test.cc

#include "stage.hh"
using namespace Stg;

class Robot {
    private:
        Model* model;
        ModelRanger* ranger;
    public:
        Robot(Model* model);
        static int update(Model* model, Robot* robot);
        static int updateRanger(ModelRanger* model, Robot* robot);
        void Enable(Stg::Model* model);
};

inline Robot::Robot(Model* model)
:model(model),
//ranger( (ModelRanger*)model->GetChild( "ranger:0" ))
ranger((ModelRanger*)model->GetUnusedModelOfType( "ranger" ))

{
    std::cout << "Enter decleration" << std::endl;
    //Robot* nBot = this;
    model->AddCallback( Model::CB_UPDATE, (model_callback_t)update, this );
    //ranger->AddCallback( Model::CB_UPDATE, (model_callback_t)updateRanger, this );
    std::cout << "before model subscribe" << std::endl;
    model->Subscribe();
    std::cout << "after model subscribe" << std::endl;
    //Enable((Stg::Model*)ranger);
    std::cout << "before ranger subscribe" << std::endl;
    ranger->Subscribe();
    std::cout << "after ranger subscribe" << std::endl;
}

inline int Robot::update(Model* model, Robot* robot)
{
    //robot->ranger->Subscribe();
    std::cout << "Enter update function" << std::endl;

    //const std::vector<ModelRanger::Sensor>& scan = robot->ranger->GetSensors();
    const std::vector< meters_t > & scan = robot->ranger->GetRanges();

    uint32_t sample_count = scan.size();
    std::cout << sample_count << std::endl;
/**
    for (int i=0; i<scan.size(); i++)
    {
        std::cout << "sesor" << i << ": " << std::endl;
        std::cout << scan[i] << std::endl;
        std::cout << std::endl;
    }
**/
    std::cout << "exit update function" << std::endl;
    return 0;
}

inline int Robot::updateRanger(ModelRanger* model, Robot* robot)
{
    return 0;
}
inline void Robot::Enable( Stg::Model* model)
{
    model->Subscribe();
}
extern "C" int Init( Model* mod, CtrlArgs* args )
{
    std::cout << "In Init" << std::endl;
  new Robot((ModelPosition*)mod);

  return 0; //ok
}