0
$\begingroup$

Rosanswers logo

System used: ROS2 foxy on Ubuntu 20.04 with PCL 1.10 installed on amd64 architecture.

I have created a C++ subscriber for ROS2 topic velodyne_points of the message type sensor_msgs::msg::PointCloud2. Since, this is ROS2 message and PCL can't handle it. So, I first convert it to something (pcl::PCLPointCloud2 cloud;) which PCL can understand using pcl_conversions::toPCL(*msg, cloud); where msg is the pointer to ROS message and cloud contains the converted PCL data. Then, I want to apply PCL's voxelGrid functionality. Since voxelGrid.setInputCloud(cloudPtr); takes the argument as a pointer(the const boost shared pointer to a PointCloud message), I create one which points to PCL "cloud" using pcl::PCLPointCloud2::Ptr cloudPtr (&cloud);, process it further and save the result in a memory location pointed by cloud_filtered. But as soon as program reaches the end of this subscriber callback function, it gives an error double free or corruption (out) on the terminal and execution stops (No error in compilation, only in execution).

I tried to understand this error but I am not trying to free some pointer explicitly which is already free. This issue happens because of the line pcl::PCLPointCloud2::Ptr cloudPtr (&cloud); because when I tried commenting this line and the voxelization steps, execution worked well. But I need to do voxelization and some more steps after it is done (ground plane segmentation, clustering etc.).

I am trying to build ROS2 package and run the executable using this command : clear && colcon build --packages-select custom_nav_stack_pkg --symlink-install && source install/local_setup.bash && ros2 run custom_nav_stack_pkg cpp_executable which looks fine to me.

Here is the code.

#include <memory>
#include <iostream>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/conversions.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/visualization/cloud_viewer.h>
#include <boost/foreach.hpp>
#include <pcl/filters/voxel_grid.h>

class MinimalSubscriber : public rclcpp::Node
{
  public:
    MinimalSubscriber()    //Constructor which has the same name as that of class followed by a parentheses. A constructor in C++ is a special method that is automatically called when an object of a class is created. 
    : Node("minimal_subscriber")
    {
      subscription_ = this->create_subscription<sensor_msgs::msg::PointCloud2>
      ("velodyne_points", 10, std::bind(&MinimalSubscriber::topic_callback, this, std::placeholders::_1));
    }

  private:
    void topic_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) const
    {
      RCLCPP_INFO(this->get_logger(), "I received the message , height is: '%d'", msg->height); //
      pcl::PCLPointCloud2 cloud;
      pcl_conversions::toPCL(*msg, cloud);

      RCLCPP_INFO(this->get_logger(), "I received the PCL message , height is: '%d'", cloud.height);
     
      std::cerr << "PointCloud before filtering: " << cloud.width * cloud.height 
             << " data points (" << pcl::getFieldsList (cloud) << ")." << std::endl;

      
      // Declaring pointer for the PCL "cloud" 
      pcl::PCLPointCloud2::Ptr cloudPtr (&cloud);

      // define a new container for the data
      pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());

      // define a voxelgrid
      pcl::VoxelGrid<pcl::PCLPointCloud2> voxelGrid;
      // set input to cloud
      voxelGrid.setInputCloud(cloudPtr);
      // set the leaf size (x, y, z)
      voxelGrid.setLeafSize(0.1, 0.1, 0.1);
      // apply the filter to dereferenced cloudVoxel
      voxelGrid.filter(*cloud_filtered);   

      std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height 
       << " data points (" << pcl::getFieldsList (*cloud_filtered) << ")." << std::endl;

      std::cin.ignore();
    }
    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscription_;

};

int main(int argc, char * argv[])
{
  std::cout << "Inside main function now" << std::endl;
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<MinimalSubscriber>());
  rclcpp::shutdown();
  return 0;
}

Running it gives me below output.

Starting >>> custom_nav_stack_pkg
Finished <<< custom_nav_stack_pkg [16.4s]                       

Summary: 1 package finished [16.7s]
Inside main function now
[INFO] [1664396894.528685765] [minimal_subscriber]: I received the message , height is: '1'
[INFO] [1664396894.528876950] [minimal_subscriber]: I received the PCL message , height is: '1'
PointCloud before filtering: 15000 data points (x y z intensity ring time).
PointCloud after filtering: 5937 data points (x y z intensity ring time).

double free or corruption (out)

Originally posted by dvy on ROS Answers with karma: 52 on 2022-09-28

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

There are a few changes needed to fix the error. The following should work:

void topic_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg_ptr) const
{
  pcl::PCLPointCloud2::Ptr cloud_in_ptr(new pcl::PCLPointCloud2);
  pcl_conversions::toPCL(*msg_ptr, *cloud_in_ptr);

  RCLCPP_INFO_STREAM(get_logger(), "[Input PointCloud] width " << cloud_in_ptr->width << " height " << cloud_in_ptr->height);

  pcl::PCLPointCloud2::Ptr cloud_out_ptr(new pcl::PCLPointCloud2);
  pcl::voxelGrid<pcl::PCLPointCloud2> voxel_grid;
  voxel_grid.setInputCloud(cloud_in_ptr);
  voxel_grid.setLeafSize(0.1, 0.1, 0.1);
  voxel_grid.filter(*cloud_out_ptr);

  RCLCPP_INFO_STREAM(get_logger(), "[Output PointCloud] width " << cloud_out_ptr->width << " height " << cloud_out_ptr->height);
}

Originally posted by ravijoshi with karma: 1744 on 2022-09-29

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by dvy on 2022-09-29:
@rajivjoshi : Thanks a lot! It worked like a charm. By the way, can you please also explain the reason for the failure and the fix?

Comment by ravijoshi on 2022-09-29:
I am glad it worked. Notice that you are calling the class directly. Pay attention to the () after PCLPointCloud2 . The pcl::PCLPointCloud2::Ptr is a smart pointer, so you just need to provide the class name only, i.e., without (). Next, the voxelGrid filter requires smart pointer as input, so I created it at the beginning and used it inside pcl_conversions::toPCL. Notice the dereferencing of pointer here.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.