From 75ccf8ee17120090df360c5014102f6e745a61bd Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 8 Aug 2023 10:49:51 +0900 Subject: [PATCH 01/16] feat(dynamic_avoidance): use decision with reason (#4547) * feat(dynamic_avoidance): use decision with reason as return value Signed-off-by: Takayuki Murooka * fix time to collision Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../dynamic_avoidance_module.hpp | 18 ++++- .../dynamic_avoidance_module.cpp | 69 ++++++++++++------- 2 files changed, 60 insertions(+), 27 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp index c93707bf84362..29b012caa231d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -235,6 +235,12 @@ class DynamicAvoidanceModule : public SceneModuleInterface std::unordered_map object_map_; }; + struct DecisionWithReason + { + bool decision; + std::string reason{""}; + }; + DynamicAvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, @@ -299,14 +305,14 @@ class DynamicAvoidanceModule : public SceneModuleInterface bool willObjectCutIn( const std::vector & ego_path, const PredictedPath & predicted_path, const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const; - bool willObjectCutOut( + DecisionWithReason willObjectCutOut( const double obj_tangent_vel, const double obj_normal_vel, const bool is_object_left, const std::optional & prev_object) const; bool isObjectFarFromPath( const PredictedObject & predicted_object, const double obj_dist_to_path) const; double calcTimeToCollision( const std::vector & ego_path, const geometry_msgs::msg::Pose & obj_pose, - const double obj_tangent_vel) const; + const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const; std::optional> calcCollisionSection( const std::vector & ego_path, const PredictedPath & obj_path) const; LatLonOffset getLateralLongitudinalOffset( @@ -326,6 +332,14 @@ class DynamicAvoidanceModule : public SceneModuleInterface std::optional calcDynamicObstaclePolygon( const DynamicAvoidanceObject & object) const; + void printIgnoreReason(const std::string & obj_uuid, const std::string & reason) + { + const auto reason_text = + "[DynamicAvoidance] Ignore obstacle (%s)" + (reason == "" ? "." : " since " + reason + "."); + RCLCPP_INFO_EXPRESSION( + getLogger(), parameters_->enable_debug_info, reason_text.c_str(), obj_uuid.c_str()); + } + std::vector target_objects_; // std::vector prev_target_objects_; std::shared_ptr parameters_; diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index 52f887e60d83e..6233c1a5dd36f 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -459,34 +459,32 @@ void DynamicAvoidanceModule::updateTargetObjects() } // 2.d. check if object will not cut out - const bool will_object_cut_out = + const auto will_object_cut_out = willObjectCutOut(object.vel, object.lat_vel, is_object_left, prev_object); - if (will_object_cut_out) { - RCLCPP_INFO_EXPRESSION( - getLogger(), parameters_->enable_debug_info, - "[DynamicAvoidance] Ignore obstacle (%s) since it will cut out.", obj_uuid.c_str()); + if (will_object_cut_out.decision) { + printIgnoreReason(obj_uuid.c_str(), will_object_cut_out.reason); continue; } - // 2.e. check if time to collision + // 2.e. check time to collision const double time_to_collision = - calcTimeToCollision(prev_module_path->points, object.pose, object.vel); + calcTimeToCollision(prev_module_path->points, object.pose, object.vel, lat_lon_offset); if ( (0 <= object.vel && parameters_->max_time_to_collision_overtaking_object < time_to_collision) || (object.vel <= 0 && parameters_->max_time_to_collision_oncoming_object < time_to_collision)) { RCLCPP_INFO_EXPRESSION( getLogger(), parameters_->enable_debug_info, - "[DynamicAvoidance] Ignore obstacle (%s) since time to collision is large.", - obj_uuid.c_str()); + "[DynamicAvoidance] Ignore obstacle (%s) since time to collision (%f) is large.", + obj_uuid.c_str(), time_to_collision); continue; } if (time_to_collision < -parameters_->duration_to_hold_avoidance_overtaking_object) { RCLCPP_INFO_EXPRESSION( getLogger(), parameters_->enable_debug_info, - "[DynamicAvoidance] Ignore obstacle (%s) since time to collision is a small negative " + "[DynamicAvoidance] Ignore obstacle (%s) since time to collision (%f) is a small negative " "value.", - obj_uuid.c_str()); + obj_uuid.c_str(), time_to_collision); continue; } @@ -574,15 +572,24 @@ DynamicAvoidanceModule::calcCollisionSection( double DynamicAvoidanceModule::calcTimeToCollision( const std::vector & ego_path, const geometry_msgs::msg::Pose & obj_pose, - const double obj_tangent_vel) const + const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const { - const double relative_velocity = getEgoSpeed() - obj_tangent_vel; + // Set maximum time-to-collision 0 if the object longitudinally overlaps ego. + // NOTE: This is to avoid objects running right beside ego even if time-to-collision is negative. const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(ego_path); + const double lon_offset_ego_to_obj = + motion_utils::calcSignedArcLength( + ego_path, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx) + + lat_lon_offset.max_lon_offset; + const double maximum_time_to_collision = + (0.0 < lon_offset_ego_to_obj) ? 0.0 : -std::numeric_limits::max(); + + const double relative_velocity = getEgoSpeed() - obj_tangent_vel; const size_t obj_seg_idx = motion_utils::findNearestSegmentIndex(ego_path, obj_pose.position); const double signed_lon_length = motion_utils::calcSignedArcLength( ego_path, getEgoPosition(), ego_seg_idx, obj_pose.position, obj_seg_idx); const double positive_relative_velocity = std::max(relative_velocity, 1.0); - return signed_lon_length / positive_relative_velocity; + return std::max(maximum_time_to_collision, signed_lon_length / positive_relative_velocity); } bool DynamicAvoidanceModule::isObjectFarFromPath( @@ -630,33 +637,45 @@ bool DynamicAvoidanceModule::willObjectCutIn( return false; } -bool DynamicAvoidanceModule::willObjectCutOut( +DynamicAvoidanceModule::DecisionWithReason DynamicAvoidanceModule::willObjectCutOut( const double obj_tangent_vel, const double obj_normal_vel, const bool is_object_left, const std::optional & prev_object) const { // Ignore oncoming object if (obj_tangent_vel < 0) { - return false; + return DecisionWithReason{false}; } - if (prev_object && prev_object->latest_time_inside_ego_path) { - if ( - parameters_->max_time_from_outside_ego_path_for_cut_out < - (clock_->now() - *prev_object->latest_time_inside_ego_path).seconds()) { - return false; - } + // Check if previous object is memorized + if (!prev_object || !prev_object->latest_time_inside_ego_path) { + return DecisionWithReason{false}; + } + if ( + parameters_->max_time_from_outside_ego_path_for_cut_out < + (clock_->now() - *prev_object->latest_time_inside_ego_path).seconds()) { + return DecisionWithReason{false}; } + // Check object's lateral velocity + std::stringstream reason; + reason << "since latest time inside ego's path is small enough (" + << (clock_->now() - *prev_object->latest_time_inside_ego_path).seconds() << "<" + << parameters_->max_time_from_outside_ego_path_for_cut_out << ")"; if (is_object_left) { if (parameters_->min_cut_out_object_lat_vel < obj_normal_vel) { - return true; + reason << ", and lateral velocity is large enough (" + << parameters_->min_cut_out_object_lat_vel << "min_cut_out_object_lat_vel) { - return true; + reason << ", and lateral velocity is large enough (" + << parameters_->min_cut_out_object_lat_vel << " DynamicAvoidanceModule::getAdjacentLanes( From cdabb78cf597df2f57606a720178122f1276bf3f Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 8 Aug 2023 10:49:59 +0900 Subject: [PATCH 02/16] feat(dynamic_avoidance): ignore oncoming crossing object (#4548) * feat(dynamic_avoidance): ignore oncoming crossing object Signed-off-by: Takayuki Murooka * remove debug print Signed-off-by: Takayuki Murooka * update config Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../config/dynamic_avoidance.param.yaml | 58 +++++++++++++++++++ .../dynamic_avoidance_module.hpp | 6 +- .../dynamic_avoidance_module.cpp | 13 +++-- .../dynamic_avoidance/manager.cpp | 24 ++++++-- 4 files changed, 89 insertions(+), 12 deletions(-) create mode 100644 planning/behavior_path_planner/config/dynamic_avoidance.param.yaml diff --git a/planning/behavior_path_planner/config/dynamic_avoidance.param.yaml b/planning/behavior_path_planner/config/dynamic_avoidance.param.yaml new file mode 100644 index 0000000000000..896c44c9cec9a --- /dev/null +++ b/planning/behavior_path_planner/config/dynamic_avoidance.param.yaml @@ -0,0 +1,58 @@ +/**: + ros__parameters: + dynamic_avoidance: + common: + enable_debug_info: true + + # avoidance is performed for the object type with true + target_object: + car: true + truck: true + bus: true + trailer: true + unknown: false + bicycle: false + motorcycle: true + pedestrian: false + + min_obstacle_vel: 0.0 # [m/s] + + successive_num_to_entry_dynamic_avoidance_condition: 5 + successive_num_to_exit_dynamic_avoidance_condition: 1 + + min_obj_lat_offset_to_ego_path: 0.0 # [m] + max_obj_lat_offset_to_ego_path: 1.0 # [m] + + cut_in_object: + min_time_to_start_cut_in: 1.0 # [s] + min_lon_offset_ego_to_object: 0.0 # [m] + + cut_out_object: + max_time_from_outside_ego_path: 2.0 # [s] + min_object_lat_vel: 0.3 # [m/s] + + crossing_object: + min_overtaking_object_vel: 1.0 + max_overtaking_object_angle: 1.05 + min_oncoming_object_vel: 0.0 + max_oncoming_object_angle: 0.523 + + front_object: + max_object_angle: 0.785 + + drivable_area_generation: + lat_offset_from_obstacle: 0.8 # [m] + max_lat_offset_to_avoid: 0.5 # [m] + + # for same directional object + overtaking_object: + max_time_to_collision: 40.0 # [s] + start_duration_to_avoid: 2.0 # [s] + end_duration_to_avoid: 4.0 # [s] + duration_to_hold_avoidance: 3.0 # [s] + + # for opposite directional object + oncoming_object: + max_time_to_collision: 15.0 # [s] + start_duration_to_avoid: 12.0 # [s] + end_duration_to_avoid: 0.0 # [s] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp index 29b012caa231d..087a39e87393a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -67,8 +67,10 @@ struct DynamicAvoidanceParameters double max_time_from_outside_ego_path_for_cut_out{0.0}; double min_cut_out_object_lat_vel{0.0}; double max_front_object_angle{0.0}; - double min_crossing_object_vel{0.0}; - double max_crossing_object_angle{0.0}; + double min_overtaking_crossing_object_vel{0.0}; + double max_overtaking_crossing_object_angle{0.0}; + double min_oncoming_crossing_object_vel{0.0}; + double max_oncoming_crossing_object_angle{0.0}; // drivable area generation double lat_offset_from_obstacle{0.0}; diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index 6233c1a5dd36f..e8570f0e310cf 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -374,11 +374,16 @@ void DynamicAvoidanceModule::updateTargetObjects() // 1.c. check if object is not crossing ego's path const double obj_angle = calcDiffAngleAgainstPath(prev_module_path->points, obj_pose); - const bool is_obstacle_crossing_path = - parameters_->max_crossing_object_angle < std::abs(obj_angle) && - parameters_->max_crossing_object_angle < M_PI - std::abs(obj_angle); + const double max_crossing_object_angle = 0.0 <= obj_tangent_vel + ? parameters_->max_overtaking_crossing_object_angle + : parameters_->max_oncoming_crossing_object_angle; + const bool is_obstacle_crossing_path = max_crossing_object_angle < std::abs(obj_angle) && + max_crossing_object_angle < M_PI - std::abs(obj_angle); + const double min_crossing_object_vel = 0.0 <= obj_tangent_vel + ? parameters_->min_overtaking_crossing_object_vel + : parameters_->min_oncoming_crossing_object_vel; const bool is_crossing_object_to_ignore = - parameters_->min_crossing_object_vel < std::abs(obj_vel) && is_obstacle_crossing_path; + min_crossing_object_vel < std::abs(obj_vel) && is_obstacle_crossing_path; if (is_crossing_object_to_ignore) { RCLCPP_INFO_EXPRESSION( getLogger(), parameters_->enable_debug_info, diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp index 87525068c8c34..e686719392e14 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp @@ -68,10 +68,14 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager( p.max_front_object_angle = node->declare_parameter(ns + "front_object.max_object_angle"); - p.min_crossing_object_vel = - node->declare_parameter(ns + "crossing_object.min_object_vel"); - p.max_crossing_object_angle = - node->declare_parameter(ns + "crossing_object.max_object_angle"); + p.min_overtaking_crossing_object_vel = + node->declare_parameter(ns + "crossing_object.min_overtaking_object_vel"); + p.max_overtaking_crossing_object_angle = + node->declare_parameter(ns + "crossing_object.max_overtaking_object_angle"); + p.min_oncoming_crossing_object_vel = + node->declare_parameter(ns + "crossing_object.min_oncoming_object_vel"); + p.max_oncoming_crossing_object_angle = + node->declare_parameter(ns + "crossing_object.max_oncoming_object_angle"); } { // drivable_area_generation @@ -152,9 +156,17 @@ void DynamicAvoidanceModuleManager::updateModuleParams( parameters, ns + "front_object.max_object_angle", p->max_front_object_angle); updateParam( - parameters, ns + "crossing_object.min_object_vel", p->min_crossing_object_vel); + parameters, ns + "crossing_object.min_overtaking_object_vel", + p->min_overtaking_crossing_object_vel); updateParam( - parameters, ns + "crossing_object.max_object_angle", p->max_crossing_object_angle); + parameters, ns + "crossing_object.max_overtaking_object_angle", + p->max_overtaking_crossing_object_angle); + updateParam( + parameters, ns + "crossing_object.min_oncoming_object_vel", + p->min_oncoming_crossing_object_vel); + updateParam( + parameters, ns + "crossing_object.max_oncoming_object_angle", + p->max_oncoming_crossing_object_angle); } { // drivable_area_generation From 95695f458e06d6a8ffdaafa806b68115a0ab212d Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Tue, 8 Aug 2023 14:26:43 +0900 Subject: [PATCH 03/16] fix(behavior_path_planner): make use of the avoid_linestring.distance parameter (#4198) Signed-off-by: Maxime CLEMENT --- .../src/utils/drivable_area_expansion/expansion.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp index 79e15bf4e68d7..7bff7bef578ce 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp @@ -137,8 +137,9 @@ multipolygon_t createExpansionPolygons( : footprint_dist; auto expansion_polygon = createExpansionPolygon(base_ls, expansion_dist, is_left); auto limited_dist = expansion_dist; - const auto uncrossable_dist_limit = - calculateDistanceLimit(base_ls, expansion_polygon, uncrossable_lines); + const auto uncrossable_dist_limit = std::max( + 0.0, calculateDistanceLimit(base_ls, expansion_polygon, uncrossable_lines) - + params.avoid_linestring_dist); if (uncrossable_dist_limit < limited_dist) { limited_dist = uncrossable_dist_limit; if (params.compensate_uncrossable_lines) { From 4e68889ec5ce4e8eef93a353e503dd298ebc7678 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 8 Aug 2023 14:38:27 +0900 Subject: [PATCH 04/16] fix(crosswalk): set distance even when decision is GO (#4551) Signed-off-by: Takayuki Murooka --- .../src/scene_crosswalk.cpp | 48 ++++++++++--------- .../src/scene_crosswalk.hpp | 9 +++- 2 files changed, 32 insertions(+), 25 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index fd3b0d76107cb..391b397dd82cd 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -252,6 +252,10 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto // Set safe or unsafe setSafe(!nearest_stop_factor); + // Set distance + // NOTE: If no stop point is inserted, distance to the virtual stop line has to be calculated. + setDistanceToStop(*path, default_stop_pose, nearest_stop_factor); + // plan Go/Stop if (isActivated()) { planGo(*path, nearest_stop_factor); @@ -402,7 +406,7 @@ std::pair CrosswalkModule::getAttentionRange( void CrosswalkModule::insertDecelPointWithDebugInfo( const geometry_msgs::msg::Point & stop_point, const float target_velocity, - PathWithLaneId & output) + PathWithLaneId & output) const { const auto stop_pose = planning_utils::insertDecelPoint(stop_point, output, target_velocity); if (!stop_pose) { @@ -1007,25 +1011,34 @@ geometry_msgs::msg::Polygon CrosswalkModule::createVehiclePolygon( return polygon; } -void CrosswalkModule::planGo( - PathWithLaneId & ego_path, const std::optional & stop_factor) +void CrosswalkModule::setDistanceToStop( + const PathWithLaneId & ego_path, + const std::optional & default_stop_pose, + const std::optional & stop_factor) { - if (!stop_factor) { - setDistance(std::numeric_limits::lowest()); - return; + // calculate stop position + const auto stop_pos = [&]() -> std::optional { + if (stop_factor) return stop_factor->stop_pose.position; + if (default_stop_pose) return default_stop_pose->position; + return std::nullopt; + }(); + + // Set distance + if (stop_pos) { + const auto & ego_pos = planner_data_->current_odometry->pose.position; + const double dist_ego2stop = calcSignedArcLength(ego_path.points, ego_pos, *stop_pos); + setDistance(dist_ego2stop); } +} +void CrosswalkModule::planGo( + PathWithLaneId & ego_path, const std::optional & stop_factor) const +{ // Plan slow down const auto target_velocity = calcTargetVelocity(stop_factor->stop_pose.position, ego_path); insertDecelPointWithDebugInfo( stop_factor->stop_pose.position, std::max(planner_param_.min_slow_down_velocity, target_velocity), ego_path); - - // Set distance - const auto & ego_pos = planner_data_->current_odometry->pose.position; - const double dist_ego2stop = - calcSignedArcLength(ego_path.points, ego_pos, stop_factor->stop_pose.position); - setDistance(dist_ego2stop); } void CrosswalkModule::planStop( @@ -1038,22 +1051,11 @@ void CrosswalkModule::planStop( return std::nullopt; }(); - if (!stop_factor) { - setDistance(std::numeric_limits::lowest()); - return; - } - // Plan stop insertDecelPointWithDebugInfo(stop_factor->stop_pose.position, 0.0, ego_path); planning_utils::appendStopReason(*stop_factor, stop_reason); velocity_factor_.set( ego_path.points, planner_data_->current_odometry->pose, stop_factor->stop_pose, VelocityFactor::UNKNOWN); - - // set distance - const auto & ego_pos = planner_data_->current_odometry->pose.position; - const double dist_ego2stop = - calcSignedArcLength(ego_path.points, ego_pos, stop_factor->stop_pose.position); - setDistance(dist_ego2stop); } } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 7ac00ab880be8..961126bf4bca1 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -295,7 +295,12 @@ class CrosswalkModule : public SceneModuleInterface const std::optional & stop_factor_for_crosswalk_users, const std::optional & stop_factor_for_stuck_vehicles); - void planGo(PathWithLaneId & ego_path, const std::optional & stop_factor); + void setDistanceToStop( + const PathWithLaneId & ego_path, + const std::optional & default_stop_pose, + const std::optional & stop_factor); + + void planGo(PathWithLaneId & ego_path, const std::optional & stop_factor) const; void planStop( PathWithLaneId & ego_path, const std::optional & nearest_stop_factor, @@ -308,7 +313,7 @@ class CrosswalkModule : public SceneModuleInterface void insertDecelPointWithDebugInfo( const geometry_msgs::msg::Point & stop_point, const float target_velocity, - PathWithLaneId & output); + PathWithLaneId & output) const; std::pair clampAttentionRangeByNeighborCrosswalks( const PathWithLaneId & ego_path, const double near_attention_range, From 2e5752f8e73aecd6938ebac03727ac5dc1b5563c Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Tue, 8 Aug 2023 18:15:46 +0900 Subject: [PATCH 05/16] feat(behavior_path_planner): add reroute availability publisher (#4543) * feat(behavior_path_planner): add reroute availability publisher Signed-off-by: yutaka * update Signed-off-by: yutaka * add comment Signed-off-by: yutaka --------- Signed-off-by: yutaka --- .../behavior_path_planner_node.hpp | 8 ++++++ .../behavior_path_planner/planner_manager.hpp | 10 ++++++++ .../src/behavior_path_planner_node.cpp | 25 +++++++++++++++++++ 3 files changed, 43 insertions(+) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index e6ed8b7063d2c..54cfccc494e8a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -42,6 +42,7 @@ #include #include #include +#include #include #include #include @@ -72,6 +73,7 @@ using steering_factor_interface::SteeringFactorInterface; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; using tier4_planning_msgs::msg::LateralOffset; +using tier4_planning_msgs::msg::RerouteAvailability; using tier4_planning_msgs::msg::Scenario; using tier4_planning_msgs::msg::StopReasonArray; using visualization_msgs::msg::Marker; @@ -102,6 +104,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr bound_publisher_; rclcpp::Publisher::SharedPtr modified_goal_publisher_; rclcpp::Publisher::SharedPtr stop_reason_publisher_; + rclcpp::Publisher::SharedPtr reroute_availability_publisher_; rclcpp::TimerBase::SharedPtr timer_; std::map::SharedPtr> path_candidate_publishers_; @@ -171,6 +174,11 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_lane_change_msg_array_publisher_; rclcpp::Publisher::SharedPtr debug_turn_signal_info_publisher_; + /** + * @brief publish reroute availability + */ + void publish_reroute_availability(); + /** * @brief publish steering factor from intersection */ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index 02e5452cc2103..f1292c2db7811 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -214,6 +214,16 @@ class PlannerManager return stop_reason_array; } + /** + * @brief check if there are approved modules. + */ + bool hasApprovedModules() const { return !approved_module_ptrs_.empty(); } + + /** + * @brief check if there are candidate modules. + */ + bool hasCandidateModules() const { return !candidate_module_ptrs_.empty(); } + /** * @brief reset root lanelet. if there are approved modules, don't reset root lanelet. * @param planner data. diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 869de6cb4d103..3e2ccd23d58e7 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -68,6 +68,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod hazard_signal_publisher_ = create_publisher("~/output/hazard_lights_cmd", 1); modified_goal_publisher_ = create_publisher("~/output/modified_goal", 1); stop_reason_publisher_ = create_publisher("~/output/stop_reasons", 1); + reroute_availability_publisher_ = + create_publisher("~/output/is_reroute_available", 1); debug_avoidance_msg_array_publisher_ = create_publisher("~/debug/avoidance_debug_message_array", 1); debug_lane_change_msg_array_publisher_ = @@ -539,6 +541,9 @@ void BehaviorPathPlannerNode::run() // compute turn signal computeTurnSignal(planner_data_, *path, output); + // publish reroute availability + publish_reroute_availability(); + // publish drivable bounds publish_bounds(*path); @@ -654,6 +659,26 @@ void BehaviorPathPlannerNode::publish_steering_factor( steering_factor_interface_ptr_->publishSteeringFactor(get_clock()->now()); } +void BehaviorPathPlannerNode::publish_reroute_availability() +{ + const bool has_approved_modules = planner_manager_->hasApprovedModules(); + const bool has_candidate_modules = planner_manager_->hasCandidateModules(); + + // In the current behavior path planner, we might get unexpected behavior when rerouting while + // modules other than lane follow are active. Therefore, rerouting will be allowed only when the + // lane follow module is running Note that if there is a approved module or candidate module, it + // means non-lane-following modules are runnning. + RerouteAvailability is_reroute_available; + is_reroute_available.stamp = this->now(); + if (has_approved_modules || has_candidate_modules) { + is_reroute_available.availability = false; + } else { + is_reroute_available.availability = true; + } + + reroute_availability_publisher_->publish(is_reroute_available); +} + void BehaviorPathPlannerNode::publish_turn_signal_debug_data(const TurnSignalDebugData & debug_data) { MarkerArray marker_array; From 1f91c838ea002c2c1ce1794746d984462893d1d6 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 8 Aug 2023 22:08:01 +0900 Subject: [PATCH 06/16] feat(avoidance): enable avoidance cancel (#4398) Signed-off-by: satoshi-ota --- .../config/avoidance/avoidance.param.yaml | 2 +- .../behavior_path_planner_avoidance_design.md | 22 +++--- .../avoidance/avoidance_module.hpp | 13 ---- .../utils/avoidance/avoidance_module_data.hpp | 4 +- .../avoidance/avoidance_module.cpp | 77 ++++++++----------- .../src/scene_module/avoidance/manager.cpp | 3 +- .../src/scene_module/lane_change/manager.cpp | 2 - 7 files changed, 48 insertions(+), 75 deletions(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index db2436f796dfe..22de79f4dc7d0 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -11,10 +11,10 @@ # avoidance module common setting enable_bound_clipping: false - enable_update_path_when_object_is_gone: false enable_force_avoidance_for_stopped_vehicle: false enable_yield_maneuver: true enable_yield_maneuver_during_shifting: false + enable_cancel_maneuver: false disable_path_update: false # drivable area setting diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md index 9be99f705f560..4e4d2258fc276 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md @@ -601,13 +601,13 @@ $$ ### Avoidance cancelling maneuver -If `enable_update_path_when_object_is_gone` parameter is true, Avoidance Module takes different actions according to the situations as follows: +If `enable_cancel_maneuver` parameter is true, Avoidance Module takes different actions according to the situations as follows: - If vehicle stops: If there is any object in the path of the vehicle, the avoidance path is generated. If this object goes away while the vehicle is stopping, the avoidance path will cancelled. - If vehicle is in motion, but avoidance maneuver doesn't started: If there is any object in the path of the vehicle, the avoidance path is generated. If this object goes away while the vehicle is not started avoidance maneuver, the avoidance path will cancelled. - If vehicle is in motion, avoidance maneuver started: If there is any object in the path of the vehicle, the avoidance path is generated,but if this object goes away while the vehicle is started avoidance maneuver, the avoidance path will not cancelled. -If `enable_update_path_when_object_is_gone` parameter is false, Avoidance Module doesn't revert generated avoidance path even if path objects are gone. +If `enable_cancel_maneuver` parameter is false, Avoidance Module doesn't revert generated avoidance path even if path objects are gone. ## How to keep the consistency of the optimize-base path generation logic @@ -621,15 +621,15 @@ The avoidance specific parameter configuration file can be located at `src/autow namespace: `avoidance.` -| Name | Unit | Type | Description | Default value | -| :------------------------------------- | :--- | :----- | :---------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| resample_interval_for_planning | [m] | double | Path resample interval for avoidance planning path. | 0.3 | -| resample_interval_for_output | [m] | double | Path resample interval for output path. Too short interval increases computational cost for latter modules. | 4.0 | -| detection_area_right_expand_dist | [m] | double | Lanelet expand length for right side to find avoidance target vehicles. | 0.0 | -| detection_area_left_expand_dist | [m] | double | Lanelet expand length for left side to find avoidance target vehicles. | 1.0 | -| enable_update_path_when_object_is_gone | [-] | bool | Reset trajectory when avoided objects are gone. If false, shifted path points remain same even though the avoided objects are gone. | false | -| enable_yield_maneuver | [-] | bool | Flag to enable yield maneuver. | false | -| enable_yield_maneuver_during_shifting | [-] | bool | Flag to enable yield maneuver during shifting. | false | +| Name | Unit | Type | Description | Default value | +| :------------------------------------ | :--- | :----- | :---------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| resample_interval_for_planning | [m] | double | Path resample interval for avoidance planning path. | 0.3 | +| resample_interval_for_output | [m] | double | Path resample interval for output path. Too short interval increases computational cost for latter modules. | 4.0 | +| detection_area_right_expand_dist | [m] | double | Lanelet expand length for right side to find avoidance target vehicles. | 0.0 | +| detection_area_left_expand_dist | [m] | double | Lanelet expand length for left side to find avoidance target vehicles. | 1.0 | +| enable_cancel_maneuver | [-] | bool | Reset trajectory when avoided objects are gone. If false, shifted path points remain same even though the avoided objects are gone. | false | +| enable_yield_maneuver | [-] | bool | Flag to enable yield maneuver. | false | +| enable_yield_maneuver_during_shifting | [-] | bool | Flag to enable yield maneuver during shifting. | false | | Name | Unit | Type | Description | Default value | | :------------------------ | ---- | ---- | ----------------------------------------------------------------------------------------------------------------------- | ------------- | diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index b6cc840a560f0..3095ec2c03251 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -195,19 +195,6 @@ class AvoidanceModule : public SceneModuleInterface */ AvoidanceState updateEgoState(const AvoidancePlanningData & data) const; - /** - * @brief check whether the ego is shifted based on shift line. - * @return result. - */ - bool isAvoidanceManeuverRunning(); - - /** - * @brief check whether the ego is in avoidance maneuver based on shift line and target object - * existence. - * @return result. - */ - bool isAvoidancePlanRunning() const; - // ego behavior update /** diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 2b1a9cf1b82d4..0263eee5fe727 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -92,8 +92,8 @@ struct AvoidanceParameters // to use this, enable_avoidance_over_same_direction need to be set to true. bool use_opposite_lane{true}; - // enable update path when if detected objects on planner data is gone. - bool enable_update_path_when_object_is_gone{false}; + // if this param is true, it reverts avoidance path when the path is no longer needed. + bool enable_cancel_maneuver{false}; // enable avoidance for all parking vehicle bool enable_force_avoidance_for_stopped_vehicle{false}; diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 601badee6a9f9..db80bc72f21ad 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -135,64 +135,53 @@ bool AvoidanceModule::isExecutionReady() const bool AvoidanceModule::canTransitSuccessState() { const auto & data = avoidance_data_; - const auto is_plan_running = isAvoidancePlanRunning(); - const bool has_avoidance_target = !data.target_objects.empty(); + // Change input lane. -> EXIT. if (!isDrivingSameLane(helper_.getPreviousDrivingLanes(), data.current_lanelets)) { - RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 500, "previous module lane is updated."); + RCLCPP_WARN(getLogger(), "Previous module lane is updated. Exit."); return true; } - const auto idx = planner_data_->findEgoIndex(data.reference_path.points); - if (idx == data.reference_path.points.size() - 1) { - arrived_path_end_ = true; - } - - constexpr double THRESHOLD = 1.0; - if ( - calcDistance2d(getEgoPose(), getPose(data.reference_path.points.back())) > THRESHOLD && - arrived_path_end_) { - RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 500, "reach path end point. exit avoidance module."); - return true; - } + helper_.setPreviousDrivingLanes(data.current_lanelets); - DEBUG_PRINT( - "is_plan_running = %d, has_avoidance_target = %d", is_plan_running, has_avoidance_target); + // Reach input path end point. -> EXIT. + { + const auto idx = planner_data_->findEgoIndex(data.reference_path.points); + if (idx == data.reference_path.points.size() - 1) { + arrived_path_end_ = true; + } - if (!is_plan_running && !has_avoidance_target) { - return true; + constexpr double THRESHOLD = 1.0; + const auto is_further_than_threshold = + calcDistance2d(getEgoPose(), getPose(data.reference_path.points.back())) > THRESHOLD; + if (is_further_than_threshold && arrived_path_end_) { + RCLCPP_WARN(getLogger(), "Reach path end point. Exit."); + return true; + } } - if ( - !has_avoidance_target && parameters_->enable_update_path_when_object_is_gone && - !isAvoidanceManeuverRunning()) { - // if dynamic objects are removed on path, change current state to reset path - return true; + const bool has_avoidance_target = !data.target_objects.empty(); + const bool has_shift_point = !path_shifter_.getShiftLines().empty(); + const bool has_base_offset = + std::abs(path_shifter_.getBaseOffset()) > parameters_->lateral_avoid_check_threshold; + + // Nothing to do. -> EXIT. + if (!has_avoidance_target) { + if (!has_shift_point && !has_base_offset) { + RCLCPP_INFO(getLogger(), "No objects. No approved shift lines. Exit."); + return true; + } } - helper_.setPreviousDrivingLanes(data.current_lanelets); - - return false; -} - -bool AvoidanceModule::isAvoidancePlanRunning() const -{ - constexpr double AVOIDING_SHIFT_THR = 0.1; - const bool has_base_offset = std::abs(path_shifter_.getBaseOffset()) > AVOIDING_SHIFT_THR; - const bool has_shift_point = (path_shifter_.getShiftLinesSize() > 0); - return has_base_offset || has_shift_point; -} -bool AvoidanceModule::isAvoidanceManeuverRunning() -{ - const auto path_idx = avoidance_data_.ego_closest_path_index; - - for (const auto & al : registered_raw_shift_lines_) { - if (path_idx > al.start_idx || is_avoidance_maneuver_starts) { - is_avoidance_maneuver_starts = true; + // Be able to canceling avoidance path. -> EXIT. + if (!has_avoidance_target) { + if (!helper_.isShifted() && parameters_->enable_cancel_maneuver) { + RCLCPP_INFO(getLogger(), "No objects. Cancel avoidance path. Exit."); return true; } } - return false; + + return false; // Keep current state. } AvoidancePlanningData AvoidanceModule::calcAvoidancePlanningData(DebugData & debug) const diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 13b3344dc7c39..a0ae98276c05e 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -55,8 +55,7 @@ AvoidanceModuleManager::AvoidanceModuleManager( p.detection_area_left_expand_dist = get_parameter(node, ns + "detection_area_left_expand_dist"); p.enable_bound_clipping = get_parameter(node, ns + "enable_bound_clipping"); - p.enable_update_path_when_object_is_gone = - get_parameter(node, ns + "enable_update_path_when_object_is_gone"); + p.enable_cancel_maneuver = get_parameter(node, ns + "enable_cancel_maneuver"); p.enable_force_avoidance_for_stopped_vehicle = get_parameter(node, ns + "enable_force_avoidance_for_stopped_vehicle"); p.enable_yield_maneuver = get_parameter(node, ns + "enable_yield_maneuver"); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index 7c12579bf34f5..0f770efb542c7 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -197,8 +197,6 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( get_parameter(node, ns + "detection_area_right_expand_dist"); p.detection_area_left_expand_dist = get_parameter(node, ns + "detection_area_left_expand_dist"); - p.enable_update_path_when_object_is_gone = - get_parameter(node, ns + "enable_update_path_when_object_is_gone"); p.enable_force_avoidance_for_stopped_vehicle = get_parameter(node, ns + "enable_force_avoidance_for_stopped_vehicle"); } From e10db1371e32cfd2ba1f12a289e2af40679e47c3 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Tue, 8 Aug 2023 23:12:26 +0900 Subject: [PATCH 07/16] fix(lane_change): add distance check for the lane change acceleration sampling (#4556) * fix(lane_change): add distance checker in acceleration sampling Signed-off-by: yutaka * fix format Signed-off-by: yutaka * update Signed-off-by: yutaka --------- Signed-off-by: yutaka --- .../src/scene_module/lane_change/normal.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 3d2bb44ad8bc0..2f7c39f94604e 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -540,6 +540,11 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( current_velocity, common_parameters, route_handler.getLateralIntervalsToPreferredLane(current_lanes.back()), max_acc); + if (max_lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { + return utils::lane_change::getAccelerationValues( + min_acc, max_acc, longitudinal_acc_sampling_num); + } + // if maximum lane change length is less than length to goal or the end of target lanes, only // sample max acc if (route_handler.isInGoalRouteSection(target_lanes.back())) { From 3c37d0a0b93e5607584532e7ccab790c4543180e Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 9 Aug 2023 00:44:19 +0900 Subject: [PATCH 08/16] feat(goal_planner): visualize planner type even when goal is not found (#4560) Signed-off-by: kosuke55 --- .../src/scene_module/goal_planner/goal_planner_module.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 7f8272e748233..b428b2fa379e4 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -1467,7 +1467,8 @@ void GoalPlannerModule::setDebugData() auto marker = createDefaultMarker( header.frame_id, header.stamp, "planner_type", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); - marker.pose = modified_goal_pose_->goal_pose; + marker.pose = modified_goal_pose_ ? modified_goal_pose_->goal_pose + : planner_data_->self_odometry->pose.pose; marker.text = magic_enum::enum_name(status_.pull_over_path->type); marker.text += " " + std::to_string(status_.current_path_idx) + "/" + std::to_string(status_.pull_over_path->partial_paths.size() - 1); From 0b44a264c19d74eaa269e918d8e95bbc0705e616 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 9 Aug 2023 00:45:39 +0900 Subject: [PATCH 09/16] fix(start_planner): add missing change of "do not request when modifed route is received #4528" (#4558) --- .../src/scene_module/start_planner/start_planner_module.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 7aa0fd2d690fc..32398c7944435 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -79,7 +79,7 @@ void StartPlannerModule::processOnExit() bool StartPlannerModule::isExecutionRequested() const { // Execute when current pose is near route start pose - const Pose & start_pose = planner_data_->route_handler->getStartPose(); + const Pose & start_pose = planner_data_->route_handler->getOriginalStartPose(); const Pose & current_pose = planner_data_->self_odometry->pose.pose; if ( tier4_autoware_utils::calcDistance2d(start_pose.position, current_pose.position) > From de7d6241403d1601f9d4c441189377e9902158a0 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 9 Aug 2023 00:45:57 +0900 Subject: [PATCH 10/16] feat(goal_planner): add life time to debug markers (#4562) Signed-off-by: kosuke55 --- .../src/scene_module/goal_planner/goal_planner_module.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index b428b2fa379e4..d767fc91409f1 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -1425,7 +1425,10 @@ void GoalPlannerModule::setDebugData() const auto header = planner_data_->route_handler->getRouteHeader(); - const auto add = [this](const MarkerArray & added) { + const auto add = [this](MarkerArray added) { + for (auto & marker : added.markers) { + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + } tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); }; From f695cb27da14980b9ad8097ca7a290b7701a402e Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 9 Aug 2023 00:52:04 +0900 Subject: [PATCH 11/16] fix(start_planner): plan freespace pull over even if any path has never been found (#4561) Signed-off-by: kosuke55 --- .../goal_planner/goal_planner_module.cpp | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index d767fc91409f1..b8219c5057f1d 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -1076,11 +1076,22 @@ bool GoalPlannerModule::isStopped() bool GoalPlannerModule::isStuck() { + constexpr double stuck_time = 5.0; + if (!isStopped(odometry_buffer_stuck_, stuck_time)) { + return false; + } + + // not found safe path + if (!status_.is_safe) { + return true; + } + + // any path has never been found if (!status_.pull_over_path) { return false; } - constexpr double stuck_time = 5.0; - return isStopped(odometry_buffer_stuck_, stuck_time) && checkCollision(getCurrentPath()); + + return checkCollision(getCurrentPath()); } bool GoalPlannerModule::hasFinishedCurrentPath() From 81d2c0c886eace2b114bd9905fe12640d31e8e37 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Wed, 9 Aug 2023 00:59:29 +0900 Subject: [PATCH 12/16] feat(mission_planner): add a guard for rerouting while not following lane (#4564) feat(mission_planner): add a guard for rerouting while not follwoing lane Signed-off-by: yutaka --- planning/mission_planner/package.xml | 1 + .../src/mission_planner/mission_planner.cpp | 27 +++++++++++++++++++ .../src/mission_planner/mission_planner.hpp | 5 ++++ 3 files changed, 33 insertions(+) diff --git a/planning/mission_planner/package.xml b/planning/mission_planner/package.xml index f5f7eefd49d7d..49e1d0de8be93 100644 --- a/planning/mission_planner/package.xml +++ b/planning/mission_planner/package.xml @@ -33,6 +33,7 @@ tf2_geometry_msgs tf2_ros tier4_autoware_utils + tier4_planning_msgs vehicle_info_util ament_lint_auto diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index 0c9b27a685583..1d37b820efff1 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -75,6 +75,7 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) plugin_loader_("mission_planner", "mission_planner::PlannerPlugin"), tf_buffer_(get_clock()), tf_listener_(tf_buffer_), + reroute_availability_(nullptr), normal_route_(nullptr), mrm_route_(nullptr) { @@ -89,6 +90,11 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) sub_odometry_ = create_subscription( "/localization/kinematic_state", rclcpp::QoS(1), std::bind(&MissionPlanner::on_odometry, this, std::placeholders::_1)); + sub_reroute_availability_ = create_subscription( + "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/output/" + "is_reroute_available", + rclcpp::QoS(1), + std::bind(&MissionPlanner::on_reroute_availability, this, std::placeholders::_1)); auto qos_transient_local = rclcpp::QoS{1}.transient_local(); vector_map_subscriber_ = create_subscription( @@ -130,6 +136,11 @@ void MissionPlanner::on_odometry(const Odometry::ConstSharedPtr msg) } } +void MissionPlanner::on_reroute_availability(const RerouteAvailability::ConstSharedPtr msg) +{ + reroute_availability_ = msg; +} + void MissionPlanner::onMap(const HADMapBin::ConstSharedPtr msg) { map_ptr_ = msg; @@ -375,6 +386,10 @@ void MissionPlanner::on_set_mrm_route( throw component_interface_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); } + if (reroute_availability_ && !reroute_availability_->availability) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following."); + } const auto prev_state = state_.state; change_state(RouteState::Message::CHANGING); @@ -444,6 +459,10 @@ void MissionPlanner::on_clear_mrm_route( if (!mrm_route_) { throw component_interface_utils::NoEffectWarning("MRM route is not set"); } + if (reroute_availability_ && !reroute_availability_->availability) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following."); + } change_state(RouteState::Message::CHANGING); @@ -574,6 +593,10 @@ void MissionPlanner::on_change_route( throw component_interface_utils::ServiceException( ResponseCode::ERROR_INVALID_STATE, "Cannot reroute in the emergency state."); } + if (reroute_availability_ && !reroute_availability_->availability) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following."); + } // set to changing state change_state(RouteState::Message::CHANGING); @@ -633,6 +656,10 @@ void MissionPlanner::on_change_route_points( throw component_interface_utils::ServiceException( ResponseCode::ERROR_INVALID_STATE, "Cannot reroute in the emergency state."); } + if (reroute_availability_ && !reroute_availability_->availability) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following."); + } change_state(RouteState::Message::CHANGING); diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/src/mission_planner/mission_planner.hpp index 4d09f72f81899..7b0427f6d01b1 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.hpp @@ -27,6 +27,7 @@ #include #include +#include #include #include @@ -55,6 +56,7 @@ using NormalRoute = planning_interface::NormalRoute; using MrmRoute = planning_interface::MrmRoute; using RouteState = planning_interface::RouteState; using Odometry = nav_msgs::msg::Odometry; +using RerouteAvailability = tier4_planning_msgs::msg::RerouteAvailability; class MissionPlanner : public rclcpp::Node { @@ -72,8 +74,11 @@ class MissionPlanner : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_odometry_; rclcpp::Subscription::SharedPtr vector_map_subscriber_; + rclcpp::Subscription::SharedPtr sub_reroute_availability_; Odometry::ConstSharedPtr odometry_; + RerouteAvailability::ConstSharedPtr reroute_availability_; void on_odometry(const Odometry::ConstSharedPtr msg); + void on_reroute_availability(const RerouteAvailability::ConstSharedPtr msg); rclcpp::Publisher::SharedPtr pub_marker_; void clear_route(); From 1c529ae5e0a83f83a4f7952b1ad1d01f341a5750 Mon Sep 17 00:00:00 2001 From: Yusuke Muramatsu Date: Wed, 9 Aug 2023 01:33:25 +0900 Subject: [PATCH 13/16] fix(image_projection_based_fusion): fix out-of-scope error (#4057) * tmp Signed-off-by: yukke42 style(pre-commit): autofix update Signed-off-by: yukke42 style(pre-commit): autofix * fix: fix association bug Signed-off-by: yukke42 --------- Signed-off-by: yukke42 Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> --- .../roi_detected_object_fusion/node.hpp | 10 +- .../src/roi_detected_object_fusion/node.cpp | 101 +++++++++++++----- 2 files changed, 79 insertions(+), 32 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 8168ad2c9f265..fe95d21eb266c 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 @@ -41,12 +41,11 @@ class RoiDetectedObjectFusionNode : public FusionNode generateDetectedObjectRoIs( - const std::vector & input_objects, const double image_width, - const double image_height, const Eigen::Affine3d & object2camera_affine, - const Eigen::Matrix4d & camera_projection); + const DetectedObjects & input_object_msg, const double image_width, const double image_height, + const Eigen::Affine3d & object2camera_affine, const Eigen::Matrix4d & camera_projection); void fuseObjectsOnImage( - const std::vector & objects, + const DetectedObjects & input_object_msg, const std::vector & image_rois, const std::map & object_roi_map); @@ -63,7 +62,8 @@ class RoiDetectedObjectFusionNode : public FusionNode passthrough_object_flags_, fused_object_flags_, ignored_object_flags_; + std::map> passthrough_object_flags_map_, fused_object_flags_map_, + ignored_object_flags_map_; }; } // namespace image_projection_based_fusion 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 37637f99f69c9..8961c0aad478f 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 @@ -34,19 +34,23 @@ RoiDetectedObjectFusionNode::RoiDetectedObjectFusionNode(const rclcpp::NodeOptio void RoiDetectedObjectFusionNode::preprocess(DetectedObjects & output_msg) { - passthrough_object_flags_.clear(); - passthrough_object_flags_.resize(output_msg.objects.size()); - fused_object_flags_.clear(); - fused_object_flags_.resize(output_msg.objects.size()); - ignored_object_flags_.clear(); - ignored_object_flags_.resize(output_msg.objects.size()); + std::vector passthrough_object_flags, fused_object_flags, ignored_object_flags; + passthrough_object_flags.resize(output_msg.objects.size()); + fused_object_flags.resize(output_msg.objects.size()); + ignored_object_flags.resize(output_msg.objects.size()); for (std::size_t obj_i = 0; obj_i < output_msg.objects.size(); ++obj_i) { if ( output_msg.objects.at(obj_i).existence_probability > fusion_params_.passthrough_lower_bound_probability_threshold) { - passthrough_object_flags_.at(obj_i) = true; + passthrough_object_flags.at(obj_i) = true; } } + + int64_t timestamp_nsec = + output_msg.header.stamp.sec * (int64_t)1e9 + output_msg.header.stamp.nanosec; + passthrough_object_flags_map_.insert(std::make_pair(timestamp_nsec, passthrough_object_flags)); + fused_object_flags_map_.insert(std::make_pair(timestamp_nsec, fused_object_flags)); + ignored_object_flags_map_.insert(std::make_pair(timestamp_nsec, ignored_object_flags)); } void RoiDetectedObjectFusionNode::fuseOnSingleImage( @@ -58,8 +62,8 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( Eigen::Affine3d object2camera_affine; { const auto transform_stamped_optional = getTransformStamped( - tf_buffer_, /*target*/ camera_info.header.frame_id, - /*source*/ input_object_msg.header.frame_id, input_object_msg.header.stamp); + tf_buffer_, /*target*/ input_roi_msg.header.frame_id, + /*source*/ input_object_msg.header.frame_id, input_roi_msg.header.stamp); if (!transform_stamped_optional) { return; } @@ -73,9 +77,9 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( camera_info.p.at(11); const auto object_roi_map = generateDetectedObjectRoIs( - input_object_msg.objects, static_cast(camera_info.width), + input_object_msg, static_cast(camera_info.width), static_cast(camera_info.height), object2camera_affine, camera_projection); - fuseObjectsOnImage(input_object_msg.objects, input_roi_msg.feature_objects, object_roi_map); + fuseObjectsOnImage(input_object_msg, input_roi_msg.feature_objects, object_roi_map); if (debugger_) { debugger_->image_rois_.reserve(input_roi_msg.feature_objects.size()); @@ -87,17 +91,25 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( } std::map RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( - const std::vector & input_objects, const double image_width, - const double image_height, const Eigen::Affine3d & object2camera_affine, - const Eigen::Matrix4d & camera_projection) + const DetectedObjects & input_object_msg, const double image_width, const double image_height, + const Eigen::Affine3d & object2camera_affine, const Eigen::Matrix4d & camera_projection) { std::map object_roi_map; - for (std::size_t obj_i = 0; obj_i < input_objects.size(); ++obj_i) { + int64_t timestamp_nsec = + input_object_msg.header.stamp.sec * (int64_t)1e9 + input_object_msg.header.stamp.nanosec; + if (passthrough_object_flags_map_.size() == 0) { + return object_roi_map; + } + if (passthrough_object_flags_map_.count(timestamp_nsec) == 0) { + return object_roi_map; + } + const auto & passthrough_object_flags = passthrough_object_flags_map_.at(timestamp_nsec); + for (std::size_t obj_i = 0; obj_i < input_object_msg.objects.size(); ++obj_i) { std::vector vertices_camera_coord; { - const auto & object = input_objects.at(obj_i); + const auto & object = input_object_msg.objects.at(obj_i); - if (passthrough_object_flags_.at(obj_i)) { + if (passthrough_object_flags.at(obj_i)) { continue; } @@ -142,7 +154,7 @@ std::map RoiDetectedObjectFusionNode::generateDet } } } - if (point_on_image_cnt == 0) { + if (point_on_image_cnt == 3) { continue; } @@ -168,13 +180,26 @@ std::map RoiDetectedObjectFusionNode::generateDet } void RoiDetectedObjectFusionNode::fuseObjectsOnImage( - const std::vector & objects __attribute__((unused)), + const DetectedObjects & input_object_msg, const std::vector & image_rois, const std::map & object_roi_map) { + int64_t timestamp_nsec = + input_object_msg.header.stamp.sec * (int64_t)1e9 + input_object_msg.header.stamp.nanosec; + if (fused_object_flags_map_.size() == 0 || ignored_object_flags_map_.size() == 0) { + return; + } + if ( + fused_object_flags_map_.count(timestamp_nsec) == 0 || + ignored_object_flags_map_.count(timestamp_nsec) == 0) { + return; + } + auto & fused_object_flags = fused_object_flags_map_.at(timestamp_nsec); + auto & ignored_object_flags = ignored_object_flags_map_.at(timestamp_nsec); + for (const auto & object_pair : object_roi_map) { const auto & obj_i = object_pair.first; - if (fused_object_flags_.at(obj_i)) { + if (fused_object_flags.at(obj_i)) { continue; } @@ -192,15 +217,15 @@ void RoiDetectedObjectFusionNode::fuseObjectsOnImage( if (max_iou > fusion_params_.min_iou_threshold) { if (fusion_params_.use_roi_probability) { if (roi_prob > fusion_params_.roi_probability_threshold) { - fused_object_flags_.at(obj_i) = true; + fused_object_flags.at(obj_i) = true; } else { - ignored_object_flags_.at(obj_i) = true; + ignored_object_flags.at(obj_i) = true; } } else { - fused_object_flags_.at(obj_i) = true; + fused_object_flags.at(obj_i) = true; } } else { - ignored_object_flags_.at(obj_i) = true; + ignored_object_flags.at(obj_i) = true; } } } @@ -234,19 +259,37 @@ void RoiDetectedObjectFusionNode::publish(const DetectedObjects & output_msg) return; } + int64_t timestamp_nsec = + output_msg.header.stamp.sec * (int64_t)1e9 + output_msg.header.stamp.nanosec; + if ( + passthrough_object_flags_map_.size() == 0 || fused_object_flags_map_.size() == 0 || + ignored_object_flags_map_.size() == 0) { + return; + } + if ( + passthrough_object_flags_map_.count(timestamp_nsec) == 0 || + fused_object_flags_map_.count(timestamp_nsec) == 0 || + ignored_object_flags_map_.count(timestamp_nsec) == 0) { + return; + } + auto & passthrough_object_flags = passthrough_object_flags_map_.at(timestamp_nsec); + auto & fused_object_flags = fused_object_flags_map_.at(timestamp_nsec); + auto & ignored_object_flags = ignored_object_flags_map_.at(timestamp_nsec); + DetectedObjects output_objects_msg, debug_fused_objects_msg, debug_ignored_objects_msg; output_objects_msg.header = output_msg.header; debug_fused_objects_msg.header = output_msg.header; debug_ignored_objects_msg.header = output_msg.header; for (std::size_t obj_i = 0; obj_i < output_msg.objects.size(); ++obj_i) { const auto & obj = output_msg.objects.at(obj_i); - if (passthrough_object_flags_.at(obj_i)) { + if (passthrough_object_flags.at(obj_i)) { output_objects_msg.objects.emplace_back(obj); } - if (fused_object_flags_.at(obj_i)) { + if (fused_object_flags.at(obj_i)) { output_objects_msg.objects.emplace_back(obj); debug_fused_objects_msg.objects.emplace_back(obj); - } else if (ignored_object_flags_.at(obj_i)) { + } + if (ignored_object_flags.at(obj_i)) { debug_ignored_objects_msg.objects.emplace_back(obj); } } @@ -255,6 +298,10 @@ void RoiDetectedObjectFusionNode::publish(const DetectedObjects & output_msg) debug_publisher_->publish("debug/fused_objects", debug_fused_objects_msg); debug_publisher_->publish("debug/ignored_objects", debug_ignored_objects_msg); + + passthrough_object_flags_map_.erase(timestamp_nsec); + fused_object_flags_map_.erase(timestamp_nsec); + fused_object_flags_map_.erase(timestamp_nsec); } } // namespace image_projection_based_fusion From 7502305eacad196ce7747b0c1d63cfa2715b37be Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Wed, 9 Aug 2023 01:56:04 +0900 Subject: [PATCH 14/16] refactor(mission_planner): use snake case instead of camel case (#4565) * feat(mission_planner): add a guard for rerouting while not follwoing lane Signed-off-by: yutaka * refactor(mission_planner): use snake case instead of camel case Signed-off-by: yutaka --------- Signed-off-by: yutaka --- .../src/mission_planner/mission_planner.cpp | 9 +++++---- .../src/mission_planner/mission_planner.hpp | 7 +++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index 1d37b820efff1..08da1260b000c 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -75,6 +75,8 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) plugin_loader_("mission_planner", "mission_planner::PlannerPlugin"), tf_buffer_(get_clock()), tf_listener_(tf_buffer_), + odometry_(nullptr), + map_ptr_(nullptr), reroute_availability_(nullptr), normal_route_(nullptr), mrm_route_(nullptr) @@ -86,7 +88,6 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) planner_ = plugin_loader_.createSharedInstance("mission_planner::lanelet2::DefaultPlanner"); planner_->initialize(this); - odometry_ = nullptr; sub_odometry_ = create_subscription( "/localization/kinematic_state", rclcpp::QoS(1), std::bind(&MissionPlanner::on_odometry, this, std::placeholders::_1)); @@ -97,9 +98,9 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) std::bind(&MissionPlanner::on_reroute_availability, this, std::placeholders::_1)); auto qos_transient_local = rclcpp::QoS{1}.transient_local(); - vector_map_subscriber_ = create_subscription( + sub_vector_map_ = create_subscription( "input/vector_map", qos_transient_local, - std::bind(&MissionPlanner::onMap, this, std::placeholders::_1)); + std::bind(&MissionPlanner::on_map, this, std::placeholders::_1)); const auto durable_qos = rclcpp::QoS(1).transient_local(); pub_marker_ = create_publisher("debug/route_marker", durable_qos); @@ -141,7 +142,7 @@ void MissionPlanner::on_reroute_availability(const RerouteAvailability::ConstSha reroute_availability_ = msg; } -void MissionPlanner::onMap(const HADMapBin::ConstSharedPtr msg) +void MissionPlanner::on_map(const HADMapBin::ConstSharedPtr msg) { map_ptr_ = msg; } diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/src/mission_planner/mission_planner.hpp index 7b0427f6d01b1..fd1b317970749 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.hpp @@ -73,11 +73,13 @@ class MissionPlanner : public rclcpp::Node PoseStamped transform_pose(const PoseStamped & input); rclcpp::Subscription::SharedPtr sub_odometry_; - rclcpp::Subscription::SharedPtr vector_map_subscriber_; + rclcpp::Subscription::SharedPtr sub_vector_map_; rclcpp::Subscription::SharedPtr sub_reroute_availability_; Odometry::ConstSharedPtr odometry_; + HADMapBin::ConstSharedPtr map_ptr_; RerouteAvailability::ConstSharedPtr reroute_availability_; void on_odometry(const Odometry::ConstSharedPtr msg); + void on_map(const HADMapBin::ConstSharedPtr msg); void on_reroute_availability(const RerouteAvailability::ConstSharedPtr msg); rclcpp::Publisher::SharedPtr pub_marker_; @@ -126,9 +128,6 @@ class MissionPlanner : public rclcpp::Node const ClearMrmRoute::Service::Request::SharedPtr req, const ClearMrmRoute::Service::Response::SharedPtr res); - HADMapBin::ConstSharedPtr map_ptr_{nullptr}; - void onMap(const HADMapBin::ConstSharedPtr msg); - component_interface_utils::Subscription::SharedPtr sub_modified_goal_; void on_modified_goal(const ModifiedGoal::Message::ConstSharedPtr msg); void on_change_route( From a195b0530bb140f7c08c6c87df580b5d196230e2 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 9 Aug 2023 03:56:13 +0900 Subject: [PATCH 15/16] feat(dynamic_avoidance): use hatched road markings (#4566) * feat(dynamic_avoidance): use hatched road markings Signed-off-by: Takayuki Murooka * add some ros parameters Signed-off-by: Takayuki Murooka * update config Signed-off-by: Takayuki Murooka * remove comment Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../dynamic_avoidance.param.yaml | 20 ++++++++++++---- .../dynamic_avoidance_module.hpp | 3 +++ .../dynamic_avoidance_module.cpp | 23 +++++++++++-------- .../dynamic_avoidance/manager.cpp | 10 ++++++++ 4 files changed, 42 insertions(+), 14 deletions(-) diff --git a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml index 336879185f48e..701f05eb89ba1 100644 --- a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml +++ b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml @@ -3,6 +3,7 @@ dynamic_avoidance: common: enable_debug_info: true + use_hatched_road_markings: true # avoidance is performed for the object type with true target_object: @@ -18,6 +19,7 @@ min_obstacle_vel: 0.0 # [m/s] successive_num_to_entry_dynamic_avoidance_condition: 5 + successive_num_to_exit_dynamic_avoidance_condition: 1 min_obj_lat_offset_to_ego_path: 0.0 # [m] max_obj_lat_offset_to_ego_path: 1.0 # [m] @@ -26,26 +28,34 @@ min_time_to_start_cut_in: 1.0 # [s] min_lon_offset_ego_to_object: 0.0 # [m] + cut_out_object: + max_time_from_outside_ego_path: 2.0 # [s] + min_object_lat_vel: 0.3 # [m/s] + crossing_object: - min_object_vel: 1.0 - max_object_angle: 1.05 + min_overtaking_object_vel: 1.0 + max_overtaking_object_angle: 1.05 + min_oncoming_object_vel: 0.0 + max_oncoming_object_angle: 0.523 front_object: max_object_angle: 0.785 drivable_area_generation: - lat_offset_from_obstacle: 1.0 # [m] + lat_offset_from_obstacle: 0.8 # [m] max_lat_offset_to_avoid: 0.5 # [m] + max_time_for_object_lat_shift: 2.0 # [s] + lpf_gain_for_lat_avoid_to_offset: 0.9 # [-] # for same directional object overtaking_object: - max_time_to_collision: 10.0 # [s] + max_time_to_collision: 40.0 # [s] start_duration_to_avoid: 2.0 # [s] end_duration_to_avoid: 4.0 # [s] duration_to_hold_avoidance: 3.0 # [s] # for opposite directional object oncoming_object: - max_time_to_collision: 15.0 # [s] + max_time_to_collision: 40.0 # [s] This value should be the same as overtaking_object's one to suppress chattering of this value for parked vehicles start_duration_to_avoid: 12.0 # [s] end_duration_to_avoid: 0.0 # [s] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp index 087a39e87393a..de41fe9359bc2 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -45,6 +45,7 @@ struct DynamicAvoidanceParameters { // common bool enable_debug_info{true}; + bool use_hatched_road_markings{true}; // obstacle types to avoid bool avoid_car{true}; @@ -75,6 +76,8 @@ struct DynamicAvoidanceParameters // drivable area generation double lat_offset_from_obstacle{0.0}; double max_lat_offset_to_avoid{0.0}; + double max_time_for_lat_shift{0.0}; + double lpf_gain_for_lat_avoid_to_offset{0.0}; double max_time_to_collision_overtaking_object{0.0}; double start_duration_to_avoid_overtaking_object{0.0}; diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index e8570f0e310cf..b5ed7cc217a56 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -270,7 +270,6 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan() debug_marker_.markers.clear(); const auto prev_module_path = getPreviousModuleOutput().path; - const auto drivable_lanes = getPreviousModuleOutput().drivable_area_info.drivable_lanes; // create obstacles to avoid (= extract from the drivable area) std::vector obstacles_for_drivable_area; @@ -285,11 +284,18 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan() } } + DrivableAreaInfo current_drivable_area_info; + current_drivable_area_info.drivable_lanes = + getPreviousModuleOutput().drivable_area_info.drivable_lanes; + current_drivable_area_info.obstacles = obstacles_for_drivable_area; + current_drivable_area_info.enable_expanding_hatched_road_markings = + parameters_->use_hatched_road_markings; + BehaviorModuleOutput output; output.path = prev_module_path; + output.drivable_area_info = utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); output.reference_path = getPreviousModuleOutput().reference_path; - output.drivable_area_info.drivable_lanes = drivable_lanes; - output.drivable_area_info.obstacles = obstacles_for_drivable_area; output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; return output; @@ -827,10 +833,9 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoid( // calculate bound min and max lateral offset const double min_bound_lat_offset = [&]() { - constexpr double object_time_to_shift = 2.0; const double lat_abs_offset_to_shift = std::max(0.0, obj_normal_vel * (is_collision_left ? -1.0 : 1.0)) * - object_time_to_shift; // TODO(murooka) use rosparam + parameters_->max_time_for_lat_shift; const double raw_min_bound_lat_offset = min_obj_lat_abs_offset - parameters_->lat_offset_from_obstacle - lat_abs_offset_to_shift; const double min_bound_lat_abs_offset_limit = @@ -850,10 +855,10 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoid( return prev_object->lat_offset_to_avoid.min_value; }(); const double filtered_min_bound_lat_offset = - prev_min_lat_avoid_to_offset - ? signal_processing::lowpassFilter( - min_bound_lat_offset, *prev_min_lat_avoid_to_offset, 0.5) // TODO(murooka) use rosparam - : min_bound_lat_offset; + prev_min_lat_avoid_to_offset ? signal_processing::lowpassFilter( + min_bound_lat_offset, *prev_min_lat_avoid_to_offset, + parameters_->lpf_gain_for_lat_avoid_to_offset) + : min_bound_lat_offset; return MinMaxValue{filtered_min_bound_lat_offset, max_bound_lat_offset}; } diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp index e686719392e14..e378d43f6497c 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp @@ -32,6 +32,7 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager( { // common std::string ns = "dynamic_avoidance.common."; p.enable_debug_info = node->declare_parameter(ns + "enable_debug_info"); + p.use_hatched_road_markings = node->declare_parameter(ns + "use_hatched_road_markings"); } { // target object @@ -82,6 +83,10 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager( std::string ns = "dynamic_avoidance.drivable_area_generation."; p.lat_offset_from_obstacle = node->declare_parameter(ns + "lat_offset_from_obstacle"); p.max_lat_offset_to_avoid = node->declare_parameter(ns + "max_lat_offset_to_avoid"); + p.max_time_for_lat_shift = + node->declare_parameter(ns + "max_time_for_object_lat_shift"); + p.lpf_gain_for_lat_avoid_to_offset = + node->declare_parameter(ns + "lpf_gain_for_lat_avoid_to_offset"); p.max_time_to_collision_overtaking_object = node->declare_parameter(ns + "overtaking_object.max_time_to_collision"); @@ -112,6 +117,7 @@ void DynamicAvoidanceModuleManager::updateModuleParams( { // common const std::string ns = "dynamic_avoidance.common."; updateParam(parameters, ns + "enable_debug_info", p->enable_debug_info); + updateParam(parameters, ns + "use_hatched_road_markings", p->use_hatched_road_markings); } { // target object @@ -174,6 +180,10 @@ void DynamicAvoidanceModuleManager::updateModuleParams( updateParam(parameters, ns + "lat_offset_from_obstacle", p->lat_offset_from_obstacle); updateParam(parameters, ns + "max_lat_offset_to_avoid", p->max_lat_offset_to_avoid); + updateParam( + parameters, ns + "max_time_for_object_lat_shift", p->max_time_for_lat_shift); + updateParam( + parameters, ns + "lpf_gain_for_lat_avoid_to_offset", p->lpf_gain_for_lat_avoid_to_offset); updateParam( parameters, ns + "overtaking_object.max_time_to_collision", From 9ac8faf5291337e072e9f22679dead91a9a26be8 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 9 Aug 2023 03:56:29 +0900 Subject: [PATCH 16/16] fix(dynamic_avoidance): minor changes with bug fix (#4567) * feat(dynamic_avoidance): use hatched road markings Signed-off-by: Takayuki Murooka * add some ros parameters Signed-off-by: Takayuki Murooka * update config Signed-off-by: Takayuki Murooka * remove comment Signed-off-by: Takayuki Murooka * feat(dynamic_avoidance): add minor changes Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../dynamic_avoidance_module.hpp | 7 ++- .../dynamic_avoidance_module.cpp | 61 ++++++------------- 2 files changed, 21 insertions(+), 47 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp index de41fe9359bc2..1f54786df83d7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -120,8 +120,10 @@ class DynamicAvoidanceModule : public SceneModuleInterface std::optional latest_time_inside_ego_path{std::nullopt}; std::vector predicted_paths{}; - MinMaxValue lon_offset_to_avoid; - MinMaxValue lat_offset_to_avoid; + // NOTE: Previous values of the following are used for low-pass filtering. + // Therefore, they has to be initialized as nullopt. + std::optional lon_offset_to_avoid{std::nullopt}; + std::optional lat_offset_to_avoid{std::nullopt}; bool is_collision_left; bool should_be_avoided{false}; @@ -306,7 +308,6 @@ class DynamicAvoidanceModule : public SceneModuleInterface bool isLabelTargetObstacle(const uint8_t label) const; void updateTargetObjects(); - std::optional> calcPathForObjectPolygon() const; bool willObjectCutIn( const std::vector & ego_path, const PredictedPath & predicted_path, const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const; diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index b5ed7cc217a56..7f69397cb3cf5 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -68,7 +68,7 @@ void appendExtractedPolygonMarker( auto marker = tier4_autoware_utils::createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "extracted_polygons", marker_array.markers.size(), visualization_msgs::msg::Marker::LINE_STRIP, - tier4_autoware_utils::createMarkerScale(0.05, 0.0, 0.0), + tier4_autoware_utils::createMarkerScale(0.1, 0.0, 0.0), tier4_autoware_utils::createMarkerColor(1.0, 0.5, 0.6, 0.8)); // NOTE: obj_poly.outer() has already duplicated points to close the polygon. @@ -349,11 +349,7 @@ void DynamicAvoidanceModule::updateTargetObjects() const auto prev_module_path = getPreviousModuleOutput().path; const auto & predicted_objects = planner_data_->dynamic_object->objects; - const auto path_points_for_object_polygon = calcPathForObjectPolygon(); - if (!path_points_for_object_polygon) { - return; - } - + const auto path_points_for_object_polygon = getPreviousModuleOutput().reference_path->points; const auto prev_objects = target_objects_manager_.getValidObjects(); // 1. Rough filtering of target objects @@ -509,9 +505,9 @@ void DynamicAvoidanceModule::updateTargetObjects() // 2.g. calculate longitudinal and lateral offset to avoid const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape); const auto lon_offset_to_avoid = calcMinMaxLongitudinalOffsetToAvoid( - *path_points_for_object_polygon, object.pose, obj_points, object.vel, time_to_collision); + path_points_for_object_polygon, object.pose, obj_points, object.vel, time_to_collision); const auto lat_offset_to_avoid = calcMinMaxLateralOffsetToAvoid( - *path_points_for_object_polygon, obj_points, is_collision_left, object.lat_vel, prev_object); + path_points_for_object_polygon, obj_points, is_collision_left, object.lat_vel, prev_object); const bool should_be_avoided = true; target_objects_manager_.updateObject( @@ -519,30 +515,6 @@ void DynamicAvoidanceModule::updateTargetObjects() } } -std::optional> DynamicAvoidanceModule::calcPathForObjectPolygon() - const -{ - const auto & ego_pose = getEgoPose(); - const auto & rh = planner_data_->route_handler; - - // get path with backward margin - lanelet::ConstLanelet current_lane; - if (!rh->getClosestLaneletWithinRoute(ego_pose, ¤t_lane)) { - RCLCPP_ERROR( - rclcpp::get_logger("behavior_path_planner").get_child("dynamic_avoidance"), - "failed to find closest lanelet within route!!!"); - return std::nullopt; - } - - constexpr double forward_length = 100.0; - const double backward_length = 50.0; - const auto current_lanes = - rh->getLaneletSequence(current_lane, ego_pose, backward_length, forward_length); - const auto path = utils::getCenterLinePath( - *rh, current_lanes, ego_pose, backward_length, forward_length, planner_data_->parameters); - return path.points; -} - [[maybe_unused]] std::optional> DynamicAvoidanceModule::calcCollisionSection( const std::vector & ego_path, const PredictedPath & obj_path) const @@ -849,10 +821,10 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoid( // filter min_bound_lat_offset const auto prev_min_lat_avoid_to_offset = [&]() -> std::optional { - if (!prev_object) { + if (!prev_object || !prev_object->lat_offset_to_avoid) { return std::nullopt; } - return prev_object->lat_offset_to_avoid.min_value; + return prev_object->lat_offset_to_avoid->min_value; }(); const double filtered_min_bound_lat_offset = prev_min_lat_avoid_to_offset ? signal_processing::lowpassFilter( @@ -867,22 +839,23 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoid( std::optional DynamicAvoidanceModule::calcDynamicObstaclePolygon( const DynamicAvoidanceObject & object) const { - auto path_points_for_object_polygon = calcPathForObjectPolygon(); - if (!path_points_for_object_polygon) { + if (!object.lon_offset_to_avoid || !object.lat_offset_to_avoid) { return std::nullopt; } + auto path_points_for_object_polygon = getPreviousModuleOutput().reference_path->points; + const size_t obj_seg_idx = - motion_utils::findNearestSegmentIndex(*path_points_for_object_polygon, object.pose.position); + motion_utils::findNearestSegmentIndex(path_points_for_object_polygon, object.pose.position); const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape); const auto lon_bound_start_idx_opt = motion_utils::insertTargetPoint( - obj_seg_idx, object.lon_offset_to_avoid.min_value, *path_points_for_object_polygon); + obj_seg_idx, object.lon_offset_to_avoid->min_value, path_points_for_object_polygon); const size_t updated_obj_seg_idx = (lon_bound_start_idx_opt && lon_bound_start_idx_opt.value() <= obj_seg_idx) ? obj_seg_idx + 1 : obj_seg_idx; const auto lon_bound_end_idx_opt = motion_utils::insertTargetPoint( - updated_obj_seg_idx, object.lon_offset_to_avoid.max_value, *path_points_for_object_polygon); + updated_obj_seg_idx, object.lon_offset_to_avoid->max_value, path_points_for_object_polygon); if (!lon_bound_start_idx_opt && !lon_bound_end_idx_opt) { // NOTE: The obstacle is longitudinally out of the ego's trajectory. @@ -892,19 +865,19 @@ std::optional DynamicAvoidanceModule::calcDynam lon_bound_start_idx_opt ? lon_bound_start_idx_opt.value() : static_cast(0); const size_t lon_bound_end_idx = lon_bound_end_idx_opt ? lon_bound_end_idx_opt.value() - : static_cast(path_points_for_object_polygon->size() - 1); + : static_cast(path_points_for_object_polygon.size() - 1); // create inner/outer bound points std::vector obj_inner_bound_points; std::vector obj_outer_bound_points; for (size_t i = lon_bound_start_idx; i <= lon_bound_end_idx; ++i) { obj_inner_bound_points.push_back(tier4_autoware_utils::calcOffsetPose( - path_points_for_object_polygon->at(i).point.pose, 0.0, - object.lat_offset_to_avoid.min_value, 0.0) + path_points_for_object_polygon.at(i).point.pose, 0.0, + object.lat_offset_to_avoid->min_value, 0.0) .position); obj_outer_bound_points.push_back(tier4_autoware_utils::calcOffsetPose( - path_points_for_object_polygon->at(i).point.pose, 0.0, - object.lat_offset_to_avoid.max_value, 0.0) + path_points_for_object_polygon.at(i).point.pose, 0.0, + object.lat_offset_to_avoid->max_value, 0.0) .position); }