How to integrate the official ouster ros2 driver into autoware.universe project

[TZECHIN6]

Checklist

• [X] I've read the contribution guidelines.
• [X] I've searched other issues and no duplicate issues were found.
• [X] I'm convinced that this is not my fault but a bug.

Description

I would like to integrate my ouster lidar (os-1 64 beam I believe) into autoware.universe project. However, I am having trouble to get the pointcloud data after the preprocessing node.

I first tried to pass the raw point cloud into the self cropped box node as below:

    cropbox_parameters = create_parameter_dict("input_frame", "output_frame")
    cropbox_parameters["negative"] = True

    vehicle_info = get_vehicle_info(context)
    cropbox_parameters["min_x"] = vehicle_info["min_longitudinal_offset"]
    cropbox_parameters["max_x"] = vehicle_info["max_longitudinal_offset"]
    cropbox_parameters["min_y"] = vehicle_info["min_lateral_offset"]
    cropbox_parameters["max_y"] = vehicle_info["max_lateral_offset"]
    cropbox_parameters["min_z"] = vehicle_info["min_height_offset"]
    cropbox_parameters["max_z"] = vehicle_info["max_height_offset"]

    nodes.append(
        ComposableNode(
            package="pointcloud_preprocessor",
            plugin="pointcloud_preprocessor::CropBoxFilterComponent",
            name="crop_box_filter_self",
            remappings=[
                ("input", "/ouster/points"),
                ("output", "self_cropped/pointcloud"),
            ],
            parameters=[cropbox_parameters],
            extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
        )
    )

The output topic of /sensing/lidar/top/self_cropped/pointcloud has no response.

At this point, the issue might caused by the missing of required data field, thus I tried to just directly pass into concatenate_data as below:

def launch_setup(context, *args, **kwargs):
    # set concat filter as a component
    concat_component = ComposableNode(
        package="pointcloud_preprocessor",
        plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent",
        name="concatenate_data",
        remappings=[
            ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"),
            ("output", "concatenated/pointcloud"),
        ],
        parameters=[
            {
                "input_topics": [
                    "/ouster/points",
                    "/sensing/lidar/left/outlier_filtered/pointcloud",
                    "/sensing/lidar/right/outlier_filtered/pointcloud",
                ],
                "output_frame": LaunchConfiguration("base_frame"),
            }
        ],
        extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
    )

Sadly, no luck for me to see the pointcloud at topic /sensing/lidar/concatenated/pointcloud...

I have checked the raw points topic /ouster/points, and ensure it has data output and can be seen in rviz.

---
header:
  stamp:
    sec: 1694597431
    nanosec: 800832957
  frame_id: ouster_top
height: 64
width: 1024
fields:
- name: x
  offset: 0
  datatype: 7
  count: 1
- name: y
  offset: 4
  datatype: 7
  count: 1
- name: z
  offset: 8
  datatype: 7
  count: 1
- name: intensity
  offset: 16
  datatype: 7
  count: 1
- name: t
  offset: 20
  datatype: 6
  count: 1
- name: reflectivity
  offset: 24
  datatype: 4
  count: 1
- name: ring
  offset: 26
  datatype: 4
  count: 1
- name: ambient
  offset: 28
  datatype: 4
  count: 1
- name: range
  offset: 32
  datatype: 6
  count: 1
is_bigendian: false
point_step: 48
row_step: 49152
data:
- 0
- 0
- 0
- 0
- 0
- 0
- 0
- 0
- 0
- 0
- 128
- 63
- 0
- 0
- 192
- 64
- 174
- 22
- 244
...
- '...'
is_dense: true

This is the tf qraph: frames_2023-09-13_17.16.22.pdf

It shows ouster_top do have a tf tree connect with base_link. So at this point I am not so sure how to fix the issue. And asking for help in here. Thanks to all who have paid kind attention to this.

Expected behavior

Able to use official ros2 ouster driver to launch ouster lidar and use it's outputed points in Autoware with preprocessing node.

Actual behavior

Error message capture from the console:

[component_container-28] [ERROR] [1694599051.940295753] [sensing.lidar.top.crop_box_filter_self]: [get_transform_matrix] timeout tf: ouster_top->base_link

Right now I cannot review points from either /sensing/lidar/top/self_cropped/pointcloud or /sensing/lidar/concatenated/pointcloud.

Steps to reproduce

1. use this official ouster ros2 driver link.

2. update the config of ouster driver launch as below:

   
   <launch>
   
   <arg name="ouster_ns" default="ouster"
   description="Override the default namespace of all ouster nodes"/>
   <arg name="sensor_hostname" default="os-serial-no.local"
   description="hostname or IP in dotted decimal form of the sensor"/>
   <arg name="udp_dest" default="169.254.248.124"
   description="hostname or IP where the sensor will send data packets"/>
   <arg name="lidar_port" default="0"
   description="port to which the sensor should send lidar data"/>
   <arg name="imu_port" default="0"
   description="port to which the sensor should send imu data"/>
   <arg name="udp_profile_lidar" default=""
   description="lidar packet profile; possible values: {
   LEGACY,
   RNG19_RFL8_SIG16_NIR16_DUAL,
   RNG19_RFL8_SIG16_NIR16,
   RNG15_RFL8_NIR8
   }"/>
   <arg name="lidar_mode" default=""
   description="resolution and rate; possible values: {
   512x10,
   512x20,
   1024x10,
   1024x20,
   2048x10,
   4096x5
   }"/>
   <arg name="timestamp_mode" default="TIME_FROM_ROS_TIME"
   description="method used to timestamp measurements; possible values: {
   TIME_FROM_INTERNAL_OSC,
   TIME_FROM_SYNC_PULSE_IN,
   TIME_FROM_PTP_1588,
   TIME_FROM_ROS_TIME
   }"/>
   <arg name="ptp_utc_tai_offset" default="-37.0"
   description="UTC/TAI offset in seconds to apply when using TIME_FROM_PTP_1588"/>
   <arg name="metadata" default=""
   description="path to write metadata file when receiving sensor data"/>
   <arg name="viz" default="false"
   description="whether to run a rviz"/>
   <arg name="rviz_config" default="$(find-pkg-share ouster_ros)/config/viz.rviz"
   description="optional rviz config file"/>
   
   <arg name="sensor_frame" default="os_sensor"
   description="sets name of choice for the sensor_frame tf frame, value can not be empty"/>
   <arg name="lidar_frame" default="ouster_top"
   description="sets name of choice for the os_lidar tf frame, value can not be empty"/>
   <arg name="imu_frame" default="os_imu"
     description="sets name of choice for the os_imu tf frame, value can not be empty"/>
   <arg name="point_cloud_frame" default=""
   description="which frame to be used when publishing PointCloud2 or LaserScan messages.
   Choose between the value of sensor_frame or lidar_frame, leaving this value empty
   would set lidar_frame to be the frame used when publishing these messages."/>
   
   <arg name="use_system_default_qos" default="false"
   description="Use the default system QoS settings"/>
   
   <arg name="proc_mask" default="IMG|PCL|IMU|SCAN" description="
   use any combination of the 4 flags to enable or disable specific processors"/>
   
   <arg name="scan_ring" default="0" description="
   use this parameter in conjunction with the SCAN flag
   and choose a value the range [0, sensor_beams_count)"/>
   
   <group>
   <push-ros-namespace namespace="$(var ouster_ns)"/>
   <node pkg="ouster_ros" exec="os_driver" name="os_driver" output="screen">
     <param name="sensor_hostname" value="$(var sensor_hostname)"/>
     <param name="udp_dest" value="$(var udp_dest)"/>
     <param name="mtp_dest" value=""/>
     <param name="mtp_main" value="false"/>
     <param name="lidar_port" value="$(var lidar_port)"/>
     <param name="imu_port" value="$(var imu_port)"/>
     <param name="udp_profile_lidar" value="$(var udp_profile_lidar)"/>
     <param name="lidar_mode" value="$(var lidar_mode)"/>
     <param name="metadata" value="$(var metadata)"/>
     <param name="sensor_frame" value="$(var sensor_frame)"/>
     <param name="lidar_frame" value="$(var lidar_frame)"/>
     <param name="imu_frame" value="$(var imu_frame)"/>
     <param name="point_cloud_frame" value="$(var point_cloud_frame)"/>
     <param name="timestamp_mode" value="$(var timestamp_mode)"/>
     <param name="ptp_utc_tai_offset" value="$(var ptp_utc_tai_offset)"/>
     <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
     <param name="proc_mask" value="$(var proc_mask)"/>
     <param name="scan_ring" value="$(var scan_ring)"/>
   </node>
   </group>
   
   <!-- HACK: configure and activate the sensor node via a process execute since state
   transition is currently not availabe through launch.xml format -->
   <executable cmd="$(find-exec ros2) lifecycle set /$(var ouster_ns)/os_driver configure"
   launch-prefix="bash -c 'sleep 0; $0 $@'" output="screen"/>
   <executable cmd="$(find-exec ros2) lifecycle set /$(var ouster_ns)/os_driver activate"
   launch-prefix="bash -c 'sleep 1; $0 $@'" output="screen"/>
   
   <include if="$(var viz)" file="$(find-pkg-share ouster_ros)/launch/rviz.launch.xml">
   <arg name="ouster_ns" value="$(var ouster_ns)"/>
   <arg name="rviz_config" value="$(var rviz_config)"/>
   </include>

basically i updated the point cloud frame to `ouster_top` and using ROS time

3. update the description:

``` ``` ouster_top_base_link: x: 0.0 y: 0.0 z: 0.0 roll: 0.0 pitch: 0.0 yaw: 0.0 ``` 4. I havent integrated the ouster into autoware workspace yet, so right now I first launching ouster, then launching autoware.launch ### Versions - OS: Ubuntu 22 - ROS2 Humble - Autoware universe (`0e5ec3109c752b3cd92deaaad4c0b51b3b8d2cf1` 2023-08-08) - ouster ros2 driver [link](https://github.com/ouster-lidar/ouster-ros/tree/ros2) ### Possible causes Might due to unmatch data field and also tf issue. As the ouster driver package is now installed at other workspace, it doesnt launch with node_container. This might also an issue... but I have difficult time to create a custom node_container. ### Additional context Any rosbag or information is needed to further debug this issue, please free to let me know, I would try my best to provided the required information.

[VRichardJP]

The problem might comes from the data layout:

fields:
- name: x
  offset: 0
  datatype: 7
  count: 1
- name: y
  offset: 4
  datatype: 7
  count: 1
- name: z
  offset: 8
  datatype: 7
  count: 1
- name: intensity
  offset: 16
  datatype: 7
  count: 1
- name: t
  offset: 20
  datatype: 6
  count: 1
- name: reflectivity
  offset: 24
  datatype: 4
  count: 1
- name: ring
  offset: 26
  datatype: 4
  count: 1
- name: ambient
  offset: 28
  datatype: 4
  count: 1
- name: range
  offset: 32
  datatype: 6
  count: 1

Point cloud data layout is defined here: https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-architecture/sensing/data-types/point-cloud/#point-cloud-fields

Which is now implemented here: https://github.com/tier4/nebula/blob/main/nebula_common/include/nebula_common/point_types.hpp

struct PointXYZIRCAEDT
{
  float x;
  float y;
  float z;
  std::uint8_t intensity;
  std::uint8_t return_type;
  std::uint16_t channel;
  float azimuth;
  float elevation;
  float distance;
  std::uint32_t time_stamp;
};

As you can see, the layout is not exactly the same. There are some fields you don't have, and some you have that don't exist.

Unfortunately, the new nebula module seems not to support Ouster Lidar (yet), so you need to modify the driver you have (or add a conversion node) so you get the same fields than above.

Not all fields are equaly important. The most important are 'x', 'y', 'z' and 'time_stamp'. In your case you have a time field, but it is named 't'. You should also check what is the format used by this field and make sure it is what Autoware expect. AFAIK, 'channel', 'azimuth' and 'distance' are only used by the ring outlier filter (which is not that important so you could remove it), and the other fields are not used.

[TZECHIN6]

Thanks for the guidance, I will try modify the driver to match the data field.

Sorry but i have a question about 'channel', 'azimuth' and 'distance' are only used by the ring outlier filter, as I saw the graph saying each node input points msg are all required XYZIRC, channel seem cannot be ignored. Also, for the first cropbox filter node, XYZIRCADT points is needed, does this mean I have to prepare all data field at the very beginning?

If i have three lidars combo, left-right velodyne lidar and top ouster lidar, whats the least required field to directly pass ouster lidar points to Cloud Transformer and Cloud Concatenator?

[VRichardJP]

It is better to follow the expected format. That said, if you remove the ring outlier filter, I think you can fill fields you don't have with 0.

[TZECHIN6]

Understood. I am trying to work on that now

[stale[bot]]

This pull request has been automatically marked as stale because it has not had recent activity.

[isouf]

@TZECHIN6 may I please ask if you managed to overcome this issue? I am facing the same issue and I believe it comes from the field type differences.

[TZECHIN6]

Currently, I don't have an ouster lidar with me. I will resume this integration task once I borrow one from the University.

In the meantime, you could check this out to see the difference between the published pointcloud type structure link.

Remark: there is no nubela driver when at the time I use autoware, so things might change right now.

[TZECHIN6]

@isouf

https://github.com/ouster-lidar/ouster-ros/issues/217

actually I have asked related question in ouster's repository, and seem they have merged a feature to allow selecting different presentation of point cloud type.

Please have a look at here: https://github.com/ouster-lidar/ouster-ros/pull/216

I haven't checked it out in detail. I will come back for this after receiving the lidar.

[isouf]

@TZECHIN6 thank you very much for your prompt response. I managed to resolve the issue by creating a node to convert the format from Ouster to Autoware. However, I need to investigate the ring outlier results further because they do not seem to be sensible. For now I have disabled the ring outlier.

[TZECHIN6]

@isouf Great to hear from you about that. Actually I do not have experience on formatting the point cloud type before, would you mind sharing a bit how you able to do such transform? I would be very appreciated.

[isouf]

@TZECHIN6 Here is the "quick" method that I made to convert the pointcloud format. Any improvement recommendations are welcomed.

// Receive the pointcloud from ouster lidar and change the format of the message so that it matches
// the format the is used in autoware pointcloud_preprocessor filters
void PointcloudFormatAdaptorNode::onPointcloud(
  const sensor_msgs::msg::PointCloud2::ConstSharedPtr pcd)
{
  // Initiate output messages
  auto pointcloud_msg = std::make_shared<sensor_msgs::msg::PointCloud2>();

  // Initiate pcl pointcloud message for the input point cloud
  pcl::PointCloud<pointcloud_format_adaptor::PointXYZITRRAR>::Ptr input_pointcloud(
    new pcl::PointCloud<pointcloud_format_adaptor::PointXYZITRRAR>);

  // Convert ros to pcl
  pcl::fromROSMsg(*pcd, *input_pointcloud);

  // Initiate pcl pointcloud message for the input point cloud
  pcl::PointCloud<nebula::drivers::PointXYZIRADT>::Ptr output_pointcloud(
    new pcl::PointCloud<nebula::drivers::PointXYZIRADT>);
  output_pointcloud->reserve(input_pointcloud->points.size());

  // Convert pcl ouster to pcl autoware
  nebula::drivers::PointXYZIRADT point{};
  for (const auto & p : input_pointcloud->points) {
    point.x = p.x;
    point.y = p.y;
    point.z = p.z;
    point.intensity = p.intensity;
    point.return_type = 0;
    point.ring = p.ring;
    point.azimuth = std::atan2(p.y, p.x);
    point.distance = p.range;
    point.time_stamp = p.t;
    output_pointcloud->points.emplace_back(point);
  }
  output_pointcloud->header = input_pointcloud->header;
  output_pointcloud->height = 1;
  output_pointcloud->width = output_pointcloud->points.size();

  // Convert pcl to ros
  pcl::toROSMsg(*output_pointcloud, *pointcloud_msg);

  // Publish updated pointcloud message
  pub_pcd_->publish(*pointcloud_msg);
}

The PointXYZITRRAR point type is defined on a separate header file as follows:

struct PointXYZITRRAR
{
  float x;
  float y;
  float z;
  float intensity;
  std::uint32_t t;
  std::uint16_t reflectivity;
  std::uint16_t ring;
  std::uint16_t ambient;
  uint32_t range;
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

[TZECHIN6]

@isouf Thanks for your sharing! I will definitely have a look about it.

If there is any update or improvement I have made, I will let you know for sure.

Might I ask which ouster driver have you used for the raw point cloud?

[isouf]

@TZECHIN6 the official ROS2 driver: https://github.com/ouster-lidar/ouster-ros/tree/ros2

[stale[bot]]

This pull request has been automatically marked as stale because it has not had recent activity.

[DarrenQu]

@isouf Thanks for you sharing! Quick question, where in the autoware.universe code do you add this node and how do you run this node?

[DarrenQu]

@isouf To follow up my last questions. Cause I'm using Ouster LiDAR with the following topic: However, autoware requires the following topic:

So, where in the autoware.universe code do you integrate the converter node?

[isouf]

@DarrenQu you will need to replace the default Velodyne drivers with Ouster and then feed the PointCloud2 message (output of Ouster drivers) directly to the pointcloud_preprocessos.

[skill121790]

@isouf Could you perhaps share what you lidar.launch.xml file looks like? I have incorporated a node that changes the format from ouster to autoware, and I have confirmed that it is outputting data in the correct format. I changed the input topics in pointcloud_preprocessor.launch.py to the output topic of my converter node, but I am still not getting any output from /sensing/lidar/concantenated/pointcloud

[TZECHIN6]

@DarrenQu you will need to replace the default Velodyne drivers with Ouster and then feed the PointCloud2 message (output of Ouster drivers) directly to the pointcloud_preprocessos.

@isouf if I directly pass the raw pointcloud from the modified driver to the pointcloud_proprocessors like the way you did, then what the nebula driver does? as seem like the raw pointcloud should firstly be published to the cropbox filter...outlier ring filter, then concatenated at the end. Right now seem it bypass all those preprocess steps.

[knzo25]

@TZECHIN6 The point type that autoware expects is now this one https://github.com/autowarefoundation/autoware.universe/blob/14fa0f3692551ec6af460284537473c5c9be6d66/common/autoware_point_types/include/autoware_point_types/types.hpp#L95 So you may need to take extra steps

[TZECHIN6]

@TZECHIN6

The point type that autoware expects is now this one

https://github.com/autowarefoundation/autoware.universe/blob/14fa0f3692551ec6af460284537473c5c9be6d66/common/autoware_point_types/include/autoware_point_types/types.hpp#L95

So you may need to take extra steps

@knzo25 thanks for the update! I have switched to Livox lidar recently, and successfully publish the required point cloud structure after modifying the official driver.

However when blocking me now is the integration to the point cloud preprocessing component container nodes, I have posted a question about it in the discussion https://github.com/orgs/autowarefoundation/discussions/5048

I know it's a bit off the topic of this issue.

[FranzAlbers]

I've adapted the method from @isouf a bit for the current Autoware PointXYZIRCAEDT point type and built a package around it so that one can directly use a composable node to subscribe to the PointCloud from the Ouster driver and provide the output to the Crop Box Filter (and the Autoware point cloud preprocessing pipeline): https://github.com/FranzAlbers/ouster_point_type_adapter