代碼分析node
#include <ros/ros.h> #include <string.h> #include <tf/transform_listener.h> #include <pcl_ros/transforms.h> #include <laser_geometry/laser_geometry.h> #include <pcl/ros/conversions.h> #include <pcl/point_cloud.h> #include <pcl/point_types.h> #include <pcl/io/pcd_io.h> #include <sensor_msgs/PointCloud.h> #include <sensor_msgs/PointCloud2.h> #include <sensor_msgs/point_cloud_conversion.h> #include "sensor_msgs/LaserScan.h" #include "pcl_ros/point_cloud.h" #include <Eigen/Dense> #include <dynamic_reconfigure/server.h> #include <ira_laser_tools/laserscan_multi_mergerConfig.h> using namespace std; //using namespace pcl; using namespace laserscan_multi_merger; class LaserscanMerger { public: LaserscanMerger(); void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan, std::string topic); void scan_to_mergedscan(const sensor_msgs::LaserScan::ConstPtr& scan); void reconfigureCallback(laserscan_multi_mergerConfig &config, uint32_t level); private: ros::NodeHandle node_; //laser_geometry::LaserProjection projector_; tf::TransformListener tfListener_; //ros::Publisher point_cloud_publisher_; ros::Publisher laser_scan_publisher_; vector<ros::Subscriber> scan_subscribers; //vector<bool> clouds_modified; //vector<pcl::PCLPointCloud2> clouds; vector<string> input_topics; void laserscan_topic_parser(); double angle_min; double angle_max; double angle_increment; double time_increment; double scan_time; double range_min; double range_max; string destination_frame; //string cloud_destination_topic; string scan_destination_topic; string laserscan_topics; }; // // void LaserscanMerger::reconfigureCallback(laserscan_multi_mergerConfig &config, uint32_t level) { this->angle_min = config.angle_min; this->angle_max = config.angle_max; this->angle_increment = config.angle_increment; this->time_increment = config.time_increment; this->scan_time = config.scan_time; this->range_min = config.range_min; this->range_max = config.range_max; } 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)//i表明laserscan_topics 的個數 { for(int j=0;j<topics.size();++j)//j表明全部topics的個數 { 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());//將這些主題全都改爲這兩個。 cout <<"Subscribing to topics:"<<' '; 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);//發佈/merged_cloud laser_scan_publisher_ = node_.advertise<sensor_msgs::LaserScan> (scan_destination_topic.c_str(), 1, false);//發佈scan_destination_topic 即/scan也是新output tfListener_.setExtrapolationLimit(ros::Duration(0.1));//??????????????? } void LaserscanMerger::scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan, std::string topic)//參數是scan 和 topic { sensor_msgs::PointCloud tmpCloud1,tmpCloud2; sensor_msgs::PointCloud2 tmpCloud3; // Verify that TF knows how to transform from the received scan to the destination scan frame tfListener_.waitForTransform(scan->header.frame_id.c_str(), destination_frame.c_str(), scan->header.stamp, ros::Duration(1)); projector_.transformLaserScanToPointCloud(scan->header.frame_id, *scan, tmpCloud1, tfListener_);//將激光雷達轉換成點雲 try { tfListener_.transformPointCloud(destination_frame.c_str(), tmpCloud1, tmpCloud2);//將激光雷達轉換成的點雲tmpCloud1經過tf轉換到merge_link併發布爲tmpCloud2 }catch (tf::TransformException ex){ROS_ERROR("%s",ex.what());return;} for(int i=0; i<input_topics.size(); ++i) { if(topic.compare(input_topics[i]) == 0) { sensor_msgs::convertPointCloudToPointCloud2(tmpCloud2,tmpCloud3);//將PointCloud類型的tmpCloud2轉換成PointCloud2類型的tmpCloud3 pcl_conversions::toPCL(tmpCloud3, clouds[i]); clouds_modified[i] = true; } } // Count how many scans we have int totalClouds = 0; for(int i=0; i<clouds_modified.size(); ++i) if(clouds_modified[i]) ++totalClouds; // Go ahead only if all subscribed scans have arrived if(totalClouds == clouds_modified.size()) { pcl::PCLPointCloud2 merged_cloud = clouds[0]; clouds_modified[0] = false; for(int i=1; i<clouds_modified.size(); ++i) { pcl::concatenatePointCloud(merged_cloud, clouds[i], merged_cloud); clouds_modified[i] = false; } point_cloud_publisher_.publish(merged_cloud);//發佈merged_cloud主題 Eigen::MatrixXf points;//表示任意大小的元素類型爲float型的矩陣變量,其大小隻有在運行時被賦值以後才能知道。 getPointCloudAsEigen(merged_cloud,points);//獲得merged_cloud主題將points按照矩陣的特徵向量形式輸出。 pointcloud_to_laserscan(points, &merged_cloud, scan);//這裏調用的是下面這個函數 }//input是cloud2類型的output是scan,上面這三個就把point_cloud輸出成laserscan了。 } void LaserscanMerger::scan_to_mergedscan(const sensor_msgs::LaserScan::ConstPtr& scan) { //將結合的merged_cloud轉換成output,並將output發佈出去。這個函數是經過括號裏的這些參數來實現函數名這個功能 sensor_msgs::LaserScanPtr output(new sensor_msgs::LaserScan());//新輸出一個output名稱的scan對象指針 //output->header = pcl_conversions::fromPCL(merged_cloud->header);//pcl轉換 output->header.frame_id = destination_frame.c_str(); output->header.stamp = scan->header.stamp; //fixes #265 output->angle_min = this->angle_min; output->angle_max = this->angle_max; output->angle_increment = this->angle_increment; output->time_increment = this->time_increment; output->scan_time = this->scan_time; output->range_min = this->range_min; output->range_max = this->range_max; uint32_t ranges_size = std::ceil((output->angle_max - output->angle_min) / output->angle_increment);//計算出掃描的個數有多少個 output->ranges.assign(ranges_size, output->range_max + 1.0);//ranges就是激光雷達掃描的點。它的範圍是比range_max範圍內的點還多一米的範圍。 for(int i=0; i<points.cols(); i++)//矩陣的列數 { const float &x = points(0,i); const float &y = points(1,i); const float &z = points(2,i);//一共三行i列 if ( std::isnan(x) || std::isnan(y) || std::isnan(z) )//若是含有非零值則對,若是是0則錯。就是有0的點都排除 { ROS_DEBUG("rejected for nan in point(%f, %f, %f)\n", x, y, z); continue; } double range_sq = y*y+x*x; double range_min_sq_ = output->range_min * output->range_min; if (range_sq < range_min_sq_) { ROS_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range_sq, range_min_sq_, x, y, z); continue;//拒絕小於範圍的點,都排除 } double angle = atan2(y, x); if (angle < output->angle_min || angle > output->angle_max) { ROS_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, output->angle_min, output->angle_max); continue;//不在範圍內的點全排除 } int index = (angle - output->angle_min) / output->angle_increment;//點的索引 if (output->ranges[index] * output->ranges[index] > range_sq) output->ranges[index] = sqrt(range_sq); } laser_scan_publisher_.publish(output);//將output發佈出去 } int main(int argc, char** argv) { ros::init(argc, argv, "laser_multi_merger"); LaserscanMerger _laser_merger; dynamic_reconfigure::Server<laserscan_multi_mergerConfig> server; dynamic_reconfigure::Server<laserscan_multi_mergerConfig>::CallbackType f; f = boost::bind(&LaserscanMerger::reconfigureCallback,&_laser_merger, _1, _2); server.setCallback(f); ros::spin(); return 0; }//也就是說整個程序是先按點雲形式結合,而後再將點雲轉化成scan