1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46
| void Node::LaunchSubscribers(const std::string& scan_topic, const int scan_num, const std::string& odom_topic, const int use_odom, const std::string& imu_topic, const int use_imu ) { if(scan_num) LaunchLaserScanSub(scan_topic, scan_num); if(use_odom) LaunchOdomSub(odom_topic, use_odom); if(use_imu) LaunchImuSub(imu_topic, use_imu); }
void Node::LaunchLaserScanSub(const std::string& scan_topic, const int scan_num) { subscribers_.push_back( { SubscribeWithHandler<sensor_msgs::LaserScan>( &Node::HandleLaserScanMessage, scan_num, scan_topic, &node_handle_, this), scan_topic} ); }
template <typename MessageType> ::ros::Subscriber SubscribeWithHandler( void (Node::*handler)(int, const std::string&, const typename MessageType::ConstPtr&), const int num, const std::string& topic, ::ros::NodeHandle* const node_handle, Node* const node) { return node_handle->subscribe<MessageType>( topic, 1, boost::function<void(const typename MessageType::ConstPtr&)>( [node, handler, num, topic](const typename MessageType::ConstPtr& msg) { (node->*handler)(num, topic, msg); }) ); }
void Node::HandleLaserScanMessage(int scan_num, const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) { ROS_INFO("range_min: %f, scan_num: %d", msg->range_min, scan_num ); }
|