Error when trying to publish a PointCloud2 message from dora to ros2

[GoAfteraDeer]

I'm trying to publish PointCloud2 message form dora to ros2 but an error occurred.
Here is the log:

2024-01-18T14:51:00.830295Z  WARN opentelemetry_system_metrics: Could not initiate NVML for observing GPU
 memory usage. Error: LibloadingError(DlOpen { desc: "libnvidia-ml.so: cannot open shared object file: No s
uch file or directory" })
    at /home/runner/.cargo/registry/src/index.crates.io-6f17d22bba15001f/opentelemetry-system-metrics-0.1.6
/src/lib.rs:79

thread '<unnamed>' panicked at 'internal error: entered unreachable code: Struct array's data type is not struct!', /home/runner/.cargo/registry/src/index.crates.io-6f17d22bba15001f/arrow-array-48.0.0/src/array/struct_array.rs:254:18
note: run with `RUST_BACKTRACE=1` environment variable to display a backtrace
--- PyO3 is resuming a panic after fetching a PanicException from Python. ---
Python stack trace below:
Traceback (most recent call last):
  File "/home/userA/dora_project/lidar-message-bridge/demo_pub_lidar_data.py", line 58, in on_event
    self.lidar_data_publisher.publish(pa.array([lidar_data_dict]))
pyo3_runtime.PanicException: internal error: entered unreachable code: Struct array's data type is not struct!
Traceback (most recent call last):
  File "<string>", line 1, in <module>
RuntimeError: Dora Runtime raised an error.

Caused by:
   0: main task failed
   1: operator op panicked: Any { .. }

Location:
    binaries/runtime/src/lib.rs:175:25

and here is my code

# demo_pub_lidar_data.py

from typing import Callable, Optional
import pyarrow as pa
from dora import DoraStatus
import dora
import numpy as np

class Operator:
    def __init__(self) -> None:
        self.ros2_context = dora.experimental.ros2_bridge.Ros2Context()
        # create ros2 node
        self.ros2_node = self.ros2_context.new_node(
            "lidar2ros",
            "/ros2_bridge",
            dora.experimental.ros2_bridge.Ros2NodeOptions(rosout=True)
        )
        # create ros2 qos
        self.topic_qos = dora.experimental.ros2_bridge.Ros2QosPolicies(
            reliable=True, max_blocking_time=0.1
        )
        # create ros2 topic
        self.lidar_data_topic = self.ros2_node.create_topic(
            "/ros2_bridge/lidar_data",
            "sensor_msgs::PointCloud2",
            self.topic_qos
        )
        # create ros2 publisher
        self.lidar_data_publisher = self.ros2_node.create_publisher(self.lidar_data_topic)
    
    def on_event(
            self,
            dora_event,
            send_output,
    ) -> DoraStatus:
        if dora_event["type"] == "INPUT":
            # create a virtual PointCloud2 message for testing the programm
            points=np.array([[225.0, -71.0, 819.8],[237.0, -24.0, 816.0],[254.0, -82.0, 772.3]])
            # print(np.asarray(points, np.float32).tostring())
            lidar_data_dict = {
                "header": {
                    "stamp": {
                        "sec": np.int32(112233),
                        "nanosec": np.uint32(332211),
                    },
                    "frame_id": "lidar_frame",
                },
                "height": np.uint32(1),
                "width": np.uint32(3),
                "fields":[{"name": "x", "offset": np.uint32(0), "datatype": np.uint8(7), "count": np.uint32(1)}, 
                          {"name": "y", "offset": np.uint32(4), "datatype": np.uint8(7), "count": np.uint32(1)},
                          {"name": "z", "offset": np.uint32(8), "datatype": np.uint8(7), "count": np.uint32(1)},],
                "is_bigendian": False,
                "point_step": np.uint32(12),
                "row_step": np.uint32(36),
                "data": np.asarray(points, np.float32).tostring(),
                "is_dense": False,
            }
            # print(pa.array([lidar_data_dict]))
            self.lidar_data_publisher.publish(pa.array([lidar_data_dict]))
        return DoraStatus.CONTINUE

# demo.yml

nodes:
  - id: lidar_data_pub
    operator:
      python: demo_pub_lidar_data.py
      inputs:
        tick: dora/timer/millis/100

I think maybe the PointCloud2 message I write in the code has smoething wrong.So i annotation the data in the lidar_data_dict and let lidar_data_dict is void dict, however, the same error occurred.

[haixuanTao]

So we currently facing an issue with array type in ros2. See: dora-rs/dora#412 so this might be an issue. In the meantime can you replace:

"data": np.asarray(points, np.float32).tostring(),

with:

                "data": np.asarray(points, np.uint8).ravel(),

As string is different from uint8

[GoAfteraDeer]

Thanks for your reply.I replace my code with

                "data": np.asarray(points, np.uint8).ravel(),

But it doesn‘t seems to work, the same error still occur.

[haixuanTao]

Just for info, @phil-opp is attempting a fix.

Can you checkout:

git clone https://github.com/dora-rs/dora
git checkout ros2-type-info
maturin develop -m apis/python/node/Cargo.toml       

Thanks in advance!

P.S: Make sure to use the right type for each field.

[phil-opp]

This should be solved by dora-rs/dora#415. I just tried your code with the new serialization implementation and it worked without throwing any errors.

[GoAfteraDeer]

Thank you very much!This problem was solved successfully.

[GoAfteraDeer]

There is a new problem.The lidar data send to ros2 has uint8 datatype using the below code:

                "data": np.asarray(points, np.uint8).ravel(),

It can send the lidar data to ros2 without throwing any errors, but the sended data can't be displayed in rviz2.This is because the sended LidarData's datatype is not correct,.The LidarData's datatype should be float32 to display the LidarData in rviz2. I tried to change the datatype:

                "data": np.asarray(points, np.float32).ravel(),

but an error occured:

  2024-01-23T07:49:19.684836Z  WARN opentelemetry_system_metrics: Could not initiate NVML for observing GPU memory usage. Error: LibloadingError(DlOpen { desc: "libnvidia-ml.so: cannot open shared object file: No such file or directory" })
    at /home/wzs/.cargo/registry/src/index.crates.io-6f17d22bba15001f/opentelemetry-system-metrics-0.1.6/src/lib.rs:79

Traceback (most recent call last):
  File "<string>", line 1, in <module>
RuntimeError: Dora Runtime raised an error.

Caused by:
   0: main task failed
   1: operator lidar_data_pub/op raised an error
   2: error in Python module at /home/wzs/dora_project/lidar-message-bridge/demo_pub_lidar_data.py
   3: Traceback (most recent call last):
        File "/home/wzs/dora_project/lidar-message-bridge/demo_pub_lidar_data.py", line 58, in on_event
          self.lidar_data_publisher.publish(pa.array([lidar_data_dict]))

      RuntimeError: publish failed

      Caused by:
          Serialization error: Serde says:not a primitive arrow_array::types::UInt8Type array

      Location:
          libraries/extensions/ros2-bridge/python/src/lib.rs:222:14

Location:
    binaries/runtime/src/operator/python.rs:28:9

It seems that the float32 datatype is not supported.

[phil-opp]

The issue is that the PointCloud2 message type specifies data as uint8 array: https://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/PointCloud2.html

I'm not familiar with the message type, but the float32 data probably needs to be encoded as u8 array in some way?

the sended data can't be displayed in rviz2.This is because the sended LidarData's datatype is not correct,.The LidarData's datatype should be float32 to display the LidarData in rviz2.

Is there any error/warning message about the wrong data type? Or is this just a guess?

[haixuanTao]

It's possible that the error comes from downcasting your data to uint8. Can you replace your line with:

               "data": np.asarray(points, np.float32).ravel().view(np.uint8),

Basically you need to .view(np.uint8) https://numpy.org/doc/stable/reference/generated/numpy.ndarray.view.html your data as uint8 to interpret your float32 as uint8.

[GoAfteraDeer]

I replaced my line with:

               "data": np.asarray(points, np.float32).ravel().view(np.uint8),

It worked. The lidar data was displayed in rviz2 successfully.
About the wrong datatype,it is not my guess.The rviz show a message: "No position transformer available for cloud",and there is a disscussion about this: https://answers.ros.org/question/197309/rviz-does-not-display-pointcloud2-if-encoding-not-float32/ .But I don't realize that the datatype shoudn't set as float32 directly.
Thanks for your help!

[phil-opp]

Great to hear that!