From 63220dfcb099ed7619d10f08982d5992ccc12b0a Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 13 Nov 2023 19:23:36 +0900 Subject: [PATCH 1/7] docs(lane_change): stopping position when an object exists ahead (#5523) * docs(lane_change): Stopping position when an object exists ahead Signed-off-by: kosuke55 * add stuck params Signed-off-by: kosuke55 * add backward_length_buffer_for_blocking_object Signed-off-by: kosuke55 * add stop for lane change Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- ...ehavior_path_planner_lane_change_design.md | 159 +++++++++++++----- .../lane_change-stop_at_terminal.drawio.svg | 94 +++++++++++ ...hange-stop_at_terminal_no_block.drawio.svg | 76 +++++++++ ...hange-stop_far_from_target_lane.drawio.svg | 85 ++++++++++ ...ane_change-stop_not_at_terminal.drawio.svg | 70 ++++++++ ..._at_terminal_no_blocking_object.drawio.svg | 76 +++++++++ 6 files changed, 518 insertions(+), 42 deletions(-) create mode 100644 planning/behavior_path_planner/image/lane_change/lane_change-stop_at_terminal.drawio.svg create mode 100644 planning/behavior_path_planner/image/lane_change/lane_change-stop_at_terminal_no_block.drawio.svg create mode 100644 planning/behavior_path_planner/image/lane_change/lane_change-stop_far_from_target_lane.drawio.svg create mode 100644 planning/behavior_path_planner/image/lane_change/lane_change-stop_not_at_terminal.drawio.svg create mode 100644 planning/behavior_path_planner/image/lane_change/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md index 2e5e486afc7c3..ee74f700c4d42 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md @@ -186,6 +186,46 @@ The following figure illustrates when the lane is blocked in multiple lane chang ![multiple-lane-changes](../image/lane_change/lane_change-when_cannot_change_lanes.png) +#### Stopping position when an object exists ahead + +When an obstacle is in front of the ego vehicle, stop with keeping a distance for lane change. +The position to be stopped depends on the situation, such as when the lane change is blocked by the target lane obstacle, or when the lane change is not needed immediately.The following shows the division in that case. + +##### When the ego vehicle is near the end of the lane change + +Regardless of the presence or absence of objects in the lane change target lane, stop by keeping the distance necessary for lane change to the object ahead. + +![stop_at_terminal_no_block](../image/lane_change/lane_change-stop_at_terminal_no_block.drawio.svg) + +![stop_at_terminal](../image/lane_change/lane_change-stop_at_terminal.drawio.svg) + +##### When the ego vehicle is not near the end of the lane change + +If there are NO objects in the lane change section of the target lane, stop by keeping the distance necessary for lane change to the object ahead. + +![stop_not_at_terminal_no_blocking_object](../image/lane_change/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg) + +If there are objects in the lane change section of the target lane, stop WITHOUT keeping the distance necessary for lane change to the object ahead. + +![stop_not_at_terminal](../image/lane_change/lane_change-stop_not_at_terminal.drawio.svg) + +##### When the target lane is far away + +When the target lane for lane change is far away and not next to the current lane, do not keep the distance necessary for lane change to the object ahead. + +![stop_far_from_target_lane](../image/lane_change/lane_change-stop_far_from_target_lane.drawio.svg) + +### Lane Change When Stuck + +The ego vehicle is considered stuck if it is stopped and meets any of the following conditions: + +- There is an obstacle in front of the current lane +- The ego vehicle is at the end of the current lane + +In this case, the safety check for lane change is relaxed compared to normal times. +Please refer to the 'stuck' section under the 'Collision checks during lane change' for more details. +The function to stop by keeping a margin against forward obstacle in the previous section is being performed to achieve this feature. + ### Lane change regulations If you want to regulate lane change on crosswalks or intersections, the lane change module finds a lane change path excluding it includes crosswalks or intersections. @@ -272,35 +312,36 @@ The last behavior will also occur if the ego vehicle has departed from the curre The following parameters are configurable in `lane_change.param.yaml`. -| Name | Unit | Type | Description | Default value | -| :------------------------------------------ | ------ | ------- | --------------------------------------------------------------------------------------------------------------- | ------------------ | -| `backward_lane_length` | [m] | double | The backward length to check incoming objects in lane change target lane. | 200.0 | -| `prepare_duration` | [m] | double | The preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | -| `backward_length_buffer_for_end_of_lane` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change | 2.0 | -| `lane_change_finish_judge_buffer` | [m] | double | The additional buffer used to confirm lane change process completion | 3.0 | -| `finish_judge_lateral_threshold` | [m] | double | Lateral distance threshold to confirm lane change process completion | 0.2 | -| `lane_changing_lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 | -| `minimum_lane_changing_velocity` | [m/s] | double | Minimum speed during lane changing process. | 2.78 | -| `prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 | -| `longitudinal_acceleration_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by longitudinal acceleration | 5 | -| `lateral_acceleration_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by lateral acceleration | 3 | -| `object_check_min_road_shoulder_width` | [m] | double | Width considered as a road shoulder if the lane does not have a road shoulder | 0.5 | -| `object_shiftable_ratio_threshold` | [-] | double | Vehicles around the center line within this distance ratio will be excluded from parking objects | 0.6 | -| `min_length_for_turn_signal_activation` | [m] | double | Turn signal will be activated if the ego vehicle approaches to this length from minimum lane change length | 10.0 | -| `length_ratio_for_turn_signal_deactivation` | [-] | double | Turn signal will be deactivated if the ego vehicle approaches to this length ratio for lane change finish point | 0.8 | -| `max_longitudinal_acc` | [-] | double | maximum longitudinal acceleration for lane change | 1.0 | -| `min_longitudinal_acc` | [-] | double | maximum longitudinal deceleration for lane change | -1.0 | -| `lateral_acceleration.velocity` | [m/s] | double | Reference velocity for lateral acceleration calculation (look up table) | [0.0, 4.0, 10.0] | -| `lateral_acceleration.min_values` | [m/ss] | double | Min lateral acceleration values corresponding to velocity (look up table) | [0.15, 0.15, 0.15] | -| `lateral_acceleration.max_values` | [m/ss] | double | Max lateral acceleration values corresponding to velocity (look up table) | [0.5, 0.5, 0.5] | -| `target_object.car` | [-] | boolean | Include car objects for safety check | true | -| `target_object.truck` | [-] | boolean | Include truck objects for safety check | true | -| `target_object.bus` | [-] | boolean | Include bus objects for safety check | true | -| `target_object.trailer` | [-] | boolean | Include trailer objects for safety check | true | -| `target_object.unknown` | [-] | boolean | Include unknown objects for safety check | true | -| `target_object.bicycle` | [-] | boolean | Include bicycle objects for safety check | true | -| `target_object.motorcycle` | [-] | boolean | Include motorcycle objects for safety check | true | -| `target_object.pedestrian` | [-] | boolean | Include pedestrian objects for safety check | true | +| Name | Unit | Type | Description | Default value | +| :------------------------------------------- | ------ | ------- | ---------------------------------------------------------------------------------------------------------------------- | ------------------ | +| `backward_lane_length` | [m] | double | The backward length to check incoming objects in lane change target lane. | 200.0 | +| `prepare_duration` | [m] | double | The preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | +| `backward_length_buffer_for_end_of_lane` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change | 3.0 | +| `backward_length_buffer_for_blocking_object` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change when there is an object in front | 3.0 | +| `lane_change_finish_judge_buffer` | [m] | double | The additional buffer used to confirm lane change process completion | 3.0 | +| `finish_judge_lateral_threshold` | [m] | double | Lateral distance threshold to confirm lane change process completion | 0.2 | +| `lane_changing_lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 | +| `minimum_lane_changing_velocity` | [m/s] | double | Minimum speed during lane changing process. | 2.78 | +| `prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 | +| `longitudinal_acceleration_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by longitudinal acceleration | 5 | +| `lateral_acceleration_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by lateral acceleration | 3 | +| `object_check_min_road_shoulder_width` | [m] | double | Width considered as a road shoulder if the lane does not have a road shoulder | 0.5 | +| `object_shiftable_ratio_threshold` | [-] | double | Vehicles around the center line within this distance ratio will be excluded from parking objects | 0.6 | +| `min_length_for_turn_signal_activation` | [m] | double | Turn signal will be activated if the ego vehicle approaches to this length from minimum lane change length | 10.0 | +| `length_ratio_for_turn_signal_deactivation` | [-] | double | Turn signal will be deactivated if the ego vehicle approaches to this length ratio for lane change finish point | 0.8 | +| `max_longitudinal_acc` | [-] | double | maximum longitudinal acceleration for lane change | 1.0 | +| `min_longitudinal_acc` | [-] | double | maximum longitudinal deceleration for lane change | -1.0 | +| `lateral_acceleration.velocity` | [m/s] | double | Reference velocity for lateral acceleration calculation (look up table) | [0.0, 4.0, 10.0] | +| `lateral_acceleration.min_values` | [m/ss] | double | Min lateral acceleration values corresponding to velocity (look up table) | [0.15, 0.15, 0.15] | +| `lateral_acceleration.max_values` | [m/ss] | double | Max lateral acceleration values corresponding to velocity (look up table) | [0.5, 0.5, 0.5] | +| `target_object.car` | [-] | boolean | Include car objects for safety check | true | +| `target_object.truck` | [-] | boolean | Include truck objects for safety check | true | +| `target_object.bus` | [-] | boolean | Include bus objects for safety check | true | +| `target_object.trailer` | [-] | boolean | Include trailer objects for safety check | true | +| `target_object.unknown` | [-] | boolean | Include unknown objects for safety check | true | +| `target_object.bicycle` | [-] | boolean | Include bicycle objects for safety check | true | +| `target_object.motorcycle` | [-] | boolean | Include motorcycle objects for safety check | true | +| `target_object.pedestrian` | [-] | boolean | Include pedestrian objects for safety check | true | ### Lane change regulations @@ -320,19 +361,53 @@ The following parameters are configurable in `lane_change.param.yaml`. The following parameters are configurable in `behavior_path_planner.param.yaml` and `lane_change.param.yaml`. -| Name | Unit | Type | Description | Default value | -| :----------------------------------------- | ------- | ------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | -| `lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 2.0 | -| `longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | -| `expected_front_deceleration` | [m/s^2] | double | The front object's maximum deceleration when the front vehicle perform sudden braking. (\*1) | -1.0 | -| `expected_rear_deceleration` | [m/s^2] | double | The rear object's maximum deceleration when the rear vehicle perform sudden braking. (\*1) | -1.0 | -| `rear_vehicle_reaction_time` | [s] | double | The reaction time of the rear vehicle driver which starts from the driver noticing the sudden braking of the front vehicle until the driver step on the brake. | 2.0 | -| `rear_vehicle_safety_time_margin` | [s] | double | The time buffer for the rear vehicle to come into complete stop when its driver perform sudden braking. | 2.0 | -| `enable_collision_check_at_prepare_phase` | [-] | boolean | Perform collision check starting from prepare phase. If `false`, collision check only evaluated for lane changing phase. | true | -| `prepare_phase_ignore_target_speed_thresh` | [m/s] | double | Ignore collision check in prepare phase of object speed that is lesser that the configured value. `enable_collision_check_at_prepare_phase` must be `true` | 0.1 | -| `check_objects_on_current_lanes` | [-] | boolean | If true, the lane change module include objects on current lanes. | true | -| `check_objects_on_other_lanes` | [-] | boolean | If true, the lane change module include objects on other lanes. | true | -| `use_all_predicted_path` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | true | +#### execution + +| Name | Unit | Type | Description | Default value | +| :---------------------------------------------------------------- | ------- | ------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| `safety_check.execution.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 2.0 | +| `safety_check.execution.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | +| `safety_check.execution.expected_front_deceleration` | [m/s^2] | double | The front object's maximum deceleration when the front vehicle perform sudden braking. (\*1) | -1.0 | +| `safety_check.execution.expected_rear_deceleration` | [m/s^2] | double | The rear object's maximum deceleration when the rear vehicle perform sudden braking. (\*1) | -1.0 | +| `safety_check.execution.rear_vehicle_reaction_time` | [s] | double | The reaction time of the rear vehicle driver which starts from the driver noticing the sudden braking of the front vehicle until the driver step on the brake. | 2.0 | +| `safety_check.execution.rear_vehicle_safety_time_margin` | [s] | double | The time buffer for the rear vehicle to come into complete stop when its driver perform sudden braking. | 2.0 | +| `safety_check.execution.enable_collision_check_at_prepare_phase` | [-] | boolean | Perform collision check starting from prepare phase. If `false`, collision check only evaluated for lane changing phase. | true | +| `safety_check.execution.prepare_phase_ignore_target_speed_thresh` | [m/s] | double | Ignore collision check in prepare phase of object speed that is lesser that the configured value. `enable_collision_check_at_prepare_phase` must be `true` | 0.1 | +| `safety_check.execution.check_objects_on_current_lanes` | [-] | boolean | If true, the lane change module include objects on current lanes. | true | +| `safety_check.execution.check_objects_on_other_lanes` | [-] | boolean | If true, the lane change module include objects on other lanes. | true | +| `safety_check.execution.use_all_predicted_path` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | true | + +##### cancel + +| Name | Unit | Type | Description | Default value | +| :------------------------------------------------------------- | ------- | ------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| `safety_check.cancel.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 1.5 | +| `safety_check.cancel.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | +| `safety_check.cancel.expected_front_deceleration` | [m/s^2] | double | The front object's maximum deceleration when the front vehicle perform sudden braking. (\*1) | -1.5 | +| `safety_check.cancel.expected_rear_deceleration` | [m/s^2] | double | The rear object's maximum deceleration when the rear vehicle perform sudden braking. (\*1) | -2.5 | +| `safety_check.cancel.rear_vehicle_reaction_time` | [s] | double | The reaction time of the rear vehicle driver which starts from the driver noticing the sudden braking of the front vehicle until the driver step on the brake. | 2.0 | +| `safety_check.cancel.rear_vehicle_safety_time_margin` | [s] | double | The time buffer for the rear vehicle to come into complete stop when its driver perform sudden braking. | 2.5 | +| `safety_check.cancel.enable_collision_check_at_prepare_phase` | [-] | boolean | Perform collision check starting from prepare phase. If `false`, collision check only evaluated for lane changing phase. | false | +| `safety_check.cancel.prepare_phase_ignore_target_speed_thresh` | [m/s] | double | Ignore collision check in prepare phase of object speed that is lesser that the configured value. `enable_collision_check_at_prepare_phase` must be `true` | 0.2 | +| `safety_check.cancel.check_objects_on_current_lanes` | [-] | boolean | If true, the lane change module include objects on current lanes. | false | +| `safety_check.cancel.check_objects_on_other_lanes` | [-] | boolean | If true, the lane change module include objects on other lanes. | false | +| `safety_check.cancel.use_all_predicted_path` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | false | + +##### stuck + +| Name | Unit | Type | Description | Default value | +| :------------------------------------------------------------ | ------- | ------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| `safety_check.stuck.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 2.0 | +| `safety_check.stuck.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | +| `safety_check.stuck.expected_front_deceleration` | [m/s^2] | double | The front object's maximum deceleration when the front vehicle perform sudden braking. (\*1) | -1.0 | +| `safety_check.stuck.expected_rear_deceleration` | [m/s^2] | double | The rear object's maximum deceleration when the rear vehicle perform sudden braking. (\*1) | -1.0 | +| `safety_check.stuck.rear_vehicle_reaction_time` | [s] | double | The reaction time of the rear vehicle driver which starts from the driver noticing the sudden braking of the front vehicle until the driver step on the brake. | 2.0 | +| `safety_check.stuck.rear_vehicle_safety_time_margin` | [s] | double | The time buffer for the rear vehicle to come into complete stop when its driver perform sudden braking. | 2.0 | +| `safety_check.stuck.enable_collision_check_at_prepare_phase` | [-] | boolean | Perform collision check starting from prepare phase. If `false`, collision check only evaluated for lane changing phase. | true | +| `safety_check.stuck.prepare_phase_ignore_target_speed_thresh` | [m/s] | double | Ignore collision check in prepare phase of object speed that is lesser that the configured value. `enable_collision_check_at_prepare_phase` must be `true` | 0.1 | +| `safety_check.stuck.check_objects_on_current_lanes` | [-] | boolean | If true, the lane change module include objects on current lanes. | true | +| `safety_check.stuck.check_objects_on_other_lanes` | [-] | boolean | If true, the lane change module include objects on other lanes. | true | +| `safety_check.stuck.use_all_predicted_path` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | true | (\*1) the value must be negative. diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-stop_at_terminal.drawio.svg b/planning/behavior_path_planner/image/lane_change/lane_change-stop_at_terminal.drawio.svg new file mode 100644 index 0000000000000..f8aab60d0991d --- /dev/null +++ b/planning/behavior_path_planner/image/lane_change/lane_change-stop_at_terminal.drawio.svg @@ -0,0 +1,94 @@ + + + + + + + + + + + + + + + + + + + + +
+
+
+ lane change margin +
+
+
+ + lane change m... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-stop_at_terminal_no_block.drawio.svg b/planning/behavior_path_planner/image/lane_change/lane_change-stop_at_terminal_no_block.drawio.svg new file mode 100644 index 0000000000000..0de2c5fa0a550 --- /dev/null +++ b/planning/behavior_path_planner/image/lane_change/lane_change-stop_at_terminal_no_block.drawio.svg @@ -0,0 +1,76 @@ + + + + + + + + + + + + + + + + + + +
+
+
+ lane change margin +
+
+
+ + lane change m... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-stop_far_from_target_lane.drawio.svg b/planning/behavior_path_planner/image/lane_change/lane_change-stop_far_from_target_lane.drawio.svg new file mode 100644 index 0000000000000..dedcc13b6ef6a --- /dev/null +++ b/planning/behavior_path_planner/image/lane_change/lane_change-stop_far_from_target_lane.drawio.svg @@ -0,0 +1,85 @@ + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-stop_not_at_terminal.drawio.svg b/planning/behavior_path_planner/image/lane_change/lane_change-stop_not_at_terminal.drawio.svg new file mode 100644 index 0000000000000..938b7b7d4b371 --- /dev/null +++ b/planning/behavior_path_planner/image/lane_change/lane_change-stop_not_at_terminal.drawio.svg @@ -0,0 +1,70 @@ + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg b/planning/behavior_path_planner/image/lane_change/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg new file mode 100644 index 0000000000000..e07a8dcbe0354 --- /dev/null +++ b/planning/behavior_path_planner/image/lane_change/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg @@ -0,0 +1,76 @@ + + + + + + + + + + + + + + + + + + +
+
+
+ lane change margin +
+
+
+ + lane change m... + + + + + + + Text is not SVG - cannot display + + + From b97ff3c595acf33169208fb185ff12e833d27cea Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Mon, 13 Nov 2023 20:17:03 +0900 Subject: [PATCH 2/7] fix(detection_by_tracker): add ignore option for each label (#5473) * fix(detection_by_tracker): add ignore for each class Signed-off-by: badai-nguyen * fix: launch Signed-off-by: badai-nguyen --------- Signed-off-by: badai-nguyen --- ...ra_lidar_fusion_based_detection.launch.xml | 1 + ...ar_radar_fusion_based_detection.launch.xml | 1 + .../detection/detection.launch.xml | 3 ++ .../lidar_based_detection.launch.xml | 1 + .../launch/perception.launch.xml | 2 ++ .../detection_by_tracker/CMakeLists.txt | 2 ++ .../config/detection_by_tracker.param.yaml | 11 ++++++ .../detection_by_tracker_core.hpp | 5 +-- .../include/utils/utils.hpp | 36 +++++++++++++++++++ .../launch/detection_by_tracker.launch.xml | 3 +- .../src/detection_by_tracker_core.cpp | 14 ++++++-- perception/detection_by_tracker/src/utils.cpp | 30 ++++++++++++++++ 12 files changed, 103 insertions(+), 6 deletions(-) create mode 100644 perception/detection_by_tracker/config/detection_by_tracker.param.yaml create mode 100644 perception/detection_by_tracker/include/utils/utils.hpp create mode 100644 perception/detection_by_tracker/src/utils.cpp diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 59fe3f13f1231..b3e9e9148ee60 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -195,6 +195,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index 11deaad5d06cc..20b1b5a4d0b27 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -220,6 +220,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index 1fcf29606891b..33994934949ac 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -73,6 +73,7 @@ + @@ -104,6 +105,7 @@ + @@ -133,6 +135,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml index 822c251ddad33..ac521934265d7 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -61,6 +61,7 @@ + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 347606d91759f..8b241db9774b3 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -18,6 +18,7 @@ + @@ -174,6 +175,7 @@ + diff --git a/perception/detection_by_tracker/CMakeLists.txt b/perception/detection_by_tracker/CMakeLists.txt index 236268438f852..51839027e0e41 100644 --- a/perception/detection_by_tracker/CMakeLists.txt +++ b/perception/detection_by_tracker/CMakeLists.txt @@ -27,6 +27,7 @@ include_directories( # Generate exe file set(DETECTION_BY_TRACKER_SRC src/detection_by_tracker_core.cpp + src/utils.cpp ) ament_auto_add_library(detection_by_tracker_node SHARED @@ -45,4 +46,5 @@ rclcpp_components_register_node(detection_by_tracker_node ament_auto_package(INSTALL_TO_SHARE launch + config ) diff --git a/perception/detection_by_tracker/config/detection_by_tracker.param.yaml b/perception/detection_by_tracker/config/detection_by_tracker.param.yaml new file mode 100644 index 0000000000000..695704050697d --- /dev/null +++ b/perception/detection_by_tracker/config/detection_by_tracker.param.yaml @@ -0,0 +1,11 @@ +/**: + ros__parameters: + tracker_ignore_label: + UNKNOWN : true + CAR : false + TRUCK : false + BUS : false + TRAILER : false + MOTORCYCLE : false + BICYCLE : false + PEDESTRIAN : false diff --git a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp index 0eacfa527750b..10affd0b94ffd 100644 --- a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp +++ b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp @@ -39,6 +39,8 @@ #include #endif +#include "utils/utils.hpp" + #include #include @@ -46,7 +48,6 @@ #include #include #include - class TrackerHandler { private: @@ -81,7 +82,7 @@ class DetectionByTracker : public rclcpp::Node std::map max_search_distance_for_merger_; std::map max_search_distance_for_divider_; - bool ignore_unknown_tracker_; + utils::TrackerIgnoreLabel tracker_ignore_; void setMaxSearchRange(); diff --git a/perception/detection_by_tracker/include/utils/utils.hpp b/perception/detection_by_tracker/include/utils/utils.hpp new file mode 100644 index 0000000000000..3f39125b95e03 --- /dev/null +++ b/perception/detection_by_tracker/include/utils/utils.hpp @@ -0,0 +1,36 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef UTILS__UTILS_HPP_ +#define UTILS__UTILS_HPP_ + +#include + +namespace utils +{ +struct TrackerIgnoreLabel +{ + bool UNKNOWN; + bool CAR; + bool TRUCK; + bool BUS; + bool TRAILER; + bool MOTORCYCLE; + bool BICYCLE; + bool PEDESTRIAN; + bool isIgnore(const uint8_t label) const; +}; // struct TrackerIgnoreLabel +} // namespace utils + +#endif // UTILS__UTILS_HPP_ diff --git a/perception/detection_by_tracker/launch/detection_by_tracker.launch.xml b/perception/detection_by_tracker/launch/detection_by_tracker.launch.xml index 79e1b642cc53c..95e7cbf16b388 100644 --- a/perception/detection_by_tracker/launch/detection_by_tracker.launch.xml +++ b/perception/detection_by_tracker/launch/detection_by_tracker.launch.xml @@ -3,10 +3,11 @@ - + + diff --git a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp index 7b436e1edd64c..2595315e1f9b2 100644 --- a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp +++ b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp @@ -160,7 +160,15 @@ DetectionByTracker::DetectionByTracker(const rclcpp::NodeOptions & node_options) objects_pub_ = create_publisher( "~/output", rclcpp::QoS{1}); - ignore_unknown_tracker_ = declare_parameter("ignore_unknown_tracker", true); + // Set parameters + tracker_ignore_.UNKNOWN = declare_parameter("tracker_ignore_label.UNKNOWN"); + tracker_ignore_.CAR = declare_parameter("tracker_ignore_label.CAR"); + tracker_ignore_.TRUCK = declare_parameter("tracker_ignore_label.TRUCK"); + tracker_ignore_.BUS = declare_parameter("tracker_ignore_label.BUS"); + tracker_ignore_.TRAILER = declare_parameter("tracker_ignore_label.TRAILER"); + tracker_ignore_.MOTORCYCLE = declare_parameter("tracker_ignore_label.MOTORCYCLE"); + tracker_ignore_.BICYCLE = declare_parameter("tracker_ignore_label.BICYCLE"); + tracker_ignore_.PEDESTRIAN = declare_parameter("tracker_ignore_label.PEDESTRIAN"); // set maximum search setting for merger/divider setMaxSearchRange(); @@ -259,7 +267,7 @@ void DetectionByTracker::divideUnderSegmentedObjects( for (const auto & tracked_object : tracked_objects.objects) { const auto & label = tracked_object.classification.front().label; - if (ignore_unknown_tracker_ && (label == Label::UNKNOWN)) continue; + if (tracker_ignore_.isIgnore(label)) continue; // change search range according to label type const float max_search_range = max_search_distance_for_divider_[label]; @@ -395,7 +403,7 @@ void DetectionByTracker::mergeOverSegmentedObjects( for (const auto & tracked_object : tracked_objects.objects) { const auto & label = tracked_object.classification.front().label; - if (ignore_unknown_tracker_ && (label == Label::UNKNOWN)) continue; + if (tracker_ignore_.isIgnore(label)) continue; // change search range according to label type const float max_search_range = max_search_distance_for_merger_[label]; diff --git a/perception/detection_by_tracker/src/utils.cpp b/perception/detection_by_tracker/src/utils.cpp new file mode 100644 index 0000000000000..29a500a24cf32 --- /dev/null +++ b/perception/detection_by_tracker/src/utils.cpp @@ -0,0 +1,30 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "utils/utils.hpp" + +#include + +namespace utils +{ +using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + +bool TrackerIgnoreLabel::isIgnore(const uint8_t label) const +{ + return (label == Label::UNKNOWN && UNKNOWN) || (label == Label::CAR && CAR) || + (label == Label::TRUCK && TRUCK) || (label == Label::BUS && BUS) || + (label == Label::TRAILER && TRAILER) || (label == Label::MOTORCYCLE && MOTORCYCLE) || + (label == Label::BICYCLE && BICYCLE) || (label == Label::PEDESTRIAN && PEDESTRIAN); +} +} // namespace utils From 6e60821fbe8a7ca5e6bd789c9e7401ba98a66fe9 Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Mon, 13 Nov 2023 20:32:22 +0900 Subject: [PATCH 3/7] feat(image_projection_based_fusion): enable to show iou value in roi_cluster_fusion debug image (#5541) * enable to show debug iou value in roi_cluster_fusion Signed-off-by: yoshiri * refactor iou draw settings Signed-off-by: yoshiri * add backgroud color to iou Signed-off-by: yoshiri * prevent object copying when debugger is not enabled Signed-off-by: yoshiri --------- Signed-off-by: yoshiri --- .../debugger.hpp | 1 + .../src/debugger.cpp | 56 ++++++++++++++++--- .../src/fusion_node.cpp | 1 + .../src/roi_cluster_fusion/node.cpp | 18 +++--- 4 files changed, 58 insertions(+), 18 deletions(-) diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/debugger.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/debugger.hpp index 8a6ac7672b3a8..e4b1913effed5 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/debugger.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/debugger.hpp @@ -50,6 +50,7 @@ class Debugger std::vector image_rois_; std::vector obstacle_rois_; std::vector obstacle_points_; + std::vector max_iou_for_image_rois_; private: void imageCallback( diff --git a/perception/image_projection_based_fusion/src/debugger.cpp b/perception/image_projection_based_fusion/src/debugger.cpp index 3f52a0de1f09d..a51c23a77aac6 100644 --- a/perception/image_projection_based_fusion/src/debugger.cpp +++ b/perception/image_projection_based_fusion/src/debugger.cpp @@ -77,6 +77,7 @@ void Debugger::clear() image_rois_.clear(); obstacle_rois_.clear(); obstacle_points_.clear(); + max_iou_for_image_rois_.clear(); } void Debugger::publishImage(const std::size_t image_id, const rclcpp::Time & stamp) @@ -84,6 +85,8 @@ void Debugger::publishImage(const std::size_t image_id, const rclcpp::Time & sta const boost::circular_buffer & image_buffer = image_buffers_.at(image_id); const image_transport::Publisher & image_pub = image_pubs_.at(image_id); + const bool draw_iou_score = + max_iou_for_image_rois_.size() > 0 && max_iou_for_image_rois_.size() == image_rois_.size(); for (std::size_t i = 0; i < image_buffer.size(); ++i) { if (image_buffer.at(i)->header.stamp != stamp) { @@ -91,22 +94,59 @@ void Debugger::publishImage(const std::size_t image_id, const rclcpp::Time & sta } auto cv_ptr = cv_bridge::toCvCopy(image_buffer.at(i), image_buffer.at(i)->encoding); - + // draw obstacle points for (const auto & point : obstacle_points_) { cv::circle( cv_ptr->image, cv::Point(static_cast(point.x()), static_cast(point.y())), 2, cv::Scalar(255, 255, 255), 3, 4); } + + // draw rois + const int img_height = static_cast(image_buffer.at(i)->height); + const int img_width = static_cast(image_buffer.at(i)->width); for (const auto & roi : obstacle_rois_) { - drawRoiOnImage( - cv_ptr->image, roi, image_buffer.at(i)->width, image_buffer.at(i)->height, - cv::Scalar(255, 0, 0)); + drawRoiOnImage(cv_ptr->image, roi, img_width, img_height, cv::Scalar(255, 0, 0)); // blue } - // TODO(yukke42): show iou_score on image for (const auto & roi : image_rois_) { - drawRoiOnImage( - cv_ptr->image, roi, image_buffer.at(i)->width, image_buffer.at(i)->height, - cv::Scalar(0, 0, 255)); + drawRoiOnImage(cv_ptr->image, roi, img_width, img_height, cv::Scalar(0, 0, 255)); // red + } + + // show iou_score on image + if (draw_iou_score) { + for (auto roi_index = 0; roi_index < static_cast(image_rois_.size()); ++roi_index) { + std::stringstream stream; + stream << std::fixed << std::setprecision(2) << max_iou_for_image_rois_.at(roi_index); + std::string iou_score = stream.str(); + + // set text position + int baseline = 3; + cv::Size textSize = cv::getTextSize(iou_score, cv::FONT_HERSHEY_SIMPLEX, 0.5, 1, &baseline); + const int text_height = static_cast(textSize.height); + const int text_width = static_cast(textSize.width); + int x = image_rois_.at(roi_index).x_offset; + int y = image_rois_.at(roi_index).y_offset; // offset for text + if (y < 0 + text_height) + y = text_height; // if roi is on the top of image, put text on lower left of roi + if (y > img_height - text_height) + y = img_height - + text_height; // if roi is on the bottom of image, put text on upper left of roi + if (x > img_width - text_width) + x = img_width - text_width; // if roi is on the right of image, put text on left of roi + if (x < 0) x = 0; // if roi is on the left of image, put text on right of roi + + // choice color by iou score + // cv::Scalar color = max_iou_for_image_rois_.at(i) > 0.5 ? cv::Scalar(0, 255, 0) : + // cv::Scalar(0, 0, 255); + cv::Scalar color = cv::Scalar(0, 0, 255); // red + + cv::rectangle( + cv_ptr->image, cv::Point(x, y - textSize.height - baseline), + cv::Point(x + textSize.width, y), cv::Scalar(255, 255, 255), + cv::FILLED); // white background + cv::putText( + cv_ptr->image, iou_score, cv::Point(x, y), cv::FONT_HERSHEY_SIMPLEX, 0.5, color, 1, + cv::LINE_AA); // text + } } image_pub.publish(cv_ptr->toImageMsg()); diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp index a540688f7e751..df797369208fe 100644 --- a/perception/image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/image_projection_based_fusion/src/fusion_node.cpp @@ -203,6 +203,7 @@ void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_ (*output_msg).header.stamp.sec * (int64_t)1e9 + (*output_msg).header.stamp.nanosec; // if matching rois exist, fuseOnSingle + // please ask maintainers before parallelize this loop because debugger is not thread safe for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { if (camera_info_map_.find(roi_i) == camera_info_map_.end()) { RCLCPP_WARN_THROTTLE( diff --git a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index 4a46c8aace696..9da9df7eff6ca 100644 --- a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -84,10 +84,6 @@ void RoiClusterFusionNode::fuseOnSingleImage( const DetectedObjectsWithFeature & input_roi_msg, const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjectsWithFeature & output_cluster_msg) { - std::vector debug_image_rois; - std::vector debug_pointcloud_rois; - std::vector debug_image_points; - Eigen::Matrix4d projection; projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2), camera_info.p.at(3), camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6), camera_info.p.at(7), @@ -100,6 +96,9 @@ void RoiClusterFusionNode::fuseOnSingleImage( tf_buffer_, /*target*/ camera_info.header.frame_id, /*source*/ input_cluster_msg.header.frame_id, camera_info.header.stamp); if (!transform_stamped_optional) { + RCLCPP_WARN_STREAM( + get_logger(), "Failed to get transform from " << input_cluster_msg.header.frame_id << " to " + << camera_info.header.frame_id); return; } transform_stamped = transform_stamped_optional.value(); @@ -151,7 +150,7 @@ void RoiClusterFusionNode::fuseOnSingleImage( max_x = std::max(static_cast(normalized_projected_point.x()), max_x); max_y = std::max(static_cast(normalized_projected_point.y()), max_y); projected_points.push_back(normalized_projected_point); - debug_image_points.push_back(normalized_projected_point); + if (debugger_) debugger_->obstacle_points_.push_back(normalized_projected_point); } } if (projected_points.empty()) { @@ -165,7 +164,7 @@ void RoiClusterFusionNode::fuseOnSingleImage( roi.width = max_x - min_x; roi.height = max_y - min_y; m_cluster_roi.insert(std::make_pair(i, roi)); - debug_pointcloud_rois.push_back(roi); + if (debugger_) debugger_->obstacle_rois_.push_back(roi); } for (const auto & feature_obj : input_roi_msg.feature_objects) { @@ -231,13 +230,12 @@ void RoiClusterFusionNode::fuseOnSingleImage( } } } - debug_image_rois.push_back(feature_obj.feature.roi); + if (debugger_) debugger_->image_rois_.push_back(feature_obj.feature.roi); + if (debugger_) debugger_->max_iou_for_image_rois_.push_back(max_iou); } + // note: debug objects are safely cleared in fusion_node.cpp if (debugger_) { - debugger_->image_rois_ = debug_image_rois; - debugger_->obstacle_rois_ = debug_pointcloud_rois; - debugger_->obstacle_points_ = debug_image_points; debugger_->publishImage(image_id, input_roi_msg.header.stamp); } } From ea12d77d1e762703201eb85cfea280223b7ab5c9 Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Tue, 14 Nov 2023 08:49:29 +0900 Subject: [PATCH 4/7] refactor(landmark_parser): refactored landmark manager (#5511) * Refactored landmark_parser Signed-off-by: Shintaro Sakoda * Renamed landmark_parser to landmark_manager Signed-off-by: Shintaro Sakoda * Fixed tag_id Signed-off-by: Shintaro Sakoda * Refactored ar_tag_based_localizer Signed-off-by: Shintaro Sakoda * style(pre-commit): autofix * Added [[nodiscard]] Signed-off-by: Shintaro Sakoda * Refactored landmark parsing and conversion Signed-off-by: Shintaro SAKODA * style(pre-commit): autofix * Added namespace Signed-off-by: Shintaro Sakoda * Fixed include Signed-off-by: Shintaro Sakoda --------- Signed-off-by: Shintaro Sakoda Signed-off-by: Shintaro SAKODA Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../landmark_based_localizer/README.md | 6 ++--- .../ar_tag_based_localizer/package.xml | 2 +- .../src/ar_tag_based_localizer.cpp | 9 +++++-- .../src/ar_tag_based_localizer.hpp | 2 +- .../CMakeLists.txt | 6 ++--- .../landmark_manager/landmark_manager.hpp} | 26 ++++++++++++++----- .../package.xml | 5 ++-- .../src/landmark_manager.cpp} | 22 ++++++++++------ 8 files changed, 51 insertions(+), 27 deletions(-) rename localization/landmark_based_localizer/{landmark_parser => landmark_manager}/CMakeLists.txt (79%) rename localization/landmark_based_localizer/{landmark_parser/include/landmark_parser/landmark_parser_core.hpp => landmark_manager/include/landmark_manager/landmark_manager.hpp} (64%) rename localization/landmark_based_localizer/{landmark_parser => landmark_manager}/package.xml (88%) rename localization/landmark_based_localizer/{landmark_parser/src/landmark_parser_core.cpp => landmark_manager/src/landmark_manager.cpp} (92%) diff --git a/localization/landmark_based_localizer/README.md b/localization/landmark_based_localizer/README.md index 35d8d78e7015c..e4c79b7941b12 100644 --- a/localization/landmark_based_localizer/README.md +++ b/localization/landmark_based_localizer/README.md @@ -23,11 +23,11 @@ This calculated ego pose is passed to the EKF, where it is fused with the twist ![node diagram](./doc_image/node_diagram.drawio.svg) -### `landmark_parser` +### `landmark_manager` The definitions of the landmarks written to the map are introduced in the next section. See `Map Specifications`. -The `landmark_parser` is a utility package to load landmarks from the map. +The `landmark_manager` is a utility package to load landmarks from the map. - Translation : The center of the four vertices of the landmark - Rotation : Let the vertex numbers be 1, 2, 3, 4 counterclockwise as shown in the next section. Direction is defined as the cross product of the vector from 1 to 2 and the vector from 2 to 3. @@ -43,7 +43,7 @@ So, if the 4 vertices are considered as forming a tetrahedron and its volume exc ## Map specifications -For this package to work correctly, the poses of the landmarks must be specified in the Lanelet2 map format that `map_loader` and `landmark_parser` can interpret. +For this package to work correctly, the poses of the landmarks must be specified in the Lanelet2 map format that `map_loader` and `landmark_manager` can interpret. The four vertices of a landmark are defined counterclockwise. diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml index a22e7b1acf6dc..24b737c309016 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml @@ -17,7 +17,7 @@ cv_bridge diagnostic_msgs image_transport - landmark_parser + landmark_manager lanelet2_extension localization_util rclcpp diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp index bc40fb1532297..a66277c699a00 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp @@ -148,8 +148,13 @@ bool ArTagBasedLocalizer::setup() void ArTagBasedLocalizer::map_bin_callback(const HADMapBin::ConstSharedPtr & msg) { - landmark_map_ = parse_landmark(msg, "apriltag_16h5", this->get_logger()); - const MarkerArray marker_msg = convert_to_marker_array_msg(landmark_map_); + const std::vector landmarks = + landmark_manager::parse_landmarks(msg, "apriltag_16h5", this->get_logger()); + for (const landmark_manager::Landmark & landmark : landmarks) { + landmark_map_[landmark.id] = landmark.pose; + } + + const MarkerArray marker_msg = landmark_manager::convert_landmarks_to_marker_array_msg(landmarks); marker_pub_->publish(marker_msg); } diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp index fe33a64b995a3..889e76eb78ad2 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp @@ -45,7 +45,7 @@ #ifndef AR_TAG_BASED_LOCALIZER_HPP_ #define AR_TAG_BASED_LOCALIZER_HPP_ -#include "landmark_parser/landmark_parser_core.hpp" +#include "landmark_manager/landmark_manager.hpp" #include #include diff --git a/localization/landmark_based_localizer/landmark_parser/CMakeLists.txt b/localization/landmark_based_localizer/landmark_manager/CMakeLists.txt similarity index 79% rename from localization/landmark_based_localizer/landmark_parser/CMakeLists.txt rename to localization/landmark_based_localizer/landmark_manager/CMakeLists.txt index 41f7c00d61383..1b6126c892d2e 100644 --- a/localization/landmark_based_localizer/landmark_parser/CMakeLists.txt +++ b/localization/landmark_based_localizer/landmark_manager/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(landmark_parser) +project(landmark_manager) find_package(autoware_cmake REQUIRED) autoware_package() @@ -12,8 +12,8 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -ament_auto_add_library(landmark_parser - src/landmark_parser_core.cpp +ament_auto_add_library(landmark_manager + src/landmark_manager.cpp ) ament_auto_package( diff --git a/localization/landmark_based_localizer/landmark_parser/include/landmark_parser/landmark_parser_core.hpp b/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp similarity index 64% rename from localization/landmark_based_localizer/landmark_parser/include/landmark_parser/landmark_parser_core.hpp rename to localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp index edf45c2a2997a..9a22ee13f60ae 100644 --- a/localization/landmark_based_localizer/landmark_parser/include/landmark_parser/landmark_parser_core.hpp +++ b/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp @@ -12,23 +12,35 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LANDMARK_PARSER__LANDMARK_PARSER_CORE_HPP_ -#define LANDMARK_PARSER__LANDMARK_PARSER_CORE_HPP_ +#ifndef LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_ +#define LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_ #include #include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp" #include +#include #include -#include #include +#include -std::map parse_landmark( +namespace landmark_manager +{ + +struct Landmark +{ + std::string id; + geometry_msgs::msg::Pose pose; +}; + +std::vector parse_landmarks( const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg, const std::string & target_subtype, const rclcpp::Logger & logger); -visualization_msgs::msg::MarkerArray convert_to_marker_array_msg( - const std::map & landmarks); +visualization_msgs::msg::MarkerArray convert_landmarks_to_marker_array_msg( + const std::vector & landmarks); + +} // namespace landmark_manager -#endif // LANDMARK_PARSER__LANDMARK_PARSER_CORE_HPP_ +#endif // LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_ diff --git a/localization/landmark_based_localizer/landmark_parser/package.xml b/localization/landmark_based_localizer/landmark_manager/package.xml similarity index 88% rename from localization/landmark_based_localizer/landmark_parser/package.xml rename to localization/landmark_based_localizer/landmark_manager/package.xml index e3daa93f81220..1a35ae6a338d1 100644 --- a/localization/landmark_based_localizer/landmark_parser/package.xml +++ b/localization/landmark_based_localizer/landmark_manager/package.xml @@ -1,9 +1,9 @@ - landmark_parser + landmark_manager 0.0.0 - The landmark_parser package + The landmark_manager package Shintaro Sakoda Masahiro Sakamoto Yamato Ando @@ -19,6 +19,7 @@ geometry_msgs lanelet2_extension rclcpp + tf2_eigen ament_cmake diff --git a/localization/landmark_based_localizer/landmark_parser/src/landmark_parser_core.cpp b/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp similarity index 92% rename from localization/landmark_based_localizer/landmark_parser/src/landmark_parser_core.cpp rename to localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp index c86b35b6115b4..6d1698daf5eae 100644 --- a/localization/landmark_based_localizer/landmark_parser/src/landmark_parser_core.cpp +++ b/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp @@ -12,16 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "landmark_parser/landmark_parser_core.hpp" +#include "landmark_manager/landmark_manager.hpp" #include "lanelet2_extension/utility/message_conversion.hpp" #include +#include #include #include -std::map parse_landmark( +namespace landmark_manager +{ + +std::vector parse_landmarks( const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg, const std::string & target_subtype, const rclcpp::Logger & logger) { @@ -32,7 +36,7 @@ std::map parse_landmark( lanelet::LaneletMapPtr lanelet_map_ptr{std::make_shared()}; lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_ptr); - std::map landmark_map; + std::vector landmarks; for (const auto & poly : lanelet_map_ptr->polygonLayer) { const std::string type{poly.attributeOr(lanelet::AttributeName::Type, "none")}; @@ -93,8 +97,8 @@ std::map parse_landmark( pose.orientation.z = q.z(); pose.orientation.w = q.w(); - // Add to map - landmark_map[marker_id] = pose; + // Add + landmarks.push_back(Landmark{marker_id, pose}); RCLCPP_INFO_STREAM(logger, "id: " << marker_id); RCLCPP_INFO_STREAM( logger, @@ -104,11 +108,11 @@ std::map parse_landmark( << pose.orientation.z << ", " << pose.orientation.w); } - return landmark_map; + return landmarks; } -visualization_msgs::msg::MarkerArray convert_to_marker_array_msg( - const std::map & landmarks) +visualization_msgs::msg::MarkerArray convert_landmarks_to_marker_array_msg( + const std::vector & landmarks) { int32_t id = 0; visualization_msgs::msg::MarkerArray marker_array; @@ -152,3 +156,5 @@ visualization_msgs::msg::MarkerArray convert_to_marker_array_msg( } return marker_array; } + +} // namespace landmark_manager From 60a51e005d99422f726e2388989dac46335e3ce0 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Tue, 14 Nov 2023 11:03:49 +0900 Subject: [PATCH 5/7] feat(obstacle_cruise_planner): use obstacle velocity based obstacle parameters (#5510) * Add extra tag for moving obstacle type Signed-off-by: Daniel Sanchez * add static and moving as parameter postfixes Signed-off-by: Daniel Sanchez * set hysteresis-based obstacle moving classification Signed-off-by: Daniel Sanchez * update config params Signed-off-by: Daniel Sanchez * Use speed norm for object classification Signed-off-by: Daniel Sanchez * changed '_' for '.' to make the code more consistent Signed-off-by: Daniel Sanchez * update documentation Signed-off-by: Daniel Sanchez * move the obstacle moving check to generateSlowDownTrajectory Signed-off-by: Daniel Sanchez * remove unnecessary reference Signed-off-by: Daniel Sanchez * add const to certain variables Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez Signed-off-by: Daniel Sanchez --- planning/obstacle_cruise_planner/README.md | 32 +++--- .../config/obstacle_cruise_planner.param.yaml | 31 ++++-- .../planner_interface.hpp | 104 +++++++++++------- .../src/planner_interface.cpp | 24 ++-- 4 files changed, 120 insertions(+), 71 deletions(-) diff --git a/planning/obstacle_cruise_planner/README.md b/planning/obstacle_cruise_planner/README.md index 260f302791079..532761b1f0cb7 100644 --- a/planning/obstacle_cruise_planner/README.md +++ b/planning/obstacle_cruise_planner/README.md @@ -230,19 +230,25 @@ $$ ### Slow down planning -| Parameter | Type | Description | -| ----------------------------------------------- | -------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | -| `slow_down.labels` | vector(string) | A vector of labels for customizing obstacle-label-based slow down behavior. Each label represents an obstacle type that will be treated differently when applying slow down. The possible labels are ("default" (Mandatory), "unknown","car","truck","bus","trailer","motorcycle","bicycle" or "pedestrian") | -| `slow_down.default.min_lat_velocity` | double | minimum velocity to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels | -| `slow_down.default.max_lat_velocity` | double | maximum velocity to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels | -| `slow_down.default.min_lat_margin` | double | minimum lateral margin to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels | -| `slow_down.default.max_lat_margin` | double | maximum lateral margin to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels | -| `(optional) slow_down."label".min_lat_velocity` | double | minimum velocity to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels` | -| `(optional) slow_down."label".max_lat_velocity` | double | maximum velocity to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels` | -| `(optional) slow_down."label".min_lat_margin` | double | minimum lateral margin to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels` | -| `(optional) slow_down."label".max_lat_margin` | double | maximum lateral margin to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels` | - -The role of the slow down planning is inserting slow down velocity in the trajectory where the trajectory points are close to the obstacles. The parameters can be customized depending on the obstacle type (see `slow_down.labels`), making it possible to adjust the slow down behavior depending if the obstacle is a pedestrian, bicycle, car, etc. +| Parameter | Type | Description | +| ----------------------------------------------------------------- | -------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | +| `slow_down.labels` | vector(string) | A vector of labels for customizing obstacle-label-based slow down behavior. Each label represents an obstacle type that will be treated differently when applying slow down. The possible labels are ("default" (Mandatory), "unknown","car","truck","bus","trailer","motorcycle","bicycle" or "pedestrian") | +| `slow_down.default.static.min_lat_velocity` | double | minimum velocity to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels and the obstacle is considered to be static, or not moving | +| `slow_down.default.static.max_lat_velocity` | double | maximum velocity to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels and the obstacle is considered to be static, or not moving | +| `slow_down.default.static.min_lat_margin` | double | minimum lateral margin to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels and the obstacle is considered to be static, or not moving | +| `slow_down.default.static.max_lat_margin` | double | maximum lateral margin to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels and the obstacle is considered to be static, or not moving | +| `slow_down.default.moving.min_lat_velocity` | double | minimum velocity to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels and the obstacle is considered to be moving | +| `slow_down.default.moving.max_lat_velocity` | double | maximum velocity to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels and the obstacle is considered to be moving | +| `slow_down.default.moving.min_lat_margin` | double | minimum lateral margin to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels and the obstacle is considered to be moving | +| `slow_down.default.moving.max_lat_margin` | double | maximum lateral margin to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels and the obstacle is considered to be moving | +| `(optional) slow_down."label".(static & moving).min_lat_velocity` | double | minimum velocity to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels`. Requires a `static` and a `moving` value | +| `(optional) slow_down."label".(static & moving).max_lat_velocity` | double | maximum velocity to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels`. Requires a `static` and a `moving` value | +| `(optional) slow_down."label".(static & moving).min_lat_margin` | double | minimum lateral margin to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels`. Requires a `static` and a `moving` value | +| `(optional) slow_down."label".(static & moving).max_lat_margin` | double | maximum lateral margin to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels`. Requires a `static` and a `moving` value | + +The role of the slow down planning is inserting slow down velocity in the trajectory where the trajectory points are close to the obstacles. The parameters can be customized depending on the obstacle type (see `slow_down.labels`), making it possible to adjust the slow down behavior depending if the obstacle is a pedestrian, bicycle, car, etc. Each obstacle type has a `static` and a `moving` parameter set, so it is possible to customize the slow down response of the ego vehicle according to the obstacle type and if it is moving or not. If an obstacle is determined to be moving, the corresponding `moving` set of parameters will be used to compute the vehicle slow down, otherwise, the `static` parameters will be used. The `static` and `moving` separation is useful for customizing the ego vehicle slow down behavior to, for example, slow down more significantly when passing stopped vehicles that might cause occlusion or that might suddenly open its doors. + +An obstacle is classified as `static` if its total speed is less than the `moving_object_speed_threshold` parameter. Furthermore, a hysteresis based approach is used to avoid chattering, it uses the `moving_object_hysteresis_range` parameter range and the obstacle's previous state (`moving` or `static`) to determine if the obstacle is moving or not. In other words, if an obstacle was previously classified as `static`, it will not change its classification to `moving` unless its total speed is greater than `moving_object_speed_threshold` + `moving_object_hysteresis_range`. Likewise, an obstacle previously classified as `moving`, will only change to `static` if its speed is lower than `moving_object_speed_threshold` - `moving_object_hysteresis_range`. The closest point on the obstacle to the ego's trajectory is calculated. Then, the slow down velocity is calculated by linear interpolation with the distance between the point and trajectory as follows. diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index 123d0dda93b7a..def2a2af78b37 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -183,15 +183,30 @@ - "default" - "pedestrian" default: - min_lat_margin: 0.2 - max_lat_margin: 1.0 - min_ego_velocity: 2.0 - max_ego_velocity: 8.0 + moving: + min_lat_margin: 0.8 + max_lat_margin: 1.3 + min_ego_velocity: 0.5 + max_ego_velocity: 1.5 + static: + min_lat_margin: 0.2 + max_lat_margin: 1.0 + min_ego_velocity: 2.0 + max_ego_velocity: 8.0 pedestrian: - min_lat_margin: 0.5 - max_lat_margin: 1.5 - min_ego_velocity: 1.0 - max_ego_velocity: 2.0 + moving: + min_lat_margin: 0.8 + max_lat_margin: 1.3 + min_ego_velocity: 0.5 + max_ego_velocity: 0.8 + static: + min_lat_margin: 0.8 + max_lat_margin: 1.3 + min_ego_velocity: 1.0 + max_ego_velocity: 2.0 + + moving_object_speed_threshold: 0.5 # [m/s] how fast the object needs to move to be considered as "moving" + moving_object_hysteresis_range: 0.1 # [m/s] hysteresis range used to prevent chattering when obstacle moves close to moving_object_speed_threshold time_margin_on_target_velocity: 1.5 # [s] diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp index d8c31fb35df9b..2e1e165a78952 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp @@ -49,6 +49,11 @@ class PlannerInterface node.create_publisher("/planning/velocity_factors/obstacle_cruise", 1); stop_speed_exceeded_pub_ = node.create_publisher("~/output/stop_speed_exceeded", 1); + + moving_object_speed_threshold = + node.declare_parameter("slow_down.moving_object_speed_threshold"); + moving_object_hysteresis_range = + node.declare_parameter("slow_down.moving_object_hysteresis_range"); } PlannerInterface() = default; @@ -183,11 +188,12 @@ class PlannerInterface const std::string & arg_uuid, const std::vector & traj_points, const std::optional & start_idx, const std::optional & end_idx, const double arg_target_vel, const double arg_feasible_target_vel, - const double arg_precise_lat_dist) + const double arg_precise_lat_dist, const bool is_moving) : uuid(arg_uuid), target_vel(arg_target_vel), feasible_target_vel(arg_feasible_target_vel), - precise_lat_dist(arg_precise_lat_dist) + precise_lat_dist(arg_precise_lat_dist), + is_moving(is_moving) { if (start_idx) { start_point = traj_points.at(*start_idx).pose; @@ -203,15 +209,17 @@ class PlannerInterface double precise_lat_dist; std::optional start_point{std::nullopt}; std::optional end_point{std::nullopt}; + bool is_moving; }; double calculateMarginFromObstacleOnCurve( const PlannerData & planner_data, const StopObstacle & stop_obstacle) const; double calculateSlowDownVelocity( - const SlowDownObstacle & obstacle, const std::optional & prev_output) const; + const SlowDownObstacle & obstacle, const std::optional & prev_output, + const bool is_obstacle_moving) const; std::optional> calculateDistanceToSlowDownWithConstraints( const PlannerData & planner_data, const std::vector & traj_points, const SlowDownObstacle & obstacle, const std::optional & prev_output, - const double dist_to_ego) const; + const double dist_to_ego, const bool is_obstacle_moving) const; struct SlowDownInfo { @@ -223,6 +231,7 @@ class PlannerInterface struct SlowDownParam { std::vector obstacle_labels{"default"}; + std::vector obstacle_moving_classification{"static", "moving"}; std::unordered_map types_map; struct ObstacleSpecificParams { @@ -233,28 +242,23 @@ class PlannerInterface }; explicit SlowDownParam(rclcpp::Node & node) { - types_map = {{ObjectClassification::UNKNOWN, "unknown"}, - {ObjectClassification::CAR, "car"}, - {ObjectClassification::TRUCK, "truck"}, - {ObjectClassification::BUS, "bus"}, - {ObjectClassification::TRAILER, "trailer"}, - {ObjectClassification::MOTORCYCLE, "motorcycle"}, - {ObjectClassification::BICYCLE, "bicycle"}, - {ObjectClassification::PEDESTRIAN, "pedestrian"}}; obstacle_labels = node.declare_parameter>("slow_down.labels", obstacle_labels); // obstacle label dependant parameters for (const auto & label : obstacle_labels) { - ObstacleSpecificParams params; - params.max_lat_margin = - node.declare_parameter("slow_down." + label + ".max_lat_margin"); - params.min_lat_margin = - node.declare_parameter("slow_down." + label + ".min_lat_margin"); - params.max_ego_velocity = - node.declare_parameter("slow_down." + label + ".max_ego_velocity"); - params.min_ego_velocity = - node.declare_parameter("slow_down." + label + ".min_ego_velocity"); - obstacle_to_param_struct_map.emplace(std::make_pair(label, params)); + for (const auto & movement_postfix : obstacle_moving_classification) { + ObstacleSpecificParams params; + params.max_lat_margin = node.declare_parameter( + "slow_down." + label + "." + movement_postfix + ".max_lat_margin"); + params.min_lat_margin = node.declare_parameter( + "slow_down." + label + "." + movement_postfix + ".min_lat_margin"); + params.max_ego_velocity = node.declare_parameter( + "slow_down." + label + "." + movement_postfix + ".max_ego_velocity"); + params.min_ego_velocity = node.declare_parameter( + "slow_down." + label + "." + movement_postfix + ".min_ego_velocity"); + obstacle_to_param_struct_map.emplace( + std::make_pair(label + "." + movement_postfix, params)); + } } // common parameters @@ -264,34 +268,49 @@ class PlannerInterface lpf_gain_lat_dist = node.declare_parameter("slow_down.lpf_gain_lat_dist"); lpf_gain_dist_to_slow_down = node.declare_parameter("slow_down.lpf_gain_dist_to_slow_down"); + + types_map = {{ObjectClassification::UNKNOWN, "unknown"}, + {ObjectClassification::CAR, "car"}, + {ObjectClassification::TRUCK, "truck"}, + {ObjectClassification::BUS, "bus"}, + {ObjectClassification::TRAILER, "trailer"}, + {ObjectClassification::MOTORCYCLE, "motorcycle"}, + {ObjectClassification::BICYCLE, "bicycle"}, + {ObjectClassification::PEDESTRIAN, "pedestrian"}}; } - ObstacleSpecificParams getObstacleParamByLabel(const ObjectClassification & label_id) const + ObstacleSpecificParams getObstacleParamByLabel( + const ObjectClassification & label_id, const bool is_obstacle_moving) const { - const std::string label = types_map.at(label_id.label); - if (obstacle_to_param_struct_map.count(label) > 0) { - return obstacle_to_param_struct_map.at(label); - } - return obstacle_to_param_struct_map.at("default"); + const std::string label = + (types_map.count(label_id.label) > 0) ? types_map.at(label_id.label) : "default"; + const std::string movement_postfix = (is_obstacle_moving) ? "moving" : "static"; + return (obstacle_to_param_struct_map.count(label + "." + movement_postfix) > 0) + ? obstacle_to_param_struct_map.at(label + "." + movement_postfix) + : obstacle_to_param_struct_map.at("default." + movement_postfix); } void onParam(const std::vector & parameters) { // obstacle type dependant parameters for (const auto & label : obstacle_labels) { - auto & param_by_obstacle_label = obstacle_to_param_struct_map[label]; - tier4_autoware_utils::updateParam( - parameters, "slow_down." + label + ".max_lat_margin", - param_by_obstacle_label.max_lat_margin); - tier4_autoware_utils::updateParam( - parameters, "slow_down." + label + ".min_lat_margin", - param_by_obstacle_label.min_lat_margin); - tier4_autoware_utils::updateParam( - parameters, "slow_down." + label + ".max_ego_velocity", - param_by_obstacle_label.max_ego_velocity); - tier4_autoware_utils::updateParam( - parameters, "slow_down." + label + ".min_ego_velocity", - param_by_obstacle_label.min_ego_velocity); + for (const auto & movement_postfix : obstacle_moving_classification) { + if (obstacle_to_param_struct_map.count(label + "." + movement_postfix) < 1) continue; + auto & param_by_obstacle_label = + obstacle_to_param_struct_map.at(label + "." + movement_postfix); + tier4_autoware_utils::updateParam( + parameters, "slow_down." + label + "." + movement_postfix + ".max_lat_margin", + param_by_obstacle_label.max_lat_margin); + tier4_autoware_utils::updateParam( + parameters, "slow_down." + label + "." + movement_postfix + ".min_lat_margin", + param_by_obstacle_label.min_lat_margin); + tier4_autoware_utils::updateParam( + parameters, "slow_down." + label + "." + movement_postfix + ".max_ego_velocity", + param_by_obstacle_label.max_ego_velocity); + tier4_autoware_utils::updateParam( + parameters, "slow_down." + label + "." + movement_postfix + ".min_ego_velocity", + param_by_obstacle_label.min_ego_velocity); + } } // common parameters @@ -313,7 +332,8 @@ class PlannerInterface double lpf_gain_dist_to_slow_down; }; SlowDownParam slow_down_param_; - + double moving_object_speed_threshold; + double moving_object_hysteresis_range; std::vector prev_slow_down_output_; // previous trajectory and distance to stop // NOTE: Previous trajectory is memorized to deal with nearest index search for overlapping or diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index 76469364cfb39..47f62016df53b 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -561,9 +561,17 @@ std::vector PlannerInterface::generateSlowDownTrajectory( const auto & obstacle = obstacles.at(i); const auto prev_output = getObjectFromUuid(prev_slow_down_output_, obstacle.uuid); + const bool is_obstacle_moving = [&]() -> bool { + const auto object_vel_norm = std::hypot(obstacle.velocity, obstacle.lat_velocity); + if (!prev_output) return object_vel_norm > moving_object_speed_threshold; + if (prev_output->is_moving) + return object_vel_norm > moving_object_speed_threshold - moving_object_hysteresis_range; + return object_vel_norm > moving_object_speed_threshold + moving_object_hysteresis_range; + }(); + // calculate slow down start distance, and insert slow down velocity const auto dist_vec_to_slow_down = calculateDistanceToSlowDownWithConstraints( - planner_data, slow_down_traj_points, obstacle, prev_output, dist_to_ego); + planner_data, slow_down_traj_points, obstacle, prev_output, dist_to_ego, is_obstacle_moving); if (!dist_vec_to_slow_down) { RCLCPP_INFO_EXPRESSION( rclcpp::get_logger("ObstacleCruisePlanner::PlannerInterface"), enable_debug_info_, @@ -648,7 +656,7 @@ std::vector PlannerInterface::generateSlowDownTrajectory( // update prev_slow_down_output_ new_prev_slow_down_output.push_back(SlowDownOutput{ obstacle.uuid, slow_down_traj_points, slow_down_start_idx, slow_down_end_idx, - stable_slow_down_vel, feasible_slow_down_vel, obstacle.precise_lat_dist}); + stable_slow_down_vel, feasible_slow_down_vel, obstacle.precise_lat_dist, is_obstacle_moving}); } // update prev_slow_down_output_ @@ -663,10 +671,11 @@ std::vector PlannerInterface::generateSlowDownTrajectory( } double PlannerInterface::calculateSlowDownVelocity( - const SlowDownObstacle & obstacle, const std::optional & prev_output) const + const SlowDownObstacle & obstacle, const std::optional & prev_output, + const bool is_obstacle_moving) const { - const auto & p = slow_down_param_.getObstacleParamByLabel(obstacle.classification); - + const auto & p = + slow_down_param_.getObstacleParamByLabel(obstacle.classification, is_obstacle_moving); const double stable_precise_lat_dist = [&]() { if (prev_output) { return signal_processing::lowpassFilter( @@ -689,15 +698,14 @@ std::optional> PlannerInterface::calculateDistanceToSlowDownWithConstraints( const PlannerData & planner_data, const std::vector & traj_points, const SlowDownObstacle & obstacle, const std::optional & prev_output, - const double dist_to_ego) const + const double dist_to_ego, const bool is_obstacle_moving) const { const double abs_ego_offset = planner_data.is_driving_forward ? std::abs(vehicle_info_.max_longitudinal_offset_m) : std::abs(vehicle_info_.min_longitudinal_offset_m); const double obstacle_vel = obstacle.velocity; - // calculate slow down velocity - const double slow_down_vel = calculateSlowDownVelocity(obstacle, prev_output); + const double slow_down_vel = calculateSlowDownVelocity(obstacle, prev_output, is_obstacle_moving); // calculate distance to collision points const double dist_to_front_collision = From ef0569f5d0e94282454951f94a5abe2963b7b79c Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 14 Nov 2023 11:31:36 +0900 Subject: [PATCH 6/7] refactor(side_shift): support new interface (#5556) Signed-off-by: satoshi-ota --- .../side_shift/side_shift_module.hpp | 26 ++----------------- .../side_shift/side_shift_module.cpp | 14 ++++------ 2 files changed, 7 insertions(+), 33 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp index 7c04cab5c081f..fce31b6db7369 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp @@ -49,8 +49,6 @@ class SideShiftModule : public SceneModuleInterface bool isExecutionReady() const override; bool isReadyForNextRequest( const double & min_request_time_sec, bool override_requests = false) const noexcept; - // TODO(someone): remove this, and use base class function - [[deprecated]] ModuleStatus updateState() override; void updateData() override; BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; @@ -70,32 +68,12 @@ class SideShiftModule : public SceneModuleInterface { } - // TODO(someone): remove this, and use base class function - [[deprecated]] BehaviorModuleOutput run() override - { - updateData(); - - if (!isWaitingApproval()) { - return plan(); - } - - // module is waiting approval. Check it. - if (isActivated()) { - RCLCPP_DEBUG(getLogger(), "Was waiting approval, and now approved. Do plan()."); - return plan(); - } else { - RCLCPP_DEBUG(getLogger(), "keep waiting approval... Do planCandidate()."); - return planWaitingApproval(); - } - } - private: - bool canTransitSuccessState() override { return false; } + bool canTransitSuccessState() override; bool canTransitFailureState() override { return false; } - bool canTransitIdleToRunningState() override { return false; } - rclcpp::Subscription::SharedPtr lateral_offset_subscriber_; + bool canTransitIdleToRunningState() override { return true; } void initVariables(); diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp index 2232c6d750d55..46332e738f82f 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp @@ -40,8 +40,6 @@ SideShiftModule::SideShiftModule( const std::unordered_map > & rtc_interface_ptr_map) : SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{parameters} { - // If lateral offset is subscribed, it approves side shift module automatically - clearWaitingApproval(); } void SideShiftModule::initVariables() @@ -80,7 +78,7 @@ void SideShiftModule::setParameters(const std::shared_ptr & bool SideShiftModule::isExecutionRequested() const { - if (current_state_ == ModuleStatus::RUNNING) { + if (getCurrentStatus() == ModuleStatus::RUNNING) { return true; } @@ -112,7 +110,7 @@ bool SideShiftModule::isReadyForNextRequest( return false; } -ModuleStatus SideShiftModule::updateState() +bool SideShiftModule::canTransitSuccessState() { // Never return the FAILURE. When the desired offset is zero and the vehicle is in the original // drivable area,this module can stop the computation and return SUCCESS. @@ -150,7 +148,7 @@ ModuleStatus SideShiftModule::updateState() no_shifted_plan); if (no_request && no_shifted_plan && no_offset_diff) { - return ModuleStatus::SUCCESS; + return true; } const auto & current_lanes = utils::getCurrentLanes(planner_data_); @@ -169,7 +167,7 @@ ModuleStatus SideShiftModule::updateState() } else { shift_status_ = SideShiftStatus::AFTER_SHIFT; } - return ModuleStatus::RUNNING; + return false; } void SideShiftModule::updateData() @@ -184,7 +182,7 @@ void SideShiftModule::updateData() } } - if (current_state_ != ModuleStatus::RUNNING && current_state_ != ModuleStatus::IDLE) { + if (getCurrentStatus() != ModuleStatus::RUNNING && getCurrentStatus() != ModuleStatus::IDLE) { return; } @@ -331,8 +329,6 @@ BehaviorModuleOutput SideShiftModule::planWaitingApproval() prev_output_ = shifted_path; - waitApproval(); - return output; } From 6e6cb40e5f91715266d9d4d15f8a0cc89a586709 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 14 Nov 2023 12:46:44 +0900 Subject: [PATCH 7/7] refactor(goal_planner): add updateStatus and reduce variables (#5546) * refactor(goal_planner): add updateStatus and reduce variables Signed-off-by: kosuke55 refactor Signed-off-by: kosuke55 refactor Signed-off-by: kosuke55 * move to updatedata Signed-off-by: kosuke55 * Update planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp Co-authored-by: Kyoichi Sugahara * style(pre-commit): autofix --------- Signed-off-by: kosuke55 Co-authored-by: Kyoichi Sugahara Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../goal_planner/goal_planner_module.hpp | 56 ++-- .../goal_planner/pull_over_planner_base.hpp | 1 + .../goal_planner/goal_planner_module.cpp | 272 +++++++++--------- 3 files changed, 152 insertions(+), 177 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index e4044d7191805..5149979ef082d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -93,50 +93,30 @@ class PullOverStatus void reset() { lane_parking_pull_over_path_ = nullptr; - current_path_idx_ = 0; - require_increment_ = true; prev_stop_path_ = nullptr; prev_stop_path_after_approval_ = nullptr; - current_lanes_.clear(); - pull_over_lanes_.clear(); - lanes_.clear(); - has_decided_path_ = false; - is_safe_dynamic_objects_ = false; + + is_safe_ = false; prev_found_path_ = false; - prev_is_safe_dynamic_objects_ = false; - has_decided_velocity_ = false; + prev_is_safe_ = false; } DEFINE_SETTER_GETTER(std::shared_ptr, lane_parking_pull_over_path) - DEFINE_SETTER_GETTER(size_t, current_path_idx) - DEFINE_SETTER_GETTER(bool, require_increment) DEFINE_SETTER_GETTER(std::shared_ptr, prev_stop_path) DEFINE_SETTER_GETTER(std::shared_ptr, prev_stop_path_after_approval) - DEFINE_SETTER_GETTER(lanelet::ConstLanelets, current_lanes) - DEFINE_SETTER_GETTER(lanelet::ConstLanelets, pull_over_lanes) - DEFINE_SETTER_GETTER(std::vector, lanes) - DEFINE_SETTER_GETTER(bool, has_decided_path) - DEFINE_SETTER_GETTER(bool, is_safe_dynamic_objects) + DEFINE_SETTER_GETTER(bool, is_safe) DEFINE_SETTER_GETTER(bool, prev_found_path) - DEFINE_SETTER_GETTER(bool, prev_is_safe_dynamic_objects) - DEFINE_SETTER_GETTER(bool, has_decided_velocity) + DEFINE_SETTER_GETTER(bool, prev_is_safe) DEFINE_SETTER_GETTER(Pose, refined_goal_pose) DEFINE_SETTER_GETTER(Pose, closest_goal_candidate_pose) private: std::shared_ptr lane_parking_pull_over_path_{nullptr}; - size_t current_path_idx_{0}; - bool require_increment_{true}; std::shared_ptr prev_stop_path_{nullptr}; std::shared_ptr prev_stop_path_after_approval_{nullptr}; - lanelet::ConstLanelets current_lanes_{}; - lanelet::ConstLanelets pull_over_lanes_{}; - std::vector lanes_{}; - bool has_decided_path_{false}; - bool is_safe_dynamic_objects_{false}; + bool is_safe_{false}; bool prev_found_path_{false}; - bool prev_is_safe_dynamic_objects_{false}; - bool has_decided_velocity_{false}; + bool prev_is_safe_{false}; Pose refined_goal_pose_{}; Pose closest_goal_candidate_pose_{}; @@ -177,6 +157,10 @@ class ThreadSafeData bool incrementPathIndex() { const std::lock_guard lock(mutex_); + if (!pull_over_path_) { + return false; + } + if (pull_over_path_->incrementPathIndex()) { last_path_idx_increment_time_ = clock_->now(); return true; @@ -393,10 +377,10 @@ class GoalPlannerModule : public SceneModuleInterface void decelerateForTurnSignal(const Pose & stop_pose, PathWithLaneId & path) const; void decelerateBeforeSearchStart( const Pose & search_start_offset_pose, PathWithLaneId & path) const; - PathWithLaneId generateStopPath(); - PathWithLaneId generateFeasibleStopPath(); + PathWithLaneId generateStopPath() const; + PathWithLaneId generateFeasibleStopPath() const; - void keepStoppedWithCurrentPath(PathWithLaneId & path); + void keepStoppedWithCurrentPath(PathWithLaneId & path) const; double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const; // status @@ -411,6 +395,7 @@ class GoalPlannerModule : public SceneModuleInterface bool hasDecidedPath() const; void decideVelocity(); bool foundPullOverPath() const; + void updateStatus(const BehaviorModuleOutput & output); // validation bool hasEnoughDistance(const PullOverPath & pull_over_path) const; @@ -433,16 +418,13 @@ class GoalPlannerModule : public SceneModuleInterface const std::vector & pull_over_path_candidates, const GoalCandidates & goal_candidates) const; - // deal with pull over partial paths - void transitionToNextPathIfFinishingCurrentPath(); - // lanes and drivable area - void setLanes(); + std::vector generateDrivableLanes() const; void setDrivableAreaInfo(BehaviorModuleOutput & output) const; // output setter - void setOutput(BehaviorModuleOutput & output); - void setStopPath(BehaviorModuleOutput & output); + void setOutput(BehaviorModuleOutput & output) const; + void setStopPath(BehaviorModuleOutput & output) const; /** * @brief Sets a stop path in the current path based on safety conditions and previous paths. @@ -453,7 +435,7 @@ class GoalPlannerModule : public SceneModuleInterface * * @param output BehaviorModuleOutput */ - void setStopPathFromCurrentPath(BehaviorModuleOutput & output); + void setStopPathFromCurrentPath(BehaviorModuleOutput & output) const; void setModifiedGoal(BehaviorModuleOutput & output) const; void setTurnSignalInfo(BehaviorModuleOutput & output) const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp index 4267261fdfe84..acb034a29d9e5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp @@ -53,6 +53,7 @@ struct PullOverPath Pose end_pose{}; std::vector debug_poses{}; size_t goal_id{}; + bool decided_velocity{false}; PathWithLaneId getFullPath() const { diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 0475cd92f1bec..202c19f1bf1db 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -255,10 +255,21 @@ void GoalPlannerModule::updateData() updateOccupancyGrid(); - // set current road lanes, pull over lanes, and drivable lane - setLanes(); - generateGoalCandidates(); + + if (!isActivated()) { + return; + } + + if (hasFinishedCurrentPath()) { + thread_safe_data_.incrementPathIndex(); + } + + if (!last_approval_data_) { + last_approval_data_ = + std::make_unique(clock_->now(), planner_data_->self_odometry->pose.pose); + decideVelocity(); + } } void GoalPlannerModule::initializeOccupancyGridMap() @@ -524,9 +535,7 @@ void GoalPlannerModule::returnToLaneParking() return; } - status_.set_has_decided_path(false); thread_safe_data_.set_pull_over_path(status_.get_lane_parking_pull_over_path()); - RCLCPP_INFO(getLogger(), "return to lane parking"); } @@ -549,9 +558,6 @@ void GoalPlannerModule::generateGoalCandidates() goal_searcher_->setPlannerData(planner_data_); goal_searcher_->setReferenceGoal(status_.get_refined_goal_pose()); thread_safe_data_.set_goal_candidates(goal_searcher_->search()); - const auto current_lanes = utils::getExtendedCurrentLanes( - planner_data_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length, false); status_.set_closest_goal_candidate_pose( goal_searcher_->getClosetGoalCandidateAlongLanes(thread_safe_data_.get_goal_candidates()) .goal_pose); @@ -689,28 +695,25 @@ void GoalPlannerModule::selectSafePullOverPath() } } -void GoalPlannerModule::setLanes() +std::vector GoalPlannerModule::generateDrivableLanes() const { - const std::lock_guard lock(mutex_); - status_.set_current_lanes(utils::getExtendedCurrentLanes( + const lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes( planner_data_, parameters_->backward_goal_search_length, parameters_->forward_goal_search_length, - /*forward_only_in_route*/ false)); - status_.set_pull_over_lanes(goal_planner_utils::getPullOverLanes( + /*forward_only_in_route*/ false); + const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes( *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length)); - status_.set_lanes(utils::generateDrivableLanesWithShoulderLanes( - status_.get_current_lanes(), status_.get_pull_over_lanes())); + parameters_->forward_goal_search_length); + return utils::generateDrivableLanesWithShoulderLanes(current_lanes, pull_over_lanes); } -void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) +void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) const { if (!thread_safe_data_.foundPullOverPath()) { // situation : not safe against static objects use stop_path setStopPath(output); } else if ( - parameters_->safety_check_params.enable_safety_check && !isSafePath() && - status_.get_has_decided_path() && isActivated()) { + parameters_->safety_check_params.enable_safety_check && !isSafePath() && isActivated()) { // situation : not safe against dynamic objects after approval // insert stop point in current path if ego is able to stop with acceleration and jerk // constraints @@ -735,34 +738,22 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) setModifiedGoal(output); // set hazard and turn signal - if (status_.get_has_decided_path()) { + if (hasDecidedPath()) { setTurnSignalInfo(output); } - - // for the next loop setOutput(). - // this is used to determine whether to generate a new stop path or keep the current stop path. - const std::lock_guard lock(mutex_); - status_.set_prev_found_path(thread_safe_data_.foundPullOverPath()); - status_.set_prev_is_safe_dynamic_objects( - parameters_->safety_check_params.enable_safety_check ? isSafePath() : true); } -void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) +void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) const { if (status_.get_prev_found_path() || !status_.get_prev_stop_path()) { // safe -> not_safe or no prev_stop_path: generate new stop_path output.path = std::make_shared(generateStopPath()); - const std::lock_guard lock(mutex_); - status_.set_prev_stop_path(output.path); - // set stop path as pull over path - auto stop_pull_over_path = std::make_shared(); - stop_pull_over_path->partial_paths.push_back(*output.path); - thread_safe_data_.set_pull_over_path(stop_pull_over_path); RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull_over path, generate stop path"); } else { // not_safe -> not_safe: use previous stop path output.path = status_.get_prev_stop_path(); + // stop_pose_ is removed in manager every loop, so need to set every loop. stop_pose_ = utils::getFirstStopPoseFromPath(*output.path); RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull_over path, use previous stop path"); @@ -770,10 +761,10 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) output.reference_path = getPreviousModuleOutput().reference_path; } -void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output) +void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output) const { // safe or not safe(no feasible stop_path found) -> not_safe: try to find new feasible stop_path - if (status_.get_prev_is_safe_dynamic_objects() || !status_.get_prev_stop_path_after_approval()) { + if (status_.get_prev_is_safe() || !status_.get_prev_stop_path_after_approval()) { auto current_path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); const auto stop_path = behavior_path_planner::utils::start_goal_planner_common::generateFeasibleStopPath( @@ -781,7 +772,7 @@ void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output parameters_->maximum_jerk_for_stop); if (stop_path) { output.path = std::make_shared(*stop_path); - status_.set_prev_stop_path_after_approval(output.path); + // status_.set_prev_stop_path_after_approval(output.path); RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, generate stop path"); } else { output.path = @@ -794,6 +785,7 @@ void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output } else { // not_safe safe(no feasible stop path found) -> not_safe: use previous stop path output.path = status_.get_prev_stop_path_after_approval(); + // stop_pose_ is removed in manager every loop, so need to set every loop. stop_pose_ = utils::getFirstStopPoseFromPath(*output.path); RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, use previous stop path"); } @@ -808,7 +800,7 @@ void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; } else { const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( - *output.path, status_.get_lanes(), planner_data_->drivable_area_expansion_parameters); + *output.path, generateDrivableLanes(), planner_data_->drivable_area_expansion_parameters); DrivableAreaInfo current_drivable_area_info; current_drivable_area_info.drivable_lanes = target_drivable_lanes; @@ -860,13 +852,10 @@ void GoalPlannerModule::updateSteeringFactor( pose, distance, SteeringFactor::GOAL_PLANNER, steering_factor_direction, type, ""); } +// NOTE: Once this function returns true, it will continue to return true thereafter. Because +// selectSafePullOverPath() will not select new path. bool GoalPlannerModule::hasDecidedPath() const { - // once decided, keep the decision - if (status_.get_has_decided_path()) { - return true; - } - // if path is not safe, not decided if (!thread_safe_data_.foundPullOverPath()) { return false; @@ -874,18 +863,15 @@ bool GoalPlannerModule::hasDecidedPath() const // if ego is sufficiently close to the start of the nearest candidate path, the path is decided const auto & current_pose = planner_data_->self_odometry->pose.pose; - const auto ego_segment_idx = motion_utils::findNearestSegmentIndex( - thread_safe_data_.get_pull_over_path()->getCurrentPath().points, current_pose, - std::numeric_limits::max(), M_PI_2); - if (!ego_segment_idx) { - return false; - } + const size_t ego_segment_idx = motion_utils::findNearestSegmentIndex( + thread_safe_data_.get_pull_over_path()->getCurrentPath().points, current_pose.position); + const size_t start_pose_segment_idx = motion_utils::findNearestSegmentIndex( thread_safe_data_.get_pull_over_path()->getCurrentPath().points, thread_safe_data_.get_pull_over_path()->start_pose.position); const double dist_to_parking_start_pose = calcSignedArcLength( thread_safe_data_.get_pull_over_path()->getCurrentPath().points, current_pose.position, - *ego_segment_idx, thread_safe_data_.get_pull_over_path()->start_pose.position, + ego_segment_idx, thread_safe_data_.get_pull_over_path()->start_pose.position, start_pose_segment_idx); return dist_to_parking_start_pose < parameters_->decide_path_distance; } @@ -894,16 +880,12 @@ void GoalPlannerModule::decideVelocity() { const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; - // decide velocity to guarantee turn signal lighting time - if (!status_.get_has_decided_velocity()) { - auto & first_path = thread_safe_data_.get_pull_over_path()->partial_paths.front(); - const auto vel = - static_cast(std::max(current_vel, parameters_->pull_over_minimum_velocity)); - for (auto & p : first_path.points) { - p.point.longitudinal_velocity_mps = std::min(p.point.longitudinal_velocity_mps, vel); - } + auto & first_path = thread_safe_data_.get_pull_over_path()->partial_paths.front(); + const auto vel = + static_cast(std::max(current_vel, parameters_->pull_over_minimum_velocity)); + for (auto & p : first_path.points) { + p.point.longitudinal_velocity_mps = std::min(p.point.longitudinal_velocity_mps, vel); } - status_.set_has_decided_velocity(true); } CandidateOutput GoalPlannerModule::planCandidate() const @@ -920,31 +902,15 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() return getPreviousModuleOutput(); } - constexpr double path_update_duration = 1.0; - resetPathCandidate(); resetPathReference(); - // Check if it needs to decide path - status_.set_has_decided_path(hasDecidedPath()); - - // Use decided path - if (status_.get_has_decided_path()) { - if (isActivated() && !last_approval_data_) { - last_approval_data_ = - std::make_unique(clock_->now(), planner_data_->self_odometry->pose.pose); - decideVelocity(); - } - transitionToNextPathIfFinishingCurrentPath(); - } else if ( - !thread_safe_data_.get_pull_over_path_candidates().empty() && - needPathUpdate(path_update_duration)) { + if (!hasDecidedPath() && needPathUpdate(1.0 /*path_update_duration*/)) { // if the final path is not decided and enough time has passed since last path update, // select safe path from lane parking pull over path candidates // and set it to thread_safe_data_.get_pull_over_path() selectSafePullOverPath(); } - // else: stop path is generated and set by setOutput() // set output and status BehaviorModuleOutput output{}; @@ -969,7 +935,7 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() } const auto distance_to_path_change = calcDistanceToPathChange(); - if (status_.get_has_decided_path()) { + if (hasDecidedPath()) { updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); } // TODO(tkhmy) add handle status TRYING @@ -980,9 +946,41 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() setStopReason(StopReason::GOAL_PLANNER, thread_safe_data_.get_pull_over_path()->getFullPath()); + updateStatus(output); + return output; } +void GoalPlannerModule::updateStatus(const BehaviorModuleOutput & output) +{ + if (status_.get_prev_found_path() || !status_.get_prev_stop_path()) { + status_.set_prev_stop_path(output.path); + } + + // for the next loop setOutput(). + // this is used to determine whether to generate a new stop path or keep the current stop path. + status_.set_prev_found_path(thread_safe_data_.foundPullOverPath()); + status_.set_prev_is_safe( + parameters_->safety_check_params.enable_safety_check ? isSafePath() : true); + + if (!isActivated()) { + return; + } + + if ( + !parameters_->safety_check_params.enable_safety_check || isSafePath() || + (!status_.get_prev_is_safe() && status_.get_prev_stop_path_after_approval())) { + return; + } + status_.set_prev_is_safe(false); + const bool is_stop_path = std::any_of( + output.path->points.begin(), output.path->points.end(), + [](const auto & point) { return point.point.longitudinal_velocity_mps == 0.0; }); + if (is_stop_path) { + status_.set_prev_stop_path_after_approval(output.path); + } +} + BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() { resetPathCandidate(); @@ -1019,7 +1017,7 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification( planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; } else { const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( - *out.path, status_.get_lanes(), planner_data_->drivable_area_expansion_parameters); + *out.path, generateDrivableLanes(), planner_data_->drivable_area_expansion_parameters); DrivableAreaInfo current_drivable_area_info; current_drivable_area_info.drivable_lanes = target_drivable_lanes; @@ -1032,7 +1030,7 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification( } const auto distance_to_path_change = calcDistanceToPathChange(); - if (status_.get_has_decided_path()) { + if (hasDecidedPath()) { updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); } updateSteeringFactor( @@ -1079,7 +1077,7 @@ void GoalPlannerModule::setParameters(const std::shared_ptrroute_handler; const auto & current_pose = planner_data_->self_odometry->pose.pose; @@ -1087,17 +1085,19 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; const double pull_over_velocity = parameters_->pull_over_velocity; - if (status_.get_current_lanes().empty()) { + const auto current_lanes = utils::getExtendedCurrentLanes( + planner_data_, common_parameters.backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + + if (current_lanes.empty()) { return PathWithLaneId{}; } // generate reference path - const auto s_current = - lanelet::utils::getArcCoordinates(status_.get_current_lanes(), current_pose).length; + const auto s_current = lanelet::utils::getArcCoordinates(current_lanes, current_pose).length; const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length); const double s_end = s_current + common_parameters.forward_path_length; - auto reference_path = - route_handler->getCenterLinePath(status_.get_current_lanes(), s_start, s_end, true); + auto reference_path = route_handler->getCenterLinePath(current_lanes, s_start, s_end, true); // if not approved stop road lane. // stop point priority is @@ -1162,19 +1162,20 @@ PathWithLaneId GoalPlannerModule::generateStopPath() return reference_path; } -PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() +PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() const { const auto & route_handler = planner_data_->route_handler; const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto & common_parameters = planner_data_->parameters; // generate stop reference path - const auto s_current = - lanelet::utils::getArcCoordinates(status_.get_current_lanes(), current_pose).length; + const auto current_lanes = utils::getExtendedCurrentLanes( + planner_data_, common_parameters.backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + const auto s_current = lanelet::utils::getArcCoordinates(current_lanes, current_pose).length; const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length); const double s_end = s_current + common_parameters.forward_path_length; - auto stop_path = - route_handler->getCenterLinePath(status_.get_current_lanes(), s_start, s_end, true); + auto stop_path = route_handler->getCenterLinePath(current_lanes, s_start, s_end, true); // calc minimum stop distance under maximum deceleration const auto min_stop_distance = calcFeasibleDecelDistance( @@ -1193,40 +1194,6 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() return stop_path; } -void GoalPlannerModule::transitionToNextPathIfFinishingCurrentPath() -{ - if (!isActivated() || !last_approval_data_) { - return; - } - - // if using arc_path and finishing current_path, get next path - // enough time for turn signal - const bool has_passed_enough_time_from_approval = - (clock_->now() - last_approval_data_->time).seconds() > - planner_data_->parameters.turn_signal_search_time; - if (!has_passed_enough_time_from_approval) { - return; - } - - // require increment only when the time passed is enough - // to prevent increment before driving - // when the end of the current path is close to the current pose - // this value should be `keep_stop_time` in keepStoppedWithCurrentPath - constexpr double keep_current_idx_time = 4.0; - const bool has_passed_enough_time_from_increment = - (clock_->now() - *thread_safe_data_.get_last_path_update_time()).seconds() > - keep_current_idx_time; - if (!has_passed_enough_time_from_increment) { - return; - } - - if (!hasFinishedCurrentPath()) { - return; - } - - thread_safe_data_.incrementPathIndex(); -} - bool GoalPlannerModule::isStopped( std::deque & odometry_buffer, const double time) { @@ -1285,13 +1252,41 @@ bool GoalPlannerModule::isStuck() bool GoalPlannerModule::hasFinishedCurrentPath() { + if (!last_approval_data_) { + return false; + } + + if (!isStopped()) { + return false; + } + + // check if enough time has passed since last approval + // this is necessary to give turn signal for enough time + const bool has_passed_enough_time_from_approval = + (clock_->now() - last_approval_data_->time).seconds() > + planner_data_->parameters.turn_signal_search_time; + if (!has_passed_enough_time_from_approval) { + return false; + } + + // require increment only when the time passed is enough + // to prevent increment before driving + // when the end of the current path is close to the current pose + // this value should be `keep_stop_time` in keepStoppedWithCurrentPath + constexpr double keep_current_idx_time = 4.0; + const bool has_passed_enough_time_from_increment = + (clock_->now() - *thread_safe_data_.get_last_path_update_time()).seconds() > + keep_current_idx_time; + if (!has_passed_enough_time_from_increment) { + return false; + } + + // check if self pose is near the end of current path const auto current_path_end = thread_safe_data_.get_pull_over_path()->getCurrentPath().points.back(); const auto & self_pose = planner_data_->self_odometry->pose.pose; - const bool is_near_target = tier4_autoware_utils::calcDistance2d(current_path_end, self_pose) < - parameters_->th_arrived_distance; - - return is_near_target && isStopped(); + return tier4_autoware_utils::calcDistance2d(current_path_end, self_pose) < + parameters_->th_arrived_distance; } bool GoalPlannerModule::isOnModifiedGoal() const @@ -1334,9 +1329,8 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const { // ego decelerates so that current pose is the point `turn_light_on_threshold_time` seconds // before starting pull_over - turn_signal.desired_start_point = last_approval_data_ && status_.get_has_decided_path() - ? last_approval_data_->pose - : current_pose; + turn_signal.desired_start_point = + last_approval_data_ && hasDecidedPath() ? last_approval_data_->pose : current_pose; turn_signal.desired_end_point = end_pose; turn_signal.required_start_point = start_pose; turn_signal.required_end_point = end_pose; @@ -1440,7 +1434,7 @@ bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) c return true; } -void GoalPlannerModule::keepStoppedWithCurrentPath(PathWithLaneId & path) +void GoalPlannerModule::keepStoppedWithCurrentPath(PathWithLaneId & path) const { constexpr double keep_stop_time = 2.0; if (!thread_safe_data_.get_last_path_idx_increment_time()) { @@ -1619,7 +1613,7 @@ bool GoalPlannerModule::isCrossingPossible( bool GoalPlannerModule::isCrossingPossible(const PullOverPath & pull_over_path) const { - const lanelet::ConstLanelets lanes = utils::transformToLanelets(status_.get_lanes()); + const lanelet::ConstLanelets lanes = utils::transformToLanelets(generateDrivableLanes()); const Pose & start_pose = pull_over_path.start_pose; const Pose & end_pose = pull_over_path.end_pose; @@ -1655,11 +1649,10 @@ bool GoalPlannerModule::isSafePath() const const std::pair terminal_velocity_and_accel = utils::start_goal_planner_common::getPairsTerminalVelocityAndAccel( thread_safe_data_.get_pull_over_path()->pairs_terminal_velocity_and_accel, - status_.get_current_path_idx()); + thread_safe_data_.get_pull_over_path()->path_idx); RCLCPP_DEBUG( getLogger(), "pairs_terminal_velocity_and_accel for goal_planner: %f, %f", terminal_velocity_and_accel.first, terminal_velocity_and_accel.second); - RCLCPP_DEBUG(getLogger(), "current_path_idx %ld", status_.get_current_path_idx()); utils::start_goal_planner_common::updatePathProperty( ego_predicted_path_params_, terminal_velocity_and_accel); // TODO(Sugahara): shoule judge is_object_front properly @@ -1680,7 +1673,7 @@ bool GoalPlannerModule::isSafePath() const pull_over_lanes, route_handler, filtered_objects, objects_filtering_params_); const double hysteresis_factor = - status_.get_prev_is_safe_dynamic_objects() ? 1.0 : parameters_->hysteresis_factor_expand_rate; + status_.get_prev_is_safe() ? 1.0 : parameters_->hysteresis_factor_expand_rate; utils::start_goal_planner_common::updateSafetyCheckTargetObjectsData( goal_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path); @@ -1742,9 +1735,8 @@ void GoalPlannerModule::setDebugData() }; if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { // Visualize pull over areas - const auto color = status_.get_has_decided_path() - ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow - : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green + const auto color = hasDecidedPath() ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow + : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green const double z = status_.get_refined_goal_pose().position.z; add(goal_planner_utils::createPullOverAreaMarkerArray( goal_searcher_->getAreaPolygons(), header, color, z)); @@ -1834,7 +1826,7 @@ void GoalPlannerModule::setDebugData() marker.text = magic_enum::enum_name(thread_safe_data_.getPullOverPlannerType()); if (thread_safe_data_.foundPullOverPath()) { marker.text += - " " + std::to_string(status_.get_current_path_idx()) + "/" + + " " + std::to_string(thread_safe_data_.get_pull_over_path()->path_idx) + "/" + std::to_string(thread_safe_data_.get_pull_over_path()->partial_paths.size() - 1); }