Revision ea068cf1 src/server/server.cpp

View differences:

src/server/server.cpp
46 46 
    printf("> initializing humotion server (on %s, middleware=%s)\n", scope.c_str(), mw.c_str());
47 47 

  		




  48
  48
  
    
    // store pointer to joint interface


  		




  49
  
  
    
    joint_interface = _joint_interface;


  		




  
  49
  
    
    joint_interface_ = _joint_interface;


  		




  50
  50
  
    

  		




  51
  51
  
    
    // tell joint interface our framerate


  		




  52
  
  
    
    joint_interface->set_framerate(MOTION_UPDATERATE);


  		




  
  52
  
    
    joint_interface_->set_framerate(MOTION_UPDATERATE);


  		




  53
  53
  
    

  		




  54
  54
  
    
    // create controller


  		




  55
  
  
    
    controller = new Controller(joint_interface);


  		




  56
  
  
    
    controller->init_motion_generators();


  		




  
  55
  
    
    controller_ = new Controller(joint_interface_);


  		




  
  56
  
    
    controller_->init_motion_generators();


  		




  57
  57
  
    

  		




  58
  58
  
    
    // start middleware


  		




  59
  59
  
    
    if (mw == "ROS") {


  		




  60
  
  
    
        middleware = new MiddlewareROS(scope, controller);


  		




  
  60
  
    
        middleware_ = new MiddlewareROS(scope, controller_);


  		




  61
  61
  
    
    } else {


  		




  62
  62
  
    
        printf("> ERROR: invalid mw '%s' given. RSB support was dropped. Please use ROS\n\n",


  		




  63
  63
  
    
               mw.c_str());


  		




  ......




  74
  74
  
    

  		




  75
  75
  
    
//! middleware still running?


  		




  76
  76
  
    
bool Server::ok() {


  		




  77
  
  
    
    return middleware->ok();


  		




  
  77
  
    
    return middleware_->ok();


  		




  78
  78
  
    
}


  		




  79
  79
  
    

  		




  80
  80
  
    
//! start main thread


  		




  81
  81
  
    
void Server::start_motion_generation_thread() {


  		




  82
  
  
    
    motion_generation_thread_ptr = new boost::thread(boost::bind(


  		




  
  82
  
    
    motion_generation_thread_ptr_ = new boost::thread(boost::bind(


  		




  83
  83
  
    
                                                         &Server::motion_generation_thread, this));


  		




  84
  84
  
    
}


  		




  85
  85
  
    

  		




  ......




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


  		




  99
  99
  
    

  		




  100
  100
  
    
    // wait for incoming joint data


  		




  101
  
  
    
    while (middleware->ok()) {


  		




  
  101
  
    
    while (middleware_->ok()) {


  		




  102
  102
  
    
        // mw tick


  		




  103
  
  
    
        middleware->tick();


  		




  
  103
  
    
        middleware_->tick();


  		




  
  104
  
    

  		




  
  105
  
    
        unsigned int incoming_data_count =


  		




  
  106
  
    
                joint_interface_->get_and_clear_incoming_position_count();


  		




  104
  107
  
    

  		




  105
  
  
    
        unsigned int incoming_data_count = joint_interface->get_and_clear_incoming_position_count();


  		




  106
  108
  
    
        if (incoming_data_count == 0) {


  		




  107
  109
  
    
            incoming_data_count_invalid++;


  		




  108
  110
  
    
            if (incoming_data_count_invalid >= MOTION_UPDATERATE) {


  		




  ......




  119
  121
  
    
    }


  		




  120
  122
  
    

  		




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


  		




  122
  
  
    
    controller->set_activated();


  		




  
  124
  
    
    controller_->set_activated();


  		




  123
  125
  
    

  		




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


  		




  125
  
  
    
    while (middleware->ok()) {


  		




  
  127
  
    
    while (middleware_->ok()) {


  		




  126
  128
  
    
        // mw tick


  		




  127
  
  
    
        middleware->tick();


  		




  
  129
  
    
        middleware_->tick();


  		




  128
  130
  
    

  		




  129
  131
  
    
        // do some logging


  		




  130
  132
  
    
        // controller->dump_angles();


  		




  131
  133
  
    

  		




  132
  134
  
    
        // calculate all targets


  		




  133
  
  
    
        controller->calculate_targets();


  		




  
  135
  
    
        controller_->calculate_targets();


  		




  134
  136
  
    

  		




  135
  137
  
    
        // publish data to joints


  		




  136
  
  
    
        controller->publish_targets();


  		




  
  138
  
    
        controller_->publish_targets();


  		




  137
  139
  
    

  		




  138
  140
  
    
        // finally execute the movement


  		




  139
  
  
    
        joint_interface->execute_motion();


  		




  
  141
  
    
        joint_interface_->execute_motion();


  		




  140
  142
  
    

  		




  141
  143
  
    
        // check if we received joint information in the last step


  		




  142
  
  
    
        unsigned int incoming_data_count = joint_interface->get_and_clear_incoming_position_count();


  		




  
  144
  
    
        unsigned int incoming_data_count =


  		




  
  145
  
    
                joint_interface_->get_and_clear_incoming_position_count();


  		




  
  146
  
    

  		




  143
  147
  
    
        if (incoming_data_count == 0) {


  		




  144
  148
  
    
            // printf("> WARNING: no incoming joint data during the last iteration...\n");


  		




  145
  149
  
    
            incoming_data_count_invalid++;


  		












Also available in:  Unified diff