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

Switch to tf_sensor_msgs for transform

Browse Source
This commit is contained in:
Paul Bovbel 2015-01-18 13:04:18 -05:00 committed by Paul Bovbel
parent 7295940808
commit 6aded38494
4 changed files with 25 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
pointcloud_to_laserscan/CMakeLists.txt
View File

@ -8,6 +8,7 @@ find_package(catkin REQUIRED COMPONENTS
sensor_msgs sensor_msgs
tf2 tf2
tf2_ros tf2_ros
tf2_sensor_msgs
geometry_msgs geometry_msgs
) )

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

@ -45,7 +45,7 @@
#include "boost/thread/mutex.hpp" #include "boost/thread/mutex.hpp"
#include "nodelet/nodelet.h" #include "nodelet/nodelet.h"
#include "tf2/buffer_core.h" #include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.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"

2
pointcloud_to_laserscan/package.xml
View File

@ -21,6 +21,7 @@
<build_depend>sensor_msgs</build_depend> <build_depend>sensor_msgs</build_depend>
<build_depend>tf2</build_depend> <build_depend>tf2</build_depend>
<build_depend>tf2_ros</build_depend> <build_depend>tf2_ros</build_depend>
<build_depend>tf2_sensor_msgs</build_depend>
<run_depend>message_filters</run_depend> <run_depend>message_filters</run_depend>
<run_depend>nodelet</run_depend> <run_depend>nodelet</run_depend>
@ -28,6 +29,7 @@
<run_depend>sensor_msgs</run_depend> <run_depend>sensor_msgs</run_depend>
<run_depend>tf2</run_depend> <run_depend>tf2</run_depend>
<run_depend>tf2_ros</run_depend> <run_depend>tf2_ros</run_depend>
<run_depend>tf2_sensor_msgs</run_depend>
<export> <export>
<nodelet plugin="${prefix}/nodelets.xml"/> <nodelet plugin="${prefix}/nodelets.xml"/>

50
pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp
View File

@ -43,6 +43,7 @@
#include <pluginlib/class_list_macros.h> #include <pluginlib/class_list_macros.h>
#include <sensor_msgs/point_cloud2_iterator.h> #include <sensor_msgs/point_cloud2_iterator.h>
#include <geometry_msgs/PointStamped.h> #include <geometry_msgs/PointStamped.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
namespace pointcloud_to_laserscan namespace pointcloud_to_laserscan
{ {
@ -153,61 +154,52 @@ namespace pointcloud_to_laserscan
output.ranges.assign(ranges_size, output.range_max + 1.0); output.ranges.assign(ranges_size, output.range_max + 1.0);
} }
// Pre-allocate for transformation sensor_msgs::PointCloud2ConstPtr cloud_out;
geometry_msgs::TransformStamped transform; sensor_msgs::PointCloud2Ptr cloud;
geometry_msgs::PointStamped in, out;
in.header = cloud_msg->header;
bool need_transform = !(output.header.frame_id == cloud_msg->header.frame_id); // Transform cloud if necessary
if(!(output.header.frame_id == cloud_msg->header.frame_id))
// Fetch transform if necessary
if(need_transform)
{ {
try try
{ {
transform = tf2_.lookupTransform(target_frame_, cloud_msg->header.frame_id, cloud_msg->header.stamp); cloud.reset(new sensor_msgs::PointCloud2);
tf2_.transform(*cloud_msg, *cloud, target_frame_, ros::Duration(tolerance_));
cloud_out = cloud;
} }
catch (tf2::TransformException ex) catch (tf2::TransformException ex)
{ {
NODELET_ERROR_STREAM("Transform failure: " << ex.what()); NODELET_ERROR_STREAM("Transform failure: " << ex.what());
return; return;
} }
}else{
cloud_out = cloud_msg;
} }
// Iterate through pointcloud // Iterate through pointcloud
sensor_msgs::PointCloud2ConstIterator<double> iter_x(*cloud_msg, "x"); sensor_msgs::PointCloud2ConstIterator<double> iter_x(*cloud_out, "x");
sensor_msgs::PointCloud2ConstIterator<double> iter_y(*cloud_msg, "y"); sensor_msgs::PointCloud2ConstIterator<double> iter_y(*cloud_out, "y");
sensor_msgs::PointCloud2ConstIterator<double> iter_z(*cloud_msg, "z"); sensor_msgs::PointCloud2ConstIterator<double> iter_z(*cloud_out, "z");
for(; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z){ for(; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z){
const double *x = &(*iter_x), *y = &(*iter_y), *z = &(*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);
// Do transform if necessary
if(need_transform){
in.point.x = *x; in.point.y = *y; in.point.z = *z;
tf2::doTransform(in, out, transform);
x = &out.point.x; y = &out.point.y; z = &out.point.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; continue;
} }
if (*z > max_height_ || *z < min_height_){ if (*iter_z > max_height_ || *iter_z < min_height_){
NODELET_DEBUG("rejected for height %f not in range (%f, %f)\n", *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(*x,*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_, *x, *y, NODELET_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range, range_min_, *iter_x, *iter_y,
*z); *iter_z);
continue; continue;
} }
double angle = atan2(*y, *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;