feat(localization): add lidar_marker_localizer

[SakodaShintaro]

Description

Related discussion: https://github.com/orgs/autowarefoundation/discussions/4190

merge with autowarefoundation/autoware_launch#861

Tests performed

1. Download sample dataset

The sample dataset was acquired in National Institute for Land and Infrastructure Management, Full-scale tunnel experiment facility. The reflectors were installed by Taisei Corporation.

2. Launch Autoware with pose_source:=ndt_lidar-marker

ros2 launch autoware_launch logging_simulator.launch.xml \
  map_path:=$HOME/autoware_map/kokusouken \
  vehicle_model:=sample_vehicle \
  sensor_model:=sample_sensor_kit \
  pose_source:=ndt_lidar-marker

3. play rosbag

sample_data_for_lidar_marker_localization2.mp4

Please visualize the following topic in RViz.

• /localization/pose_estimator/lidar_marker_localizer/pointcloud_preprocessor/ring_filter/pointcloud
  input lidar pointcloud
• /localization/pose_estimator/lidar_marker_localizer/lidar_marker_localizer/debug/marker_mapped
  marker information listed in the vector map
• /localization/pose_estimator/lidar_marker_localizer/lidar_marker_localizer/debug/marker_detected
  detected marker positions
• /localization/pose_estimator/lidar_marker_localizer/lidar_marker_localizer/debug/marker_pointcloud
  detected marker pointcloud
• /localization/pose_estimator/lidar_marker_localizer/lidar_marker_localizer/debug/pose_with_covariance
  estimated position by lidar_marker_localizer

Effects on system behavior

Not applicable.

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

• The PR follows the pull request guidelines.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

• There are no open discussions or they are tracked via tickets.

After all checkboxes are checked, anyone who has write access can merge the PR.

[github-actions]

Documentation URL: https://autowarefoundation.github.io/autoware.universe/pr-5573/
Modified URLs:

• https://autowarefoundation.github.io/autoware.universe/pr-5573/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/

[Motsu-san]

Thank you for your pull-request.
I used this function on some tunnels and it was worked well.

[SakodaShintaro]

I checked this PR by TIER IV internal Evaluator.
https://evaluation.tier4.jp/evaluation/reports/cf23ec1f-f7c8-54e8-b9a2-5b549945d7b4?project_id=prd_jt

And I noticed that there is a issue with

tier4_localization_launch: Cannot locate rosdep definition for [lidar_marker_localizer]

I will fix it.

[SakodaShintaro]

Sorry, I realized that the above problem has already been resolved at edabd49

[codecov]

Codecov Report

Attention: Patch coverage is 0% with 294 lines in your changes missing coverage. Please review.

Project coverage is 29.22%. Comparing base (44e0535) to head (0521fd7).
Report is 18 commits behind head on main.

Files	Patch %	Lines
...ar_marker_localizer/src/lidar_marker_localizer.cpp	0.00%	294 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #5573      +/-   ##
==========================================
+ Coverage   24.02%   29.22%   +5.20%     
==========================================
  Files        1384     1602     +218     
  Lines      102177   116425   +14248     
  Branches    38956    49966   +11010     
==========================================
+ Hits        24544    34022    +9478     
+ Misses      75138    73256    -1882     
- Partials     2495     9147    +6652     

Flag	Coverage Δ		*Carryforward flag
differential	0.00% <0.00%> (?)
total	29.35% <ø> (+5.33%)	⬆️	Carriedforward from 37665bd

*This pull request uses carry forward flags. Click here to find out more.

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[TaikiYamada4]

@SakodaShintaro @YamatoAndo
I see that the localization is going well, except I can't see the topic /localization/pose_estimator/lidar_marker_localizer/lidar_marker_localizer/debug/marker_mapped on rviz. What should it be like on rviz?

ros2 topic echo works and returns something like this.

header:
    stamp:
      sec: 1723008684
      nanosec: 791924782
    frame_id: map
  ns: landmark_text
  id: 18
  type: 9
  action: 0
  pose:
    position:
      x: 16518.486750000855
      y: 97361.17539999983
      z: 28.375
    orientation:
      x: 0.5453159159375941
      y: -0.45014503421135593
      z: -0.4501450342113559
      w: 0.5453159159375941
  scale:
    x: 0.0
    y: 0.0
    z: 0.5
  color:
    r: 1.0
    g: 0.0
    b: 0.0
    a: 1.0
  lifetime:
    sec: 0
    nanosec: 0
  frame_locked: false
  points: []
  colors: []
  texture_resource: ''
  texture:
    header:
      stamp:
        sec: 0
        nanosec: 0
      frame_id: ''
    format: ''
    data: []
  uv_coordinates: []
  text: (0)
  mesh_resource: ''
  mesh_file:
    filename: ''
    data: []
  mesh_use_embedded_materials: false

[YamatoAndo]

@TaikiYamada4
Please set "transient local"

[TaikiYamada4]

@YamatoAndo

Thanks! I found it, so the current PR here is approvable for me 👍

[SakodaShintaro]

I was also able to confirm that it works too. This PR is ready to merge.

[cavadibrahimli1]

Problem

I am encountering an issue where, despite successfully launching Autoware and loading the map, I am unable to see the car or point clouds in the RViz display. Additionally, this problem persists even after I attempt to initialize with GNSS.

Commands Used

I used the following commands to launch the simulation and play the rosbag:

ros2 launch autoware_launch logging_simulator.launch.xml map_path:=$HOME/autoware_map/kokusouken vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit pose_source:=ndt_lidar-marker

ros2 bag play autoware_map/kokusouken/rosbag/

Steps to Reproduce

1. Launch Autoware with the provided commands.
2. Open RViz and observe that the car and point clouds are not visible.
3. Attempt to initialize with GNSS, but the issue remains unresolved.

Expected Behavior

The car and point clouds should be visible in the RViz display, and GNSS initialization should allow proper visualization of the vehicle's position and sensor data.

Actual Behavior

Neither the car nor the point clouds are visible in RViz, even after attempting GNSS initialization.

Additional Information

• Running on UBUNTU 22.04 with ROS 2 humble
• Attached a screenshot of the RViz display showing the current state.

[TaikiYamada4]

@cavadibrahimli1

The rosbag doesn't contain the /clock topic so you need the --clock 200 option to your ros2 bag play command, but did you try that?

[cavadibrahimli1]

Tests Performed

1. Start Autoware

ros2 launch autoware_launch logging_simulator.launch.xml \
  map_path:=$HOME/autoware_map/kokusouken \
  vehicle_model:=sample_vehicle \
  sensor_model:=sample_sensor_kit \
  pose_source:=ndt_lidar-marker

2. Correct Way of Running Rosbag

ros2 bag play autoware_map/kokusouken/rosbag/ --clock 200

@TaikiYamada4

3. Play Rosbag Video

• A video was recorded to demonstrate the correct playback of the rosbag and successful visualization of the following topics in RViz:

/localization/pose_estimator/lidar_marker_localizer/pointcloud_preprocessor/ring_filter/pointcloud
/localization/pose_estimator/lidar_marker_localizer/lidar_marker_localizer/debug/marker_mapped
/localization/pose_estimator/lidar_marker_localizer/lidar_marker_localizer/debug/marker_detected
/localization/pose_estimator/lidar_marker_localizer/lidar_marker_localizer/debug/marker_pointcloud
/localization/pose_estimator/lidar_marker_localizer/lidar_marker_localizer/debug/pose_with_covariance

Screencast.from.08-13-2024.01.26.21.PM.webm

Effects on System Behavior

• System Initialization: The system initializes correctly when launching Autoware with the specified command, loading the map, vehicle model, and sensor configurations without issues.

• Rosbag Playback: Playing the rosbag with the --clock 200 option generally ensures synchronized data processing, accurately reflecting real-time behavior. However, there are occasional instances where the rosbag deviates from the expected path, which may require manual intervention or resetting the playback to correct the trajectory.

• Data Visualization: The visualized topics in RViz, such as point clouds and marker positions, display as expected, confirming that the system’s localization and sensor data processing functions are operating correctly.

• Performance Considerations: During testing, the system performed well under normal conditions. However, it was observed that the system might freeze if a swap file is not created, particularly during memory-intensive operations. Creating a swap file is recommended to prevent these freezes and ensure smooth performance.

• Resource Usage: The system’s resource usage remains within acceptable limits under typical conditions. However, memory usage can spike during extensive data processing, emphasizing the need for adequate memory management, including the use of a swap file, to maintain system stability.

Pre-review Checklist for the PR Author

[x] I've confirmed the contribution guidelines.
[x] The PR follows the pull request guidelines.

In-review Checklist for the PR Reviewers

[ ] The PR follows the pull request guidelines.

Post-review Checklist for the PR Author

[ ] There are no open discussions, or they are tracked via tickets.

[TaikiYamada4]

@cavadibrahimli1
Is your comment an approval or something else?
It seems your video shows us the mismatch between the observed pointcloud and the pointcloud map.
Do you have any reasons why?

[TaikiYamada4]

@cavadibrahimli1
This is a friendly reminder about my comment. What do you think about my previous comment?

[cavadibrahimli1]

@TaikiYamada4 Thank you for your follow-up. I apologize for not responding sooner—I got caught up with some other urgent tasks and unintentionally missed your earlier comment.

I think the problem is with my computer. Because when I did the same test with my other computer, it worked without any problems. But in this test, there is a slight drift.

What do you think?

[TaikiYamada4]

But in this test, there is a slight drift. What do you think?

It depends on what kind of drift is happening. Are you talking about the vehicle doesn't match expecially in the Z coordinates?
In your video it looks like the NDT is not working at all, but are you sure the ndt is outputting topics?

Plus, I want to ask your stance on this PR. Is your comment meant as an approval or a request change?

[cavadibrahimli1]

@TaikiYamada4 Thank you for pointing that out. After investigating, I realized the issue was due to a problem with my PC’s Autoware setup, which caused the slight drift in the test. The NDT was indeed functioning correctly, and there are no issues with the code itself.

I’m approving this PR as the problem was on my end, not with the submission. Thank you for your patience and thorough review.