laserscan_multi_merger代码解析

laserscan_multi_merger是基于ROS的将多个激光雷达传感器扫描的topic整合成一个topic的工具。

先上代码，然后一句一句解读

#include

#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include  
#include "sensor_msgs/LaserScan.h"
#include "pcl_ros/point_cloud.h"
#include
#include
#include


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 pointcloud_to_laserscan(Eigen::MatrixXf points, pcl::PCLPointCloud2 *merged_cloud, 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 scan_subscribers;
    vector clouds_modified;


    vector clouds;
    vector 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);
vector tokens;
copy(istream_iterator(iss), istream_iterator(), back_inserter >(tokens));


vector tmp_input_topics;


for(int i=0;i {
       for(int j=0;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);
}
}
}


sort(tmp_input_topics.begin(),tmp_input_topics.end());
std::vector::iterator last = std::unique(tmp_input_topics.begin(), tmp_input_topics.end());
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[i].shutdown();


input_topics = tmp_input_topics;
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 {
                scan_subscribers[i] = node_.subscribe (input_topics[i].c_str(), 1, boost::bind(&LaserscanMerger::scanCallback,this, _1, input_topics[i]));
clouds_modified[i] = false;
cout << input_topics[i] << " ";
}
}
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 (cloud_destination_topic.c_str(), 1, false);
laser_scan_publisher_ = node_.advertise (scan_destination_topic.c_str(), 1, false);


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


void LaserscanMerger::scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan, std::string 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);
}catch (tf::TransformException ex){ROS_ERROR("%s",ex.what());return;}


for(int i=0; i {
if(topic.compare(input_topics[i]) == 0)
{
sensor_msgs::convertPointCloudToPointCloud2(tmpCloud2,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 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 {
pcl::concatenatePointCloud(merged_cloud, clouds[i], merged_cloud);
clouds_modified[i] = false;
}

point_cloud_publisher_.publish(merged_cloud);


Eigen::MatrixXf points;
getPointCloudAsEigen(merged_cloud,points);


pointcloud_to_laserscan(points, &merged_cloud, scan);
}
}


void LaserscanMerger::pointcloud_to_laserscan(Eigen::MatrixXf points, pcl::PCLPointCloud2 *merged_cloud, const sensor_msgs::LaserScan::ConstPtr& scan)
{
sensor_msgs::LaserScanPtr output(new sensor_msgs::LaserScan());
output->header = pcl_conversions::fromPCL(merged_cloud->header);
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);


for(int i=0; i {
const float &x = points(0,i);
const float &y = points(1,i);
const float &z = points(2,i);


if ( std::isnan(x) || std::isnan(y) || std::isnan(z) )
{
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);
}


int main(int argc, char** argv)
{
ros::init(argc, argv, "laser_multi_merger");


    LaserscanMerger _laser_merger;


    dynamic_reconfigure::Server server;
    dynamic_reconfigure::Server::CallbackType f;


    f = boost::bind(&LaserscanMerger::reconfigureCallback,&_laser_merger, _1, _2);
server.setCallback(f);


ros::spin();


return 0;
}