Ask Your Question
0

Passing pcl::shared_ptr to subscriber (ROS2 Foxy)

asked 2021-09-21 04:53:31 -0600

morten gravatar image

I am trying to write a pointcloud filtering node in ROS2 foxy using pcl but I am running into some problems I think are resulting from the publishing.

The resulting error is

~/ros2_ws$ colcon build --symlink-install --packages-select pointcloud_filters
Starting >>> pointcloud_filters
--- stderr: pointcloud_filters                                
** WARNING ** io features related to pcap will be disabled
** WARNING ** io features related to png will be disabled
** WARNING ** io features related to libusb-1.0 will be disabled
/usr/bin/ld: CMakeFiles/pointcloud_filters_node.dir/src/pointcloud_filters_node.cpp.o: in function `rclcpp::Publisher<pcl::PointCloud<pcl::PointXYZI>, std::allocator<void> >::Publisher(rclcpp::node_interfaces::NodeBaseInterface*, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, rclcpp::QoS const&, rclcpp::PublisherOptionsWithAllocator<std::allocator<void> > const&)':
pointcloud_filters_node.cpp:(.text._ZN6rclcpp9PublisherIN3pcl10PointCloudINS1_9PointXYZIEEESaIvEEC2EPNS_15node_interfaces17NodeBaseInterfaceERKNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEERKNS_3QoSERKNS_29PublisherOptionsWithAllocatorIS5_EE[_ZN6rclcpp9PublisherIN3pcl10PointCloudINS1_9PointXYZIEEESaIvEEC5EPNS_15node_interfaces17NodeBaseInterfaceERKNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEERKNS_3QoSERKNS_29PublisherOptionsWithAllocatorIS5_EE]+0x6e): undefined reference to `rosidl_message_type_support_t const* rosidl_typesupport_cpp::get_message_type_support_handle<pcl::PointCloud<pcl::PointXYZI> >()'
collect2: error: ld returned 1 exit status
make[2]: *** [CMakeFiles/pointcloud_filters_node.dir/build.make:317: pointcloud_filters_node] Error 1
make[1]: *** [CMakeFiles/Makefile2:78: CMakeFiles/pointcloud_filters_node.dir/all] Error 2
make: *** [Makefile:141: all] Error 2
---
Failed   <<< pointcloud_filters [12.3s, exited with code 2]

Summary: 0 packages finished [12.4s]
  1 package failed: pointcloud_filters
  1 package had stderr output: pointcloud_filters

so its complaining about some undefined reference. I'm not entirely sure what the problem is but it seems like it could be similar to https://answers.ros.org/question/3117.... I looked at the interfaces for sensor_msgs/msg/PointCloud2 and sensor_msgs/msg./PointField and it looks identical to pcl::PointCloud so maybe I should manually transfer the attributes?

CPP file:

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"

#include "pcl_conversions/pcl_conversions.h"

#include "pcl/filters/radius_outlier_removal.h"
#include "pcl/impl/point_types.hpp"
#include "pcl/point_cloud.h"

class PCFilter : public rclcpp::Node {
public:
  PCFilter() : Node("pointcloud_filters") {
    pub_ = this->create_publisher<pcl::PointCloud<pcl::PointXYZI>>(
        "filters_output", 1);
    sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
        "filters_input", 1,
        std::bind(&PCFilter::cloud_callback, this, std::placeholders::_1));

    this->declare_parameter("pointcloud_filters/neighbors", 1);
    this->declare_parameter("pointcloud_filters/radius", 0.2);
    this->declare_parameter("pointcloud_filters/distance", 5.0);
    this->declare_parameter("pointcloud_filters/intensity_threshold", 8);

    neighbors_ = this->get_parameter("pointcloud_filters/neighbors").as_int();
    radius_ = this->get_parameter("pointcloud_filters/radius").as_double();
    distance_ = this->get_parameter("pointcloud_filters/distance").as_double();
    intensity_threshold_ =
        this->get_parameter("pointcloud_filters/intensity_threshold").as_int();

    RCLCPP_INFO(this->get_logger(),
                "Filter "
                "pararms:\n\tneighbors:%d\t\nradius:%f\n\tdistance:%"
                "f\n\tintensity_threshold:%d",
                neighbors_, radius_, distance_, intensity_threshold_);
  }

private:
  void cloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(
        new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered(
        new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_dust_filtered(
        new pcl::PointCloud<pcl::PointXYZI>);

    pcl::fromROSMsg(*msg, *cloud);

    if (cloud->size() > 0) {
      // Filter dust based on low intensity within a certain distance from the
      // lidar
      double dist = 0.0;
      for (std::size_t n = 0; n < cloud->size(); n++) {
        dist = std::sqrt(std::pow(cloud->points[n].x, 2) +
                         std::pow(cloud->points[n].y, 2) +
                         std::pow(cloud->points[n].z, 2));
        if (dist < distance_) {
          if (cloud->points[n].intensity > intensity_threshold_) {
            cloud_dust_filtered->push_back(cloud->points[n]);
          }
        } else {
          cloud_dust_filtered->push_back(cloud->points[n]);
        }
      }

      cloud_dust_filtered->header.frame_id = cloud->header.frame_id;
      cloud_dust_filtered->header.stamp = cloud->header.stamp;

      // Create radius outlier removal filter
      pcl::RadiusOutlierRemoval<pcl::PointXYZI> outrem;
      outrem.setInputCloud(cloud_dust_filtered);
      outrem.setRadiusSearch(radius_);
      outrem.setMinNeighborsInRadius(neighbors_);
      // Apply filter
      outrem.filter ...
(more)
edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
0

answered 2021-09-22 07:53:06 -0600

morten gravatar image

updated 2021-09-22 08:16:10 -0600

A potential solution I have found: taking more advantage of the pcl_conversions library.

Instead of trying to publish pcl::PointCloud<pcl::PointXYZI>> utilize the pcl::toROSMsg conversion by

void cloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(
        new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered(
        new pcl::PointCloud<pcl::PointXYZI>);
    pcl::fromROSMsg(*msg, *cloud);

    pass_.setInputCloud(cloud);
    pass_.filter(*cloud_filtered);

    // new things
    sensor_msgs::msg::PointCloud2 output;
    pcl::toROSMsg(*cloud_filtered, output);

    pub_->publish(output);
}

and thereby changing the subscriber to

PCFilter() : Node("pointcloud_filters") {
    pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>>("filters_output", 1); 
...}

Note: My understanding is constantly converting with pcl::fromROSMsg and pcl::toROSMsg add unnecessary computations, looking into how to avoid this. Not so familiar with pointclouds in ROS

edit flag offensive delete link more
0

answered 2021-09-21 08:15:10 -0600

Ranjit Kathiriya gravatar image

updated 2021-09-21 08:42:03 -0600

Hello there,

I think your code is having a minor typo mistake.

PCFilter() : Node("pointcloud_filters") {
    pub_ = this->create_publisher<pcl::PointCloud<pcl::PointXYZI>>("filters_output", 1); 
    # Typo Extra >

    ...}

On this line you have added extra ">" Can you please have a look at this. <pcl::PointCloud<pcl::PointXYZI>>


Instead of that you can use: #q187588

edit flag offensive delete link more

Comments

I think the extra is necessary, because the this->create_publisher<Template> asks first for the message type, where the pcl::PointCloud<template> asks for the point type. So I think both are necessary.

morten gravatar image morten  ( 2021-09-21 08:26:56 -0600 )edit

You should probably look at this solution. #q187588. Your error is of publishing line you can use this alternative provided in an answer of this question.

Ranjit Kathiriya gravatar image Ranjit Kathiriya  ( 2021-09-21 08:33:37 -0600 )edit

This is in ros1, I don't think the same solution works here.

morten gravatar image morten  ( 2021-09-22 07:51:00 -0600 )edit

Can you please! try this I think it will work,

 typedef pcl::PointCloud<pcl::PointXYZI> PCLCloud;
 graspub.publish(*grasp_points);
 ros::Publisher graspub = this->create_publisher<PCLCloud>("filters_output", 1);
Ranjit Kathiriya gravatar image Ranjit Kathiriya  ( 2021-09-22 07:57:59 -0600 )edit

Doesn't work, same error error message. Doesn't make sense either that the error source should be having a full contained template definition with an extra layer.

I replied with an alternative, working solution I found. It may not be the most effective however.

morten gravatar image morten  ( 2021-09-22 08:12:05 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2021-09-21 04:53:31 -0600

Seen: 49 times

Last updated: Sep 22 '21