Revision 5d7ba19c

View differences:

examples/meka/src/mekajointinterface.cpp
143 143 
    p.positions.push_back(joint_target[ID_NECK_PAN] * M_PI / 180.0);
144 144 

  		




  145
  145
  
    
    p.time_from_start = ros::Duration(1.2 * 1.0 / humotion::server::Server::MOTION_UPDATERATE);


  		




  146
  
  
    
    p.time_from_start = ros::Duration(1.2 * 1.0 / humotion::server::Server::MOTION_UPDATERATE);


  		




  147
  146
  
    

  		




  148
  147
  
    
    msg.points.push_back(p);


  		




  149
  148
  
    

  		












  

    
      include/humotion/server/controller.h
    
  





  46
  46
  
    

  		




  47
  47
  
    
    void set_gaze_target(GazeState s);


  		




  48
  48
  
    
    void set_mouth_target(MouthState s);


  		




  
  49
  
    
    void set_activated(void);


  		




  49
  50
  
    

  		




  50
  51
  
    
private:


  		




  
  52
  
    
    bool activated;


  		




  51
  53
  
    
    void add_motion_generator(MotionGenerator *m);


  		




  52
  54
  
    
    GazeState relative_gaze_to_absolute_gaze(GazeState relative);


  		




  53
  55
  
    

  		












  

    
      src/server/controller.cpp
    
  





  37
  37
  
    
using namespace humotion::server;


  		




  38
  38
  
    

  		




  39
  39
  
    
//! constructor


  		




  40
  
  
    
Controller::Controller(JointInterface *j){


  		




  
  40
  
    
Controller::Controller(JointInterface *j) : activated(false) {


  		




  41
  41
  
    
    joint_interface = j;


  		




  42
  42
  
    
}


  		




  43
  43
  
    

  		




  ......




  121
  121
  
    
    return absolute_gaze;


  		




  122
  122
  
    
}


  		




  123
  123
  
    

  		




  124
  
  
    

  		




  
  124
  
    
//! activate controller


  		




  
  125
  
    
void Controller::set_activated(void){


  		




  
  126
  
    
    activated = true;


  		




  
  127
  
    
}


  		




  125
  128
  
    

  		




  126
  129
  
    
//! update gaze target:


  		




  127
  130
  
    
//! \param GazeState with target values for the overall gaze


  		




  128
  131
  
    
void Controller::set_gaze_target(GazeState new_gaze_target){


  		




  
  132
  
    
    if (!activated){


  		




  
  133
  
    
        //not yet initialized, ignore incoming targets


  		




  
  134
  
    
        return;


  		




  
  135
  
    
    }


  		




  
  136
  
    

  		




  129
  137
  
    
    GazeState target_gaze;


  		




  130
  138
  
    

  		




  131
  139
  
    
    //relative or absolute gaze update?


  		




  ......




  146
  154
  
    
//! update mouth state:


  		




  147
  155
  
    
//! \param MouthState with target values for the mouth joints


  		




  148
  156
  
    
void Controller::set_mouth_target(MouthState s){


  		




  
  157
  
    
    if (!activated){


  		




  
  158
  
    
        //not yet initialized, ignore incoming targets


  		




  
  159
  
    
        return;


  		




  
  160
  
    
    }


  		




  
  161
  
    

  		




  149
  162
  
    
    for(motion_generator_vector_t::iterator it = motion_generator_vector.begin(); it<motion_generator_vector.end(); it++){


  		




  150
  163
  
    
        (*it)->set_mouth_target(s);


  		




  151
  164
  
    
    }


  		












  

    
      src/server/middleware_ros.cpp
    
  





  37
  37
  
    
using namespace humotion::server;


  		




  38
  38
  
    

  		




  39
  39
  
    
//! constructor


  		




  40
  
  
    
MiddlewareROS::MiddlewareROS(string scope, Controller *c) : Middleware(scope, c){


  		




  41
  
  
    
    printf("> using ROS middleware\n");


  		




  
  40
  
    
MiddlewareROS::MiddlewareROS(string scope, Controller *c)


  		




  
  41
  
    
    : Middleware(scope, c){


  		




  42
  42
  
    

  		




  
  43
  
    
    printf("> using ROS middleware\n");


  		




  43
  44
  
    
    //start ros core?


  		




  44
  45
  
    
    if (ros::isInitialized()){


  		




  45
  46
  
    
        //oh another process is doing ros comm, fine, we do not need to call it


  		




  ......




  104
  105
  
    

  		




  105
  106
  
    
//! callback to handle incoming gaze  target


  		




  106
  107
  
    
void MiddlewareROS::incoming_gaze_target(const humotion::gaze::ConstPtr& msg){


  		




  107
  
  
    
    ///printf("> incoming gaze_target [P:%3.1f, T:%3.1f, R:%3.1f] %s\n", msg->pan, msg->tilt, msg->roll, msg->type==humotion::gaze::ABSOLUTE?"ABSOLUTE":"RELATIVE");


  		




  108
  108
  
    
    GazeState gaze_state;


  		




  
  109
  
    
    ///printf("> incoming gaze_target [P:%3.1f, T:%3.1f, R:%3.1f] %s\n", msg->pan, msg->tilt, msg->roll, msg->type==humotion::gaze::ABSOLUTE?"ABSOLUTE":"RELATIVE");


  		




  109
  110
  
    

  		




  110
  111
  
    
    gaze_state.type = msg->type;


  		




  111
  112
  
    
    gaze_state.pan  = msg->pan;


  		












  

    
      src/server/server.cpp
    
  





  100
  100
  
    

  		




  101
  101
  
    
    printf("> one loop = %3.1fms\n",loop_delay);


  		




  102
  102
  
    

  		




  
  103
  
    
    printf("> waiting for incoming joint states\n");


  		




  
  104
  
    

  		




  103
  105
  
    
    //wait for incoming joint data:


  		




  104
  106
  
    
    while (middleware->ok()){


  		




  105
  107
  
    
        //mw tick


  		




  ......




  122
  124
  
    
    }


  		




  123
  125
  
    

  		




  124
  126
  
    
    printf("> joint data arrived, control loop active.\n");


  		




  
  127
  
    
    controller->set_activated();


  		




  125
  128
  
    

  		




  126
  129
  
    
    //fine, data is arriving, activate and run control loop:


  		




  127
  130
  
    
    while (middleware->ok()){


  		












Also available in:  Unified diff