ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Newbie question on working with pcl_ros

asked 2020-08-21 09:51:13 -0600

bjoernolav gravatar image

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 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
2

answered 2020-08-22 10:49:10 -0600

fergs gravatar image

This really isn't a PCL thing, it's C++ and pointers. The issue with your original code is these lines:

const PointCloud *points_in;
if (blah) {
    PointCloud transformed_points;
    points_in = &transformed_points;
}

transformed_points is a local variable, so it is created on the stack. When you hit the end of the if() block, it goes out of scope - your pointer is now pointing at an invalid address. When you access it, you'll corrupt your stack!

In your newer code, you create a shared_ptr and assign it storage with the "new" command:

const PointCloud::Ptr points_in (new PointCloud);

This is on the heap, not the stack, and so it continues to be valid later on.

edit flag offensive delete link more

Comments

Thanks a lot for your answer! It really helped understanding what was going on :)

bjoernolav gravatar image bjoernolav  ( 2020-08-23 02:02:02 -0600 )edit

Question Tools

2 followers

Stats

Asked: 2020-08-21 09:51:13 -0600

Seen: 359 times

Last updated: Aug 22 '20