Newbie question on working with pcl_ros
I'm used to ROS/python, but due to performance requirements I'm writing a node in C++ for filtering pointcloud data. Specifically, I'm inputting a sensor_msgs/PointCloud2
message using pcl_ros
, checking if they originate from a "occupied" point in a occupancy grid and publishing "occupied" and "free" points on separate topics. It also transforms the points into the same frame as the occupancy grid.
All the work is being done in a callback function for the input topic. Initially, I wanted to make it optionally to transform the points, so therefore I attempted to make a separate pointer points_in
referring to either the raw points or the transformed points. The code sort of works, but sometimes the points used in the filtering part seems to have very high values (etc. x=-27e41) - which made me think that I've done something stupid, e.g. letting the underlying data be manipulated by something in a way.
After some trial and error/tutorial copying I've made it work by replacing const PointCloud *points_in
with const PointCloud::Ptr points_in (new PointCloud);
. The problem is that I don't really understand what caused the problem - could anyone explain me what I did wrong, or point me in a direction? I realize that this may be a trivial thing that I probably should have learned in a tutorial somewhere, but I really appreciate any pointers!
Original callback function - not working:
void pointcloud_cb(const PointCloud::ConstPtr& msg) {
ROS_INFO_STREAM("Received cloud " << *msg);
ros::Time trans_begin = ros::Time::now();
const PointCloud *points_in;
// Transform points
if (_transform_to_map_frame_id) {
PointCloud transformed_points;
std::string map_frame_id = "piren";//TODO: Check frame stuff //_occupancy_grid.getMapFrameId();
ROS_INFO_STREAM("Looking for transform from " << msg->header.frame_id << " to " << map_frame_id);
_tf_listener->waitForTransform(map_frame_id, msg->header.frame_id, pcl_conversions::fromPCL((*msg).header.stamp), ros::Duration(0.05));
if (!pcl_ros::transformPointCloud<PointType>(map_frame_id, *msg, transformed_points, *_tf_listener)) {
ROS_WARN_STREAM_THROTTLE(5.0,
"Failed to lookup transform, discarding points.");
}
points_in = &transformed_points;
}
else {
points_in = &(*msg);
}
_points_transformed_pub.publish(*points_in);
// Filter points
ros::Time filter_begin = ros::Time::now();
int occupied = 0;
PointCloud::Ptr free_points (new PointCloud);
PointCloud::Ptr occupied_points (new PointCloud);
BOOST_FOREACH (const PointType& pt, points_in->points){
//TODO: Check frame stuff: //if (_occupancy_grid.point_occupied<PointType>(points_in->header.frame_id, pt)) {
if (_occupancy_grid.point_occupied<PointType>("piren_ENU", pt)) {
// Point is occupied
++occupied;
if (_publish_occupied){occupied_points->points.push_back(pt);}
}
else{
//Point is free
free_points->points.push_back(pt);
}
}
ros::Time filter_end = ros::Time::now();
// Headers
free_points->height = 1;
free_points->width = points_in->size() - occupied;
free_points->header = points_in->header;
_points_free_pub.publish(free_points);
if (_publish_occupied){
occupied_points->height = 1;
occupied_points->width = occupied;
occupied_points->header=points_in->header;
_points_occupied_pub.publish(occupied_points);
}
ROS_INFO_STREAM("Received " << msg->size() << " points. " <<
"trans_dur: " << filter_begin - trans_begin << "s, " <<
"filt_dur: " << filter_end - filter_begin << "s, " <<
"points occupied: " << occupied <<
"(" << points_in->size() - occupied << " points free).");
}
New callback function - working:
void pointcloud_cb(const PointCloud::ConstPtr& msg) {
ROS_INFO_STREAM("Received cloud " << *msg);
ros::Time trans_begin = ros::Time::now();
const PointCloud::Ptr points_in (new PointCloud);
// Transform points
if (_transform_to_map_frame_id) {
std::string map_frame_id = "piren";//TODO: Check frame stuff //_occupancy_grid.getMapFrameId();
ROS_INFO_STREAM("Looking for transform from " << msg->header.frame_id << " to " << map_frame_id);
_tf_listener->waitForTransform(map_frame_id, msg->header.frame_id, pcl_conversions::fromPCL ...