Link autoware_lidar_centerpoint to autoware_multi_object_tracker for vehicle tracking

[vollkornholztuer]

Note

Before starting, I'd like to inform you, that I am a beginner in terms of ROS2 and the usage of Autoware. I might have skipped essential things, because i simply didn't know better. I also would like you to know that i searched to the best of my abilities on the internet.

EDIT: If more context is needed, please tell me and i will try my best to provide information.

I'm currently experiencing the following issue:

I want to track vehicles in a series of pointclouds with the help of the autoware_lidar_centerpoint and autoware_multi_object_tracker. I'm currently at the state, where the pointclouds are fed into autoware_lidar_centerpoint and the results are published after each frame/pointcloud at around 10 Hz.

I remapped the db3 file and output of autoware_lidar_centerpoint accordingly, in order to serve it as an input for autoware_multi_object_tracker like this:

Remap of bag-file for autoware_lidar_centerpoint:
ros2 bag play -l 171720_corrected.db3 --remap /pointcloud2:=/sensing/lidar/pointcloud

Remap of autoware_lidar_centerpoint-output for autoware_mutli_object_tracker:
ros2 launch autoware_lidar_centerpoint lidar_centerpoint.launch.xml output/objects:=/perception/object_recognition/detection/objects

After that, i just launch autoware_multi_object_tracker without any changes:
ros2 launch autoware_multi_object_tracker multi_object_tracker.launch.xml

After doing all of that, this is how my node-graph looks like in rqt:

Echoing the /objects topic yields nothing, the terminal stays empty. The output topic for autoware_lidar_centerpoint, which was remapped, yields results as expected. It looks something like this:

autoware_lidar_centerpoint results

header:
  stamp:
    sec: 1730210174
    nanosec: 835973978
  frame_id: scala3s
objects:
- existence_probability: 0.7329742908477783
  classification:
  - label: 7
    probability: 1.0
  kinematics:
    pose_with_covariance:
      pose:
        position:
          x: 45.4932746887207
          y: 9.119370460510254
          z: 0.440185546875
        orientation:
          x: 0.0
          y: -0.0
          z: -0.9999759873873287
          w: -0.006929981871322616
      covariance:
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
    has_position_covariance: false
    orientation_availability: 0
    twist_with_covariance:
      twist:
        linear:
          x: 0.0
          y: 0.0
          z: 0.0
        angular:
          x: 0.0
          y: 0.0
          z: 0.0
      covariance:
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
      - 0.0
    has_twist: false
    has_twist_covariance: false
  shape:
    type: 0
    footprint:
      points: []
    dimensions:
      x: 0.8530513048171997
      y: 1.0110468864440918
      z: 1.8267512321472168

Ideally, i would like to have such a result at the end: https://www.youtube.com/watch?v=5sxa38YBVDQ
Besides that, I'd like to save the results of the tracked objects, but that's an unrelated issue.

I hope you can help me. Thank you very much for reading.

[amadeuszsz]

Hello @vollkornholztuer

Thank you for using Autoware and detailed description of your issue, I really appreciate it!

For your custom scenario, you can't get expected result because of ROS time. Since you use rosbag, time differs from runtime time and you need to explicitly indicate it for all the nodes. You can do it by adding:

      <param name="use_sim_time" value="true"/>

to both launch files within node definition.

FYI, for Autoware rosbag replay use case and its launch files, we use autoware_global_parameter_loader which override this parameter. If you willing to not edit original launch files, you can create your own custom launch file and do same way as it is done for Autoware launch files with global parameter loader.

[vollkornholztuer]

As of now, I've done the following steps and made some progress:

I created both for autoware_lidar_centerpoint and autoware_multi_object_tracker custom launch.xml-files in order to preserve the original ones. Inside of them i both added the <param name="use_sim_time" value="true"/> line for the use of the parameter. This is how both xml-files look like now:

lidar_centerpoint.launch_for_multi_object_tracker.xml

<?xml version="1.0"?>
<launch>
  <arg name="input/pointcloud" default="/sensing/lidar/pointcloud"/>
  <arg name="output/objects" default="detected_objects"/>
  <arg name="data_path" default="$(env HOME)/autoware_data" description="packages data and artifacts directory path"/>
  <arg name="model_name" default="centerpoint_tiny" description="options: `centerpoint` or `centerpoint_tiny`"/>
  <arg name="model_path" default="$(var data_path)/lidar_centerpoint"/>
  <arg name="model_param_path" default="$(find-pkg-share autoware_lidar_centerpoint)/config/$(var model_name).param.yaml"/>
  <arg name="ml_package_param_path" default="$(var model_path)/$(var model_name)_ml_package.param.yaml"/>
  <arg name="class_remapper_param_path" default="$(var model_path)/detection_class_remapper.param.yaml"/>
  <arg name="build_only" default="false" description="shutdown node after TensorRT engine file is built"/>

  <arg name="use_pointcloud_container" default="false" description="use pointcloud_container"/>
  <arg name="pointcloud_container_name" default="pointcloud_container" description="pointcloud_container name"/>

  <group unless="$(var use_pointcloud_container)">
    <node pkg="autoware_lidar_centerpoint" exec="autoware_lidar_centerpoint_node" name="lidar_centerpoint" output="screen">
      <remap from="~/input/pointcloud" to="$(var input/pointcloud)"/>
      <!-- Here, remap the lidar detection output so it publishes to 'detected_objects' -->
      <remap from="~/output/objects" to="$(var output/objects)"/>
      <param from="$(var model_param_path)" allow_substs="true"/>
      <param from="$(var ml_package_param_path)" allow_substs="true"/>
      <param from="$(var class_remapper_param_path)"/>
      <param name="build_only" value="$(var build_only)"/>
      <param name="use_sim_time" value="true"/>
    </node>
  </group>
  <!-- Similarly update the composable node group if using pointcloud container -->
  <group if="$(var use_pointcloud_container)">
    <load_composable_node target="$(var pointcloud_container_name)">
      <composable_node pkg="autoware_lidar_centerpoint" plugin="autoware::lidar_centerpoint::LidarCenterPointNode" name="lidar_centerpoint">
        <remap from="~/input/pointcloud" to="$(var input/pointcloud)"/>
        <remap from="~/output/objects" to="$(var output/objects)"/>
        <param from="$(var model_param_path)" allow_substs="true"/>
        <param from="$(var ml_package_param_path)" allow_substs="true"/>
        <param from="$(var class_remapper_param_path)"/>
        <param name="build_only" value="$(var build_only)"/>
      </composable_node>
    </load_composable_node>
  </group>
</launch>

multi_object_tracker.launch_for_lidar_centerpoint.xml

<?xml version="1.0" encoding="UTF-8"?>
<launch>
  <arg name="selected_input_channels" default="['detected_objects']"/>
  <arg name="output" default="objects"/>
  <arg name="tracker_setting_path" default="$(find-pkg-share autoware_multi_object_tracker)/config/multi_object_tracker_node.param.yaml"/>
  <arg name="data_association_matrix_path" default="$(find-pkg-share autoware_multi_object_tracker)/config/data_association_matrix.param.yaml"/>
  <arg name="input_channels_path" default="$(find-pkg-share autoware_multi_object_tracker)/config/input_channels.param.yaml"/>

  <node pkg="autoware_multi_object_tracker" exec="multi_object_tracker_node" name="multi_object_tracker" output="both">
    <param name="selected_input_channels" value="$(var selected_input_channels)"/>
    <remap from="output" to="$(var output)"/>
    <param from="$(var tracker_setting_path)"/>
    <param from="$(var data_association_matrix_path)"/>
    <param from="$(var input_channels_path)"/>
    <param name="use_sim_time" value="true"/>
  </node>
</launch>

Now the /objects topic, which is the output for autoware_multi_object_tracker recieves messages, although not entirely as expected. This is how it looks like:

ros2 topic echo /objects

header:
  stamp:
    sec: 1722532643
    nanosec: 682935862
  frame_id: scala3s
objects: []
---
header:
  stamp:
    sec: 1722532643
    nanosec: 782059814
  frame_id: scala3s
objects: []
---
header:
  stamp:
    sec: 1722532643
    nanosec: 881443702
  frame_id: scala3s
objects: []
---
header:
  stamp:
    sec: 1722532643
    nanosec: 980819086
  frame_id: scala3s
objects: []
---

According to the documentation of autoware_multi_object_tracker, I'm supposed to pass an array of detected_objects to autoware_multi_object_tracker. For reference, here's a screenshot from that website:

Do i need an extra node, which takes the detected objects from autoware_lidar_centerpoint for each frame/point cloud and converts them into an array? Or what would the best practice be to succeed in that?

Thank you for the help already and thank you in advance.

[amadeuszsz]

@vollkornholztuer
From multi object tracker node I noticed that we assume input (from lidar_centerpoint in your case) defined in base_link frame ID, which is transformed later to world frame ID (set as map in default config file). Therefore:

• Set your input cloud to lidar_centerpoint in base_link.
• Provide transform base_link -> map to your TF tree.

Also you can add --clock flag while playing rosbag if problem with timestamp occurs.

In order to confirm it works, I played sample rosbag, launched both launch files with custom topic and I can see proper output in rviz and console:

(container) autoware@npc2405014:/workspace $ ros2 topic echo /perception/object_recognition/detection/objects2 --no-arr
---
header:
  stamp:
    sec: 1685068411
    nanosec: 633646336
  frame_id: base_link
objects: '<sequence type: autoware_perception_msgs/msg/DetectedObject, length: 9>'
---
(container) autoware@npc2405014:/workspace $ ros2 topic echo /objects2 --no-arr
header:
  stamp:
    sec: 1685068413
    nanosec: 132955904
  frame_id: map
objects: '<sequence type: autoware_perception_msgs/msg/TrackedObject, length: 9>'

[vollkornholztuer]

@amadeuszsz, thank you for your answer.

Just so i understand it correctly, autoware_multi_object_tracker expects DetectedObject-messages with the frame_id called base_link instead of scala3s? Therefore, before i put the pointcloud into autoware_lidar_centerpoint: do i have to rename the frame_id to base_link? Or can i use a node to rename the frame_id of the DetectedObjects-message after the detection with autoware_lidar_centerpoint?

Also, is there the possibility to change the expected frame_id for autoware_multi_object_tracker?

[amadeuszsz]

@vollkornholztuer

Just so i understand it correctly, autoware_multi_object_tracker expects DetectedObject-messages with the frame_id called base_link instead of scala3s? Therefore, before i put the pointcloud into autoware_lidar_centerpoint: do i have to rename the frame_id to base_link? Or can i use a node to rename the frame_id of the DetectedObjects-message after the detection with autoware_lidar_centerpoint?

If you have correct TF tree, you can use one of autoware_pointcloud_preprocessor, e.g. crop_box_filter and define input_frame and output_frame as base_link. You can also just change frame_id of your pointcloud.

Also, is there the possibility to change the expected frame_id for autoware_multi_object_tracker?

No, since this frame is hardcoded and not exposed as ROS parameter for overriding purposes.
I didn't reproduce your issue, but looking into code, if link does not exist, transformation fails resulting empty array while processing objects.

[vollkornholztuer]

@amadeuszsz

Thank you very much for the information. I think i will just change the frame_id of the pointclouds to make it work, as it seems the easiest way to do it in my opinion. As soon as i will make it work, I will update you.

[vollkornholztuer]

@vollkornholztuer

Just so i understand it correctly, autoware_multi_object_tracker expects DetectedObject-messages with the frame_id called base_link instead of scala3s? Therefore, before i put the pointcloud into autoware_lidar_centerpoint: do i have to rename the frame_id to base_link? Or can i use a node to rename the frame_id of the DetectedObjects-message after the detection with autoware_lidar_centerpoint?

If you have correct TF tree, you can use one of autoware_pointcloud_preprocessor, e.g. crop_box_filter and define input_frame and output_frame as base_link. You can also just change frame_id of your pointcloud.

Also, is there the possibility to change the expected frame_id for autoware_multi_object_tracker?

No, since this frame is hardcoded and not exposed as ROS parameter for overriding purposes. I didn't reproduce your issue, but looking into code, if link does not exist, transformation fails resulting empty array while processing objects.

I managed to convert my bag file, so the frame_id is now set to base_link. After also changing the expected frame_id inside of the multi_object_tracker_node.param.yaml-configuration file to base_link, the cars were detected and tracked.

Although it isn't the best practice, i used this way, because i also had to do some de-warping corrections to my pointclouds. This issue is unrelated though.

Thank you very much for your help :)