motion_planning container crashes after route planning

[tfnh621]

Checklist

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

Description

After placing obstacles on the road, the planning container crashes when the route is planned.

Expected behavior

The container should not crash.

Actual behavior

The container crashes and exits.

Steps to reproduce

1. Follow the source installtion tutorial to setup Autoware.
2. Edit the parameter file ~/autoware/install/autoware_launch/share/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml as follows:

--- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml
+++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml
@@ -9,7 +9,7 @@
     voxel_grid_x: 0.05                       # voxel grid x parameter for filtering pointcloud [m]
     voxel_grid_y: 0.05                       # voxel grid y parameter for filtering pointcloud [m]
     voxel_grid_z: 100000.0                   # voxel grid z parameter for filtering pointcloud [m]
-    use_predicted_objects: False            # whether to use predicted objects [-]
+    use_predicted_objects: True            # whether to use predicted objects [-]
     publish_obstacle_polygon: False          # whether to publish obstacle polygon [-]
     predicted_object_filtering_threshold: 1.5 # threshold for filtering predicted objects (valid only publish_obstacle_polygon true) [m]

@@ -28,7 +28,7 @@
         pedestrian_lateral_margin: 0.0       # margin of pedestrian footprint [m]
         unknown_lateral_margin: 0.0          # margin of unknown footprint [m]
         step_length: 1.0                     # step length for pointcloud search range [m]
-        enable_stop_behind_goal_for_obstacle: True # enable extend trajectory after goal lane for obstacle detection
+        enable_stop_behind_goal_for_obstacle: False # enable extend trajectory after goal lane for obstacle detection


     slow_down_planner:

3. Run the planning simulator.

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

4. Open another terminal and publish the topics with the following commands:

ros2 topic pub /initialpose geometry_msgs/msg/PoseWithCovarianceStamped "{header: {stamp: {sec: 1705974599, nanosec: 113561686}, frame_id: map}, pose: {pose: {position: {x: 3717.705810546875, 'y': 73685.9140625, z: 0}, orientation: {x: 0, 'y': 0, z: 0.8732685278259389, w: 0.48723924134732566}}, covariance: [0.25, 0, 0, 0, 0, 0, 0, 0.25, 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.06853891909122467]}}" --once
ros2 topic pub /planning/mission_planning/goal geometry_msgs/msg/PoseStamped "{header: {stamp: {sec: 1705976853, nanosec: 528831885}, frame_id: map}, pose: {position: {x: 3709.70556640625, 'y': 73700.921875, z: 0}, orientation: {x: 0, 'y': 0, z: 0.8512186083890675, w: 0.5248112810641357}}}" --once
sleep 4
ros2 service call /api/operation_mode/change_to_autonomous autoware_adapi_v1_msgs/srv/ChangeOperationMode
sleep 16
ros2 topic pub /simulation/dummy_perception_publisher/object_info dummy_perception_publisher/msg/Object "{header: {stamp: {sec: 1705976667, nanosec: 813447065}, frame_id: map}, id: {uuid: [198, 13, 152, 87, 154, 93, 104, 54, 131, 255, 144, 238, 163, 207, 47, 96]}, initial_state: {pose_covariance: {pose: {position: {x: 3700.91455078125, 'y': 73711.9765625, z: 0}, orientation: {x: 0, 'y': 0, z: 0.019533422790518368, w: 0.9998092044955812}}, covariance: [0.0008999999845400453, 0, 0, 0, 0, 0, 0, 0.0008999999845400453, 0, 0, 0, 0, 0, 0, 0.0008999999845400453, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.007615434937179089]}, twist_covariance: {twist: {linear: {x: 0, 'y': 0, z: 0}, angular: {x: 0, 'y': 0, z: 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]}, accel_covariance: {accel: {linear: {x: 0, 'y': 0, z: 0}, angular: {x: 0, 'y': 0, z: 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]}}, classification: {label: 3, probability: 1}, shape: {type: 0, footprint: {points: []}, dimensions: {x: 10.5, 'y': 2.5, z: 3.5}}, max_velocity: 33.29999923706055, min_velocity: -33.29999923706055, action: 0}" --once
ros2 topic pub /planning/mission_planning/goal geometry_msgs/msg/PoseStamped "{header: {stamp: {sec: 1705976823, nanosec: 664244821}, frame_id: map}, pose: {position: {x: 3712.005615234375, 'y': 73718.3203125, z: 0}, orientation: {x: 0, 'y': 0, z: 0.2581943401458496, w: 0.9660929989999147}}}" --once

5. After the second route planning, notice that the component container is exited

[component_container_mt-30] terminate called after throwing an instance of 'std::out_of_range'
[component_container_mt-30]   what():  vector::_M_range_check: __n (which is 1) >= this->size() (which is 1)
[component_container_mt-30] *** Aborted at 1705992680 (unix time) try "date -d @1705992680" if you are using GNU date ***
[component_container_mt-30] PC: @                0x0 (unknown)
[component_container_mt-30] *** SIGABRT (@0x3e8000762a7) received by PID 484007 (TID 0x7ff7987f8640) from PID 484007; stack trace: ***
[component_container_mt-30]     @     0x7ff7c02d6046 (unknown)
[component_container_mt-30]     @     0x7ff7c39ad520 (unknown)
[component_container_mt-30]     @     0x7ff7c3a019fc pthread_kill
[component_container_mt-30]     @     0x7ff7c39ad476 raise
[component_container_mt-30]     @     0x7ff7c39937f3 abort
[component_container_mt-30]     @     0x7ff7c3c58b9e (unknown)
[component_container_mt-30]     @     0x7ff7c3c6420c (unknown)
[component_container_mt-30]     @     0x7ff7c3c64277 std::terminate()
[component_container_mt-30]     @     0x7ff7c3c644d8 __cxa_throw
[component_container_mt-30]     @     0x7ff7c3c5b4cd (unknown)
[component_container_mt-30]     @     0x7ff696c9ade9 motion_planning::ObstacleStopPlannerNode::filterObstacles()
[component_container_mt-30]     @     0x7ff696ca5126 motion_planning::ObstacleStopPlannerNode::searchPredictedObject()
[component_container_mt-30]     @     0x7ff696caa96f motion_planning::ObstacleStopPlannerNode::onTrigger()
[component_container_mt-30]     @     0x7ff696cd9bc7 std::_Function_handler<>::_M_invoke()
[component_container_mt-30]     @     0x7ff7b07b25c7 _ZNSt8__detail9__variant17__gen_vtable_implINS0_12_Multi_arrayIPFNS0_21__deduce_visit_resultIvEEOZN6rclcpp23AnySubscriptionCallbackIN27autoware_auto_planning_msgs3msg11Trajectory_ISaIvEEESA_E8dispatchESt10shared_ptrISB_ERKNS5_11MessageInfoEEUlOT_E_RSt7variantIJSt8functionIFvRKSB_EESN_IFvSP_SH_EESN_IFvRKNS5_17SerializedMessageEEESN_IFvSW_SH_EESN_IFvSt10unique_ptrISB_St14default_deleteISB_EEEESN_IFvS14_SH_EESN_IFvS11_ISU_S12_ISU_EEEESN_IFvS1A_SH_EESN_IFvSD_ISO_EEESN_IFvS1F_SH_EESN_IFvSD_ISV_EEESN_IFvS1K_SH_EESN_IFvRKS1F_EESN_IFvS1Q_SH_EESN_IFvRKS1K_EESN_IFvS1W_SH_EESN_IFvSE_EESN_IFvSE_SH_EESN_IFvSD_ISU_EEESN_IFvS25_SH_EEEEEJEEESt16integer_sequenceImJLm8EEEE14__visit_invokeESL_S2B_
[component_container_mt-30]     @     0x7ff696d35fa5 rclcpp::Subscription<>::handle_message()
[component_container_mt-30]     @     0x7ff7c3f007bc rclcpp::Executor::execute_subscription()
[component_container_mt-30]     @     0x7ff7c3f00fbf rclcpp::Executor::execute_any_executable()
[component_container_mt-30]     @     0x7ff7c3f0827a rclcpp::executors::MultiThreadedExecutor::run()
[component_container_mt-30]     @     0x7ff7c3c92253 (unknown)
[component_container_mt-30]     @     0x7ff7c39ffac3 (unknown)
[component_container_mt-30]     @     0x7ff7c3a91850 (unknown)
[component_container_mt-30]     @                0x0 (unknown)
[ERROR] [component_container_mt-30]: process has died [pid 484007, exit code -6, cmd '/opt/ros/humble/lib/rclcpp_components/component_container_mt --ros-args -r __node:=motion_planning_container -r __ns:=/planning/scenario_planning/lane_driving/motion_planning -p use_sim_time:=False -p wheel_radius:=0.383 -p wheel_width:=0.235 -p wheel_base:=2.79 -p wheel_tread:=1.64 -p front_overhang:=1.0 -p rear_overhang:=1.1 -p left_overhang:=0.128 -p right_overhang:=0.128 -p vehicle_height:=2.5 -p max_steer_angle:=0.7'].

Versions

• OS: Ubuntu 22.04
• ROS 2: humble
• autoware.universe: e4cae6f

Possible causes

No response

Additional context

No response

[yuki-takagi-66]

@yta56116
Thank you for reporting the issue.
This issue has been fixed by #6254
If you have any other issues, we welcome your pull request.