diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 89bcbce9e5656..6b4daaa4b1773 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -175,7 +175,7 @@ planning/behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi@tier4.jp planning/behavior_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner_common/** fumiya.watanabe@tier4.jp isamu.takagi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_run_out_module/** makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_stop_line_module/** fumiya.watanabe@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp zhe.shen@tier4.jp planning/behavior_velocity_template_module/** daniel.sanchez@tier4.jp diff --git a/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp b/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp index 9388a0a354d50..b43102ecbd16d 100644 --- a/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp @@ -1221,7 +1221,6 @@ std::optional DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoi prev_object->ref_path_points_for_obj_poly, ref_path_points_for_obj_poly.at(obj_point_idx).point.pose.position)); - std::cerr << paths_lat_diff << std::endl; constexpr double min_paths_lat_diff = 0.3; if (paths_lat_diff < min_paths_lat_diff) { return true; diff --git a/planning/behavior_path_side_shift_module/README.md b/planning/behavior_path_side_shift_module/README.md index b76e549eb602f..6a460d8423cbd 100644 --- a/planning/behavior_path_side_shift_module/README.md +++ b/planning/behavior_path_side_shift_module/README.md @@ -2,6 +2,29 @@ (For remote control) Shift the path to left or right according to an external instruction. +## Overview of the Side Shift Module Process + +1. Receive the required lateral offset input. +2. Update the `requested_lateral_offset_` under the following conditions: + a. Verify if the last update time has elapsed. + b. Ensure the required lateral offset value is different from the previous one. +3. Insert the shift points into the path if the side shift module's status is not in the SHIFTING status. + +Please be aware that `requested_lateral_offset_` is continuously updated with the latest values and is not queued. + +## Statuses of the Side Shift + +The side shift has three distinct statuses. Note that during the SHIFTING status, the path cannot be updated: + +1. BEFORE_SHIFT: Preparing for shift. +2. SHIFTING: Currently in the process of shifting. +3. AFTER_SHIFT: Shift completed. + +
+ ![case1](images/side_shift_status.drawio.svg){width=1000} +
side shift status
+
+ ## Flowchart ```plantuml @@ -27,7 +50,7 @@ stop @enduml ``` -```plantuml --> +```plantuml @startuml skinparam monochrome true skinparam defaultTextAlignment center diff --git a/planning/behavior_path_side_shift_module/images/side_shift_status.drawio.svg b/planning/behavior_path_side_shift_module/images/side_shift_status.drawio.svg new file mode 100644 index 0000000000000..447a00236d2b6 --- /dev/null +++ b/planning/behavior_path_side_shift_module/images/side_shift_status.drawio.svg @@ -0,0 +1,101 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+ + SideShiftStatus + + ::SHIFTING +
+ + +
+
+ + +
+
+ SideShiftStatus + ::BEFORE_SHIFT + +
+ +
+
+ + +
+
SIDE SHIFT START POINT
+
+ + +
+
SIDE SHIFT END POINT
+
+ + +
+
+ SideShiftStatus + ::AFTER_SHIFT + +
+ +
+
+ + diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index aeefd18357a5c..b26864eb450d5 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -16,7 +16,7 @@ Use cases include: - pull out from the shoulder lane to the road lane centerline.
- ![case2](images/start_from_start_from_road_shoulder.drawio.svg){width=1000} + ![case2](images/start_from_road_shoulder.drawio.svg){width=1000}
pull out from the shoulder lane
diff --git a/planning/behavior_velocity_crosswalk_module/README.md b/planning/behavior_velocity_crosswalk_module/README.md index 37c3fb58049a6..ce231659ccf78 100644 --- a/planning/behavior_velocity_crosswalk_module/README.md +++ b/planning/behavior_velocity_crosswalk_module/README.md @@ -2,7 +2,7 @@ ## Role -This module judges whether the ego should stop in front of the crosswalk in order to provide safe passage of crosswalk users such as pedestrians and bicycles based on the objects' behavior and surround traffic. +This module judges whether the ego should stop in front of the crosswalk in order to provide safe passage for crosswalk users, such as pedestrians and bicycles, based on the objects' behavior and surround traffic.
![crosswalk_module](docs/crosswalk_module.svg){width=1100} @@ -14,7 +14,7 @@ This module judges whether the ego should stop in front of the crosswalk in orde #### Target Object -The target object's type is filtered by the following parameters in the `object_filtering.target_object` namespace. +The crosswalk module handles objects of the types defined by the following parameters in the `object_filtering.target_object` namespace. | Parameter | Unit | Type | Description | | ------------ | ---- | ---- | ---------------------------------------------- | @@ -23,13 +23,13 @@ The target object's type is filtered by the following parameters in the `object_ | `bicycle` | [-] | bool | whether to look and stop by BICYCLE objects | | `motorcycle` | [-] | bool | whether to look and stop by MOTORCYCLE objects | -In order to detect crosswalk users crossing outside the crosswalk as well, the crosswalk module creates an attention area around the crosswalk shown as the yellow polygon in the figure. If the object's predicted path collides with the attention area, the object will be targeted for yield. +In order to handle the crosswalk users crossing the neighborhood but outside the crosswalk, the crosswalk module creates an attention area around the crosswalk, shown as the yellow polygon in the figure. If the object's predicted path collides with the attention area, the object will be targeted for yield.
![crosswalk_attention_range](docs/crosswalk_attention_range.svg){width=600}
-The parameter is in the `object_filtering.target_object` namespace. +The neighborhood is defined by the following parameter in the `object_filtering.target_object` namespace. | Parameter | Unit | Type | Description | | --------------------------- | ---- | ------ | ------------------------------------------------------------------------------------------------- | @@ -51,13 +51,13 @@ When the stop line does **NOT** exist in the lanelet map, the stop position is c -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. +As an exceptional case, if a pedestrian (or 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 by `stop_distance_from_object` and pedestrian position, not at the stop line.
![far_object_threshold](docs/far_object_threshold.drawio.svg){width=700}
-In the `stop_position` namespace, +In the `stop_position` namespace, the following parameters are defined. | Parameter | | Type | Description | | ------------------------------ | --- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | @@ -69,12 +69,12 @@ In the `stop_position` namespace, #### Yield decision The module makes a decision to yield only when the pedestrian traffic light is **GREEN** or **UNKNOWN**. -Calculating the collision point, the decision is based on the following variables. +The decision is based on the following variables, along with the calculation of the collision point. -- TTC: Time-To-Collision which is the time for the **ego** to reach the virtual collision point. -- TTV: Time-To-Vehicle which is the time for the **object** to reach the virtual collision point. +- Time-To-Collision (TTC): The time for the **ego** to reach the virtual collision point. +- Time-To-Vehicle (TTV): The time for the **object** to reach the virtual collision point. -Depending on the relative relationship between TTC and TTV, the ego's behavior at crosswalks can be classified into three categories based on [1] +We classify ego behavior at crosswalks into three categories according to the relative relationship between TTC and TTV [1]. - A. **TTC >> TTV**: The object has enough time to cross before the ego. - No stop planning. @@ -92,10 +92,12 @@ Depending on the relative relationship between TTC and TTV, the ego's behavior a -The boundary of A and B is interpolated from `ego_pass_later_margin_x` and `ego_pass_later_margin_y`. In the case of the upper figure, `ego_pass_later_margin_x` is `{0, 1, 2}` and `ego_pass_later_margin_y` is `{1, 4, 6}` for example. -The same way, the boundary of B and C is calculated from `ego_pass_first_margin_x` and `ego_pass_first_margin_y`. In the case of the upper figure, `ego_pass_first_margin_x` is `{3, 5}` and `ego_pass_first_margin_y` is `{0, 1}` for example. +The boundary of A and B is interpolated from `ego_pass_later_margin_x` and `ego_pass_later_margin_y`. +In the case of the upper figure, `ego_pass_later_margin_x` is `{0, 1, 2}` and `ego_pass_later_margin_y` is `{1, 4, 6}`. +In the same way, the boundary of B and C is calculated from `ego_pass_first_margin_x` and `ego_pass_first_margin_y`. +In the case of the upper figure, `ego_pass_first_margin_x` is `{3, 5}` and `ego_pass_first_margin_y` is `{0, 1}`. -In the `pass_judge` namespace, +In the `pass_judge` namespace, the following parameters are defined. | Parameter | | Type | Description | | ---------------------------------- | ----- | ------ | ----------------------------------------------------------------------------------------------------------------------------------------------- | @@ -108,17 +110,17 @@ In the `pass_judge` namespace, ### Smooth Yield Decision -When the object is stopped around the crosswalk but has no intention to walk, the ego will yield the object forever. -To prevent this dead lock behavior, the ego will cancel the yield depending on the situation. +If the object is stopped near the crosswalk but has no intention of walking, a situation can arise in which the ego continues to yield the right-of-way to the object. +To prevent such a deadlock situation, the ego will cancel yielding depending on the situation. -#### When there is no traffic light +#### Cases without traffic lights -For the object stopped around the crosswalk but has no intention to walk (\*1), when the ego keeps stopping to yield for a certain time (\*2), the ego cancels the yield and start driving. +For the object stopped around the crosswalk but has no intention to walk (\*1), after the ego has keep stopping to yield for a specific time (\*2), the ego cancels the yield and starts driving. \*1: The time is calculated by the interpolation of distance between the object and crosswalk with `distance_map_for_no_intention_to_walk` and `timeout_map_for_no_intention_to_walk`. -In the `pass_judge` namespace, +In the `pass_judge` namespace, the following parameters are defined. | Parameter | | Type | Description | | --------------------------------------- | ----- | ------ | --------------------------------------------------------------------------------- | @@ -126,19 +128,19 @@ In the `pass_judge` namespace, | `timeout_map_for_no_intention_to_walk` | [[s]] | double | timeout map to calculate the timeout for no intention to walk with interpolation | \*2: -In the `pass_judge` namespace, +In the `pass_judge` namespace, the following parameters are defined. | Parameter | | Type | Description | | ---------------------------- | --- | ------ | ----------------------------------------------------------------------------------------------------------------------- | | `timeout_ego_stop_for_yield` | [s] | double | If the ego maintains the stop for this amount of time, then the ego proceeds, assuming it has stopped long time enough. | -#### When there is traffic light +#### Cases with traffic lights -For the object stopped around the crosswalk but has no intention to walk (\*1), the ego will cancel the yield without stopping. +The ego will cancel the yield without stopping when the object stops around the crosswalk but has no intention to walk (\*1). This comes from the assumption that the object has no intention to walk since it is stopped even though the pedestrian traffic light is green. \*1: -The crosswalk user's intention to walk is calculated in the same way as `When there is no traffic light`. +The crosswalk user's intention to walk is calculated in the same way as `Cases without traffic lights`.
@@ -152,12 +154,12 @@ The crosswalk user's intention to walk is calculated in the same way as `When th #### New Object Handling Due to the perception's limited performance where the tree or poll is recognized as a pedestrian or the tracking failure in the crowd or occlusion, even if the surrounding environment does not change, the new pedestrian (= the new ID's pedestrian) may suddenly appear unexpectedly. -When this happens when the ego is going to pass the crosswalk, the ego will stop suddenly. +If this happens while the ego is going to pass the crosswalk, the ego will stop suddenly. -To fix this issue, the option `disable_yield_for_new_stopped_object` is prepared. -If set to true, the new stopped object will be ignored during the yield decision around the crosswalk with a traffic light. +To deal with this issue, the option `disable_yield_for_new_stopped_object` is prepared. +If true is set, the yield decisions around the crosswalk with a traffic light will ignore the new stopped object. -In the `pass_judge` namespace, +In the `pass_judge` namespace, the following parameters are defined. | Parameter | | Type | Description | | -------------------------------------- | --- | ---- | ------------------------------------------------------------------------------------------------ | @@ -165,11 +167,9 @@ In the `pass_judge` namespace, ### Safety Slow Down Behavior -In current autoware implementation if there are no target objects around a crosswalk, ego vehicle -will not slow down for the crosswalk. However, if ego vehicle to slow down to a certain speed in -such cases is wanted then it is possible by adding some tags to the related crosswalk definition as -it is instructed -in [lanelet2_format_extension.md](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md) +In the current autoware implementation, if no target object is detected around a crosswalk, the ego vehicle will not slow down for the crosswalk. +However, it may be desirable to slow down in situations, for example, where there are blind spots. +Such a situation can be handled by setting some tags to the related crosswalk as instructed in the [lanelet2_format_extension.md](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md) document. | Parameter | | Type | Description | @@ -182,7 +182,7 @@ document. ### Stuck Vehicle Detection The feature will make the ego not to stop on the crosswalk. -When there are low-speed or stopped vehicle ahead of the crosswalk, and there is not enough space between the crosswalk and the vehicle, the crosswalk module will plan to stop before the crosswalk even if there are no pedestrians or bicycles. +When there is a low-speed or stopped vehicle ahead of the crosswalk, and there is not enough space between the crosswalk and the vehicle, the crosswalk module plans to stop before the crosswalk even if there are no pedestrians or bicycles. `min_acc`, `min_jerk`, and `max_jerk` are met. If the ego cannot stop before the crosswalk with these parameters, the stop position will move forward. @@ -190,7 +190,7 @@ When there are low-speed or stopped vehicle ahead of the crosswalk, and there is ![stuck_vehicle_attention_range](docs/stuck_vehicle_detection.svg){width=600} -In the `stuck_vehicle` namespace, +In the `stuck_vehicle` namespace, the following parameters are defined. | Parameter | Unit | Type | Description | | ---------------------------------- | ------- | ------ | ----------------------------------------------------------------------- | @@ -203,7 +203,7 @@ In the `stuck_vehicle` namespace, ### Others -In the `common` namespace, +In the `common` namespace, the following parameters are defined. | Parameter | Unit | Type | Description | | ----------------------------- | ---- | ------ | ----------------------------------------------------------------------------------------------------------------------------------------------- | @@ -231,10 +231,10 @@ In the `common` namespace, - Ego footprints' polygon to calculate the collision check. - Pink polygons - Object footprints' polygon to calculate the collision check. -- The color of crosswalk - - Considering the traffic light's color, red means the target crosswalk and white means the ignored crosswalk. +- The color of crosswalks + - Considering the traffic light's color, red means the target crosswalk, and white means the ignored crosswalk. - Texts - - It shows the module id, TTC, TTV, and the module state. + - It shows the module ID, TTC, TTV, and the module state. ### Visualization of Time-To-Collision diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 273db320f1027..aa5dfed1e4bc5 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -50,15 +50,15 @@ min_jerk: -1.0 # min jerk [m/sss] max_jerk: 1.0 # max jerk [m/sss] - 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) + stop_object_velocity_threshold: 0.25 # [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: true # for the crosswalks with traffic signal disable_yield_for_new_stopped_object: true # for the crosswalks with traffic signal # If the pedestrian does not move for X seconds after the ego has stopped in front the crosswalk, the module judge that the pedestrian has no intention to walk and allows the ego to proceed. distance_map_for_no_intention_to_walk: [1.0, 5.0] # [m] ascending order - timeout_map_for_no_intention_to_walk: [3.0, 0.0] # [s] - timeout_ego_stop_for_yield: 3.0 # [s] If the ego maintains the stop for this amount of time, then the ego proceeds, assuming it has stopped long time enough. + timeout_map_for_no_intention_to_walk: [1.0, 0.0] # [s] + timeout_ego_stop_for_yield: 1.0 # [s] If the ego maintains the stop for this amount of time, then the ego proceeds, assuming it has stopped long time enough. # param for target object filtering object_filtering: diff --git a/planning/behavior_velocity_run_out_module/config/run_out.param.yaml b/planning/behavior_velocity_run_out_module/config/run_out.param.yaml index 5534228c1b86f..196f7c6296cb4 100644 --- a/planning/behavior_velocity_run_out_module/config/run_out.param.yaml +++ b/planning/behavior_velocity_run_out_module/config/run_out.param.yaml @@ -3,8 +3,8 @@ run_out: detection_method: "Object" # [-] candidate: Object, ObjectWithoutPath, Points use_partition_lanelet: true # [-] whether to use partition lanelet map data - suppress_on_crosswalk: true # [-] whether to suppress on crosswalk lanelet: - specify_decel_jerk: false # [-] whether to specify jerk when ego decelerates + suppress_on_crosswalk: true # [-] whether to suppress on crosswalk lanelet + specify_decel_jerk: true # [-] whether to specify jerk when ego decelerates stop_margin: 2.5 # [m] the vehicle decelerates to be able to stop with this margin passing_margin: 1.1 # [m] the vehicle begins to accelerate if the vehicle's front in predicted position is ahead of the obstacle + this margin deceleration_jerk: -0.3 # [m/s^3] ego decelerates with this jerk when stopping for obstacles @@ -12,15 +12,7 @@ detection_span: 1.0 # [m] calculate collision with this span to reduce calculation time min_vel_ego_kmph: 3.6 # [km/h] min velocity to calculate time to collision - detection_area: - margin_behind: 0.5 # [m] ahead margin for detection area length - margin_ahead: 1.0 # [m] behind margin for detection area length - - # This area uses points not filtered by vector map to ensure safety - mandatory_area: - decel_jerk: -1.2 # [m/s^3] deceleration jerk for obstacles in this area - - # parameter to create abstracted dynamic obstacles + # Parameter to create abstracted dynamic obstacles dynamic_obstacle: use_mandatory_area: false # [-] whether to use mandatory detection area assume_fixed_velocity: @@ -34,26 +26,42 @@ time_step: 0.5 # [sec] time step for each path step. used for creating dynamic obstacles from points or objects without path points_interval: 0.1 # [m] divide obstacle points into groups with this interval, and detect only lateral nearest point. used only for Points method - # approach if ego has stopped in the front of the obstacle for a certain amount of time - approaching: - enable: false - margin: 0.0 # [m] distance on how close ego approaches the obstacle - limit_vel_kmph: 3.0 # [km/h] limit velocity for approaching after stopping - - # parameters for the change of state. used only when approaching is enabled - state: - stop_thresh: 0.01 # [m/s] threshold to decide if ego is stopping - stop_time_thresh: 3.0 # [sec] threshold for stopping time to transit to approaching state - disable_approach_dist: 3.0 # [m] end the approaching state if distance to the obstacle is longer than this value - keep_approach_duration: 1.0 # [sec] keep approach state for this duration to avoid chattering of state transition - - # parameter to avoid sudden stopping + # Parameter to prevent sudden stopping. + # If the deceleration jerk and acceleration exceed this value upon inserting a stop point, + # the deceleration will be moderated to stay under this value. slow_down_limit: - enable: true + enable: false max_jerk: -0.7 # [m/s^3] minimum jerk deceleration for safe brake. max_acc : -2.0 # [m/s^2] minimum accel deceleration for safe brake. - # prevent abrupt stops caused by false positives in perception + # Parameter to prevent abrupt stops caused by false positives in perception ignore_momentary_detection: enable: true time_threshold: 0.15 # [sec] ignores detections that persist for less than this duration + + # Typically used when the "detection_method" is set to ObjectWithoutPath or Points + # Approach if the ego has stopped in front of the obstacle for a certain period + # This avoids the issue of the ego continuously stopping in front of the obstacle + approaching: + enable: false + margin: 0.0 # [m] distance on how close ego approaches the obstacle + limit_vel_kmph: 3.0 # [km/h] limit velocity for approaching after stopping + # Parameters for state change when "approaching" is enabled + state: + stop_thresh: 0.01 # [m/s] threshold to decide if ego is stopping + stop_time_thresh: 3.0 # [sec] threshold for stopping time to transit to approaching state + disable_approach_dist: 3.0 # [m] end the approaching state if distance to the obstacle is longer than this value + keep_approach_duration: 1.0 # [sec] keep approach state for this duration to avoid chattering of state transition + + # Only used when "detection_method" is set to Points + # Filters points by the detection area polygon to reduce computational cost + # The detection area is calculated based on the current velocity and acceleration and deceleration jerk constraints + detection_area: + margin_behind: 0.5 # [m] ahead margin for detection area length + margin_ahead: 1.0 # [m] behind margin for detection area length + + # Only used when "detection_method" is set to Points + # Points in this area are detected even if it is in the no obstacle segmentation area + # The mandatory area is calculated based on the current velocity and acceleration and "decel_jerk" constraints + mandatory_area: + decel_jerk: -1.2 # [m/s^3] deceleration jerk for obstacles in this area diff --git a/planning/behavior_velocity_run_out_module/package.xml b/planning/behavior_velocity_run_out_module/package.xml index 809579b383461..606c63ea3448c 100644 --- a/planning/behavior_velocity_run_out_module/package.xml +++ b/planning/behavior_velocity_run_out_module/package.xml @@ -10,6 +10,7 @@ Tomoya KimuraShumpei WakabayashiTakayuki Murooka + Kosuke TakeuchiApache License 2.0 diff --git a/planning/behavior_velocity_run_out_module/src/manager.cpp b/planning/behavior_velocity_run_out_module/src/manager.cpp index 3ba9bf8bf52e6..f2ef366cbe121 100644 --- a/planning/behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/behavior_velocity_run_out_module/src/manager.cpp @@ -105,16 +105,14 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) p.enable = getOrDeclareParameter(node, ns_a + ".enable"); p.margin = getOrDeclareParameter(node, ns_a + ".margin"); p.limit_vel_kmph = getOrDeclareParameter(node, ns_a + ".limit_vel_kmph"); - } - { - auto & p = planner_param_.state_param; - const std::string ns_s = ns + ".state"; - p.stop_thresh = getOrDeclareParameter(node, ns_s + ".stop_thresh"); - p.stop_time_thresh = getOrDeclareParameter(node, ns_s + ".stop_time_thresh"); - p.disable_approach_dist = getOrDeclareParameter(node, ns_s + ".disable_approach_dist"); - p.keep_approach_duration = - getOrDeclareParameter(node, ns_s + ".keep_approach_duration"); + const std::string ns_as = ns_a + ".state"; + p.state.stop_thresh = getOrDeclareParameter(node, ns_as + ".stop_thresh"); + p.state.stop_time_thresh = getOrDeclareParameter(node, ns_as + ".stop_time_thresh"); + p.state.disable_approach_dist = + getOrDeclareParameter(node, ns_as + ".disable_approach_dist"); + p.state.keep_approach_duration = + getOrDeclareParameter(node, ns_as + ".keep_approach_duration"); } { diff --git a/planning/behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_run_out_module/src/scene.cpp index b14152863d8d0..50b69fc6139c9 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_run_out_module/src/scene.cpp @@ -46,7 +46,7 @@ RunOutModule::RunOutModule( planner_param_(planner_param), dynamic_obstacle_creator_(std::move(dynamic_obstacle_creator)), debug_ptr_(debug_ptr), - state_machine_(std::make_unique(planner_param.state_param)) + state_machine_(std::make_unique(planner_param.approaching.state)) { velocity_factor_.init(PlanningBehavior::UNKNOWN); diff --git a/planning/behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_run_out_module/src/utils.hpp index 5524c0f76049d..c4976a119dd00 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/behavior_velocity_run_out_module/src/utils.hpp @@ -85,13 +85,6 @@ struct MandatoryArea float decel_jerk; }; -struct ApproachingParam -{ - bool enable; - float margin; - float limit_vel_kmph; -}; - struct StateParam { float stop_thresh; @@ -100,6 +93,14 @@ struct StateParam float keep_approach_duration; }; +struct ApproachingParam +{ + bool enable; + float margin; + float limit_vel_kmph; + StateParam state; +}; + struct SlowDownLimit { bool enable; @@ -143,7 +144,6 @@ struct PlannerParam DetectionArea detection_area; MandatoryArea mandatory_area; ApproachingParam approaching; - StateParam state_param; DynamicObstacleParam dynamic_obstacle; SlowDownLimit slow_down_limit; Smoother smoother; diff --git a/planning/behavior_velocity_walkway_module/config/walkway.param.yaml b/planning/behavior_velocity_walkway_module/config/walkway.param.yaml index f21e3d12db56f..07f493edcde12 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: 1.0 # [s] stop time at stop position + stop_duration: 0.1 # [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/external_velocity_limit_selector/README.md b/planning/external_velocity_limit_selector/README.md index 2dc76dd3aec6e..92579bfd0abce 100644 --- a/planning/external_velocity_limit_selector/README.md +++ b/planning/external_velocity_limit_selector/README.md @@ -51,7 +51,17 @@ Example: ## Parameters -{{ json_to_markdown("planning/external_velocity_limit_selector/schema/external_velocity_limit_selector.schema.json") }} +| Parameter | Type | Description | +| ----------------- | ------ | ------------------------------------------ | +| `max_velocity` | double | default max velocity [m/s] | +| `normal.min_acc` | double | minimum acceleration [m/ss] | +| `normal.max_acc` | double | maximum acceleration [m/ss] | +| `normal.min_jerk` | double | minimum jerk [m/sss] | +| `normal.max_jerk` | double | maximum jerk [m/sss] | +| `limit.min_acc` | double | minimum acceleration to be observed [m/ss] | +| `limit.max_acc` | double | maximum acceleration to be observed [m/ss] | +| `limit.min_jerk` | double | minimum jerk to be observed [m/sss] | +| `limit.max_jerk` | double | maximum jerk to be observed [m/sss] | ## Assumptions / Known limits diff --git a/planning/external_velocity_limit_selector/config/default.param.yaml b/planning/external_velocity_limit_selector/config/default.param.yaml new file mode 100644 index 0000000000000..8023776e191ba --- /dev/null +++ b/planning/external_velocity_limit_selector/config/default.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + # motion state constraints + max_velocity: 20.0 # max velocity limit [m/s] diff --git a/planning/external_velocity_limit_selector/config/default_common.param.yaml b/planning/external_velocity_limit_selector/config/default_common.param.yaml new file mode 100644 index 0000000000000..a23570a5fc791 --- /dev/null +++ b/planning/external_velocity_limit_selector/config/default_common.param.yaml @@ -0,0 +1,15 @@ +/**: + ros__parameters: + # constraints param for normal driving + normal: + min_acc: -0.5 # min deceleration [m/ss] + max_acc: 1.0 # max acceleration [m/ss] + min_jerk: -0.5 # min jerk [m/sss] + max_jerk: 1.0 # max jerk [m/sss] + + # constraints to be observed + limit: + min_acc: -2.5 # min deceleration limit [m/ss] + max_acc: 1.0 # max acceleration limit [m/ss] + min_jerk: -1.5 # min jerk limit [m/sss] + max_jerk: 1.5 # max jerk limit [m/sss] diff --git a/planning/external_velocity_limit_selector/config/external_velocity_limit_selector.param.yaml b/planning/external_velocity_limit_selector/config/external_velocity_limit_selector.param.yaml deleted file mode 100644 index 472b9370607b5..0000000000000 --- a/planning/external_velocity_limit_selector/config/external_velocity_limit_selector.param.yaml +++ /dev/null @@ -1,18 +0,0 @@ -/**: - ros__parameters: - # constraints param for normal driving - normal: - min_acc: -0.5 # min deceleration [m/ss] - max_acc: 1.0 # max acceleration [m/ss] - min_jerk: -0.5 # min jerk [m/sss] - max_jerk: 1.0 # max jerk [m/sss] - - # constraints to be observed - limit: - min_acc: -2.5 # min deceleration limit [m/ss] - max_acc: 1.0 # max acceleration limit [m/ss] - min_jerk: -1.5 # min jerk limit [m/sss] - max_jerk: 1.5 # max jerk limit [m/sss] - - # motion state constraints - max_velocity: 20.0 # max velocity limit [m/s] diff --git a/planning/external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml b/planning/external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml index c499c0741f09f..5ef089f3d3ee7 100644 --- a/planning/external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml +++ b/planning/external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml @@ -1,6 +1,7 @@ - + + @@ -10,7 +11,8 @@ - + + diff --git a/planning/external_velocity_limit_selector/schema/external_velocity_limit_selector.schema.json b/planning/external_velocity_limit_selector/schema/external_velocity_limit_selector.schema.json deleted file mode 100644 index 36fdaf31cbc6f..0000000000000 --- a/planning/external_velocity_limit_selector/schema/external_velocity_limit_selector.schema.json +++ /dev/null @@ -1,85 +0,0 @@ -{ - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "Parameters for external velocity limit selector node", - "type": "object", - "definitions": { - "external_velocity_limit_selector": { - "type": "object", - "properties": { - "max_velocity": { - "type": "number", - "description": "max velocity limit [m/s]", - "default": 20.0 - }, - "normal": { - "type": "object", - "properties": { - "min_acc": { - "type": "number", - "description": "min deceleration [m/ss]", - "default": -0.5 - }, - "max_acc": { - "type": "number", - "description": "max acceleration [m/ss]", - "default": 1.0 - }, - "min_jerk": { - "type": "number", - "description": "min jerk [m/sss]", - "default": -0.5 - }, - "max_jerk": { - "type": "number", - "description": "max jerk [m/sss]", - "default": 1.0 - } - }, - "required": ["min_acc", "max_acc", "min_jerk", "max_jerk"] - }, - "limit": { - "type": "object", - "properties": { - "min_acc": { - "type": "number", - "description": "min deceleration to be observed [m/ss]", - "default": -2.5 - }, - "max_acc": { - "type": "number", - "description": "max acceleration to be observed [m/ss]", - "default": 1.0 - }, - "min_jerk": { - "type": "number", - "description": "min jerk to be observed [m/sss]", - "default": -1.5 - }, - "max_jerk": { - "type": "number", - "description": "max jerk to be observed [m/sss]", - "default": 1.5 - } - }, - "required": ["min_acc", "max_acc", "min_jerk", "max_jerk"] - } - }, - "required": ["max_velocity", "normal", "limit"], - "additionalProperties": false - } - }, - "properties": { - "/**": { - "type": "object", - "properties": { - "ros__parameters": { - "$ref": "#/definitions/external_velocity_limit_selector" - } - }, - "required": ["ros__parameters"], - "additionalProperties": false - } - }, - "required": ["/**"], - "additionalProperties": false -} 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 d8b9c942a986a..aad4bd7e17757 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -85,15 +85,15 @@ obstacle_traj_angle_threshold : 1.22 # [rad] = 70 [deg], yaw threshold of crossing obstacle against the nearest trajectory point for cruise or stop stop: - max_lat_margin: 0.0 # lateral margin between obstacle and trajectory band with ego's width + max_lat_margin: 0.2 # lateral margin between obstacle and trajectory band with ego's width crossing_obstacle: collision_time_margin : 4.0 # time threshold of collision between obstacle adn ego for cruise or stop [s] cruise: max_lat_margin: 1.0 # lateral margin between obstacle and trajectory band with ego's width outside_obstacle: - obstacle_velocity_threshold : 3.0 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s] - ego_obstacle_overlap_time_threshold : 1.0 # time threshold to decide cut-in obstacle for cruise or stop [s] + obstacle_velocity_threshold : 3.5 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s] + ego_obstacle_overlap_time_threshold : 2.0 # time threshold to decide cut-in obstacle for cruise or stop [s] max_prediction_time_for_collision_check : 20.0 # prediction time to check collision between obstacle and ego slow_down: @@ -188,7 +188,7 @@ static: min_lat_margin: 0.2 max_lat_margin: 1.0 - min_ego_velocity: 2.0 + min_ego_velocity: 4.0 max_ego_velocity: 8.0 pedestrian: moving: diff --git a/sensing/gnss_poser/README.md b/sensing/gnss_poser/README.md index 1ec7dc1f516ad..6799a1f79ae89 100644 --- a/sensing/gnss_poser/README.md +++ b/sensing/gnss_poser/README.md @@ -4,6 +4,11 @@ The `gnss_poser` is a node that subscribes gnss sensing messages and calculates vehicle pose with covariance. +This node subscribes to NavSatFix to publish the pose of **base_link**. The data in NavSatFix represents the antenna's position. Therefore, it performs a coordinate transformation using the tf from `base_link` to the antenna's position. The frame_id of the antenna's position refers to NavSatFix's `header.frame_id`. +(**Note that `header.frame_id` in NavSatFix indicates the antenna's frame_id, not the Earth or reference ellipsoid.** [See also NavSatFix definition.](https://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/NavSatFix.html)) + +If the transformation from `base_link` to the antenna cannot be obtained, it outputs the pose of the antenna position without performing coordinate transformation. + ## Inner-workings / Algorithms ## Inputs / Outputs diff --git a/sensing/gnss_poser/config/gnss_poser.param.yaml b/sensing/gnss_poser/config/gnss_poser.param.yaml index 30be98bba8cf1..1c00b43c2066a 100644 --- a/sensing/gnss_poser/config/gnss_poser.param.yaml +++ b/sensing/gnss_poser/config/gnss_poser.param.yaml @@ -2,7 +2,6 @@ ros__parameters: base_frame: base_link gnss_base_frame: gnss_base_link - gnss_frame: gnss map_frame: map buff_epoch: 1 use_gnss_ins_orientation: true diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp index 2118d33bc4b30..97ca1d8bffe77 100644 --- a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp +++ b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp @@ -89,10 +89,9 @@ class GNSSPoser : public rclcpp::Node rclcpp::Publisher::SharedPtr fixed_pub_; MapProjectorInfo::Message projector_info_; - std::string base_frame_; - std::string gnss_frame_; - std::string gnss_base_frame_; - std::string map_frame_; + const std::string base_frame_; + const std::string gnss_base_frame_; + const std::string map_frame_; bool received_map_projector_info_ = false; bool use_gnss_ins_orientation_; diff --git a/sensing/gnss_poser/schema/gnss_poser.schema.json b/sensing/gnss_poser/schema/gnss_poser.schema.json index 1d63bb2f4850e..6d9bc716e121a 100644 --- a/sensing/gnss_poser/schema/gnss_poser.schema.json +++ b/sensing/gnss_poser/schema/gnss_poser.schema.json @@ -11,11 +11,6 @@ "default": "base_link", "description": "frame id for base_frame" }, - "gnss_frame": { - "type": "string", - "default": "gnss", - "description": "frame id for gnss_frame" - }, "gnss_base_frame": { "type": "string", "default": "gnss_base_link", diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index 3a18dca815f12..ec58226273441 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -31,7 +31,6 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) tf2_listener_(tf2_buffer_), tf2_broadcaster_(*this), base_frame_(declare_parameter("base_frame")), - gnss_frame_(declare_parameter("gnss_frame")), gnss_base_frame_(declare_parameter("gnss_base_frame")), map_frame_(declare_parameter("map_frame")), use_gnss_ins_orientation_(declare_parameter("use_gnss_ins_orientation")), @@ -142,8 +141,9 @@ void GNSSPoser::callbackNavSatFix( // get TF from gnss_antenna to base_link auto tf_gnss_antenna2base_link_msg_ptr = std::make_shared(); + const std::string gnss_frame = nav_sat_fix_msg_ptr->header.frame_id; getStaticTransform( - base_frame_, gnss_frame_, tf_gnss_antenna2base_link_msg_ptr, nav_sat_fix_msg_ptr->header.stamp); + base_frame_, gnss_frame, tf_gnss_antenna2base_link_msg_ptr, nav_sat_fix_msg_ptr->header.stamp); tf2::Transform tf_gnss_antenna2base_link{}; tf2::fromMsg(tf_gnss_antenna2base_link_msg_ptr->transform, tf_gnss_antenna2base_link); diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index d096130e65f05..4c3a3aed24d90 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -280,9 +280,10 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node * @brief get z-position from trajectory * @param [in] x current x-position * @param [in] y current y-position + * @param [in] prev_odometry odometry calculated in the previous step * @return get z-position from trajectory */ - double get_z_pose_from_trajectory(const double x, const double y); + double get_z_pose_from_trajectory(const double x, const double y, const Odometry & prev_odometry); /** * @brief get transform from two frame_ids diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 8de5be9d71503..37ec5b33014a7 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -371,9 +371,10 @@ void SimplePlanningSimulator::on_timer() } // set current state + const auto prev_odometry = current_odometry_; current_odometry_ = to_odometry(vehicle_model_ptr_, ego_pitch_angle); current_odometry_.pose.pose.position.z = get_z_pose_from_trajectory( - current_odometry_.pose.pose.position.x, current_odometry_.pose.pose.position.y); + current_odometry_.pose.pose.position.x, current_odometry_.pose.pose.position.y, prev_odometry); current_velocity_ = to_velocity_report(vehicle_model_ptr_); current_steer_ = to_steering_report(vehicle_model_ptr_); @@ -429,6 +430,7 @@ void SimplePlanningSimulator::on_initialpose(const PoseWithCovarianceStamped::Co set_initial_state_with_transform(initial_pose, initial_twist); initial_pose_ = msg; + current_odometry_.pose = msg->pose; } void SimplePlanningSimulator::on_initialtwist(const TwistStamped::ConstSharedPtr msg) @@ -591,11 +593,12 @@ void SimplePlanningSimulator::set_initial_state(const Pose & pose, const Twist & is_initialized_ = true; } -double SimplePlanningSimulator::get_z_pose_from_trajectory(const double x, const double y) +double SimplePlanningSimulator::get_z_pose_from_trajectory( + const double x, const double y, const Odometry & prev_odometry) { // calculate closest point on trajectory if (!current_trajectory_ptr_) { - return 0.0; + return prev_odometry.pose.pose.position.z; } const double max_sqrt_dist = std::numeric_limits::max(); @@ -616,7 +619,7 @@ double SimplePlanningSimulator::get_z_pose_from_trajectory(const double x, const return current_trajectory_ptr_->points.at(index).pose.position.z; } - return 0.0; + return prev_odometry.pose.pose.position.z; } TransformStamped SimplePlanningSimulator::get_transform_msg(