diff --git a/planning/behavior_velocity_crosswalk_module/README.md b/planning/behavior_velocity_crosswalk_module/README.md index 95abf3b00f970..218989482ec97 100644 --- a/planning/behavior_velocity_crosswalk_module/README.md +++ b/planning/behavior_velocity_crosswalk_module/README.md @@ -17,20 +17,26 @@ The manager launch crosswalk scene modules when the reference path conflicts cro #### Common parameters -| Parameter | Type | Description | -| ---------------------- | ---- | ------------------------------- | -| `show_processing_time` | bool | whether to show processing time | +| Parameter | Type | Description | +| ----------------------------- | ---- | ------------------------------- | +| `common.show_processing_time` | bool | whether to show processing time | + +#### Parameters for input data + +| Parameter | Type | Description | +| ------------------------------------ | ------ | ---------------------------------------------- | +| `common.traffic_light_state_timeout` | double | [s] timeout threshold for traffic light signal | #### Parameters for stop position -The crosswalk module determines a stop position at least `stop_margin` away from the object. +The crosswalk module determines a stop position at least `stop_distance_from_object` away from the object.
- ![stop_margin](docs/stop_margin.svg){width=1000} + ![stop_distance_from_object](docs/stop_distance_from_object.svg){width=1000}
stop margin
-The stop line is the reference point for the stopping position of the vehicle, but if there is no stop line in front of the crosswalk, the position `stop_line_distance` meters before the crosswalk is the virtual stop line for the vehicle. Then, if the stop position determined from `stop_margin` exists in front of the stop line determined from the HDMap or `stop_line_distance`, the actual stop position is determined according to `stop_margin` in principle, and vice versa. +The stop line is the reference point for the stopping position of the vehicle, but if there is no stop line in front of the crosswalk, the position `stop_distance_from_crosswalk` meters before the crosswalk is the virtual stop line for the vehicle. Then, if the stop position determined from `stop_distance_from_object` exists in front of the stop line determined from the HDMap or `stop_distance_from_crosswalk`, the actual stop position is determined according to `stop_distance_from_object` in principle, and vice versa.
![stop_line](docs/stop_line.svg){width=700} @@ -38,27 +44,27 @@ The stop line is the reference point for the stopping position of the vehicle, b
- ![stop_line_distance](docs/stop_line_distance.svg){width=700} + ![stop_distance_from_crosswalk](docs/stop_distance_from_crosswalk.svg){width=700}
virtual stop point
-On the other hand, if pedestrian (bicycle) is crossing **wide** crosswalks seen in scramble intersections, and the pedestrian position is more than `stop_line_margin` meters away from the stop line, the actual stop position is determined to be `stop_margin` and pedestrian position, not at the stop line. +On the other hand, if pedestrian (bicycle) is crossing **wide** crosswalks seen in scramble intersections, and the pedestrian position is more than `far_object_threshold` meters away from the stop line, the actual stop position is determined to be `stop_distance_from_object` and pedestrian position, not at the stop line.
- ![stop_line_margin](docs/stop_line_margin.svg){width=1000} + ![far_object_threshold](docs/far_object_threshold.svg){width=1000}
stop at wide crosswalk
See the workflow in algorithms section. -| Parameter | Type | Description | -| ------------------------- | ------ | ----------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `stop_margin` | double | [m] the vehicle decelerates to be able to stop in front of object with margin | -| `stop_line_distance` | double | [m] make stop line away from crosswalk when no explicit stop line exists | -| `stop_line_margin` | double | [m] if objects cross X meters behind the stop line, the stop position is determined according to the object position (stop_margin meters before the object) | -| `stop_position_threshold` | double | [m] threshold for check whether the vehicle stop in front of crosswalk | +| Parameter | Type | Description | +| -------------------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `stop_position.stop_distance_from_object` | double | [m] the vehicle decelerates to be able to stop in front of object with margin | +| `stop_position.stop_distance_from_crosswalk` | double | [m] make stop line away from crosswalk when no explicit stop line exists | +| `stop_position.far_object_threshold` | double | [m] if objects cross X meters behind the stop line, the stop position is determined according to the object position (stop_distance_from_object meters before the object) | +| `stop_position.stop_position_threshold` | double | [m] threshold for check whether the vehicle stop in front of crosswalk | -#### Parameters for ego velocity +#### Parameters for ego's slow down velocity | Parameter | Type | Description | | --------------------- | ------ | --------------------------------------------------------------------------------------------------------------------------- | @@ -76,32 +82,26 @@ If there are low speed or stop vehicle ahead of the crosswalk, and there is not
stuck vehicle attention range
-| Parameter | Type | Description | -| ------------------------------- | ------ | ---------------------------------------------------------------------- | -| `stuck_vehicle_velocity` | double | [m/s] maximum velocity threshold whether the vehicle is stuck | -| `max_lateral_offset` | double | [m] maximum lateral offset for stuck vehicle position should be looked | -| `stuck_vehicle_attention_range` | double | [m] the detection area is defined as X meters behind the crosswalk | +| Parameter | Type | Description | +| ------------------------------------------------ | ------ | ---------------------------------------------------------------------- | +| `stuck_vehicle.stuck_vehicle_velocity` | double | [m/s] maximum velocity threshold whether the vehicle is stuck | +| `stuck_vehicle.max_stuck_vehicle_lateral_offset` | double | [m] maximum lateral offset for stuck vehicle position should be looked | +| `stuck_vehicle.stuck_vehicle_attention_range` | double | [m] the detection area is defined as X meters behind the crosswalk | #### Parameters for pass judge logic Also see algorithm section. -| Parameter | Type | Description | -| -------------------------------- | ------ | -------------------------------------------------------------------------------------------------------------------------------------------------- | -| `ego_pass_first_margin` | double | [s] time margin for ego pass first situation | -| `ego_pass_later_margin` | double | [s] time margin for object pass first situation | -| `stop_object_velocity_threshold` | double | [m/s] velocity threshold for the module to judge whether the objects is stopped | -| `min_object_velocity` | double | [m/s] minimum object velocity (compare the estimated velocity by perception module with this parameter and adopt the larger one to calculate TTV.) | -| `max_yield_timeout` | double | [s] if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first. | -| `ego_yield_query_stop_duration` | double | [s] the amount of time which ego should be stopping to query whether it yields or not. | - -#### Parameters for input data - -| Parameter | Type | Description | -| ------------------ | ------ | ---------------------------------------------- | -| `tl_state_timeout` | double | [s] timeout threshold for traffic light signal | +| Parameter | Type | Description | +| ------------------------------------------- | ------ | -------------------------------------------------------------------------------------------------------------------------------------------------- | +| `pass_judge.ego_pass_first_margin` | double | [s] time margin for ego pass first situation | +| `pass_judge.ego_pass_later_margin` | double | [s] time margin for object pass first situation | +| `pass_judge.stop_object_velocity_threshold` | double | [m/s] velocity threshold for the module to judge whether the objects is stopped | +| `pass_judge.min_object_velocity` | double | [m/s] minimum object velocity (compare the estimated velocity by perception module with this parameter and adopt the larger one to calculate TTV.) | +| `pass_judge.timeout_no_intention_to_walk` | double | [s] if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first. | +| `pass_judge.timeout_ego_stop_for_yield` | double | [s] the amount of time which ego should be stopping to query whether it yields or not. | -#### Parameters for target area & object +#### Parameters for object filtering As a countermeasure against pedestrians attempting to cross outside the crosswalk area, this module watches not only the crosswalk zebra area but also in front of and behind space of the crosswalk, and if there are pedestrians or bicycles attempting to pass through the watch area, this module judges whether ego should pass or stop. @@ -131,15 +131,15 @@ The stop position is determined by the existence of the stop line defined by the ```plantuml start -:calculate stop point from **stop_margin** (POINT-1); +:calculate stop point from **stop_distance_from_object** (POINT-1); if (There is the stop line in front of the crosswalk?) then (yes) :calculate stop point from stop line (POINT-2.1); else (no) - :calculate stop point from **stop_line_distance** (POINT-2.2); + :calculate stop point from **stop_distance_from_crosswalk** (POINT-2.2); endif if (The distance ego to **POINT-1** is shorter than the distance ego to **POINT-2**) then (yes) :ego stops at POINT-1; -else if (The distance ego to **POINT-1** is longer than the distance ego to **POINT-2** + **stop_line_margin**) then (yes) +else if (The distance ego to **POINT-1** is longer than the distance ego to **POINT-2** + **far_object_threshold**) then (yes) :ego stops at POINT-1; else (no) :ego stops at POINT-2; @@ -191,7 +191,7 @@ end #### Dead lock prevention -If there are objects stop within a radius of `min_object_velocity * ego_pass_later_margin` meters from virtual collision point, this module judges that ego should stop based on the pass judge logic described above at all times. In such a situation, even if the pedestrian has no intention of crossing, ego continues the stop decision on the spot. So, this module has another logic for dead lock prevention, and if the object continues to stop for more than `max_yield_timeout` seconds after ego stops in front of the crosswalk, this module judges that the object has no intention of crossing and switches from **STOP** state to **PASS** state. The parameter `stop_object_velocity_threshold` is used to judge whether the objects are stopped or not. In addition, if the object starts to move after the module judges that the object has no intention of crossing, this module judges whether ego should stop or not once again. +If there are objects stop within a radius of `min_object_velocity * ego_pass_later_margin` meters from virtual collision point, this module judges that ego should stop based on the pass judge logic described above at all times. In such a situation, even if the pedestrian has no intention of crossing, ego continues the stop decision on the spot. So, this module has another logic for dead lock prevention, and if the object continues to stop for more than `timeout_no_intention_to_walk` seconds after ego stops in front of the crosswalk, this module judges that the object has no intention of crossing and switches from **STOP** state to **PASS** state. The parameter `stop_object_velocity_threshold` is used to judge whether the objects are stopped or not. In addition, if the object starts to move after the module judges that the object has no intention of crossing, this module judges whether ego should stop or not once again.
![no-intension](docs/no-intension.svg){width=1000} diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 8a4b80a44d84d..edd37092ba49d 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -1,41 +1,51 @@ /**: ros__parameters: crosswalk: - show_processing_time: false # [-] whether to show processing time + common: + show_processing_time: false # [-] whether to show processing time + # param for input data + traffic_light_state_timeout: 3.0 # [s] timeout threshold for traffic light signal # param for stop position - stop_line_distance: 3.5 # [m] make stop line away from crosswalk when no explicit stop line exists - stop_line_margin: 10.0 # [m] if objects cross X meters behind the stop line, the stop position is determined according to the object position (stop_margin meters before the object) - stop_margin: 2.0 # [m] the vehicle decelerates to be able to stop in front of object with margin - stop_position_threshold: 1.0 # [m] threshold for check whether the vehicle stop in front of crosswalk + stop_position: + stop_position_threshold: 1.0 # [m] threshold to check whether the vehicle stops in front of crosswalk for yielding - # param for ego velocity - min_slow_down_velocity: 2.78 # [m/s] target vehicle velocity when module receive slow down command from FOA (2.78 m/s = 10.0 kmph) - max_slow_down_jerk: -1.5 # [m/sss] minimum jerk deceleration for safe brake - max_slow_down_accel: -2.5 # [m/ss] minimum accel deceleration for safe brake - no_relax_velocity: 2.78 # [m/s] if the current velocity is less than X m/s, ego always stops at the stop position(not relax deceleration constraints 2.78 m/s = 10 kmph) + # For the Lanelet2 map with no explicit stop lines + stop_distance_from_crosswalk: 3.5 # [m] make stop line away from crosswalk + # For the case where the crosswalk width is very wide + far_object_threshold: 10.0 # [m] If objects cross X meters behind the stop line, the stop position is determined according to the object position (stop_distance_from_object meters before the object). + # For the case where the stop position is determined according to the object position. + stop_distance_from_object: 2.0 # [m] the vehicle decelerates to be able to stop in front of object with margin - # param for stuck vehicle - stuck_vehicle_velocity: 1.0 # [m/s] maximum velocity threshold whether the vehicle is stuck - max_lateral_offset: 2.0 # [m] maximum lateral offset for stuck vehicle position should be looked - stuck_vehicle_attention_range: 10.0 # [m] the detection area is defined as X meters behind the crosswalk + # param for ego's slow down velocity + slow_down: + min_slow_down_velocity: 2.78 # [m/s] target vehicle velocity when module receive slow down command from FOA (2.78 m/s = 10.0 kmph) + max_slow_down_jerk: -1.5 # [m/sss] minimum jerk deceleration for safe brake + max_slow_down_accel: -2.5 # [m/ss] minimum accel deceleration for safe brake + no_relax_velocity: 2.78 # [m/s] if the current velocity is less than X m/s, ego always stops at the stop position(not relax deceleration constraints 2.78 m/s = 10 kmph) - # param for pass judge logic - disable_stop_for_yield_cancel: false - ego_pass_first_margin: 6.0 # [s] time margin for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) - ego_pass_later_margin: 10.0 # [s] time margin for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) - stop_object_velocity_threshold: 0.28 # [m/s] velocity threshold for the module to judge whether the objects is stopped (0.28 m/s = 1.0 kmph) - min_object_velocity: 1.39 # [m/s] minimum object velocity (compare the estimated velocity by perception module with this parameter and adopt the larger one to calculate TTV. 1.39 m/s = 5.0 kmph) - max_yield_timeout: 3.0 # [s] if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first. - ego_yield_query_stop_duration: 3.0 # [s] the amount of time which ego should be stopping to query whether it yields or not + # param for stuck vehicles + stuck_vehicle: + stuck_vehicle_velocity: 1.0 # [m/s] maximum velocity threshold whether the vehicle is stuck + max_stuck_vehicle_lateral_offset: 2.0 # [m] maximum lateral offset for stuck vehicle position should be looked + stuck_vehicle_attention_range: 10.0 # [m] the detection area is defined as X meters behind the crosswalk - # param for input data - tl_state_timeout: 3.0 # [s] timeout threshold for traffic light signal + # param for pass judge logic + pass_judge: + ego_pass_first_margin: 6.0 # [s] time margin for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) + ego_pass_later_margin: 10.0 # [s] time margin for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) + stop_object_velocity_threshold: 0.28 # [m/s] velocity threshold for the module to judge whether the objects is stopped (0.28 m/s = 1.0 kmph) + min_object_velocity: 1.39 # [m/s] minimum object velocity (compare the estimated velocity by perception module with this parameter and adopt the larger one to calculate TTV. 1.39 m/s = 5.0 kmph) + ## param for yielding + disable_stop_for_yield_cancel: false + timeout_no_intention_to_walk: 3.0 # [s] if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first. + timeout_ego_stop_for_yield: 3.0 # [s] the amount of time which ego should be stopping to query whether it yields or not - # param for target area & object - crosswalk_attention_range: 1.0 # [m] the detection area is defined as -X meters before the crosswalk to +X meters behind the crosswalk - target_object: - unknown: true # [-] whether to look and stop by UNKNOWN objects - bicycle: true # [-] whether to look and stop by BICYCLE objects - motorcycle: true # [-] whether to look and stop by MOTORCYCLE objects (tmp: currently it is difficult for perception modules to detect bicycles and motorcycles separately.) - pedestrian: true # [-] whether to look and stop by PEDESTRIAN objects + # param for target object filtering + object_filtering: + crosswalk_attention_range: 1.0 # [m] the detection area is defined as -X meters before the crosswalk to +X meters behind the crosswalk + target_object: + unknown: true # [-] whether to look and stop by UNKNOWN objects + bicycle: true # [-] whether to look and stop by BICYCLE objects + motorcycle: true # [-] whether to look and stop by MOTORCYCLE objects (tmp: currently it is difficult for perception modules to detect bicycles and motorcycles separately.) + pedestrian: true # [-] whether to look and stop by PEDESTRIAN objects diff --git a/planning/behavior_velocity_crosswalk_module/docs/stop_line.svg b/planning/behavior_velocity_crosswalk_module/docs/stop_line.svg index d4724cba3cbbb..c2d486a7d3541 100644 --- a/planning/behavior_velocity_crosswalk_module/docs/stop_line.svg +++ b/planning/behavior_velocity_crosswalk_module/docs/stop_line.svg @@ -129,13 +129,13 @@
- stop_margin + stop_distance_from_object [m]
- stop_margin [m] + stop_distance_from_object [m] diff --git a/planning/behavior_velocity_crosswalk_module/docs/stop_line_distance.svg b/planning/behavior_velocity_crosswalk_module/docs/stop_line_distance.svg index 54bb567d29fbf..3842601ae246c 100644 --- a/planning/behavior_velocity_crosswalk_module/docs/stop_line_distance.svg +++ b/planning/behavior_velocity_crosswalk_module/docs/stop_line_distance.svg @@ -51,13 +51,13 @@
- stop_line_distance + stop_distance_from_crosswalk [m]
- stop_line_distance [m] + stop_distance_from_crosswalk [m] @@ -131,13 +131,13 @@
- stop_margin + stop_distance_from_object [m]
- stop_margin [m] + stop_distance_from_object [m] diff --git a/planning/behavior_velocity_crosswalk_module/docs/stop_line_margin.svg b/planning/behavior_velocity_crosswalk_module/docs/stop_line_margin.svg index e1b5122ea1d13..ea7fd9be3ff41 100644 --- a/planning/behavior_velocity_crosswalk_module/docs/stop_line_margin.svg +++ b/planning/behavior_velocity_crosswalk_module/docs/stop_line_margin.svg @@ -51,13 +51,13 @@
- stop_line_margin + far_object_threshold [m]
- stop_line_margin [m] + far_object_threshold [m] @@ -158,13 +158,13 @@
- stop_margin + stop_distance_from_object [m]
- stop_margin [m] + stop_distance_from_object [m] diff --git a/planning/behavior_velocity_crosswalk_module/docs/stop_margin.svg b/planning/behavior_velocity_crosswalk_module/docs/stop_margin.svg index b2ab4b9c0442b..924f642ad08bb 100644 --- a/planning/behavior_velocity_crosswalk_module/docs/stop_margin.svg +++ b/planning/behavior_velocity_crosswalk_module/docs/stop_margin.svg @@ -46,13 +46,13 @@
- stop_margin + stop_distance_from_object [m]
- stop_margin [m] + stop_distance_from_object [m] @@ -125,7 +125,7 @@
The module determines a stop position at least - stop_margin + stop_distance_from_object away from the object's predicted path.
diff --git a/planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_attention_range.svg b/planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_attention_range.svg index 9b3c9ce18f1f5..3ac1a94fcec78 100644 --- a/planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_attention_range.svg +++ b/planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_attention_range.svg @@ -154,13 +154,13 @@
- max_lateral_offset + max_stuck_vehicle_lateral_offset [m]
- max_lateral_offset [m] + max_stuck_vehicle_lateral_offset [m] diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index a3f2568b82f90..3ab1e25fa7d29 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -34,46 +34,61 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) // for crosswalk parameters auto & cp = crosswalk_planner_param_; - cp.show_processing_time = node.declare_parameter(ns + ".show_processing_time"); + cp.show_processing_time = node.declare_parameter(ns + ".common.show_processing_time"); + + // param for input data + cp.traffic_light_state_timeout = + node.declare_parameter(ns + ".common.traffic_light_state_timeout"); // param for stop position - cp.stop_line_distance = node.declare_parameter(ns + ".stop_line_distance"); - cp.stop_margin = node.declare_parameter(ns + ".stop_margin"); - cp.stop_line_margin = node.declare_parameter(ns + ".stop_line_margin"); - cp.stop_position_threshold = node.declare_parameter(ns + ".stop_position_threshold"); + cp.stop_distance_from_crosswalk = + node.declare_parameter(ns + ".stop_position.stop_distance_from_crosswalk"); + cp.stop_distance_from_object = + node.declare_parameter(ns + ".stop_position.stop_distance_from_object"); + cp.far_object_threshold = + node.declare_parameter(ns + ".stop_position.far_object_threshold"); + cp.stop_position_threshold = + node.declare_parameter(ns + ".stop_position.stop_position_threshold"); // param for ego velocity - cp.min_slow_down_velocity = node.declare_parameter(ns + ".min_slow_down_velocity"); - cp.max_slow_down_jerk = node.declare_parameter(ns + ".max_slow_down_jerk"); - cp.max_slow_down_accel = node.declare_parameter(ns + ".max_slow_down_accel"); - cp.no_relax_velocity = node.declare_parameter(ns + ".no_relax_velocity"); + cp.min_slow_down_velocity = + node.declare_parameter(ns + ".slow_down.min_slow_down_velocity"); + cp.max_slow_down_jerk = node.declare_parameter(ns + ".slow_down.max_slow_down_jerk"); + cp.max_slow_down_accel = node.declare_parameter(ns + ".slow_down.max_slow_down_accel"); + cp.no_relax_velocity = node.declare_parameter(ns + ".slow_down.no_relax_velocity"); // param for stuck vehicle - cp.stuck_vehicle_velocity = node.declare_parameter(ns + ".stuck_vehicle_velocity"); - cp.max_lateral_offset = node.declare_parameter(ns + ".max_lateral_offset"); + cp.stuck_vehicle_velocity = + node.declare_parameter(ns + ".stuck_vehicle.stuck_vehicle_velocity"); + cp.max_stuck_vehicle_lateral_offset = + node.declare_parameter(ns + ".stuck_vehicle.max_stuck_vehicle_lateral_offset"); cp.stuck_vehicle_attention_range = - node.declare_parameter(ns + ".stuck_vehicle_attention_range"); + node.declare_parameter(ns + ".stuck_vehicle.stuck_vehicle_attention_range"); // param for pass judge logic - cp.ego_pass_first_margin = node.declare_parameter(ns + ".ego_pass_first_margin"); - cp.ego_pass_later_margin = node.declare_parameter(ns + ".ego_pass_later_margin"); - cp.stop_object_velocity = node.declare_parameter(ns + ".stop_object_velocity_threshold"); - cp.min_object_velocity = node.declare_parameter(ns + ".min_object_velocity"); + cp.ego_pass_first_margin = + node.declare_parameter(ns + ".pass_judge.ego_pass_first_margin"); + cp.ego_pass_later_margin = + node.declare_parameter(ns + ".pass_judge.ego_pass_later_margin"); + cp.stop_object_velocity = + node.declare_parameter(ns + ".pass_judge.stop_object_velocity_threshold"); + cp.min_object_velocity = node.declare_parameter(ns + ".pass_judge.min_object_velocity"); cp.disable_stop_for_yield_cancel = - node.declare_parameter(ns + ".disable_stop_for_yield_cancel"); - cp.max_yield_timeout = node.declare_parameter(ns + ".max_yield_timeout"); - cp.ego_yield_query_stop_duration = - node.declare_parameter(ns + ".ego_yield_query_stop_duration"); - - // param for input data - cp.tl_state_timeout = node.declare_parameter(ns + ".tl_state_timeout"); + node.declare_parameter(ns + ".pass_judge.disable_stop_for_yield_cancel"); + cp.timeout_no_intention_to_walk = + node.declare_parameter(ns + ".pass_judge.timeout_no_intention_to_walk"); + cp.timeout_ego_stop_for_yield = + node.declare_parameter(ns + ".pass_judge.timeout_ego_stop_for_yield"); // param for target area & object - cp.crosswalk_attention_range = node.declare_parameter(ns + ".crosswalk_attention_range"); - cp.look_unknown = node.declare_parameter(ns + ".target_object.unknown"); - cp.look_bicycle = node.declare_parameter(ns + ".target_object.bicycle"); - cp.look_motorcycle = node.declare_parameter(ns + ".target_object.motorcycle"); - cp.look_pedestrian = node.declare_parameter(ns + ".target_object.pedestrian"); + cp.crosswalk_attention_range = + node.declare_parameter(ns + ".object_filtering.crosswalk_attention_range"); + cp.look_unknown = node.declare_parameter(ns + ".object_filtering.target_object.unknown"); + cp.look_bicycle = node.declare_parameter(ns + ".object_filtering.target_object.bicycle"); + cp.look_motorcycle = + node.declare_parameter(ns + ".object_filtering.target_object.motorcycle"); + cp.look_pedestrian = + node.declare_parameter(ns + ".object_filtering.target_object.pedestrian"); } void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 9b2bf6a8c761d..120f8ec8581be 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -239,12 +239,12 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto *path, sparse_resample_path, p_stop_line, path_intersects, default_stop_pose); // Decide to stop for stuck vehicle - const auto stop_factor_for_stucked_vehicles = checkStopForStuckedVehicles( + const auto stop_factor_for_stuck_vehicles = checkStopForStuckVehicles( sparse_resample_path, objects_ptr->objects, path_intersects, default_stop_pose); // Get nearest stop factor const auto nearest_stop_factor = - getNearestStopFactor(*path, stop_factor_for_crosswalk_users, stop_factor_for_stucked_vehicles); + getNearestStopFactor(*path, stop_factor_for_crosswalk_users, stop_factor_for_stuck_vehicles); recordTime(3); // Set safe or unsafe @@ -274,14 +274,15 @@ std::optional> CrosswalkModule::get const auto p_stop_lines = getLinestringIntersects( ego_path, lanelet::utils::to2D(stop_line).basicLineString(), ego_pos, 2); if (!p_stop_lines.empty()) { - return std::make_pair(p_stop_lines.front(), -planner_param_.stop_margin - base_link2front); + return std::make_pair( + p_stop_lines.front(), -planner_param_.stop_distance_from_object - base_link2front); } } // If stop lines are not found in the LL2 map. if (!path_intersects.empty()) { return std::make_pair( - path_intersects.front(), -planner_param_.stop_line_distance - base_link2front); + path_intersects.front(), -planner_param_.stop_distance_from_crosswalk - base_link2front); } return {}; @@ -342,7 +343,7 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( const auto dist_ego2cp = calcSignedArcLength(sparse_resample_path.points, ego_pos, cp.collision_point) - - planner_param_.stop_margin; + planner_param_.stop_distance_from_object; if (!nearest_stop_info || dist_ego2cp - base_link2front < nearest_stop_info->second) { nearest_stop_info = std::make_pair(cp.collision_point, dist_ego2cp - base_link2front); } @@ -357,7 +358,7 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( // Check if the ego should stop beyond the stop line. const bool stop_at_stop_line = dist_ego_to_stop < nearest_stop_info->second && - nearest_stop_info->second < dist_ego_to_stop + planner_param_.stop_line_margin; + nearest_stop_info->second < dist_ego_to_stop + planner_param_.far_object_threshold; if (stop_at_stop_line) { // Stop at the stop line @@ -367,7 +368,7 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( } else { // Stop beyond the stop line const auto stop_pose = calcLongitudinalOffsetPose( - ego_path.points, nearest_stop_info->first, planner_param_.stop_margin); + ego_path.points, nearest_stop_info->first, planner_param_.stop_distance_from_object); if (stop_pose) { return createStopFactor(*stop_pose, stop_factor_points); } @@ -643,7 +644,7 @@ CollisionPoint CrosswalkModule::createCollisionPoint( CollisionPoint collision_point{}; collision_point.collision_point = nearest_collision_point; collision_point.time_to_collision = - std::max(0.0, dist_ego2cp - planner_param_.stop_margin - base_link2front) / + std::max(0.0, dist_ego2cp - planner_param_.stop_distance_from_object - base_link2front) / std::max(ego_vel.x, min_ego_velocity); collision_point.time_to_vehicle = std::max(0.0, dist_obj2cp) / velocity; @@ -756,7 +757,7 @@ Polygon2d CrosswalkModule::getAttentionArea( return attention_area; } -std::optional CrosswalkModule::checkStopForStuckedVehicles( +std::optional CrosswalkModule::checkStopForStuckVehicles( const PathWithLaneId & ego_path, const std::vector & objects, const std::vector & path_intersects, const std::optional & stop_pose) const @@ -777,7 +778,7 @@ std::optional CrosswalkModule::checkStopForStuckedVehicles( const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; const auto lateral_offset = calcLateralOffset(ego_path.points, obj_pos); - if (planner_param_.max_lateral_offset < std::abs(lateral_offset)) { + if (planner_param_.max_stuck_vehicle_lateral_offset < std::abs(lateral_offset)) { continue; } @@ -800,7 +801,7 @@ std::optional CrosswalkModule::checkStopForStuckedVehicles( std::optional CrosswalkModule::getNearestStopFactor( const PathWithLaneId & ego_path, const std::optional & stop_factor_for_crosswalk_users, - const std::optional & stop_factor_for_stucked_vehicles) + const std::optional & stop_factor_for_stuck_vehicles) { const auto get_distance_to_stop = [&](const auto stop_factor) -> std::optional { const auto & ego_pos = planner_data_->current_odometry->pose.position; @@ -809,20 +810,19 @@ std::optional CrosswalkModule::getNearestStopFactor( }; const auto dist_to_stop_for_crosswalk_users = get_distance_to_stop(stop_factor_for_crosswalk_users); - const auto dist_to_stop_for_stucked_vehicles = - get_distance_to_stop(stop_factor_for_stucked_vehicles); + const auto dist_to_stop_for_stuck_vehicles = get_distance_to_stop(stop_factor_for_stuck_vehicles); if (dist_to_stop_for_crosswalk_users) { - if (dist_to_stop_for_stucked_vehicles) { - if (*dist_to_stop_for_stucked_vehicles < *dist_to_stop_for_crosswalk_users) { - return stop_factor_for_stucked_vehicles; + if (dist_to_stop_for_stuck_vehicles) { + if (*dist_to_stop_for_stuck_vehicles < *dist_to_stop_for_crosswalk_users) { + return stop_factor_for_stuck_vehicles; } } return stop_factor_for_crosswalk_users; } - if (dist_to_stop_for_stucked_vehicles) { - return stop_factor_for_stucked_vehicles; + if (dist_to_stop_for_stuck_vehicles) { + return stop_factor_for_stuck_vehicles; } return {}; @@ -836,7 +836,7 @@ void CrosswalkModule::updateObjectState(const double dist_ego_to_stop) const bool is_ego_yielding = [&]() { const auto has_reached_stop_point = dist_ego_to_stop < planner_param_.stop_position_threshold; - return planner_data_->isVehicleStopped(planner_param_.ego_yield_query_stop_duration) && + return planner_data_->isVehicleStopped(planner_param_.timeout_ego_stop_for_yield) && has_reached_stop_point; }(); @@ -866,7 +866,7 @@ bool CrosswalkModule::isRedSignalForPedestrians() const } if ( - planner_param_.tl_state_timeout < + planner_param_.traffic_light_state_timeout < (clock_->now() - traffic_signal_stamped->header.stamp).seconds()) { continue; } diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 6a26cf4f96e29..228be0d62ae29 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -63,9 +63,9 @@ class CrosswalkModule : public SceneModuleInterface { bool show_processing_time; // param for stop position - double stop_margin; - double stop_line_distance; - double stop_line_margin; + double stop_distance_from_object; + double stop_distance_from_crosswalk; + double far_object_threshold; double stop_position_threshold; // param for ego velocity float min_slow_down_velocity; @@ -74,7 +74,7 @@ class CrosswalkModule : public SceneModuleInterface double no_relax_velocity; // param for stuck vehicle double stuck_vehicle_velocity; - double max_lateral_offset; + double max_stuck_vehicle_lateral_offset; double stuck_vehicle_attention_range; // param for pass judge logic double ego_pass_first_margin; @@ -82,10 +82,10 @@ class CrosswalkModule : public SceneModuleInterface double stop_object_velocity; double min_object_velocity; bool disable_stop_for_yield_cancel; - double max_yield_timeout; - double ego_yield_query_stop_duration; + double timeout_no_intention_to_walk; + double timeout_ego_stop_for_yield; // param for input data - double tl_state_timeout; + double traffic_light_state_timeout; // param for target area & object double crosswalk_attention_range; bool look_unknown; @@ -116,7 +116,7 @@ class CrosswalkModule : public SceneModuleInterface time_to_start_stopped = now; } const bool intent_to_cross = - (now - *time_to_start_stopped).seconds() < planner_param.max_yield_timeout; + (now - *time_to_start_stopped).seconds() < planner_param.timeout_no_intention_to_walk; if ((is_ego_yielding || planner_param.disable_stop_for_yield_cancel) && !intent_to_cross) { state = State::FULLY_STOPPED; } else { @@ -193,7 +193,7 @@ class CrosswalkModule : public SceneModuleInterface const std::vector & path_intersects, const std::optional & default_stop_pose); - std::optional checkStopForStuckedVehicles( + std::optional checkStopForStuckVehicles( const PathWithLaneId & ego_path, const std::vector & objects, const std::vector & path_intersects, const std::optional & stop_pose) const; @@ -205,7 +205,7 @@ class CrosswalkModule : public SceneModuleInterface std::optional getNearestStopFactor( const PathWithLaneId & ego_path, const std::optional & stop_factor_for_crosswalk_users, - const std::optional & stop_factor_for_stucked_vehicles); + const std::optional & stop_factor_for_stuck_vehicles); void planGo(PathWithLaneId & ego_path, const std::optional & stop_factor); diff --git a/planning/behavior_velocity_walkway_module/config/walkway.param.yaml b/planning/behavior_velocity_walkway_module/config/walkway.param.yaml index b9f278796b8ae..f21e3d12db56f 100644 --- a/planning/behavior_velocity_walkway_module/config/walkway.param.yaml +++ b/planning/behavior_velocity_walkway_module/config/walkway.param.yaml @@ -1,5 +1,5 @@ /**: ros__parameters: walkway: - stop_duration_sec: 1.0 # [s] stop time at stop position - stop_line_distance: 3.5 # [m] make stop line away from crosswalk when no explicit stop line exists + stop_duration: 1.0 # [s] stop time at stop position + stop_distance_from_crosswalk: 3.5 # [m] make stop line away from crosswalk when no explicit stop line exists diff --git a/planning/behavior_velocity_walkway_module/src/manager.cpp b/planning/behavior_velocity_walkway_module/src/manager.cpp index f2e844063568a..d0c1a47408929 100644 --- a/planning/behavior_velocity_walkway_module/src/manager.cpp +++ b/planning/behavior_velocity_walkway_module/src/manager.cpp @@ -34,8 +34,9 @@ WalkwayModuleManager::WalkwayModuleManager(rclcpp::Node & node) // for walkway parameters auto & wp = walkway_planner_param_; - wp.stop_line_distance = node.declare_parameter(ns + ".stop_line_distance"); - wp.stop_duration_sec = node.declare_parameter(ns + ".stop_duration_sec"); + wp.stop_distance_from_crosswalk = + node.declare_parameter(ns + ".stop_distance_from_crosswalk"); + wp.stop_duration = node.declare_parameter(ns + ".stop_duration"); } void WalkwayModuleManager::launchNewModules(const PathWithLaneId & path) diff --git a/planning/behavior_velocity_walkway_module/src/scene_walkway.cpp b/planning/behavior_velocity_walkway_module/src/scene_walkway.cpp index 7979ff88bd52a..af153b56b915e 100644 --- a/planning/behavior_velocity_walkway_module/src/scene_walkway.cpp +++ b/planning/behavior_velocity_walkway_module/src/scene_walkway.cpp @@ -79,7 +79,7 @@ std::optional> WalkwayModule::getSt if (!path_intersects.empty()) { const auto p_stop_line = path_intersects.front(); const auto dist_ego_to_stop = calcSignedArcLength(ego_path.points, ego_pos, p_stop_line) - - planner_param_.stop_line_distance; + planner_param_.stop_distance_from_crosswalk; return std::make_pair(dist_ego_to_stop, p_stop_line); } } @@ -112,8 +112,9 @@ bool WalkwayModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_ } const auto & p_stop = p_stop_line->second; - const auto stop_line_distance = exist_stopline_in_map ? 0.0 : planner_param_.stop_line_distance; - const auto margin = stop_line_distance + base_link2front; + const auto stop_distance_from_crosswalk = + exist_stopline_in_map ? 0.0 : planner_param_.stop_distance_from_crosswalk; + const auto margin = stop_distance_from_crosswalk + base_link2front; const auto stop_pose = calcLongitudinalOffsetPose(input.points, p_stop, -margin); if (!stop_pose) { @@ -141,9 +142,8 @@ bool WalkwayModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_ const double distance_threshold = 1.0; debug_data_.stop_judge_range = distance_threshold; - const auto stop_at_stop_point = - signed_arc_dist_to_stop_point < distance_threshold && - planner_data_->isVehicleStopped(planner_param_.stop_duration_sec); + const auto stop_at_stop_point = signed_arc_dist_to_stop_point < distance_threshold && + planner_data_->isVehicleStopped(planner_param_.stop_duration); if (stop_at_stop_point) { // If ego vehicle is after walkway stop and stopped then move to stop state diff --git a/planning/behavior_velocity_walkway_module/src/scene_walkway.hpp b/planning/behavior_velocity_walkway_module/src/scene_walkway.hpp index 5580da39f2546..c372a5835f795 100644 --- a/planning/behavior_velocity_walkway_module/src/scene_walkway.hpp +++ b/planning/behavior_velocity_walkway_module/src/scene_walkway.hpp @@ -39,8 +39,8 @@ class WalkwayModule : public SceneModuleInterface public: struct PlannerParam { - double stop_line_distance; - double stop_duration_sec; + double stop_distance_from_crosswalk; + double stop_duration; }; WalkwayModule( const int64_t module_id, const lanelet::LaneletMapPtr & lanelet_map_ptr,