laserscan_multi_merger代码解析
2016-03-28 20:08
316 查看
laserscan_multi_merger是基于ROS的将多个激光雷达传感器扫描的topic整合成一个topic的工具。
先上代码,然后一句一句解读
#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 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<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);
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);
}
}
}
sort(tmp_input_topics.begin(),tmp_input_topics.end());
std::vector<string>::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.size(); ++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<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] << " ";
}
}
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);
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<input_topics.size(); ++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<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);
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<points.cols(); 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<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;
}
先上代码,然后一句一句解读
#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 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<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);
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);
}
}
}
sort(tmp_input_topics.begin(),tmp_input_topics.end());
std::vector<string>::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.size(); ++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<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] << " ";
}
}
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);
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<input_topics.size(); ++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<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);
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<points.cols(); 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<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;
}
相关文章推荐
- JAVA学习笔记(三)
- 开发环境配置-eclipse插件的安装与卸载-3
- java.lang.nullpointerexceptiond的原因
- c++作业2
- qt中的多线程
- 单链表(C语言)
- c语言:循环队列的实现
- Teach Yourself Programming in Ten Years
- C++Primer第5版读书笔记(第9章)
- bzoj4377 Kurs szybkiego czytania 数学
- 共同学习Java源码--常用数据类型--String(四)
- a simple example for spring AOP
- eclipse安装SVN插件
- Spring3.2.6中事件驱动模型实现原理深入源码分析
- java 保留小数点后两位
- Windows下yaf的扩展
- Spring AOP原理——Java中的动态代理机制
- RxJava学习一:初识
- JAVA学习笔记(三)
- poj/hdu/zoj矩阵连乘题集+解题代码 矩阵28