From c12918739275ce5344f0b5a0e8b4b2ac1a059509 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Thu, 27 Jun 2024 10:15:39 +0900 Subject: [PATCH 1/6] feat(start_planner): yaw threshold for rss check (#7657) * add param to customize yaw th Signed-off-by: Daniel Sanchez * add param to other modules Signed-off-by: Daniel Sanchez * docs Signed-off-by: Daniel Sanchez * update READMEs with params Signed-off-by: Daniel Sanchez * fix LC README Signed-off-by: Daniel Sanchez * use normalized yaw diff Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- .../README.md | 15 +++++------ .../config/goal_planner.param.yaml | 1 + .../src/goal_planner_module.cpp | 6 +++-- .../src/manager.cpp | 5 ++++ .../README.md | 1 + .../config/lane_change.param.yaml | 1 + .../utils/data_structs.hpp | 1 + .../src/manager.cpp | 2 ++ .../src/scene.cpp | 6 ++++- .../behavior_path_planner_safety_check.md | 4 +-- .../path_safety_checker_parameters.hpp | 5 +++- .../path_safety_checker/safety_check.hpp | 11 +++++--- .../path_safety_checker/safety_check.cpp | 21 ++++++++++++---- .../README.md | 23 +++++++++-------- .../config/start_planner.param.yaml | 2 ++ .../start_planner_module.hpp | 12 ++++----- .../src/manager.cpp | 11 +++++++- .../src/start_planner_module.cpp | 25 +++++++++++-------- .../static_obstacle_avoidance.param.yaml | 1 + .../data_structs.hpp | 2 ++ .../parameter_helper.hpp | 2 ++ .../static_obstacle_avoidance.schema.json | 5 ++++ .../src/scene.cpp | 3 ++- 23 files changed, 114 insertions(+), 51 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md index 1be3e04914cda..3e155aba0af2e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md @@ -382,13 +382,14 @@ In addition, the safety check has a time hysteresis, and if the path is judged " ### Parameters for safety check -| Name | Unit | Type | Description | Default value | -| :----------------------- | :--- | :----- | :------------------------------------------------------------------------------------------------------- | :--------------------------- | -| enable_safety_check | [-] | bool | flag whether to use safety check | true | -| method | [-] | string | method for safety check. `RSS` or `integral_predicted_polygon` | `integral_predicted_polygon` | -| keep_unsafe_time | [s] | double | safety check Hysteresis time. if the path is judged "safe" for the time it is finally treated as "safe". | 3.0 | -| check_all_predicted_path | - | bool | Flag to check all predicted paths | true | -| publish_debug_marker | - | bool | Flag to publish debug markers | false | +| Name | Unit | Type | Description | Default value | +| :----------------------------------- | :---- | :----- | :------------------------------------------------------------------------------------------------------- | :--------------------------- | +| enable_safety_check | [-] | bool | flag whether to use safety check | true | +| method | [-] | string | method for safety check. `RSS` or `integral_predicted_polygon` | `integral_predicted_polygon` | +| keep_unsafe_time | [s] | double | safety check Hysteresis time. if the path is judged "safe" for the time it is finally treated as "safe". | 3.0 | +| check_all_predicted_path | - | bool | Flag to check all predicted paths | true | +| publish_debug_marker | - | bool | Flag to publish debug markers | false | +| `collision_check_yaw_diff_threshold` | [rad] | double | Maximum yaw difference between ego and object when executing rss-based collision checking | 3.1416 | #### Parameters for RSS safety check diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml index 915d256aba1db..53e06631a81d5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml @@ -180,6 +180,7 @@ time_horizon: 10.0 # hysteresis factor to expand/shrink polygon with the value hysteresis_factor_expand_rate: 1.0 + collision_check_yaw_diff_threshold: 3.1416 # temporary backward_path_length: 100.0 forward_path_length: 100.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 2ac0b9493e67a..6caeb411f6680 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -2319,8 +2319,10 @@ std::pair GoalPlannerModule::isSafePath( return autoware::behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS( pull_over_path, ego_predicted_path, filtered_objects, collision_check, planner_data->parameters, safety_check_params->rss_params, - objects_filtering_params->use_all_predicted_path, hysteresis_factor); - } else if (parameters.safety_check_params.method == "integral_predicted_polygon") { + objects_filtering_params->use_all_predicted_path, hysteresis_factor, + safety_check_params->collision_check_yaw_diff_threshold); + } + if (parameters.safety_check_params.method == "integral_predicted_polygon") { return utils::path_safety_checker::checkSafetyWithIntegralPredictedPolygon( ego_predicted_path, vehicle_info_, filtered_objects, objects_filtering_params->check_all_predicted_path, diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp index 901fe351fc68b..b079db9babf31 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp @@ -357,6 +357,8 @@ void GoalPlannerModuleManager::init(rclcpp::Node * node) p.safety_check_params.method = node->declare_parameter(safety_check_ns + "method"); p.safety_check_params.hysteresis_factor_expand_rate = node->declare_parameter(safety_check_ns + "hysteresis_factor_expand_rate"); + p.safety_check_params.collision_check_yaw_diff_threshold = + node->declare_parameter(safety_check_ns + "collision_check_yaw_diff_threshold"); p.safety_check_params.backward_path_length = node->declare_parameter(safety_check_ns + "backward_path_length"); p.safety_check_params.forward_path_length = @@ -778,6 +780,9 @@ void GoalPlannerModuleManager::updateModuleParams( updateParam( parameters, safety_check_ns + "hysteresis_factor_expand_rate", p->safety_check_params.hysteresis_factor_expand_rate); + updateParam( + parameters, safety_check_ns + "collision_check_yaw_diff_threshold", + p->safety_check_params.collision_check_yaw_diff_threshold); updateParam( parameters, safety_check_ns + "backward_path_length", p->safety_check_params.backward_path_length); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index a0870fe428c8a..5790be377b7aa 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -725,6 +725,7 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi | `check_objects_on_current_lanes` | [-] | boolean | If true, the lane change module check objects on current lanes when performing collision assessment. | false | | `check_objects_on_other_lanes` | [-] | boolean | If true, the lane change module include objects on other lanes. when performing collision assessment | false | | `use_all_predicted_path` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | true | +| `safety_check.collision_check_yaw_diff_threshold` | [rad] | double | Maximum yaw difference between ego and object when executing rss-based collision checking | 3.1416 | #### safety constraints during lane change path is computed diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml index 1ab33514c5f24..d2f695071649a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml @@ -30,6 +30,7 @@ # safety check safety_check: allow_loose_check_for_cancel: true + collision_check_yaw_diff_threshold: 3.1416 execution: expected_front_deceleration: -1.0 expected_rear_deceleration: -1.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index e2afb6abeec03..ffd2754acc38f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -137,6 +137,7 @@ struct LaneChangeParameters // safety check bool allow_loose_check_for_cancel{true}; + double collision_check_yaw_diff_threshold{3.1416}; utils::path_safety_checker::RSSparams rss_params{}; utils::path_safety_checker::RSSparams rss_params_for_abort{}; utils::path_safety_checker::RSSparams rss_params_for_stuck{}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index 61ff54e8f99f3..22c93f7d4bfd5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -102,6 +102,8 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node) // safety check p.allow_loose_check_for_cancel = getOrDeclareParameter(*node, parameter("safety_check.allow_loose_check_for_cancel")); + p.collision_check_yaw_diff_threshold = getOrDeclareParameter( + *node, parameter("safety_check.collision_check_yaw_diff_threshold")); p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter( *node, parameter("safety_check.execution.longitudinal_distance_min_threshold")); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 505bb3ef93340..fd96701fe088e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -30,6 +30,7 @@ #include #include +#include #include #include #include @@ -2048,6 +2049,8 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( lane_change_parameters_->lane_expansion_left_offset, lane_change_parameters_->lane_expansion_right_offset); + constexpr double collision_check_yaw_diff_threshold{M_PI}; + for (const auto & obj : collision_check_objects) { auto current_debug_data = utils::path_safety_checker::createObjectDebug(obj); const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( @@ -2056,7 +2059,8 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( for (const auto & obj_path : obj_predicted_paths) { const auto collided_polygons = utils::path_safety_checker::getCollidedPolygons( path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0, - get_max_velocity_for_safety_check(), current_debug_data.second); + get_max_velocity_for_safety_check(), collision_check_yaw_diff_threshold, + current_debug_data.second); if (collided_polygons.empty()) { utils::path_safety_checker::updateCollisionCheckDebugMap( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md b/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md index 4733fe7832da6..5511202c390b1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md @@ -48,7 +48,7 @@ $$ rss_{dist} = v_{rear} (t_{reaction} + t_{margin}) + \frac{v_{rear}^2}{2|a_{rear, decel}|} - \frac{v_{front}^2}{2|a_{front, decel|}} $$ -where $V_{front}$, $v_{rear}$ are front and rear vehicle velocity respectively and $a_{rear, front}$, $a_{rear, decel}$ are front and rear vehicle deceleration. +where $V_{front}$, $v_{rear}$ are front and rear vehicle velocity respectively and $a_{rear, front}$, $a_{rear, decel}$ are front and rear vehicle deceleration. Note that RSS distance is normally used for objects traveling in the same direction, if the yaw difference between a given ego pose and object pose is more than a user-defined yaw difference threshold, the rss collision check will be skipped for that specific pair of poses. #### 5. Create extended ego and target object polygons @@ -56,7 +56,7 @@ In this step, we compute extended ego and target object polygons. The extended p ![extended_polygons](../images/path_safety_checker/extended_polygons.drawio.svg) -As the picture shows, we expand the rear object polygon. For the longitudinal side, we extend it with the RSS distance, and for the lateral side, we extend it by the lateral margin +As the picture shows, we expand the rear object polygon. For the longitudinal side, we extend it with the RSS distance, and for the lateral side, we extend it by the lateral margin. #### 6. Check overlap diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp index 313f9df471938..0e7d1cee65f02 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -23,6 +23,7 @@ #include +#include #include #include #include @@ -203,7 +204,9 @@ struct SafetyCheckParams /// possible values: ["RSS", "integral_predicted_polygon"] double keep_unsafe_time{0.0}; ///< Time to keep unsafe before changing to safe. double hysteresis_factor_expand_rate{ - 0.0}; ///< Hysteresis factor to expand/shrink polygon with the value. + 0.0}; ///< Hysteresis factor to expand/shrink polygon with the value. + double collision_check_yaw_diff_threshold{ + 3.1416}; ///< threshold yaw difference between ego and object. double backward_path_length{0.0}; ///< Length of the backward lane for path generation. double forward_path_length{0.0}; ///< Length of the forward path lane for path generation. RSSparams rss_params{}; ///< Parameters related to the RSS model. diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp index 3f1094dfd70b6..d0d19b6b8fed2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp @@ -26,6 +26,7 @@ #include #include +#include #include namespace autoware::behavior_path_planner::utils::path_safety_checker @@ -98,7 +99,8 @@ bool checkSafetyWithRSS( const std::vector & ego_predicted_path, const std::vector & objects, CollisionCheckDebugMap & debug_map, const BehaviorPathPlannerParameters & parameters, const RSSparams & rss_params, - const bool check_all_predicted_path, const double hysteresis_factor); + const bool check_all_predicted_path, const double hysteresis_factor, + const double yaw_difference_th); /** * @brief Iterate the points in the ego and target's predicted path and @@ -111,6 +113,8 @@ bool checkSafetyWithRSS( * @param common_parameters The common parameters used in behavior path planner. * @param front_object_deceleration The deceleration of the object in the front.(used in RSS) * @param rear_object_deceleration The deceleration of the object in the rear.(used in RSS) + * @param yaw_difference_th maximum yaw difference between any given ego path pose and object + * predicted path pose. * @param debug The debug information for collision checking. * @return true if distance is safe. */ @@ -120,7 +124,7 @@ bool checkCollision( const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, - const double hysteresis_factor, CollisionCheckDebug & debug); + const double hysteresis_factor, const double yaw_difference_th, CollisionCheckDebug & debug); /** * @brief Iterate the points in the ego and target's predicted path and @@ -142,7 +146,8 @@ std::vector getCollidedPolygons( const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, - const double hysteresis_factor, const double max_velocity_limit, CollisionCheckDebug & debug); + const double hysteresis_factor, const double max_velocity_limit, const double yaw_difference_th, + CollisionCheckDebug & debug); bool checkPolygonsIntersects( const std::vector & polys_1, const std::vector & polys_2); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index 7bb8c13aa65fb..b05d869cca6b5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -26,6 +26,9 @@ #include #include +#include + +#include #include namespace autoware::behavior_path_planner::utils::path_safety_checker @@ -482,7 +485,8 @@ bool checkSafetyWithRSS( const std::vector & ego_predicted_path, const std::vector & objects, CollisionCheckDebugMap & debug_map, const BehaviorPathPlannerParameters & parameters, const RSSparams & rss_params, - const bool check_all_predicted_path, const double hysteresis_factor) + const bool check_all_predicted_path, const double hysteresis_factor, + const double yaw_difference_th) { // Check for collisions with each predicted path of the object const bool is_safe = !std::any_of(objects.begin(), objects.end(), [&](const auto & object) { @@ -495,7 +499,7 @@ bool checkSafetyWithRSS( obj_predicted_paths.begin(), obj_predicted_paths.end(), [&](const auto & obj_path) { const bool has_collision = !utils::path_safety_checker::checkCollision( planned_path, ego_predicted_path, object, obj_path, parameters, rss_params, - hysteresis_factor, current_debug_data.second); + hysteresis_factor, yaw_difference_th, current_debug_data.second); utils::path_safety_checker::updateCollisionCheckDebugMap( debug_map, current_debug_data, !has_collision); @@ -559,11 +563,12 @@ bool checkCollision( const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, - const double hysteresis_factor, CollisionCheckDebug & debug) + const double hysteresis_factor, const double yaw_difference_th, CollisionCheckDebug & debug) { const auto collided_polygons = getCollidedPolygons( planned_path, predicted_ego_path, target_object, target_object_path, common_parameters, - rss_parameters, hysteresis_factor, std::numeric_limits::max(), debug); + rss_parameters, hysteresis_factor, std::numeric_limits::max(), yaw_difference_th, + debug); return collided_polygons.empty(); } @@ -573,7 +578,8 @@ std::vector getCollidedPolygons( const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, - double hysteresis_factor, const double max_velocity_limit, CollisionCheckDebug & debug) + double hysteresis_factor, const double max_velocity_limit, const double yaw_difference_th, + CollisionCheckDebug & debug) { { debug.ego_predicted_path = predicted_ego_path; @@ -604,6 +610,11 @@ std::vector getCollidedPolygons( const auto & ego_polygon = interpolated_data->poly; const auto ego_velocity = std::min(interpolated_data->velocity, max_velocity_limit); + const double ego_yaw = tf2::getYaw(ego_pose.orientation); + const double object_yaw = tf2::getYaw(obj_pose.orientation); + const double yaw_difference = autoware::universe_utils::normalizeRadian(ego_yaw - object_yaw); + if (std::abs(yaw_difference) > yaw_difference_th) continue; + // check overlap if (boost::geometry::overlaps(ego_polygon, obj_polygon)) { debug.unsafe_reason = "overlap_polygon"; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md index 066708891652a..dba4a0cfce5e7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md @@ -436,17 +436,18 @@ Parameters under `target_filtering` are related to filtering target objects for Parameters under `safety_check_params` define the configuration for safety check. -| Name | Unit | Type | Description | Default value | -| :--------------------------------------------- | :--- | :----- | :------------------------------------------ | :------------ | -| enable_safety_check | - | bool | Flag to enable safety check | true | -| check_all_predicted_path | - | bool | Flag to check all predicted paths | true | -| publish_debug_marker | - | bool | Flag to publish debug markers | false | -| rss_params.rear_vehicle_reaction_time | [s] | double | Reaction time for rear vehicles | 2.0 | -| rss_params.rear_vehicle_safety_time_margin | [s] | double | Safety time margin for rear vehicles | 1.0 | -| rss_params.lateral_distance_max_threshold | [m] | double | Maximum lateral distance threshold | 2.0 | -| rss_params.longitudinal_distance_min_threshold | [m] | double | Minimum longitudinal distance threshold | 3.0 | -| rss_params.longitudinal_velocity_delta_time | [s] | double | Delta time for longitudinal velocity | 0.8 | -| hysteresis_factor_expand_rate | - | double | Rate to expand/shrink the hysteresis factor | 1.0 | +| Name | Unit | Type | Description | Default value | +| :--------------------------------------------- | :--- | :----- | :---------------------------------------------------------------------------------------- | :------------ | +| enable_safety_check | - | bool | Flag to enable safety check | true | +| check_all_predicted_path | - | bool | Flag to check all predicted paths | true | +| publish_debug_marker | - | bool | Flag to publish debug markers | false | +| rss_params.rear_vehicle_reaction_time | [s] | double | Reaction time for rear vehicles | 2.0 | +| rss_params.rear_vehicle_safety_time_margin | [s] | double | Safety time margin for rear vehicles | 1.0 | +| rss_params.lateral_distance_max_threshold | [m] | double | Maximum lateral distance threshold | 2.0 | +| rss_params.longitudinal_distance_min_threshold | [m] | double | Minimum longitudinal distance threshold | 3.0 | +| rss_params.longitudinal_velocity_delta_time | [s] | double | Delta time for longitudinal velocity | 0.8 | +| hysteresis_factor_expand_rate | - | double | Rate to expand/shrink the hysteresis factor | 1.0 | +| collision_check_yaw_diff_threshold | - | double | Maximum yaw difference between ego and object when executing rss-based collision checking | 1.578 | ## **Path Generation** diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml index ce7ead87b54c7..d39a320a19e14 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml @@ -143,8 +143,10 @@ lateral_distance_max_threshold: 2.0 longitudinal_distance_min_threshold: 3.0 longitudinal_velocity_delta_time: 0.8 + extended_polygon_policy: "along_path" # [-] select "rectangle" or "along_path" # hysteresis factor to expand/shrink polygon hysteresis_factor_expand_rate: 1.0 + collision_check_yaw_diff_threshold: 1.578 # temporary backward_path_length: 30.0 forward_path_length: 100.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp index 7554098414442..b07d6497463ef 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp @@ -77,7 +77,7 @@ struct PullOutStatus // record if the ego has departed from the start point bool has_departed{false}; - PullOutStatus() {} + PullOutStatus() = default; }; class StartPlannerModule : public SceneModuleInterface @@ -90,7 +90,7 @@ class StartPlannerModule : public SceneModuleInterface std::unordered_map> & objects_of_interest_marker_interface_ptr_map); - ~StartPlannerModule() + ~StartPlannerModule() override { if (freespace_planner_timer_) { freespace_planner_timer_->cancel(); @@ -296,16 +296,16 @@ class StartPlannerModule : public SceneModuleInterface PathWithLaneId getCurrentPath() const; void planWithPriority( const std::vector & start_pose_candidates, const Pose & refined_start_pose, - const Pose & goal_pose, const std::string search_priority); + const Pose & goal_pose, const std::string & search_priority); PathWithLaneId generateStopPath() const; lanelet::ConstLanelets getPathRoadLanes(const PathWithLaneId & path) const; std::vector generateDrivableLanes(const PathWithLaneId & path) const; void updatePullOutStatus(); void updateStatusAfterBackwardDriving(); PredictedObjects filterStopObjectsInPullOutLanes( - const lanelet::ConstLanelets & pull_out_lanes, const geometry_msgs::msg::Point & current_pose, - const double velocity_threshold, const double object_check_backward_distance, - const double object_check_forward_distance) const; + const lanelet::ConstLanelets & pull_out_lanes, const geometry_msgs::msg::Point & current_point, + const double velocity_threshold, const double object_check_forward_distance, + const double object_check_backward_distance) const; bool needToPrepareBlinkerBeforeStartDrivingForward() const; bool hasReachedFreespaceEnd() const; bool hasReachedPullOutEnd() const; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp index 9451bb3870648..873bab1043485 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp @@ -283,6 +283,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) node->declare_parameter(safety_check_ns + "forward_path_length"); p.safety_check_params.publish_debug_marker = node->declare_parameter(safety_check_ns + "publish_debug_marker"); + p.safety_check_params.collision_check_yaw_diff_threshold = + node->declare_parameter(safety_check_ns + "collision_check_yaw_diff_threshold"); } // RSSparams @@ -298,6 +300,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) node->declare_parameter(rss_ns + "longitudinal_distance_min_threshold"); p.safety_check_params.rss_params.longitudinal_velocity_delta_time = node->declare_parameter(rss_ns + "longitudinal_velocity_delta_time"); + p.safety_check_params.rss_params.extended_polygon_policy = + node->declare_parameter(rss_ns + "extended_polygon_policy"); } // surround moving obstacle check @@ -367,7 +371,6 @@ void StartPlannerModuleManager::updateModuleParams( updateParam( parameters, ns + "extra_width_margin_for_rear_obstacle", p->extra_width_margin_for_rear_obstacle); - updateParam>( parameters, ns + "collision_check_margins", p->collision_check_margins); updateParam( @@ -658,6 +661,9 @@ void StartPlannerModuleManager::updateModuleParams( updateParam( parameters, safety_check_ns + "publish_debug_marker", p->safety_check_params.publish_debug_marker); + updateParam( + parameters, safety_check_ns + "collision_check_yaw_diff_threshold", + p->safety_check_params.collision_check_yaw_diff_threshold); } const std::string rss_ns = safety_check_ns + "rss_params."; { @@ -676,6 +682,9 @@ void StartPlannerModuleManager::updateModuleParams( updateParam( parameters, rss_ns + "longitudinal_velocity_delta_time", p->safety_check_params.rss_params.longitudinal_velocity_delta_time); + updateParam( + parameters, rss_ns + "extended_polygon_policy", + p->safety_check_params.rss_params.extended_polygon_policy); } std::string surround_moving_obstacle_check_ns = ns + "surround_moving_obstacle_check."; { diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index 9400f3ff40286..08d5ea4365575 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -34,7 +34,9 @@ #include #include +#include #include +#include #include #include #include @@ -65,7 +67,7 @@ StartPlannerModule::StartPlannerModule( { lane_departure_checker_ = std::make_shared(); lane_departure_checker_->setVehicleInfo(vehicle_info_); - autoware::lane_departure_checker::Param lane_departure_checker_params; + autoware::lane_departure_checker::Param lane_departure_checker_params{}; lane_departure_checker_params.footprint_extra_margin = parameters->lane_departure_check_expansion_margin; @@ -104,7 +106,7 @@ void StartPlannerModule::onFreespacePlannerTimer() std::optional current_status_opt{std::nullopt}; std::optional parameters_opt{std::nullopt}; std::optional pull_out_status_opt{std::nullopt}; - bool is_stopped; + bool is_stopped{false}; // making a local copy of thread sensitive data { @@ -501,7 +503,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const // Get the closest target obj width in the relevant lanes const auto closest_object_width = std::invoke([&]() -> std::optional { double arc_length_to_closet_object = std::numeric_limits::max(); - double closest_object_width = -1.0; + std::optional closest_object_width = std::nullopt; std::for_each( target_objects_on_lane.on_current_lane.begin(), target_objects_on_lane.on_current_lane.end(), [&](const auto & o) { @@ -512,7 +514,6 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const arc_length_to_closet_object = arc_length; closest_object_width = o.shape.dimensions.y; }); - if (closest_object_width < 0.0) return std::nullopt; return closest_object_width; }); if (!closest_object_width) return false; @@ -837,7 +838,7 @@ PathWithLaneId StartPlannerModule::getCurrentPath() const void StartPlannerModule::planWithPriority( const std::vector & start_pose_candidates, const Pose & refined_start_pose, - const Pose & goal_pose, const std::string search_priority) + const Pose & goal_pose, const std::string & search_priority) { if (start_pose_candidates.empty()) return; @@ -1112,8 +1113,8 @@ void StartPlannerModule::updateStatusAfterBackwardDriving() waitApproval(); // To enable approval of the forward path, the RTC status is removed. removeRTCStatus(); - for (auto itr = uuid_map_.begin(); itr != uuid_map_.end(); ++itr) { - itr->second = generateUUID(); + for (auto & itr : uuid_map_) { + itr.second = generateUUID(); } } @@ -1303,7 +1304,7 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() return ignore_signal_.value() == id; }; - const auto update_ignore_signal = [this](const lanelet::Id & id, const bool is_ignore) { + const auto update_ignore_signal = [](const lanelet::Id & id, const bool is_ignore) { return is_ignore ? std::make_optional(id) : std::nullopt; }; @@ -1422,10 +1423,12 @@ bool StartPlannerModule::isSafePath() const merged_target_object.insert( merged_target_object.end(), target_objects_on_lane.on_shoulder_lane.begin(), target_objects_on_lane.on_shoulder_lane.end()); + return autoware::behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS( pull_out_path, ego_predicted_path, merged_target_object, debug_data_.collision_check, planner_data_->parameters, safety_check_params_->rss_params, - objects_filtering_params_->use_all_predicted_path, hysteresis_factor); + objects_filtering_params_->use_all_predicted_path, hysteresis_factor, + safety_check_params_->collision_check_yaw_diff_threshold); } bool StartPlannerModule::isGoalBehindOfEgoInSameRouteSegment() const @@ -1693,7 +1696,7 @@ void StartPlannerModule::setDebugData() // safety check if (parameters_->safety_check_params.enable_safety_check) { - if (debug_data_.ego_predicted_path.size() > 0) { + if (!debug_data_.ego_predicted_path.empty()) { const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath( debug_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution); add( @@ -1702,7 +1705,7 @@ void StartPlannerModule::setDebugData() debug_marker_); } - if (debug_data_.filtered_objects.objects.size() > 0) { + if (!debug_data_.filtered_objects.objects.empty()) { add( createObjectsMarkerArray( debug_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9), diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml index 324be9a73439e..80d2128bf70e0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml @@ -174,6 +174,7 @@ safety_check_backward_distance: 100.0 # [m] hysteresis_factor_expand_rate: 1.5 # [-] hysteresis_factor_safe_count: 3 # [-] + collision_check_yaw_diff_threshold: 3.1416 # [rad] # predicted path parameters min_velocity: 1.38 # [m/s] max_velocity: 50.0 # [m/s] diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp index f05230f4ec19d..dbedbfe1f70cc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp @@ -223,6 +223,8 @@ struct AvoidanceParameters size_t hysteresis_factor_safe_count; double hysteresis_factor_expand_rate{0.0}; + double collision_check_yaw_diff_threshold{3.1416}; + bool consider_front_overhang{true}; bool consider_rear_overhang{true}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp index b761939589968..a79a67d5fdd4a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp @@ -199,6 +199,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node) getOrDeclareParameter(*node, ns + "hysteresis_factor_expand_rate"); p.hysteresis_factor_safe_count = getOrDeclareParameter(*node, ns + "hysteresis_factor_safe_count"); + p.collision_check_yaw_diff_threshold = + getOrDeclareParameter(*node, ns + "collision_check_yaw_diff_threshold"); } // safety check predicted path params diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json index 266a4cf8a23ab..1e0753a288a28 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json @@ -927,6 +927,11 @@ "description": "Hysteresis count that be used for chattering prevention.", "default": 10 }, + "collision_check_yaw_diff_threshold": { + "type": "number", + "description": "Max yaw difference between ego and object when doing collision check", + "default": 3.1416 + }, "min_velocity": { "type": "number", "description": "Minimum velocity of the ego vehicle's predicted path.", diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index b0fd1b5c1b8ab..824466a1ad3ad 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -781,7 +781,8 @@ bool StaticObstacleAvoidanceModule::isSafePath( for (const auto & obj_path : obj_predicted_paths) { if (!utils::path_safety_checker::checkCollision( shifted_path.path, ego_predicted_path, object, obj_path, p, parameters_->rss_params, - hysteresis_factor, current_debug_data.second)) { + hysteresis_factor, parameters_->collision_check_yaw_diff_threshold, + current_debug_data.second)) { utils::path_safety_checker::updateCollisionCheckDebugMap( debug.collision_check, current_debug_data, false); From be0277126343fce40bc00a8dbbe56ba93dd6897d Mon Sep 17 00:00:00 2001 From: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com> Date: Thu, 27 Jun 2024 11:04:46 +0900 Subject: [PATCH 2/6] fix(autoware_behavior_velocity_planner_common): remove lane_id check from arc_lane_util (#7710) * fix(arc_lane_util): remove lane_id check from arc_lane_util Signed-off-by: Y.Hisaki * modify test_arc_lane_util.cpp Signed-off-by: Y.Hisaki --------- Signed-off-by: Y.Hisaki --- .../src/scene.cpp | 4 ++-- .../src/scene_no_stopping_area.cpp | 2 +- .../utilization/arc_lane_util.hpp | 19 ++++--------------- .../src/utilization/arc_lane_util.cpp | 4 ++-- .../test/src/test_arc_lane_util.cpp | 5 +---- .../src/scene.cpp | 2 +- .../src/scene.cpp | 2 +- 7 files changed, 12 insertions(+), 26 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp index 7abd88d59f398..9cbe962e88cae 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp @@ -83,7 +83,7 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // Get stop point const auto stop_point = arc_lane_utils::createTargetPoint( - original_path, stop_line, lane_id_, planner_param_.stop_margin, + original_path, stop_line, planner_param_.stop_margin, planner_data_->vehicle_info_.max_longitudinal_offset_m); if (!stop_point) { return true; @@ -128,7 +128,7 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * if (planner_param_.use_dead_line) { // Use '-' for margin because it's the backward distance from stop line const auto dead_line_point = arc_lane_utils::createTargetPoint( - original_path, stop_line, lane_id_, -planner_param_.dead_line_margin, + original_path, stop_line, -planner_param_.dead_line_margin, planner_data_->vehicle_info_.max_longitudinal_offset_m); if (dead_line_point) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index d007416dccb47..0478aea34a44f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -125,7 +125,7 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason return true; } const auto stop_point = arc_lane_utils::createTargetPoint( - original_path, stop_line.value(), lane_id_, planner_param_.stop_margin, + original_path, stop_line.value(), planner_param_.stop_margin, planner_data_->vehicle_info_.max_longitudinal_offset_m); if (!stop_point) { setSafe(true); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp index 77b7d25f9f46e..769932259e3e6 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp @@ -61,20 +61,9 @@ std::optional checkCollision( template std::optional findCollisionSegment( const T & path, const geometry_msgs::msg::Point & stop_line_p1, - const geometry_msgs::msg::Point & stop_line_p2, const size_t target_lane_id) + const geometry_msgs::msg::Point & stop_line_p2) { for (size_t i = 0; i < path.points.size() - 1; ++i) { - const auto & prev_lane_ids = path.points.at(i).lane_ids; - const auto & next_lane_ids = path.points.at(i + 1).lane_ids; - - const bool is_target_lane_in_prev_lane = - std::find(prev_lane_ids.begin(), prev_lane_ids.end(), target_lane_id) != prev_lane_ids.end(); - const bool is_target_lane_in_next_lane = - std::find(next_lane_ids.begin(), next_lane_ids.end(), target_lane_id) != next_lane_ids.end(); - if (!is_target_lane_in_prev_lane && !is_target_lane_in_next_lane) { - continue; - } - const auto & p1 = autoware::universe_utils::getPoint(path.points.at(i)); // Point before collision point const auto & p2 = @@ -92,12 +81,12 @@ std::optional findCollisionSegment( template std::optional findCollisionSegment( - const T & path, const LineString2d & stop_line, const size_t target_lane_id) + const T & path, const LineString2d & stop_line) { const auto stop_line_p1 = convertToGeomPoint(stop_line.at(0)); const auto stop_line_p2 = convertToGeomPoint(stop_line.at(1)); - return findCollisionSegment(path, stop_line_p1, stop_line_p2, target_lane_id); + return findCollisionSegment(path, stop_line_p1, stop_line_p2); } template @@ -194,7 +183,7 @@ geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffse std::optional createTargetPoint( const tier4_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, - const size_t lane_id, const double margin, const double vehicle_offset); + const double margin, const double vehicle_offset); } // namespace arc_lane_utils } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp index 3e029174b288d..da6ef7262de74 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp @@ -109,10 +109,10 @@ std::optional findOffsetSegment( std::optional createTargetPoint( const tier4_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, - const size_t lane_id, const double margin, const double vehicle_offset) + const double margin, const double vehicle_offset) { // Find collision segment - const auto collision_segment = findCollisionSegment(path, stop_line, lane_id); + const auto collision_segment = findCollisionSegment(path, stop_line); if (!collision_segment) { // No collision return {}; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp index ddedec80dc0f3..db9a63169e565 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp @@ -49,14 +49,11 @@ TEST(findCollisionSegment, nominal) * **/ auto path = test::generatePath(0.0, 0.0, 5.0, 0.0, 6); - for (auto & point : path.points) { - point.lane_ids.push_back(100); - } LineString2d stop_line; stop_line.emplace_back(Point2d(3.5, 3.0)); stop_line.emplace_back(Point2d(3.5, -3.0)); - auto segment = arc_lane_utils::findCollisionSegment(path, stop_line, 100); + auto segment = arc_lane_utils::findCollisionSegment(path, stop_line); EXPECT_EQ(segment->first, static_cast(3)); EXPECT_DOUBLE_EQ(segment->second.x, 3.5); EXPECT_DOUBLE_EQ(segment->second.y, 0.0); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp index 6075706a672d0..04ea5ca872098 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp @@ -52,7 +52,7 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop // Calculate stop pose and insert index const auto stop_point = arc_lane_utils::createTargetPoint( - *path, stop_line, lane_id_, planner_param_.stop_margin, + *path, stop_line, planner_param_.stop_margin, planner_data_->vehicle_info_.max_longitudinal_offset_m); // If no collision found, do nothing diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp index d27cc23223630..1bf26d9f95c6a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp @@ -379,7 +379,7 @@ std::optional VirtualTrafficLightModule::getPathIndexOfFirstEndLine() end_line_p2.y = end_line.back().y(); const auto collision = - arc_lane_utils::findCollisionSegment(module_data_.path, end_line_p1, end_line_p2, lane_id_); + arc_lane_utils::findCollisionSegment(module_data_.path, end_line_p1, end_line_p2); if (!collision) { continue; } From d88de1808cfdb5ed7cacf7fe069753848a4bdbe2 Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Thu, 27 Jun 2024 11:48:07 +0900 Subject: [PATCH 3/6] docs(rtc_replayer): fix wrong link (#7714) * fix link for rtc_replayer Signed-off-by: Go Sakayori * delete RTC replayer header Signed-off-by: Go Sakayori * fix Signed-off-by: Go Sakayori --------- Signed-off-by: Go Sakayori --- planning/.pages | 1 - planning/autoware_rtc_interface/README.md | 4 ++++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/planning/.pages b/planning/.pages index 0ebebc3c6ac7d..2e3dd92011d19 100644 --- a/planning/.pages +++ b/planning/.pages @@ -79,7 +79,6 @@ nav: - 'RTC Interface': planning/autoware_rtc_interface - 'Additional Tools': - 'Remaining Distance Time Calculator': planning/autoware_remaining_distance_time_calculator - - 'RTC Replayer': planning/rtc_replayer - 'Planning Debug Tools': - 'About Planning Debug Tools': https://github.com/autowarefoundation/autoware_tools/tree/main/planning/planning_debug_tools - 'Stop Reason Visualizer': https://github.com/autowarefoundation/autoware_tools/blob/main/planning/planning_debug_tools/doc-stop-reason-visualizer.md diff --git a/planning/autoware_rtc_interface/README.md b/planning/autoware_rtc_interface/README.md index 19d200592bcf3..635f692266f7e 100644 --- a/planning/autoware_rtc_interface/README.md +++ b/planning/autoware_rtc_interface/README.md @@ -185,6 +185,10 @@ Return `true` if `uuid` is registered. If `uuid` is registered, return `true`. If not, return `false`. +## Debugging Tools + +There is a debugging tool called [RTC replayer](https://github.com/autowarefoundation/autoware_tools/tree/main/planning/autoware_rtc_replayer) for the RTC interface. + ## Assumptions / Known limits ## Future extensions / Unimplemented parts From 228b6afa232dcee07b99c9a7cd87824feb1d7751 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 27 Jun 2024 12:47:28 +0900 Subject: [PATCH 4/6] refactor(static_obstacle_avoidance): organize params for drivable lane (#7715) * refactor(static_obstacle_avoidance): organize params for drivable lane Signed-off-by: satoshi-ota * Update planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json --------- Signed-off-by: satoshi-ota --- .../README.md | 38 +++++++++++-------- .../static_obstacle_avoidance.param.yaml | 8 +++- .../data_structs.hpp | 8 +--- .../parameter_helper.hpp | 3 +- .../static_obstacle_avoidance.schema.json | 17 +++------ .../src/utils.cpp | 12 +++--- 6 files changed, 45 insertions(+), 41 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md index b27723ab2066f..ec139b9a8b099 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md @@ -760,7 +760,7 @@ Basically, this module tries to generate avoidance path in order to keep lateral But if there isn't enough space to keep `soft_margin` distance, this module shortens soft constraint lateral margin. The parameter `soft_margin` is a maximum value of soft constraint, and actual soft margin can be a value between 0.0 and `soft_margin`. On the other hand, this module definitely keeps `hard_margin` or `hard_margin_for_parked_vehicle` depending on the situation. Thus, the minimum value of total lateral margin is `hard_margin`/`hard_margin_for_parked_vehicle`, and the maximum value is the sum of `hard_margin`/`hard_margin_for_parked_vehicle` and `soft_margin`. -Following figure shows the situation where this module shortens lateral soft constraint in order not to drive opposite direction lane when user set a parameter `use_opposite_lane` to `false`. +Following figure shows the situation where this module shortens lateral soft constraint in order not to drive opposite direction lane when user set a parameter `use_lane_type` to `same_direction_lane`. ![fig](./images/path_generation/adjust_margin.png) @@ -784,7 +784,7 @@ On the other hand, if the lateral distance is larger than `hard_margin`/`hard_ma ### When there is not enough space -This module inserts stop point only when the ego can potentially avoid the object. So, if it is not able to keep distance more than `hard_margin`/`hard_margin_for_parked_vehicle`, this module does nothing. Following figure shows the situation where this module is not able to keep enough lateral distance when user set a parameter `use_opposite_lane` to `false`. +This module inserts stop point only when the ego can potentially avoid the object. So, if it is not able to keep distance more than `hard_margin`/`hard_margin_for_parked_vehicle`, this module does nothing. Following figure shows the situation where this module is not able to keep enough lateral distance when user set a parameter `use_lane_type` to `same_direction_lane`. ![fig](./images/path_generation/do_nothing.png) @@ -804,15 +804,19 @@ Usable lane for avoidance module can be selected by config file. ```yaml ... - use_adjacent_lane: true - use_opposite_lane: true + # drivable lane setting. this module is able to use not only current lane but also right/left lane + # if the current lane(=lanelt::Lanelet) and the rignt/left lane share the boundary(=lanelet::Linestring) in HDMap. + # "current_lane" : use only current lane. this module doesn't use adjacent lane to avoid object. + # "same_direction_lane" : this module uses same direction lane to avoid object if need. + # "opposite_direction_lane": this module uses both same direction and opposite direction lane. + use_lane_type: "opposite_direction_lane" ``` -When user set parameter both `use_adjacent_lane` and `use_opposite_lane` to `true`, it is possible to use opposite lane. +When user set parameter `use_lane_type` to `opposite_direction_lane`, it is possible to use opposite lane. ![fig](./images/path_generation/opposite_direction.png) -When user only set parameter `use_adjacent_lane` to `true`, the module doesn't create path that overlaps opposite lane. +When user set parameter `use_lane_type` to `same_direction_lane`, the module doesn't create path that overlaps opposite lane. ![fig](./images/path_generation/same_direction.png) @@ -972,21 +976,25 @@ This module supports drivable area expansion for following polygons defined in H Please set the flags to `true` when user wants to make it possible to use those areas in avoidance maneuver. ```yaml +# drivable lane setting. this module is able to use not only current lane but also right/left lane +# if the current lane(=lanelt::Lanelet) and the rignt/left lane share the boundary(=lanelet::Linestring) in HDMap. +# "current_lane" : use only current lane. this module doesn't use adjacent lane to avoid object. +# "same_direction_lane" : this module uses same direction lane to avoid object if need. +# "opposite_direction_lane": this module uses both same direction and opposite direction lane. +use_lane_type: "opposite_direction_lane" # drivable area setting -use_adjacent_lane: true -use_opposite_lane: true use_intersection_areas: true use_hatched_road_markings: true use_freespace_areas: true ``` -| | | | -| --------------------- | ---------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| adjacent lane | ![fig](./images/advanced/avoidance_same_direction.png) | | -| opposite lane | ![fig](./images/advanced/avoidance_opposite_direction.png) | | -| intersection area | ![fig](./images/advanced/avoidance_intersection.png) | The intersection area is defined on Lanelet map. See [here](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md) | -| hatched road markings | ![fig](./images/advanced/avoidance_zebra.png) | The hatched road marking is defined on Lanelet map. See [here](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md#hatched-road-markings-area) | -| freespace area | ![fig](./images/advanced/avoidance_freespace.png) | The freespace area is defined on Lanelet map. (unstable) | +| | | | +| -------------------------------------- | ---------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| use_lane_type: same_direction_lane | ![fig](./images/advanced/avoidance_same_direction.png) | | +| use_lane_type: opposite_direction_lane | ![fig](./images/advanced/avoidance_opposite_direction.png) | | +| intersection area | ![fig](./images/advanced/avoidance_intersection.png) | The intersection area is defined on Lanelet map. See [here](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md) | +| hatched road markings | ![fig](./images/advanced/avoidance_zebra.png) | The hatched road marking is defined on Lanelet map. See [here](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md#hatched-road-markings-area) | +| freespace area | ![fig](./images/advanced/avoidance_freespace.png) | The freespace area is defined on Lanelet map. (unstable) | ## Future extensions / Unimplemented parts diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml index 80d2128bf70e0..3087ccc93934b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml @@ -5,9 +5,13 @@ resample_interval_for_planning: 0.3 # [m] FOR DEVELOPER resample_interval_for_output: 4.0 # [m] FOR DEVELOPER + # drivable lane setting. this module is able to use not only current lane but also right/left lane + # if the current lane(=lanelet::Lanelet) and the right/left lane share the boundary(=lanelet::Linestring) in HDMap. + # "current_lane" : use only current lane. this module doesn't use adjacent lane to avoid object. + # "same_direction_lane" : this module uses same direction lane to avoid object if need. + # "opposite_direction_lane": this module uses both same direction and opposite direction lane. + use_lane_type: "opposite_direction_lane" # drivable area setting - use_adjacent_lane: true - use_opposite_lane: true use_intersection_areas: true use_hatched_road_markings: true use_freespace_areas: true diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp index dbedbfe1f70cc..d02b39047e71c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp @@ -101,12 +101,8 @@ struct AvoidanceParameters // computational cost for latter modules. double resample_interval_for_output = 3.0; - // enable avoidance to be perform only in lane with same direction - bool use_adjacent_lane{true}; - - // enable avoidance to be perform in opposite lane direction - // to use this, enable_avoidance_over_same_direction need to be set to true. - bool use_opposite_lane{true}; + // drivable lane config + std::string use_lane_type{"current_lane"}; // if this param is true, it reverts avoidance path when the path is no longer needed. bool enable_cancel_maneuver{false}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp index a79a67d5fdd4a..84cf7c4e33d26 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp @@ -46,8 +46,7 @@ AvoidanceParameters getParameter(rclcpp::Node * node) // drivable area { const std::string ns = "avoidance."; - p.use_adjacent_lane = getOrDeclareParameter(*node, ns + "use_adjacent_lane"); - p.use_opposite_lane = getOrDeclareParameter(*node, ns + "use_opposite_lane"); + p.use_lane_type = getOrDeclareParameter(*node, ns + "use_lane_type"); p.use_intersection_areas = getOrDeclareParameter(*node, ns + "use_intersection_areas"); p.use_hatched_road_markings = getOrDeclareParameter(*node, ns + "use_hatched_road_markings"); diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json index 1e0753a288a28..db3215fa8d238 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json @@ -22,15 +22,11 @@ "description": "Path generation method.", "default": "shift_line_base" }, - "use_adjacent_lane": { - "type": "boolean", - "description": "Extend avoidance trajectory to adjacent lanes that has same direction. If false, avoidance only happen in current lane.", - "default": "true" - }, - "use_opposite_lane": { - "type": "boolean", - "description": "Extend avoidance trajectory to opposite direction lane. `use_adjacent_lane` must be `true` to take effects.", - "default": "true" + "use_lane_type": { + "type": "string", + "enum": ["current_lane", "same_direction_lane", "opposite_direction_lane"], + "description": "Drivable lane configuration.", + "default": "opposite_direction_lane" }, "use_hatched_road_markings": { "type": "boolean", @@ -1447,8 +1443,7 @@ "resample_interval_for_planning", "resample_interval_for_output", "path_generation_method", - "use_adjacent_lane", - "use_opposite_lane", + "use_lane_type", "use_hatched_road_markings", "use_intersection_areas", "use_freespace_areas", diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index 70de9e9499917..8d0f9904b5bad 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -2238,14 +2238,16 @@ DrivableLanes generateExpandedDrivableLanes( current_drivable_lanes.left_lane = lanelet; current_drivable_lanes.right_lane = lanelet; - if (!parameters->use_adjacent_lane) { + if (parameters->use_lane_type == "current_lane") { return current_drivable_lanes; } + const auto use_opposite_lane = parameters->use_lane_type == "opposite_direction_lane"; + // 1. get left/right side lanes const auto update_left_lanelets = [&](const lanelet::ConstLanelet & target_lane) { - const auto all_left_lanelets = route_handler->getAllLeftSharedLinestringLanelets( - target_lane, parameters->use_opposite_lane, true); + const auto all_left_lanelets = + route_handler->getAllLeftSharedLinestringLanelets(target_lane, use_opposite_lane, true); if (!all_left_lanelets.empty()) { current_drivable_lanes.left_lane = all_left_lanelets.back(); // leftmost lanelet pushUniqueVector( @@ -2254,8 +2256,8 @@ DrivableLanes generateExpandedDrivableLanes( } }; const auto update_right_lanelets = [&](const lanelet::ConstLanelet & target_lane) { - const auto all_right_lanelets = route_handler->getAllRightSharedLinestringLanelets( - target_lane, parameters->use_opposite_lane, true); + const auto all_right_lanelets = + route_handler->getAllRightSharedLinestringLanelets(target_lane, use_opposite_lane, true); if (!all_right_lanelets.empty()) { current_drivable_lanes.right_lane = all_right_lanelets.back(); // rightmost lanelet pushUniqueVector( From bcf06770c50106d6693efe12303b500b5684b4ce Mon Sep 17 00:00:00 2001 From: Yukihiro Saito Date: Thu, 27 Jun 2024 13:36:44 +0900 Subject: [PATCH 5/6] feat(image_projection_based_fusion): unrectify image projection based fusion (#7053) * unrecify 3d point for image projection based fusion Signed-off-by: Yukihiro Saito * style(pre-commit): autofix * add try-catch code for exception handling Signed-off-by: Yukihito Saito * Update perception/image_projection_based_fusion/src/utils/utils.cpp Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> * style(pre-commit): autofix * remove litter Signed-off-by: Yukihito Saito * revert try-catch Signed-off-by: Yukihito Saito * Update perception/image_projection_based_fusion/src/utils/utils.cpp Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> * fix: resolve conflict Signed-off-by: Taekjin LEE --------- Signed-off-by: Yukihiro Saito Signed-off-by: Yukihito Saito Signed-off-by: Taekjin LEE Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> Co-authored-by: Taekjin LEE --- .../roi_detected_object_fusion/node.hpp | 5 ++- .../roi_pointcloud_fusion/node.hpp | 2 ++ .../segmentation_pointcloud_fusion/node.hpp | 2 ++ .../utils/utils.hpp | 5 +++ .../image_projection_based_fusion/package.xml | 1 + .../src/pointpainting_fusion/node.cpp | 16 ++++----- .../src/roi_cluster_fusion/node.cpp | 34 ++++++++----------- .../src/roi_detected_object_fusion/node.cpp | 21 ++++-------- .../src/roi_pointcloud_fusion/node.cpp | 20 +++++------ .../segmentation_pointcloud_fusion/node.cpp | 22 +++++------- .../src/utils/utils.cpp | 10 ++++++ 11 files changed, 69 insertions(+), 69 deletions(-) diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp index 65e03c6bc6316..d61ced593de78 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp @@ -18,6 +18,8 @@ #include "autoware/universe_utils/ros/debug_publisher.hpp" #include "image_projection_based_fusion/fusion_node.hpp" +#include + #include "autoware_perception_msgs/msg/object_classification.hpp" #include @@ -45,7 +47,8 @@ class RoiDetectedObjectFusionNode std::map generateDetectedObjectRoIs( const DetectedObjects & input_object_msg, const double image_width, const double image_height, - const Eigen::Affine3d & object2camera_affine, const Eigen::Matrix4d & camera_projection); + const Eigen::Affine3d & object2camera_affine, + const image_geometry::PinholeCameraModel & pinhole_camera_model); void fuseObjectsOnImage( const DetectedObjects & input_object_msg, diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp index 4cbc0990352e6..4d4fd8d2dac42 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp @@ -17,6 +17,8 @@ #include "image_projection_based_fusion/fusion_node.hpp" +#include + #include #include namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp index c458c17bed793..26bcfd5ed802a 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp @@ -17,6 +17,8 @@ #include "image_projection_based_fusion/fusion_node.hpp" +#include + #include #include diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp index ffa1666396a1d..107e0d6c179a8 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp @@ -42,6 +42,7 @@ #include "autoware_perception_msgs/msg/shape.hpp" #include "tier4_perception_msgs/msg/detected_object_with_feature.hpp" +#include #include #include #include @@ -60,6 +61,10 @@ struct PointData float distance; size_t orig_index; }; + +Eigen::Vector2d calcRawImageProjectedPoint( + const image_geometry::PinholeCameraModel & pinhole_camera_model, const cv::Point3d & point3d); + std::optional getTransformStamped( const tf2_ros::Buffer & tf_buffer, const std::string & target_frame_id, const std::string & source_frame_id, const rclcpp::Time & time); diff --git a/perception/image_projection_based_fusion/package.xml b/perception/image_projection_based_fusion/package.xml index 8d5a2ef1fe519..00ffdc8fd5528 100644 --- a/perception/image_projection_based_fusion/package.xml +++ b/perception/image_projection_based_fusion/package.xml @@ -21,6 +21,7 @@ autoware_universe_utils cv_bridge euclidean_cluster + image_geometry image_transport lidar_centerpoint message_filters diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp index 7973c22cc1d78..a7c83ac7614a8 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -307,9 +307,9 @@ void PointPaintingFusionNode::fuseOnSingleImage( const auto class_offset = painted_pointcloud_msg.fields.at(4).offset; const auto p_step = painted_pointcloud_msg.point_step; // projection matrix - Eigen::Matrix3f camera_projection; // use only x,y,z - camera_projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2), - camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6); + image_geometry::PinholeCameraModel pinhole_camera_model; + pinhole_camera_model.fromCameraInfo(camera_info); + Eigen::Vector3f point_lidar, point_camera; /** dc : don't care @@ -342,15 +342,15 @@ dc | dc dc dc dc ||zc| continue; } // project - Eigen::Vector3f normalized_projected_point = camera_projection * Eigen::Vector3f(p_x, p_y, p_z); + Eigen::Vector2d projected_point = + calcRawImageProjectedPoint(pinhole_camera_model, cv::Point3d(p_x, p_y, p_z)); + // iterate 2d bbox for (const auto & feature_object : objects) { sensor_msgs::msg::RegionOfInterest roi = feature_object.feature.roi; // paint current point if it is inside bbox int label2d = feature_object.object.classification.front().label; - if ( - !isUnknown(label2d) && - isInsideBbox(normalized_projected_point.x(), normalized_projected_point.y(), roi, p_z)) { + if (!isUnknown(label2d) && isInsideBbox(projected_point.x(), projected_point.y(), roi, p_z)) { data = &painted_pointcloud_msg.data[0]; auto p_class = reinterpret_cast(&output[stride + class_offset]); for (const auto & cls : isClassTable_) { @@ -361,7 +361,7 @@ dc | dc dc dc dc ||zc| #if 0 // Parallelizing loop don't support push_back if (debugger_) { - debug_image_points.push_back(normalized_projected_point); + debug_image_points.push_back(projected_point); } #endif } 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 bc786dd460d12..51987cbbcab84 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 @@ -83,10 +83,8 @@ void RoiClusterFusionNode::fuseOnSingleImage( const DetectedObjectsWithFeature & input_roi_msg, const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjectsWithFeature & output_cluster_msg) { - 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), - camera_info.p.at(8), camera_info.p.at(9), camera_info.p.at(10), camera_info.p.at(11); + image_geometry::PinholeCameraModel pinhole_camera_model; + pinhole_camera_model.fromCameraInfo(camera_info); // get transform from cluster frame id to camera optical frame id geometry_msgs::msg::TransformStamped transform_stamped; @@ -133,23 +131,19 @@ void RoiClusterFusionNode::fuseOnSingleImage( continue; } - Eigen::Vector4d projected_point = - projection * Eigen::Vector4d(*iter_x, *iter_y, *iter_z, 1.0); - Eigen::Vector2d normalized_projected_point = Eigen::Vector2d( - projected_point.x() / projected_point.z(), projected_point.y() / projected_point.z()); + Eigen::Vector2d projected_point = + calcRawImageProjectedPoint(pinhole_camera_model, cv::Point3d(*iter_x, *iter_y, *iter_z)); if ( - 0 <= static_cast(normalized_projected_point.x()) && - static_cast(normalized_projected_point.x()) <= - static_cast(camera_info.width) - 1 && - 0 <= static_cast(normalized_projected_point.y()) && - static_cast(normalized_projected_point.y()) <= - static_cast(camera_info.height) - 1) { - min_x = std::min(static_cast(normalized_projected_point.x()), min_x); - min_y = std::min(static_cast(normalized_projected_point.y()), min_y); - 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); - if (debugger_) debugger_->obstacle_points_.push_back(normalized_projected_point); + 0 <= static_cast(projected_point.x()) && + static_cast(projected_point.x()) <= static_cast(camera_info.width) - 1 && + 0 <= static_cast(projected_point.y()) && + static_cast(projected_point.y()) <= static_cast(camera_info.height) - 1) { + min_x = std::min(static_cast(projected_point.x()), min_x); + min_y = std::min(static_cast(projected_point.y()), min_y); + max_x = std::max(static_cast(projected_point.x()), max_x); + max_y = std::max(static_cast(projected_point.y()), max_y); + projected_points.push_back(projected_point); + if (debugger_) debugger_->obstacle_points_.push_back(projected_point); } } if (projected_points.empty()) { diff --git a/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp index 762d3e0ee51b1..7d85ecb2f9200 100644 --- a/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp @@ -86,15 +86,12 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( object2camera_affine = transformToEigen(transform_stamped_optional.value().transform); } - Eigen::Matrix4d camera_projection; - camera_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), camera_info.p.at(8), camera_info.p.at(9), camera_info.p.at(10), - camera_info.p.at(11); + image_geometry::PinholeCameraModel pinhole_camera_model; + pinhole_camera_model.fromCameraInfo(camera_info); const auto object_roi_map = generateDetectedObjectRoIs( input_object_msg, static_cast(camera_info.width), - static_cast(camera_info.height), object2camera_affine, camera_projection); + static_cast(camera_info.height), object2camera_affine, pinhole_camera_model); fuseObjectsOnImage(input_object_msg, input_roi_msg.feature_objects, object_roi_map); if (debugger_) { @@ -109,7 +106,8 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( std::map RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( const DetectedObjects & input_object_msg, const double image_width, const double image_height, - const Eigen::Affine3d & object2camera_affine, const Eigen::Matrix4d & camera_projection) + const Eigen::Affine3d & object2camera_affine, + const image_geometry::PinholeCameraModel & pinhole_camera_model) { std::map object_roi_map; int64_t timestamp_nsec = @@ -148,13 +146,8 @@ RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( continue; } - Eigen::Vector2d proj_point; - { - Eigen::Vector4d proj_point_hom = - camera_projection * Eigen::Vector4d(point.x(), point.y(), point.z(), 1.0); - proj_point = Eigen::Vector2d( - proj_point_hom.x() / (proj_point_hom.z()), proj_point_hom.y() / (proj_point_hom.z())); - } + Eigen::Vector2d proj_point = calcRawImageProjectedPoint( + pinhole_camera_model, cv::Point3d(point.x(), point.y(), point.z())); min_x = std::min(proj_point.x(), min_x); min_y = std::min(proj_point.y(), min_y); diff --git a/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp index ea5c2005bdd46..7f19402d9e565 100644 --- a/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp @@ -101,10 +101,9 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( } // transform pointcloud to camera optical frame id - 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), - camera_info.p.at(8), camera_info.p.at(9), camera_info.p.at(10), camera_info.p.at(11); + image_geometry::PinholeCameraModel pinhole_camera_model; + pinhole_camera_model.fromCameraInfo(camera_info); + geometry_msgs::msg::TransformStamped transform_stamped; { const auto transform_stamped_optional = getTransformStamped( @@ -143,10 +142,8 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( if (transformed_z <= 0.0) { continue; } - Eigen::Vector4d projected_point = - projection * Eigen::Vector4d(transformed_x, transformed_y, transformed_z, 1.0); - Eigen::Vector2d normalized_projected_point = Eigen::Vector2d( - projected_point.x() / projected_point.z(), projected_point.y() / projected_point.z()); + Eigen::Vector2d projected_point = calcRawImageProjectedPoint( + pinhole_camera_model, cv::Point3d(transformed_x, transformed_y, transformed_z)); for (std::size_t i = 0; i < output_objs.size(); ++i) { auto & feature_obj = output_objs.at(i); const auto & check_roi = feature_obj.feature.roi; @@ -156,10 +153,9 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( continue; } if ( - check_roi.x_offset <= normalized_projected_point.x() && - check_roi.y_offset <= normalized_projected_point.y() && - check_roi.x_offset + check_roi.width >= normalized_projected_point.x() && - check_roi.y_offset + check_roi.height >= normalized_projected_point.y()) { + check_roi.x_offset <= projected_point.x() && check_roi.y_offset <= projected_point.y() && + check_roi.x_offset + check_roi.width >= projected_point.x() && + check_roi.y_offset + check_roi.height >= projected_point.y()) { std::memcpy( &cluster.data[clusters_data_size.at(i)], &input_pointcloud_msg.data[offset], point_step); clusters_data_size.at(i) += point_step; diff --git a/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp b/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp index 0096b91f7a3b6..06cc0fd2c61f2 100644 --- a/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp @@ -65,11 +65,9 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( if (mask.cols == 0 || mask.rows == 0) { return; } - 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), - camera_info.p.at(8), camera_info.p.at(9), camera_info.p.at(10), camera_info.p.at(11), 0.0, 0.0, - 0.0, 1.0; + image_geometry::PinholeCameraModel pinhole_camera_model; + pinhole_camera_model.fromCameraInfo(camera_info); + geometry_msgs::msg::TransformStamped transform_stamped; // transform pointcloud from frame id to camera optical frame id { @@ -113,14 +111,11 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( continue; } - Eigen::Vector4d projected_point = - projection * Eigen::Vector4d(transformed_x, transformed_y, transformed_z, 1.0); - Eigen::Vector2d normalized_projected_point = Eigen::Vector2d( - projected_point.x() / projected_point.z(), projected_point.y() / projected_point.z()); + Eigen::Vector2d projected_point = calcRawImageProjectedPoint( + pinhole_camera_model, cv::Point3d(transformed_x, transformed_y, transformed_z)); - bool is_inside_image = - normalized_projected_point.x() > 0 && normalized_projected_point.x() < camera_info.width && - normalized_projected_point.y() > 0 && normalized_projected_point.y() < camera_info.height; + bool is_inside_image = projected_point.x() > 0 && projected_point.x() < camera_info.width && + projected_point.y() > 0 && projected_point.y() < camera_info.height; if (!is_inside_image) { copyPointCloud( input_pointcloud_msg, point_step, global_offset, output_cloud, output_pointcloud_size); @@ -129,8 +124,7 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( // skip filtering pointcloud where semantic id out of the defined list uint8_t semantic_id = mask.at( - static_cast(normalized_projected_point.y()), - static_cast(normalized_projected_point.x())); + static_cast(projected_point.y()), static_cast(projected_point.x())); if (static_cast(semantic_id) >= filter_semantic_label_target_.size()) { copyPointCloud( input_pointcloud_msg, point_step, global_offset, output_cloud, output_pointcloud_size); diff --git a/perception/image_projection_based_fusion/src/utils/utils.cpp b/perception/image_projection_based_fusion/src/utils/utils.cpp index 2e997f3e8e60f..aeec2886a801a 100644 --- a/perception/image_projection_based_fusion/src/utils/utils.cpp +++ b/perception/image_projection_based_fusion/src/utils/utils.cpp @@ -13,8 +13,18 @@ // limitations under the License. #include "image_projection_based_fusion/utils/utils.hpp" + namespace image_projection_based_fusion { +Eigen::Vector2d calcRawImageProjectedPoint( + const image_geometry::PinholeCameraModel & pinhole_camera_model, const cv::Point3d & point3d) +{ + const cv::Point2d rectified_image_point = pinhole_camera_model.project3dToPixel(point3d); + + const cv::Point2d raw_image_point = pinhole_camera_model.unrectifyPoint(rectified_image_point); + + return Eigen::Vector2d(raw_image_point.x, raw_image_point.y); +} std::optional getTransformStamped( const tf2_ros::Buffer & tf_buffer, const std::string & target_frame_id, From 0cc45ae1783a4dc71e01d90c074086cf3c94e281 Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Thu, 27 Jun 2024 13:47:30 +0900 Subject: [PATCH 6/6] fix(obstacle_stop_planner): fix unreadVariable warning (#7626) Signed-off-by: Ryuta Kambe --- .../src/adaptive_cruise_control.cpp | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp index e9058f234e98f..1ab368b6f2a8d 100644 --- a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp +++ b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp @@ -639,14 +639,11 @@ double AdaptiveCruiseController::calcTargetVelocity_D( return 0.0; } - double add_vel_d = 0; - - add_vel_d = diff_vel; + double add_vel_d = diff_vel; if (add_vel_d >= 0) { - diff_vel *= param_.d_coeff_pos; - } - if (add_vel_d < 0) { - diff_vel *= param_.d_coeff_neg; + add_vel_d *= param_.d_coeff_pos; + } else { + add_vel_d *= param_.d_coeff_neg; } add_vel_d = boost::algorithm::clamp(add_vel_d, -param_.d_max_vel_norm, param_.d_max_vel_norm);