Revision ea068cf1 src/server/middleware_ros.cpp

View differences:

src/server/middleware_ros.cpp
42 42 
    // start ros core?
43 43 
    if (ros::isInitialized()) {
44 44 
        // oh another process is doing ros comm, fine, we do not need to call it
45 
        tick_necessary = false;
45 
        tick_necessary_ = false;
46 46 
    } else {
47 47 
        // we have to take care of ros
48 48 
        printf("> no active ros middleware, calling ros::init() "
......
52 52 
        printf("> registering on ROS as node %s\n", node_name.c_str());
53 53 
        ros::M_string no_remapping;
54 54 
        ros::init(no_remapping, node_name);
55 
        tick_necessary = true;
55 
        tick_necessary_ = true;
56 56 
    }
57 57 

  		




  58
  58
  
    
    // create node handle


  		




  59
  59
  
    
    ros::NodeHandle n;


  		




  
  60
  
    
    ros::NodeHandle pnh("~");


  		




  60
  61
  
    

  		




  61
  62
  
    
    // set up subscribers


  		




  62
  
  
    
    mouth_target_subscriber = n.subscribe(scope + "/humotion/mouth/target", 150,


  		




  
  63
  
    
    mouth_target_subscriber_ = n.subscribe(scope + "/humotion/mouth/target", 150,


  		




  63
  64
  
    
                                          &MiddlewareROS::incoming_mouth_target ,


  		




  64
  65
  
    
                                          this, ros::TransportHints().unreliable());


  		




  65
  
  
    
    gaze_target_subscriber  = n.subscribe(scope + "/humotion/gaze/target",  150,


  		




  
  66
  
    
    gaze_target_subscriber_  = n.subscribe(scope + "/humotion/gaze/target",  150,


  		




  66
  67
  
    
                                          &MiddlewareROS::incoming_gaze_target,


  		




  67
  68
  
    
                                          this, ros::TransportHints().unreliable());


  		




  
  69
  
    

  		




  
  70
  
    
    // set up dynamic reconfiguration service


  		




  
  71
  
    
    attach_to_reconfiguration_server(pnh);


  		




  
  72
  
    
}


  		




  
  73
  
    

  		




  
  74
  
    
//! callback for incoming dynamic reconfigure requests:


  		




  
  75
  
    
void MiddlewareROS::dynamic_reconfigure_callback(const humotion::humotionConfig &config,


  		




  
  76
  
    
                                                  uint32_t level) {


  		




  
  77
  
    
  /*// ignore incoming requests as long cam is not set up properly


  		




  
  78
  
    
  if (!hasValidHandle()) {


  		




  
  79
  
    
      return;


  		




  
  80
  
    
  }


  		




  
  81
  
    

  		




  
  82
  
    
  // use some tricks to iterate through all config entries:


  		




  
  83
  
    
  std::vector<ximea_camera::xiAPIConfig::AbstractParamDescriptionConstPtr>::const_iterator _i;


  		




  
  84
  
    
  for (_i = config.__getParamDescriptions__().begin();


  		




  
  85
  
    
       _i != config.__getParamDescriptions__().end(); ++_i) {


  		




  
  86
  
    
      boost::any val;


  		




  
  87
  
    
      boost::shared_ptr<const ximea_camera::xiAPIConfig::AbstractParamDescription>


  		




  
  88
  
    
              description = *_i;


  		




  
  89
  
    

  		




  
  90
  
    
      // fetch actual value:


  		




  
  91
  
    
      description->getValue(config, val);


  		




  
  92
  
    

  		




  
  93
  
    
      //  std::cout << description->name << " " << description->type << "\n";


  		




  
  94
  
    

  		




  
  95
  
    
      // copy data to ximea api:


  		




  
  96
  
    
      if (description->type == "double") {


  		




  
  97
  
    
          dynamicReconfigureFloat(description->name.c_str(),


  		




  
  98
  
    
                                    static_cast<float>(boost::any_cast<double>(val)));


  		




  
  99
  
    
      } else if (description->type == "bool") {


  		




  
  100
  
    
          dynamicReconfigureInt(description->name.c_str(), (boost::any_cast<bool>(val))?1:0);


  		




  
  101
  
    
      } else if (description->type == "int") {


  		




  
  102
  
    
          dynamicReconfigureInt(description->name.c_str(), boost::any_cast<int>(val));


  		




  
  103
  
    
      } else {


  		




  
  104
  
    
          std::cerr << "ERROR: unsupported config type " << description->type  << "\n";


  		




  
  105
  
    
      }


  		




  
  106
  
    
    }*/


  		




  
  107
  
    
}


  		




  
  108
  
    

  		




  
  109
  
    
//! attach to dynamic reconfigure server


  		




  
  110
  
    
void MiddlewareROS::attach_to_reconfiguration_server(ros::NodeHandle priv_nodehandle) {


  		




  
  111
  
    
    ROS_DEBUG("connecting to dynamic reconfiguration server");


  		




  
  112
  
    
    ros::NodeHandle reconf_node(priv_nodehandle, "configuration");


  		




  
  113
  
    

  		




  
  114
  
    
    reconf_server_ = new dynamic_reconfigure::Server<humotion::humotionConfig>(reconf_node);


  		




  
  115
  
    
    reconf_server_->setCallback(boost::bind(


  		




  
  116
  
    
                                    &MiddlewareROS::dynamic_reconfigure_callback, this, _1, _2));


  		




  68
  117
  
    
}


  		




  69
  118
  
    

  		




  70
  119
  
    
//! destructor


  		




  ......




  79
  128
  
    

  		




  80
  129
  
    
//! do a single tick


  		




  81
  130
  
    
void MiddlewareROS::tick() {


  		




  82
  
  
    
    if (tick_necessary) {


  		




  
  131
  
    
    if (tick_necessary_) {


  		




  83
  132
  
    
        ros::spinOnce();


  		




  84
  133
  
    
    }


  		




  85
  134
  
    
}


  		




  ......




  97
  146
  
    
    mouth_state.opening_center = msg->opening.center;


  		




  98
  147
  
    
    mouth_state.opening_right  = msg->opening.right;


  		




  99
  148
  
    

  		




  100
  
  
    
    controller->set_mouth_target(mouth_state);


  		




  
  149
  
    
    controller_->set_mouth_target(mouth_state);


  		




  101
  150
  
    
}


  		




  102
  151
  
    

  		




  103
  152
  
    

  		




  ......




  134
  183
  
    

  		




  135
  184
  
    
    gaze_state.timestamp.set(msg->gaze_timestamp.sec, msg->gaze_timestamp.nsec);


  		




  136
  185
  
    

  		




  137
  
  
    
    controller->set_gaze_target(gaze_state);


  		




  
  186
  
    
    controller_->set_gaze_target(gaze_state);


  		




  138
  187
  
    
}


  		




  139
  188
  
    

  		












Also available in:  Unified diff