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

Print complete message received in ROS2 C++ subscriber callback

asked 2022-09-28 10:54:52 -0500

dvy gravatar image

updated 2022-09-28 11:05:49 -0500

Hi,

System used: ROS2 foxy on Ubuntu 20.04

I have a sensor_msgs::msg::PointCloud2 subscriber written in C++ (by taking reference of MinimalSubscriber example) using the below code.

#include <memory>
#include <iostream>

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

class MinimalSubscriber : public rclcpp::Node
{
  public:

    MinimalSubscriber()
    : 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); //
      std::cout << "Complete ROS cloud is: " << msg.get() << std::endl;      

    }
    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscription_;
};

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

I am a beginner to C++ and so far, I understand that msg is a SharedPtr, so it should contain the memory address where the message is received. I can print the height in pointcloud using the statement RCLCPP_INFO(this->get_logger(), "I received the message , height is: '%d'", msg->height);. std::cout << "Complete ROS cloud is: " << msg.get() << std::endl; prints the memory address.

[INFO] [1664378910.644389664] [minimal_subscriber]: I received the message , height is: '1'
Inside main function nowComplete ROS cloud is: 0x559143aafa70

I am not able to figure out a way to print the entire PointCloud2 message using std::cout or RCLCPP_INFO(this->get_logger(). I searched the internet and tried dereferencing (by putting a * before the shared pointer) so that the cout line becomes std::cout << "Complete ROS cloud is: " << *msg.get() << std::endl; but then my code editor (VSCode) gives the below error

no operator "<<" matches these operandsC/C++(349) cpp_node.cpp(23, 46): operand types are: std::basic_ostream<char, std::char_traits<char>> << sensor_msgs::msg::PointCloud2_<std::allocator<void>>

Please suggest a way to print the entire message in C++ subscriber (either cout/ RCLCPP_INFO/ something else).

Thanks in advance, Dev

edit retag flag offensive close merge delete

Comments

I am not able to figure out a way to print the entire PointCloud2 ...

Why do you want to print a PointCloud2? It will definitely make the node terribly slow at run time.

ravijoshi gravatar image ravijoshi  ( 2022-09-29 05:30:56 -0500 )edit

@ravijoshi : Yes, I know. I want to print it just for debugging/ verification purposes and to expand my C++ knowledge as well.

dvy gravatar image dvy  ( 2022-09-29 08:48:38 -0500 )edit
1

... I want to print it ...

You have to print each field of the PointCloud2 message.

... for debugging/ verification purposes ...

I would use a debugger for it. For example, gdb. But if you prefer a UI based solution, I recommend vscode with ROS extension. Simply, put a breakpoint and hover your mouse over the variable!

ravijoshi gravatar image ravijoshi  ( 2022-09-30 01:50:53 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2022-09-29 22:46:54 -0500

JustinBlack02 gravatar image

A PointCloud2 is simply a raw memory buffer, to interpret it, you need to use a PointCloud2Iterator which can be found here.

#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/point_cloud2_iterator.hpp>

void topic_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) 
{
  sensor_msgs::PointCloud2Iterator<float> iter_x(*msg, "x");
  sensor_msgs::PointCloud2Iterator<float> iter_y(*msg, "y");
  sensor_msgs::PointCloud2Iterator<float> iter_z(*msg, "z");

  for(size_t i = 0; i < msg->height * msg->width; ++i, ++iter_x, ++iter_y, ++iter_z) {
    std::cout << "coords: " << *iter_x << ", " << *iter_y << ", " << *iter_z << '\n';
  }
}

That being said, printing to console is a terrible way of debugging. I highly recommend you learn to use a debugger.

edit flag offensive delete link more

Comments

@JustinBlack02 and @ravijoshi : Thanks for the guidance. I will try to follow it.

dvy gravatar image dvy  ( 2022-09-30 09:25:30 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2022-09-28 10:54:52 -0500

Seen: 1,914 times

Last updated: Sep 29 '22