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

Refactor to allow debug messages from node and nodelet

Browse Source
This commit is contained in:
Paul Bovbel 2015-01-15 12:03:52 -05:00 committed by Paul Bovbel
parent d604c43332
commit eafe961c0c
8 changed files with 209 additions and 253 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
pointcloud_to_laserscan/CMakeLists.txt
View File

@ -23,7 +23,7 @@ include_directories(
${catkin_INCLUDE_DIRS}
)
add_library(pointcloud_to_laserscan src/pointcloud_to_laserscan_base.cpp src/pointcloud_to_laserscan_nodelet.cpp)
add_library(pointcloud_to_laserscan src/pointcloud_to_laserscan_nodelet.cpp)
target_link_libraries(pointcloud_to_laserscan ${catkin_LIBRARIES})
add_executable(pointcloud_to_laserscan_node src/pointcloud_to_laserscan_node.cpp)

40
pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_base.h → pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h
View File

@ -38,8 +38,8 @@
* Author: Paul Bovbel
*/
#ifndef POINTCLOUD_TO_LASERSCAN_ROS
#define POINTCLOUD_TO_LASERSCAN_ROS
#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>
@ -50,49 +50,45 @@
#include <pcl/ros/conversions.h>
#include <tf/transform_listener.h>
#include <nodelet/nodelet.h>
namespace pointcloud_to_laserscan
{
typedef pcl::PointXYZ Point;
typedef pcl::PointCloud<Point> PointCloud;
{
typedef pcl::PointXYZ Point;
typedef pcl::PointCloud<Point> PointCloud;
/**
* Class to process incoming pointclouds into laserscans. Some initial code was pulled from the defunct turtlebot
* pointcloud_to_laserscan implementation.
*/
class PointCloudToLaserScanBase
{
public:
class PointCloudToLaserScanNodelet : public nodelet::Nodelet
{
PointCloudToLaserScanBase(ros::NodeHandle& nh, ros::NodeHandle& private_nh);
~PointCloudToLaserScanBase();
public:
PointCloudToLaserScanNodelet() { };
private:
private:
virtual void onInit();
void cloudCb(const PointCloud::ConstPtr& cloud);
void cloudCb(const PointCloud::ConstPtr &cloud_msg);
void connectCb();
void disconnectCb();
ros::NodeHandle nh_;
ros::NodeHandle private_nh_;
ros::NodeHandle nh_, private_nh_;
ros::Publisher pub_;
ros::Subscriber sub_;
tf::TransformListener tf_;
boost::mutex connect_mutex_;
// ROS Parameters
unsigned int input_queue_size_;
std::string target_frame_;
double min_height_, max_height_, angle_min_, angle_max_, angle_increment_, scan_time_, range_min_, range_max_;
bool use_inf_;
};
};
} // pointcloud_to_laserscan
} // pointcloud_to_laserscan
#endif
#endif // POINTCLOUD_TO_LASERSCAN_POINTCLOUD_TO_LASERSCAN_NODELET

23
pointcloud_to_laserscan/launch/sample_node.launch
View File

@ -12,12 +12,27 @@
<!-- run pointcloud_to_laserscan node -->
<node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan">
<param name="use_inf" value="true"/>
<param name="use_concurrency" value="true"/>
<param name="target_frame" value="base_link"/>
<remap from="cloud_in" to="$(arg camera)/depth_registered/points_processed"/>
<remap from="scan" to="$(arg camera)/scan"/>
<rosparam>
target_frame: base_link
min_height: 0.0
max_height: 1.0
angle_min: -1.5708 # -M_PI/2
angle_max: 1.5708 # M_PI/2
angle_increment: 0.087 # M_PI/360.0
scan_time: 0.3333
range_min: 0.45
range_max: 4.0
use_inf: true
# Concurrency level, affects number of pointclouds queued for processing and number of threads used
# 0 : Detect number of cores
# 1 : Single threaded
# 2->inf : Parallelism level
concurrency_level: 0
</rosparam>
</node>

23
pointcloud_to_laserscan/launch/sample_nodelet.launch
View File

@ -12,12 +12,27 @@
<!-- push pointcloud_to_laserscan nodelet into sensor's nodelet manager-->
<node pkg="nodelet" type="nodelet" name="pointcloud_to_laserscan" args="load pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet $(arg camera)_nodelet_manager">
<param name="use_inf" value="true"/>
<param name="use_concurrency" value="true"/>
<param name="target_frame" value="base_link"/>
<remap from="cloud_in" to="$(arg camera)/depth_registered/points_processed"/>
<remap from="scan" to="$(arg camera)/scan"/>
<rosparam>
target_frame: base_link
min_height: 0.0
max_height: 1.0
angle_min: -1.5708 # -M_PI/2
angle_max: 1.5708 # M_PI/2
angle_increment: 0.087 # M_PI/360.0
scan_time: 0.3333
range_min: 0.45
range_max: 4.0
use_inf: true
# Concurrency level, affects number of pointclouds queued for processing, thread number governed by nodelet manager
# 0 : Detect number of cores
# 1 : Single threaded
# 2->inf : Parallelism level
concurrency_level: 0
</rosparam>
</node>

2
pointcloud_to_laserscan/package.xml
View File

@ -14,6 +14,7 @@
<url type="repository">https://github.com/ros-perception/perception_pcl</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>dynamic_reconfigure</build_depend>
<build_depend>libpcl-all-dev</build_depend>
<build_depend>message_filters</build_depend>
@ -22,6 +23,7 @@
<build_depend>roscpp</build_depend>
<build_depend>roslaunch</build_depend>
<build_depend>sensor_msgs</build_depend>
<run_depend>dynamic_reconfigure</run_depend>
<run_depend>libpcl-all-dev</run_depend>
<run_depend>message_filters</run_depend>

186
pointcloud_to_laserscan/src/pointcloud_to_laserscan_base.cpp
View File

@ -1,186 +0,0 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2010-2012, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*
*/
/*
* Author: Paul Bovbel
*/
#include <pointcloud_to_laserscan/pointcloud_to_laserscan_base.h>
#include <sensor_msgs/LaserScan.h>
#include <pcl_ros/transforms.h>
#include <math.h>
namespace pointcloud_to_laserscan
{
PointCloudToLaserScanBase::PointCloudToLaserScanBase(ros::NodeHandle& nh, ros::NodeHandle& private_nh) :
nh_(nh), private_nh_(private_nh)
{
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);
bool use_concurrency;
private_nh_.param<bool>("use_concurrency", use_concurrency, true);
private_nh_.param<bool>("use_inf", use_inf_, true);
boost::mutex::scoped_lock lock(connect_mutex_);
//Only allow #threads pointclouds to wait in queue
if(use_concurrency){
input_queue_size_ = boost::thread::hardware_concurrency();
}else{
input_queue_size_ = 1;
}
pub_ = nh.advertise<sensor_msgs::LaserScan>("scan", 10,
boost::bind(&PointCloudToLaserScanBase::connectCb, this),
boost::bind(&PointCloudToLaserScanBase::disconnectCb, this));
}
PointCloudToLaserScanBase::~PointCloudToLaserScanBase(){
sub_.shutdown();
}
void PointCloudToLaserScanBase::connectCb() {
boost::mutex::scoped_lock lock(connect_mutex_);
if (!sub_ && pub_.getNumSubscribers() > 0) {
ROS_DEBUG("Connecting to depth topic.");
sub_ = nh_.subscribe("cloud_in", input_queue_size_, &PointCloudToLaserScanBase::cloudCb, this);
}
}
void PointCloudToLaserScanBase::disconnectCb() {
boost::mutex::scoped_lock lock(connect_mutex_);
if (pub_.getNumSubscribers() == 0) {
ROS_DEBUG("Unsubscribing from depth topic.");
sub_.shutdown();
}
}
void PointCloudToLaserScanBase::cloudCb(const PointCloud::ConstPtr& cloud_msg){
//pointer to pointcloud data to transform to laserscan
PointCloud::ConstPtr cloud_scan;
std_msgs::Header cloud_header = pcl_conversions::fromPCL(cloud_msg->header);
//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_;
//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_;
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{
ROS_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;
}
//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);
}
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) ){
//ROS_DEBUG("rejected for nan in point(%f, %f, %f)\n", x, y, z);
continue;
}
if (z > max_height_ || z < min_height_){
//ROS_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_){
//ROS_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){
//ROS_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);
}
} // pointcloud_to_laserscan

33
pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp
View File

@ -39,24 +39,27 @@
*/
#include <ros/ros.h>
#include <pointcloud_to_laserscan/pointcloud_to_laserscan_base.h>
#include <nodelet/loader.h>
int main(int argc, char **argv){
ros::init(argc, argv, "pointcloud_to_laserscan");
ros::NodeHandle nh;
ros::NodeHandle private_nh("~");
bool use_concurrency;
ros::init(argc, argv, "pointcloud_to_laserscan_node");
ros::NodeHandle private_nh("~");
int concurrency_level;
private_nh.param<int>("concurrency_level", concurrency_level, 0);
private_nh.param<bool>("use_concurrency", use_concurrency, false);
nodelet::Loader nodelet;
nodelet::M_string remap(ros::names::getRemappings());
nodelet::V_string nargv;
std::string nodelet_name = ros::this_node::getName();
nodelet.load(nodelet_name, "pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet", remap, nargv);
pointcloud_to_laserscan::PointCloudToLaserScanBase node(nh, private_nh);
boost::shared_ptr<ros::MultiThreadedSpinner> spinner;
if(concurrency_level) {
spinner.reset(new ros::MultiThreadedSpinner(concurrency_level));
}else{
spinner.reset(new ros::MultiThreadedSpinner());
}
spinner->spin();
return 0;
if(use_concurrency) {
ros::MultiThreadedSpinner spinner;
spinner.spin();
}else{
ros::spin();
}
return 0;
}

153
pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp
View File

@ -38,39 +38,150 @@
* Author: Paul Bovbel
*/
#include <pointcloud_to_laserscan/pointcloud_to_laserscan_base.h>
#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>
namespace pointcloud_to_laserscan
{
class PointCloudToLaserScanNodelet : public nodelet::Nodelet
{
public:
PointCloudToLaserScanNodelet() {};
void PointCloudToLaserScanNodelet::onInit()
{
nh_ = getMTPrivateNodeHandle();
private_nh_ = getMTPrivateNodeHandle();
~PointCloudToLaserScanNodelet() {}
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:
virtual void onInit()
{
bool use_concurrency;
getPrivateNodeHandle().param<bool>("use_concurrency", use_concurrency, false);
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);
if(use_concurrency){
cloud_to_scan.reset(new PointCloudToLaserScanBase(getMTNodeHandle(), getPrivateNodeHandle()));
}else{
cloud_to_scan.reset(new PointCloudToLaserScanBase(getNodeHandle(), getPrivateNodeHandle()));
}
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::shared_ptr<PointCloudToLaserScanBase> cloud_to_scan;
};
// 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));
}
void PointCloudToLaserScanNodelet::cloudCb(const PointCloud::ConstPtr &cloud_msg)
{
//pointer to pointcloud data to transform to laserscan
PointCloud::ConstPtr cloud_scan;
std_msgs::Header cloud_header = pcl_conversions::fromPCL(cloud_msg->header);
//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_;
//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_;
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;
}
//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);
}
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;
}
}
pub_.publish(output);
}
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);
}
}
void PointCloudToLaserScanNodelet::disconnectCb()
{
boost::mutex::scoped_lock lock(connect_mutex_);
if (pub_.getNumSubscribers() == 0) {
NODELET_DEBUG("Unsubscribing from depth topic.");
sub_.shutdown();
}
}
}
#include <pluginlib/class_list_macros.h>
PLUGINLIB_DECLARE_CLASS(pointcloud_to_laserscan, PointCloudToLaserScanNodelet, pointcloud_to_laserscan::PointCloudToLaserScanNodelet, nodelet::Nodelet);