对LaserscanMerger::laserscan_topic_parser()的代码分析

void LaserscanMerger::laserscan_topic_parser()

{

// LaserScan topics to subscribe

ros::master::V_TopicInfo topics;

ros::master::getTopics(topics);

istringstream iss(laserscan_topics);//获取两个激光雷达的topic

vector<string> tokens;

copy(istream_iterator<string>(iss), istream_iterator<string>(), back_inserter<vector<string> >(tokens));

vector<string> tmp_input_topics;

for(int i=0;i<tokens.size();++i)

{

for(int j=0;j<topics.size();++j)

{

if( (tokens[i].compare(topics[j].name) == 0) && (topics[j].datatype.compare("sensor_msgs/LaserScan") == 0) )

{

tmp_input_topics.push_back(topics[j].name);

}

}

}//从所有topic里选出两个激光雷达的topic并赋给tmp_input_topics

sort(tmp_input_topics.begin(),tmp_input_topics.end());//按大小调整一下顺序；

std::vector<string>::iterator last = std::unique(tmp_input_topics.begin(), tmp_input_topics.end());//去掉重复的；比如有两个topic同名就去掉一个

tmp_input_topics.erase(last, tmp_input_topics.end());

// Do not re-subscribe if the topics are the same

if( (tmp_input_topics.size() != input_topics.size()) || !equal(tmp_input_topics.begin(),tmp_input_topics.end(),input_topics.begin()))

{

// Unsubscribe from previous topics

for(int i=0; i<scan_subscribers.size(); ++i)//如果是if这种情况，则先不从当前的主题订阅，即先把scan_subscriber关闭。

scan_subscribers[i].shutdown();

input_topics = tmp_input_topics;//再将tmp这个赋给input这个。此时的input就是laserscan_topics的两个topic了

if(input_topics.size() > 0)

{

scan_subscribers.resize(input_topics.size());

clouds_modified.resize(input_topics.size());

clouds.resize(input_topics.size());

ROS_INFO("Subscribing to topics\t%ld", scan_subscribers.size());//将这些主题全都改成这两个。

for(int i=0; i<input_topics.size(); ++i)

{

scan_subscribers[i] = node_.subscribe<sensor_msgs::LaserScan> (input_topics[i].c_str(), 1, boost::bind(&LaserscanMerger::scanCallback,this, _1, input_topics[i]));

clouds_modified[i] = false;

cout << input_topics[i] << " ";

}

}//scan_subscriber订阅input_topics的两个主题

else

ROS_INFO("Not subscribed to any topic.");

}

}

LaserscanMerger::LaserscanMerger()

{

ros::NodeHandle nh("~");

nh.getParam("destination_frame", destination_frame);

nh.getParam("cloud_destination_topic", cloud_destination_topic);

nh.getParam("scan_destination_topic", scan_destination_topic);

nh.getParam("laserscan_topics", laserscan_topics);

this->laserscan_topic_parser();

point_cloud_publisher_ = node_.advertise<sensor_msgs::PointCloud2> (cloud_destination_topic.c_str(), 1, false);

laser_scan_publisher_ = node_.advertise<sensor_msgs::LaserScan> (scan_destination_topic.c_str(), 1, false);//发布scan_destination_topic 即/scan

tfListener_.setExtrapolationLimit(ros::Duration(0.1));

}