Print complete message received in ROS2 C++ subscriber callback
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
Asked by dvy on 2022-09-28 10:54:52 UTC
Answers
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.
Asked by JustinBlack02 on 2022-09-29 22:46:54 UTC
Comments
@JustinBlack02 and @ravijoshi : Thanks for the guidance. I will try to follow it.
Asked by dvy on 2022-09-30 09:25:30 UTC
Comments
Why do you want to print a
PointCloud2
? It will definitely make the node terribly slow at run time.Asked by ravijoshi on 2022-09-29 05:30:56 UTC
@ravijoshi : Yes, I know. I want to print it just for debugging/ verification purposes and to expand my C++ knowledge as well.
Asked by dvy on 2022-09-29 08:48:38 UTC
You have to print each field of the
PointCloud2
message.I would use a debugger for it. For example,
gdb
. But if you prefer a UI based solution, I recommendvscode
with ROS extension. Simply, put a breakpoint and hover your mouse over the variable!Asked by ravijoshi on 2022-09-30 01:50:53 UTC