Failing to get "time" field value using fromROSMsg() function

[alonsollorente]

Hi everyone!

I'm having a pretty annoying bug related with ROS2 LiDAR data processing with the PCL library. My goal is to just apply the fromROSMsg() function to my sensor_msgs.msg.PointCloud2 lidar data coming from some recordings I have in a rosbag.

This is the code I'm using to achieve that:

#include <rclcpp/rclcpp.hpp>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/conversions.h>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <iostream>
#include <cstring>

namespace velodyne_ros {
  struct EIGEN_ALIGN16 Point {
      float x;
      float y;
      float z;
      float intensity;
      uint16_t ring;
      uint32_t time;
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  };
}  // namespace velodyne_ros

POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point,
    (float, x, x)
    (float, y, y)
    (float, z, z)
    (float, intensity, intensity)
    (uint16_t, ring, ring)
    (uint32_t, time, time)
)

class PointCloudProcessor : public rclcpp::Node {
public:
    PointCloudProcessor() : Node("pointcloud_processor") {
        // Create a subscription to the point cloud topic
        subscription_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
            "/VLP16/velodyne_points", 
            1, 
            std::bind(&PointCloudProcessor::pointCloudCallback, this, std::placeholders::_1)
        );
    }

private:
    void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) {

        msg->fields[5].name = "time";
        msg->fields[4].name = "ring";

        RCLCPP_INFO(this->get_logger(), "----------------------------------------------------------------------------");

        // Print values from sensor_msgs.msg/PointCloud2 message from the LiDAR sensor.
        RCLCPP_INFO(this->get_logger(), "Raw PointCloud fields:");
        auto it = std::find_if(msg->fields.begin(), msg->fields.end(),
            [](const sensor_msgs::msg::PointField& field) {
                return field.name == "time";
            });

        // Print all fields and their properties
        for (const auto& field : msg->fields) {
            RCLCPP_INFO(this->get_logger(), "Field name: %s", field.name.c_str());
            RCLCPP_INFO(this->get_logger(), "  Offset: %u", field.offset);
            RCLCPP_INFO(this->get_logger(), "  Data type: %u", field.datatype);
            RCLCPP_INFO(this->get_logger(), "  Count: %u", field.count);
        }

        if (it != msg->fields.end()) {
            // Get the offset for the time_stamp field
            size_t time_stamp_offset = it->offset;

            // Access the point data
            const uint8_t* point_ptr = msg->data.data();

            // Print values directly from ROS message
            RCLCPP_INFO(this->get_logger(), "Raw point data:");
            for (size_t i = 0; i < 5; ++i) {
                // Extract field values based on offsets and types
                float x = *reinterpret_cast<const float*>(point_ptr + i * msg->point_step + 0);
                float y = *reinterpret_cast<const float*>(point_ptr + i * msg->point_step + 4);
                float z = *reinterpret_cast<const float*>(point_ptr + i * msg->point_step + 8);
                float intensity = *reinterpret_cast<const float*>(point_ptr + i * msg->point_step + 12);
                uint16_t channel = *reinterpret_cast<const uint16_t*>(point_ptr + i * msg->point_step + 16);
                double time_stamp = *reinterpret_cast<const double*>(point_ptr + i * msg->point_step + time_stamp_offset);

                // Log the values
                RCLCPP_INFO(this->get_logger(), "Point %zu: x=%f, y=%f, z=%f, intensity=%f, ring=%u, time=%f", 
                            i, x, y, z, intensity, channel, time_stamp);
            }
        } else {
            RCLCPP_INFO(this->get_logger(), "Field 'time_stamp' not found.");
        }

        // Convert ROS message to PCL PointCloud
        pcl::PointCloud<velodyne_ros::Point> pcl_cloud;
        pcl::fromROSMsg(*msg, pcl_cloud);

        // Print values from the PCL PointCloud
        RCLCPP_INFO(this->get_logger(), "PCL PointCloud data:");
        if (!pcl_cloud.empty()) {
            for (size_t i = 0; i < 5; ++i) {
                if (i < pcl_cloud.size()) { // Ensure index is within bounds
                float time = static_cast<float>(pcl_cloud.points[i].time);
                    RCLCPP_INFO(this->get_logger(), "Point %zu: x=%f, y=%f, z=%f, intensity=%f, ring=%u, time=%f", 
                                i, pcl_cloud.points[i].x, pcl_cloud.points[i].y, pcl_cloud.points[i].z, 
                                pcl_cloud.points[i].intensity, pcl_cloud.points[i].ring, time);
                }
            }
        } else {
            RCLCPP_INFO(this->get_logger(), "PCL PointCloud is empty.");
        }
    }

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

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<PointCloudProcessor>());
    rclcpp::shutdown();
    return 0;
}

As key considerations to understand the code, it's important for you to know that, originally, the name of the ring and time fields of the raw message were channel and time_stamp. That's why, before the processing of the point cloud, I wrote:

msg->fields[5].name = "time";
msg->fields[4].name = "ring";

And accordingly in the point/point cloud definition for the PCL library, I defined:

namespace velodyne_ros {
  struct EIGEN_ALIGN16 Point {
      float x;
      float y;
      float z;
      float intensity;
      uint16_t ring;
      uint32_t time;
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  };
}  // namespace velodyne_ros

POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point,
    (float, x, x)
    (float, y, y)
    (float, z, z)
    (float, intensity, intensity)
    (uint16_t, ring, ring)
    (uint32_t, time, time)
)

As you will see in the next attached figure, the time it's defined in uint32_t, since the data type is defined as number 6 (see sensor_msgs/PointField ROS2 definiton).

Executing this script while running a bag file, will end up in giving me the following output:

As you can see, all the values from the raw point cloud data are mapped into the pcl object, but the time. It is a bit confusing, since the ring values are mapped but not the time. I'm not getting any error from PCL side, so I don't know if it's something from the visualization part or something dumb that I'm missing.

If you need any further details, do not hesitate to ask!

Thanks in advance! 👍🏼

Some details about my environment:

• OS: Ubuntu 22.04LTS
• Compiler: To compile and execute I'm using a ROS2 environment with the RCLCPP library.
• PCL Version: 1.12

[mvieth]

@alonsollorente this line looks wrong: double time_stamp = *reinterpret_cast<const double*>(point_ptr + i * msg->point_step + time_stamp_offset); It basically means: "Interpret these eight bytes as a double". But if you know that time is stored as a std::uint32_t, the following would be correct: std::uint32_t time_stamp = *reinterpret_cast<const std::uint32_t*>(point_ptr + i * msg->point_step + time_stamp_offset);, and correspondingly in RCLCPP_INO: time=%u Also: this is not very robust:

msg->fields[5].name = "time";
msg->fields[4].name = "ring";

I would suggest to at least check whether the original names are time_stamp and channel, respectively.

[alonsollorente]

@mvieth Thanks for your reply! Concerning your suggestion, if I run the code as you suggested I don't get the time value even in the raw data.

Concerning your second suggestion, is true that this is not the desirable way to go, but as I cannot change the field's name in another way this was a temporary solution to try to use the fromROSMsg() function. Furthermore, the original fields are named channel and time_stamp.

[mvieth]

Are you absolutely sure that the time should be a non-zero value, or is it possible that the message indeed only contains "0" in this field for all points? Have you perhaps checked the documentation of the lidar sensor, or similar? I see that e.g. the intensity is also zero for all points, is this expected? Maybe the sensor just does not fill these two fields?

Concerning your second suggestion, is true that this is not the desirable way to go, but as I cannot change the field's name in another way this was a temporary solution to try to use the fromROSMsg() function. Furthermore, the original fields are named channel and time_stamp.

My suggestion was to do something like:

if(msg->fields.size() >= 6 && msg->fields[5].name == "time_stamp") {
    msg->fields[5].name = "time";
} else {
    // throw error
}
if(msg->fields.size() >= 5 && msg->fields[4].name == "channel") {
    msg->fields[4].name = "ring";
} else {
    // throw error
}

This would protect you if (for whatever reason):

• the fields are differently ordered than you expect
• the two fields do not exist

[alonsollorente]

I'm actually using the simulated LiDAR data from the AWSIM project. Could it be that the numbers that I was seeing were not really measurings? Why would they appear, then?

Regarding the intensity value, it is known that is zero since was configured like that in the simulation environment.

[alonsollorente]

@mvieth Problem solved! You were right and the problem was not on the function itself, but on the data side. I wasn't publishing the timestamp properly. I fixed some missing links in the simulator side and everything is working easy. Thank you for your time and help!

[mvieth]

You're welcome. Great that you found the solution.