优雅地订阅话题

优雅地订阅话题 | 沉默杀手

这段代码其实是模仿cartographer的LaunchSubscribers函数，订阅多个传感器的数据话题，同时可以获得主函数里多个参数的赋值

主函数长这样

int main(int argc, char** argv)
{
    ros::init(argc, argv, "launch_subscribers");
    ros::NodeHandle nh;
    Node node;
    node.LaunchSubscribers("base_scan", 1, "/base_odometry/odom", 1, "/torso_lift_imu/data", 1);
    ros::spin();
}

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}  );
}

// 这里也可以把 int 改为 bool
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::*handler)部分，参数依次为int,  string&,  MessageType::ConstPtr&
// scan_num 就是main函数里的1
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 );
}