apoorva/greenhouse
2
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Refactor with tf2 and message filters

Browse Source
This commit is contained in:
Paul Bovbel 2015-01-15 15:22:48 -05:00 committed by Paul Bovbel
parent 17db2afd94
commit 3fb881f8eb
4 changed files with 135 additions and 125 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
pointcloud_to_laserscan/CMakeLists.txt
View File

@ -2,21 +2,19 @@ cmake_minimum_required(VERSION 2.8.3)
project(pointcloud_to_laserscan)
find_package(catkin REQUIRED COMPONENTS
dynamic_reconfigure
message_filters
nodelet
pcl_ros
roscpp
sensor_msgs
roslaunch
tf2
tf2_ros
geometry_msgs
)
find_package(PCL REQUIRED)
catkin_package(
INCLUDE_DIRS include
LIBRARIES pointcloud_to_laserscan
CATKIN_DEPENDS dynamic_reconfigure libpcl-all-dev message_filters nodelet pcl_ros roscpp sensor_msgs
CATKIN_DEPENDS roscpp message_filters nodelet sensor_msgs tf2 tf2_ros
)
include_directories(

32
pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h
View File

@ -41,22 +41,21 @@
#ifndef POINTCLOUD_TO_LASERSCAN_POINTCLOUD_TO_LASERSCAN_NODELET
#define POINTCLOUD_TO_LASERSCAN_POINTCLOUD_TO_LASERSCAN_NODELET
#include <ros/ros.h>
#include <boost/thread/mutex.hpp>
#include "ros/ros.h"
#include "boost/thread/mutex.hpp"
#include <pcl/point_cloud.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/ros/conversions.h>
#include "nodelet/nodelet.h"
#include "tf2/buffer_core.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/message_filter.h"
#include "message_filters/subscriber.h"
#include "sensor_msgs/PointCloud2.h"
#include <tf/transform_listener.h>
#include <nodelet/nodelet.h>
namespace pointcloud_to_laserscan
{
typedef pcl::PointXYZ Point;
typedef pcl::PointCloud<Point> PointCloud;
typedef message_filters::Subscriber<sensor_msgs::PointCloud2> FilteredSub;
typedef tf2_ros::MessageFilter<sensor_msgs::PointCloud2> MessageFilter;
/**
* Class to process incoming pointclouds into laserscans. Some initial code was pulled from the defunct turtlebot
* pointcloud_to_laserscan implementation.
@ -65,12 +64,12 @@ namespace pointcloud_to_laserscan
{
public:
PointCloudToLaserScanNodelet() { };
PointCloudToLaserScanNodelet();
private:
virtual void onInit();
void cloudCb(const PointCloud::ConstPtr &cloud_msg);
void cloudCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg);
void connectCb();
@ -78,10 +77,13 @@ namespace pointcloud_to_laserscan
ros::NodeHandle nh_, private_nh_;
ros::Publisher pub_;
ros::Subscriber sub_;
tf::TransformListener tf_;
boost::mutex connect_mutex_;
tf2_ros::Buffer tf2_;
tf2_ros::TransformListener tf2_listener_;
boost::shared_ptr<FilteredSub> sub_;
boost::shared_ptr<MessageFilter> message_filter_;
// ROS Parameters
unsigned int input_queue_size_;
std::string target_frame_;

2
pointcloud_to_laserscan/package.xml
View File

@ -24,6 +24,7 @@
<build_depend>roslaunch</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>tf2</build_depend>
<build_depend>tf2_ros</build_depend>
<run_depend>dynamic_reconfigure</run_depend>
<run_depend>libpcl-all-dev</run_depend>
@ -33,6 +34,7 @@
<run_depend>roscpp</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>tf2</run_depend>
<run_depend>tf2_ros</run_depend>
<export>
<nodelet plugin="${prefix}/nodelets.xml"/>

218
pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp
View File

@ -39,147 +39,155 @@
*/
#include <pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h>
#include <nodelet/nodelet.h>
#include <sensor_msgs/LaserScan.h>
#include <pcl_ros/transforms.h>
#include <math.h>
#include <pluginlib/class_list_macros.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <geometry_msgs/Point32.h>
namespace pointcloud_to_laserscan
{
PointCloudToLaserScanNodelet::PointCloudToLaserScanNodelet()
: tf2_(), tf2_listener_(tf2_)
{ }
void PointCloudToLaserScanNodelet::onInit()
{
nh_ = getMTNodeHandle();
private_nh_ = getMTPrivateNodeHandle();
nh_ = getMTNodeHandle();
private_nh_ = getMTPrivateNodeHandle();
private_nh_.param<std::string>("target_frame", target_frame_, "");
private_nh_.param<double>("min_height", min_height_, 0.0);
private_nh_.param<double>("max_height", max_height_, 1.0);
private_nh_.param<std::string>("target_frame", target_frame_, "");
private_nh_.param<double>("min_height", min_height_, 0.0);
private_nh_.param<double>("max_height", max_height_, 1.0);
private_nh_.param<double>("angle_min", angle_min_, -M_PI/2.0);
private_nh_.param<double>("angle_max", angle_max_, M_PI/2.0);
private_nh_.param<double>("angle_increment", angle_increment_, M_PI/360.0);
private_nh_.param<double>("scan_time", scan_time_, 1.0/30.0);
private_nh_.param<double>("range_min", range_min_, 0.45);
private_nh_.param<double>("range_max", range_max_, 4.0);
private_nh_.param<double>("angle_min", angle_min_, -M_PI/2.0);
private_nh_.param<double>("angle_max", angle_max_, M_PI/2.0);
private_nh_.param<double>("angle_increment", angle_increment_, M_PI/360.0);
private_nh_.param<double>("scan_time", scan_time_, 1.0/30.0);
private_nh_.param<double>("range_min", range_min_, 0.45);
private_nh_.param<double>("range_max", range_max_, 4.0);
int concurrency_level;
private_nh_.param<int>("concurrency_level", concurrency_level, true);
private_nh_.param<bool>("use_inf", use_inf_, true);
int concurrency_level;
private_nh_.param<int>("concurrency_level", concurrency_level, true);
private_nh_.param<bool>("use_inf", use_inf_, true);
boost::mutex::scoped_lock lock(connect_mutex_);
boost::mutex::scoped_lock lock(connect_mutex_);
// Only queue one pointcloud per running thread
if(concurrency_level > 0)
{
input_queue_size_ = concurrency_level;
}else{
input_queue_size_ = boost::thread::hardware_concurrency();
}
// Only queue one pointcloud per running thread
if(concurrency_level > 0)
{
input_queue_size_ = concurrency_level;
}else{
input_queue_size_ = boost::thread::hardware_concurrency();
}
pub_ = nh_.advertise<sensor_msgs::LaserScan>("scan", 10,
boost::bind(&PointCloudToLaserScanNodelet::connectCb, this),
boost::bind(&PointCloudToLaserScanNodelet::disconnectCb, this));
pub_ = nh_.advertise<sensor_msgs::LaserScan>("scan", 10,
boost::bind(&PointCloudToLaserScanNodelet::connectCb, this),
boost::bind(&PointCloudToLaserScanNodelet::disconnectCb, this));
}
void PointCloudToLaserScanNodelet::connectCb()
{
boost::mutex::scoped_lock lock(connect_mutex_);
if (!sub_ && pub_.getNumSubscribers() > 0) {
NODELET_DEBUG("Connecting to depth topic.");
sub_ = nh_.subscribe("cloud_in", input_queue_size_, &PointCloudToLaserScanNodelet::cloudCb, this);
boost::mutex::scoped_lock lock(connect_mutex_);
if (!sub_ && pub_.getNumSubscribers() > 0) {
NODELET_DEBUG("Got a subscriber to laserscan, starting subscriber to point cloud");
sub_.reset(new FilteredSub(nh_, "cloud_in", input_queue_size_));
if(!target_frame_.empty())
{
message_filter_.reset(new MessageFilter(*sub_, tf2_, target_frame_, input_queue_size_, nh_));
message_filter_->registerCallback(boost::bind(&PointCloudToLaserScanNodelet::cloudCb, this, _1));
}else{
sub_->registerCallback(boost::bind(&PointCloudToLaserScanNodelet::cloudCb, this, _1));
}
}
}
void PointCloudToLaserScanNodelet::disconnectCb()
{
boost::mutex::scoped_lock lock(connect_mutex_);
if (pub_.getNumSubscribers() == 0) {
NODELET_DEBUG("Unsubscribing from depth topic.");
sub_.shutdown();
boost::mutex::scoped_lock lock(connect_mutex_);
if (pub_.getNumSubscribers() == 0) {
NODELET_DEBUG("No subscibers to laserscan, shutting down subscriber to point cloud");
if(!target_frame_.empty())
{
message_filter_.reset();
}
sub_.reset();
}
}
void PointCloudToLaserScanNodelet::cloudCb(const PointCloud::ConstPtr &cloud_msg)
void PointCloudToLaserScanNodelet::cloudCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg)
{
//pointer to pointcloud data to transform to laserscan
PointCloud::ConstPtr cloud_scan;
if(target_frame_.empty()){
target_frame_ = cloud_msg->header.frame_id;
}
std_msgs::Header cloud_header = pcl_conversions::fromPCL(cloud_msg->header);
//build laserscan output
sensor_msgs::LaserScan output;
output.header = cloud_msg->header;
output.header.frame_id = target_frame_;
output.angle_min = angle_min_;
output.angle_max = angle_max_;
output.angle_increment = angle_increment_;
output.time_increment = 0.0;
output.scan_time = scan_time_;
output.range_min = range_min_;
output.range_max = range_max_;
//build laserscan output
sensor_msgs::LaserScan output;
output.header = cloud_header;
output.angle_min = angle_min_;
output.angle_max = angle_max_;
output.angle_increment = angle_increment_;
output.time_increment = 0.0;
output.scan_time = scan_time_;
output.range_min = range_min_;
output.range_max = range_max_;
//determine amount of rays to create
uint32_t ranges_size = std::ceil((output.angle_max - output.angle_min) / output.angle_increment);
//decide if pointcloud needs to be transformed to a target frame
if(!target_frame_.empty() && cloud_header.frame_id != target_frame_){
output.header.frame_id = target_frame_;
//determine if laserscan rays with no obstacle data will evaluate to infinity or max_range
if(use_inf_){
output.ranges.assign(ranges_size, std::numeric_limits<double>::infinity());
}else{
output.ranges.assign(ranges_size, output.range_max + 1.0);
}
if(tf_.waitForTransform(cloud_header.frame_id, target_frame_, cloud_header.stamp, ros::Duration(10.0))){
PointCloud::Ptr cloud_tf(new PointCloud);
pcl_ros::transformPointCloud(target_frame_, *cloud_msg, *cloud_tf, tf_);
cloud_scan = cloud_tf;
}else{
NODELET_WARN_STREAM_THROTTLE(1.0, "Can't transform cloud with frame " << cloud_header.frame_id << " into "
"lasercan with frame " << target_frame_);
return;
}
}else{
cloud_scan = cloud_msg;
sensor_msgs::PointCloud2ConstIterator<float> iter_x(*cloud_msg, "x");
sensor_msgs::PointCloud2ConstIterator<float> iter_y(*cloud_msg, "y");
sensor_msgs::PointCloud2ConstIterator<float> iter_z(*cloud_msg, "z");
geometry_msgs::Point32 point;
for(; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z){
point.x = *iter_x;
point.y = *iter_y;
point.z = *iter_z;
if(!(output.header.frame_id == cloud_msg->header.frame_id)){
point = tf2_.transform(point, output.header.frame_id, output.header.stamp, cloud_msg->header.frame_id);
}
//determine amount of rays to create
uint32_t ranges_size = std::ceil((output.angle_max - output.angle_min) / output.angle_increment);
//determine if laserscan rays with no obstacle data will evaluate to infinity or max_range
if(use_inf_){
output.ranges.assign(ranges_size, std::numeric_limits<double>::infinity());
}else{
output.ranges.assign(ranges_size, output.range_max + 1.0);
if ( std::isnan(point.x) || std::isnan(point.y) || std::isnan(point.z) ){
NODELET_DEBUG("rejected for nan in point(%f, %f, %f)\n", point.x, point.y, point.z);
continue;
}
for (PointCloud::const_iterator it = cloud_scan->begin(); it != cloud_scan->end(); ++it){
const float &x = it->x;
const float &y = it->y;
const float &z = it->z;
if ( std::isnan(x) || std::isnan(y) || std::isnan(z) ){
NODELET_DEBUG("rejected for nan in point(%f, %f, %f)\n", x, y, z);
continue;
}
if (z > max_height_ || z < min_height_){
NODELET_DEBUG("rejected for height %f not in range (%f, %f)\n", z, min_height_, max_height_);
continue;
}
double range = hypot(x,y);
if (range < range_min_){
NODELET_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range, range_min_, x, y, z);
continue;
}
double angle = atan2(y, x);
if (angle < output.angle_min || angle > output.angle_max){
NODELET_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, output.angle_min, output.angle_max);
continue;
}
//overwrite range at laserscan ray if new range is smaller
int index = (angle - output.angle_min) / output.angle_increment;
if (range < output.ranges[index]){
output.ranges[index] = range;
}
if (point.z > max_height_ || point.z < min_height_){
NODELET_DEBUG("rejected for height %f not in range (%f, %f)\n", point.z, min_height_, max_height_);
continue;
}
pub_.publish(output);
double range = hypot(point.x,point.y);
if (range < range_min_){
NODELET_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range, range_min_, point.x, point.y, point.z);
continue;
}
double angle = atan2(point.y, point.x);
if (angle < output.angle_min || angle > output.angle_max){
NODELET_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, output.angle_min, output.angle_max);
continue;
}
//overwrite range at laserscan ray if new range is smaller
int index = (angle - output.angle_min) / output.angle_increment;
if (range < output.ranges[index]){
output.ranges[index] = range;
}
}
pub_.publish(output);
}
}