feat(obstacle_cruise_planner): support pointcloud-based obstacles

[mitukou1109]

Description

Make obstacle_cruise_planner compatible with sensor_msgs::msg::PointCloud2-based obstacles.

The node subscribes to a sensor_msgs::msg::PointCloud2 topic and converts the point cloud into obstacles by the following steps:

1. transform the cloud into the trajectory frame
2. downsample the cloud by voxel grid and apply Euclidean clustering to get candidates of obstacles
3. check if each point in a cluster is within the stop / slow down margin
4. when any points are inside the stop / slow down area, create a new obstacle object containing:
   • the nearest point from the ego for stop obstacles
   • the nearest and the farthest from the ego for slow down obstacles

Stop and slow down planning are performed with the same trajectory generation algorithm as for ML-based (autoware_perception_msgs::msg::PredictedObject) obstacles.

Cruising and yielding are not supported since point cloud-based obstacles have no velocity information.

Margin adjustment on curves (#4952) is also unsupported due to the following reason:

• Point cloud-based obstacles do not have shape information.
• Unlike PredictedObject polygons, point clouds do not necessarily capture the entire obstacle. This could lead the margin to be smaller than in the ML-based case.

launch PR: autowarefoundation/autoware_launch#980

Tests performed

Psim

ros2 launch autoware_launch planning_simulator.launch.xml map_path:=$HOME/autoware_map/sample-map-planning vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit

with the following changes to obstacle_cruise_planner.param.yaml:

/**:
  ros__parameters:
    common:
     stop_obstacle_type:
        unknown: false
        car: false
        truck: false
        bus: false
        trailer: false
        motorcycle: false
        bicycle: false
        pedestrian: false
        pointcloud: true

     slow_down_obstacle_type:
        unknown: false
        car: false
        truck: false
        bus: false
        trailer: false
        motorcycle: false
        bicycle: false
        pedestrian: false
        pointcloud: true

    behavior_determination:
      pointcloud_search_radius: 5.0
      pointcloud_voxel_grid_x: 0.05
      pointcloud_voxel_grid_y: 0.05
      pointcloud_voxel_grid_z: 100000.0
      pointcloud_cluster_tolerance: 1.0
      pointcloud_min_cluster_size: 1
      pointcloud_max_cluster_size: 100000

Screencast.from.2024.06.27.16.40.32.webm

Evaluator:

• w/ point cloud handling disabled: https://evaluation.tier4.jp/evaluation/reports/a6893361-4d9d-567f-8a2b-e92f49148286?project_id=prd_jt
• w/ both ML-based objects and point cloud handling enabled: https://evaluation.tier4.jp/evaluation/reports/1a144b5e-943e-53d0-b7ab-7ea016e29f48?project_id=prd_jt

Effects on system behavior

It is confirmed that the changes do not affect the behavior when the point cloud handling is disabled.

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.
• The PR has been properly tested.
• The PR has been reviewed by the code owners.

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.
• The PR is ready for merge.

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

[satoshi-ota]

Could you explain the purpose of this variable?

[satoshi-ota]

Why did you calculate centroid of pointcloud?

[satoshi-ota]

[imo] it's a little bit difficult to understand this condition.

This is my suggestion.

  if (
    (obstacle.source_type == Obstacle::SourceType::PREDICTED_OBJECT &&
     !isStopObstacle(obstacle.classification.label)) {
    return std::nullopt;
  }

  if (
    (obstacle.source_type == Obstacle::SourceType::POINTCLOUD && !use_pointcloud_for_stop_)) {
    return std::nullopt;
  }

[satoshi-ota]

same as https://github.com/autowarefoundation/autoware.universe/pull/6907/files#r1656411386.

[codecov]

Codecov Report

Attention: Patch coverage is 18.39080% with 142 lines in your changes missing coverage. Please review.

Project coverage is 28.32%. Comparing base (c9aa86c) to head (8e88283).
Report is 1 commits behind head on main.

Files	Patch %	Lines
...ning/autoware_obstacle_cruise_planner/src/node.cpp	19.13%	126 Missing and 5 partials ⚠️
...utoware/obstacle_cruise_planner/common_structs.hpp	0.00%	10 Missing ⚠️
..._obstacle_cruise_planner/src/planner_interface.cpp	0.00%	1 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #6907      +/-   ##
==========================================
- Coverage   28.69%   28.32%   -0.37%     
==========================================
  Files        1587     1585       -2     
  Lines      116143   115614     -529     
  Branches    49559    49305     -254     
==========================================
- Hits        33326    32750     -576     
- Misses      73723    73871     +148     
+ Partials     9094     8993     -101     

Flag	Coverage Δ		*Carryforward flag
differential	6.62% <18.39%> (?)
total	28.34% <ø> (-0.35%)	⬇️	Carriedforward from 6a70824

*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.