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

Fix multithreaded lazy pub sub

Browse Source
This commit is contained in:
Paul Bovbel 2015-01-22 09:21:45 -05:00 committed by Paul Bovbel
parent e739e0f297
commit 040f68c31f
2 changed files with 77 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -46,7 +46,6 @@
#include "nodelet/nodelet.h" #include "nodelet/nodelet.h"
#include "tf2_ros/buffer.h" #include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/message_filter.h" #include "tf2_ros/message_filter.h"
#include "message_filters/subscriber.h" #include "message_filters/subscriber.h"
#include "sensor_msgs/PointCloud2.h" #include "sensor_msgs/PointCloud2.h"
@ -54,7 +53,6 @@
namespace pointcloud_to_laserscan namespace pointcloud_to_laserscan
{ {
typedef message_filters::Subscriber<sensor_msgs::PointCloud2> FilteredSub;
typedef tf2_ros::MessageFilter<sensor_msgs::PointCloud2> MessageFilter; typedef tf2_ros::MessageFilter<sensor_msgs::PointCloud2> MessageFilter;
/** /**
* Class to process incoming pointclouds into laserscans. Some initial code was pulled from the defunct turtlebot * Class to process incoming pointclouds into laserscans. Some initial code was pulled from the defunct turtlebot
@ -82,8 +80,7 @@ namespace pointcloud_to_laserscan
boost::mutex connect_mutex_; boost::mutex connect_mutex_;
tf2_ros::Buffer tf2_; tf2_ros::Buffer tf2_;
tf2_ros::TransformListener tf2_listener_; message_filters::Subscriber<sensor_msgs::PointCloud2> sub_;
boost::shared_ptr<FilteredSub> sub_;
boost::shared_ptr<MessageFilter> message_filter_; boost::shared_ptr<MessageFilter> message_filter_;
// ROS Parameters // ROS Parameters

131
pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp
View File

@ -47,25 +47,22 @@
namespace pointcloud_to_laserscan namespace pointcloud_to_laserscan
{ {
PointCloudToLaserScanNodelet::PointCloudToLaserScanNodelet() PointCloudToLaserScanNodelet::PointCloudToLaserScanNodelet() {}
: tf2_(), tf2_listener_(tf2_)
{ }
void PointCloudToLaserScanNodelet::onInit() void PointCloudToLaserScanNodelet::onInit()
{ {
nh_ = getMTNodeHandle(); boost::mutex::scoped_lock lock(connect_mutex_);
private_nh_ = getMTPrivateNodeHandle(); private_nh_ = getPrivateNodeHandle();
private_nh_.param<std::string>("target_frame", target_frame_, ""); private_nh_.param<std::string>("target_frame", target_frame_, "");
private_nh_.param<double>("tolerance", tolerance_, 0.01); private_nh_.param<double>("tolerance", tolerance_, 0.01);
private_nh_.param<double>("min_height", min_height_, 0.0); private_nh_.param<double>("min_height", min_height_, 0.0);
private_nh_.param<double>("max_height", max_height_, 1.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_min", angle_min_, -M_PI / 2.0);
private_nh_.param<double>("angle_max", angle_max_, 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>("angle_increment", angle_increment_, M_PI / 360.0);
private_nh_.param<double>("scan_time", scan_time_, 1.0/30.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_min", range_min_, 0.45);
private_nh_.param<double>("range_max", range_max_, 4.0); private_nh_.param<double>("range_max", range_max_, 4.0);
@ -73,65 +70,79 @@ namespace pointcloud_to_laserscan
private_nh_.param<int>("concurrency_level", concurrency_level, true); private_nh_.param<int>("concurrency_level", concurrency_level, true);
private_nh_.param<bool>("use_inf", use_inf_, true); private_nh_.param<bool>("use_inf", use_inf_, true);
boost::mutex::scoped_lock lock(connect_mutex_); //Check if explicitly single threaded, otherwise, let nodelet manager dictate thread pool size
if (concurrency_level == 1)
{
nh_ = getNodeHandle();
}
else
{
nh_ = getMTNodeHandle();
}
// Only queue one pointcloud per running thread // Only queue one pointcloud per running thread
if(concurrency_level > 0) if (concurrency_level > 0)
{ {
input_queue_size_ = concurrency_level; input_queue_size_ = concurrency_level;
}else{ }
else
{
input_queue_size_ = boost::thread::hardware_concurrency(); input_queue_size_ = boost::thread::hardware_concurrency();
} }
// if pointcloud target frame specified, we need to filter by transform availability
if (!target_frame_.empty())
{
message_filter_.reset(new MessageFilter(sub_, tf2_, target_frame_, input_queue_size_, nh_));
message_filter_->setTolerance(ros::Duration(tolerance_));
message_filter_->registerCallback(boost::bind(&PointCloudToLaserScanNodelet::cloudCb, this, _1));
message_filter_->registerFailureCallback(boost::bind(&PointCloudToLaserScanNodelet::failureCb, this, _1, _2));
}
else // otherwise setup direct subscription
{
sub_.registerCallback(boost::bind(&PointCloudToLaserScanNodelet::cloudCb, this, _1));
}
pub_ = nh_.advertise<sensor_msgs::LaserScan>("scan", 10, pub_ = nh_.advertise<sensor_msgs::LaserScan>("scan", 10,
boost::bind(&PointCloudToLaserScanNodelet::connectCb, this), boost::bind(&PointCloudToLaserScanNodelet::connectCb, this),
boost::bind(&PointCloudToLaserScanNodelet::disconnectCb, this)); boost::bind(&PointCloudToLaserScanNodelet::disconnectCb, this));
} }
void PointCloudToLaserScanNodelet::connectCb() void PointCloudToLaserScanNodelet::connectCb()
{ {
boost::mutex::scoped_lock lock(connect_mutex_); boost::mutex::scoped_lock lock(connect_mutex_);
if (!sub_ && pub_.getNumSubscribers() > 0) { if (pub_.getNumSubscribers() > 0 && sub_.getSubscriber().getNumPublishers() == 0)
{
NODELET_INFO("Got a subscriber to scan, starting subscriber to pointcloud"); NODELET_INFO("Got a subscriber to scan, starting subscriber to pointcloud");
sub_.reset(new FilteredSub(nh_, "cloud_in", input_queue_size_)); sub_.subscribe(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_->setTolerance(ros::Duration(tolerance_));
message_filter_->registerCallback(boost::bind(&PointCloudToLaserScanNodelet::cloudCb, this, _1));
message_filter_->registerFailureCallback(boost::bind(&PointCloudToLaserScanNodelet::failureCb, this, _1, _2));
}else{
sub_->registerCallback(boost::bind(&PointCloudToLaserScanNodelet::cloudCb, this, _1));
}
} }
} }
void PointCloudToLaserScanNodelet::failureCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg,
tf2_ros::filter_failure_reasons::FilterFailureReason reason){
NODELET_WARN_STREAM_THROTTLE(3.0, "Can't transform pointcloud from frame " << cloud_msg->header.frame_id << " to "
<< message_filter_->getTargetFramesString() << " with tolerance " << tolerance_);
}
void PointCloudToLaserScanNodelet::disconnectCb() void PointCloudToLaserScanNodelet::disconnectCb()
{ {
boost::mutex::scoped_lock lock(connect_mutex_); boost::mutex::scoped_lock lock(connect_mutex_);
if (pub_.getNumSubscribers() == 0) { if (pub_.getNumSubscribers() == 0)
{
NODELET_INFO("No subscibers to scan, shutting down subscriber to pointcloud"); NODELET_INFO("No subscibers to scan, shutting down subscriber to pointcloud");
if(!target_frame_.empty()) sub_.unsubscribe();
{
message_filter_.reset();
}
sub_.reset();
} }
} }
void PointCloudToLaserScanNodelet::failureCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg,
tf2_ros::filter_failure_reasons::FilterFailureReason reason)
{
NODELET_WARN_STREAM_THROTTLE(1.0, "Can't transform pointcloud from frame " << cloud_msg->header.frame_id << " to "
<< message_filter_->getTargetFramesString() << " with tolerance " << tolerance_);
}
void PointCloudToLaserScanNodelet::cloudCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg) void PointCloudToLaserScanNodelet::cloudCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg)
{ {
//build laserscan output //build laserscan output
sensor_msgs::LaserScan output; sensor_msgs::LaserScan output;
output.header = cloud_msg->header; output.header = cloud_msg->header;
if(!target_frame_.empty()){ if (!target_frame_.empty())
{
output.header.frame_id = target_frame_; output.header.frame_id = target_frame_;
} }
@ -147,9 +158,12 @@ namespace pointcloud_to_laserscan
uint32_t ranges_size = std::ceil((output.angle_max - output.angle_min) / output.angle_increment); 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 //determine if laserscan rays with no obstacle data will evaluate to infinity or max_range
if(use_inf_){ if (use_inf_)
{
output.ranges.assign(ranges_size, std::numeric_limits<double>::infinity()); output.ranges.assign(ranges_size, std::numeric_limits<double>::infinity());
}else{ }
else
{
output.ranges.assign(ranges_size, output.range_max + 1.0); output.ranges.assign(ranges_size, output.range_max + 1.0);
} }
@ -157,7 +171,7 @@ namespace pointcloud_to_laserscan
sensor_msgs::PointCloud2Ptr cloud; sensor_msgs::PointCloud2Ptr cloud;
// Transform cloud if necessary // Transform cloud if necessary
if(!(output.header.frame_id == cloud_msg->header.frame_id)) if (!(output.header.frame_id == cloud_msg->header.frame_id))
{ {
try try
{ {
@ -170,43 +184,50 @@ namespace pointcloud_to_laserscan
NODELET_ERROR_STREAM("Transform failure: " << ex.what()); NODELET_ERROR_STREAM("Transform failure: " << ex.what());
return; return;
} }
}else{ }
else
{
cloud_out = cloud_msg; cloud_out = cloud_msg;
} }
// Iterate through pointcloud // Iterate through pointcloud
sensor_msgs::PointCloud2ConstIterator<float> iter_x(*cloud_out, "x"); for (sensor_msgs::PointCloud2ConstIterator<float>
sensor_msgs::PointCloud2ConstIterator<float> iter_y(*cloud_out, "y"); iter_x(*cloud_out, "x"), iter_y(*cloud_out, "y"), iter_z(*cloud_out, "z");
sensor_msgs::PointCloud2ConstIterator<float> iter_z(*cloud_out, "z"); iter_x != iter_x.end();
++iter_x, ++iter_y, ++iter_z)
{
for(; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z){ if (std::isnan(*iter_x) || std::isnan(*iter_y) || std::isnan(*iter_z))
{
if ( std::isnan(*iter_x) || std::isnan(*iter_y) || std::isnan(*iter_z) ){
NODELET_DEBUG("rejected for nan in point(%f, %f, %f)\n", *iter_x, *iter_y, *iter_z); NODELET_DEBUG("rejected for nan in point(%f, %f, %f)\n", *iter_x, *iter_y, *iter_z);
continue; continue;
} }
if (*iter_z > max_height_ || *iter_z < min_height_){ if (*iter_z > max_height_ || *iter_z < min_height_)
{
NODELET_DEBUG("rejected for height %f not in range (%f, %f)\n", *iter_z, min_height_, max_height_); NODELET_DEBUG("rejected for height %f not in range (%f, %f)\n", *iter_z, min_height_, max_height_);
continue; continue;
} }
double range = hypot(*iter_x,*iter_y); double range = hypot(*iter_x, *iter_y);
if (range < range_min_){ if (range < range_min_)
{
NODELET_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range, range_min_, *iter_x, *iter_y, NODELET_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range, range_min_, *iter_x, *iter_y,
*iter_z); *iter_z);
continue; continue;
} }
double angle = atan2(*iter_y, *iter_x); double angle = atan2(*iter_y, *iter_x);
if (angle < output.angle_min || angle > output.angle_max){ 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); NODELET_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, output.angle_min, output.angle_max);
continue; continue;
} }
//overwrite range at laserscan ray if new range is smaller //overwrite range at laserscan ray if new range is smaller
int index = (angle - output.angle_min) / output.angle_increment; int index = (angle - output.angle_min) / output.angle_increment;
if (range < output.ranges[index]){ if (range < output.ranges[index])
{
output.ranges[index] = range; output.ranges[index] = range;
} }