From da02c2e51fb0471526f0ce13695bd24820e21bf4 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Wed, 20 Dec 2023 18:10:51 +0900 Subject: [PATCH 1/3] fix(behavior_path_planner): publish steering factor from scene modules (#5914) Signed-off-by: Fumiya Watanabe --- .../include/behavior_path_planner/planner_manager.hpp | 2 ++ 1 file changed, 2 insertions(+) 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 7ceafb6f428de..3ccc92d04025f 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 @@ -286,6 +286,8 @@ class PlannerManager module_ptr->publishRTCStatus(); + module_ptr->publishSteeringFactor(); + module_ptr->publishObjectsOfInterestMarker(); processing_time_.at(module_ptr->name()) += stop_watch_.toc(module_ptr->name(), true); From 19d74b35e375f8088f85fd045dd7fd718bc8ee57 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Mon, 18 Mar 2024 17:34:26 +0900 Subject: [PATCH 2/3] feat(avoidance): (TEMPORARY) steering factor for avoidance maneuver Signed-off-by: satoshi-ota --- .../behavior_path_avoidance_module/scene.hpp | 48 ++++++++++++++++++- .../src/scene.cpp | 34 +++++++++++++ 2 files changed, 80 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp index 882806177b4e0..e3844f7eb43ea 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp @@ -122,10 +122,30 @@ class AvoidanceModule : public SceneModuleInterface calcSignedArcLength(path.points, ego_position, left_shift.finish_pose.position); rtc_interface_ptr_map_.at("left")->updateCooperateStatus( left_shift.uuid, true, start_distance, finish_distance, clock_->now()); + + const auto record_start_time = !is_recording_ && is_record_necessary_ && helper_->isShifted(); + const auto record_end_time = is_recording_ && !helper_->isShifted(); + const auto steering_factor = [&]() { + if (record_start_time) { + is_recording_ = true; + RCLCPP_DEBUG(getLogger(), "start left avoidance maneuver. get time stamp."); + return uint16_t(100); + } + + if (record_end_time) { + is_recording_ = false; + is_record_necessary_ = false; + RCLCPP_DEBUG(getLogger(), "end avoidance maneuver. get time stamp."); + return uint16_t(200); + } + + return SteeringFactor::LEFT; + }(); + if (finish_distance > -1.0e-03) { steering_factor_interface_ptr_->updateSteeringFactor( {left_shift.start_pose, left_shift.finish_pose}, {start_distance, finish_distance}, - PlanningBehavior::AVOIDANCE, SteeringFactor::LEFT, SteeringFactor::TURNING, ""); + PlanningBehavior::AVOIDANCE, steering_factor, SteeringFactor::TURNING, ""); } } @@ -136,10 +156,30 @@ class AvoidanceModule : public SceneModuleInterface calcSignedArcLength(path.points, ego_position, right_shift.finish_pose.position); rtc_interface_ptr_map_.at("right")->updateCooperateStatus( right_shift.uuid, true, start_distance, finish_distance, clock_->now()); + + const auto record_start_time = !is_recording_ && is_record_necessary_ && helper_->isShifted(); + const auto record_end_time = is_recording_ && !helper_->isShifted(); + const auto steering_factor = [&]() { + if (record_start_time) { + is_recording_ = true; + RCLCPP_DEBUG(getLogger(), "start right avoidance maneuver. get time stamp."); + return uint16_t(100); + } + + if (record_end_time) { + is_recording_ = false; + is_record_necessary_ = false; + RCLCPP_DEBUG(getLogger(), "end avoidance maneuver. get time stamp."); + return uint16_t(200); + } + + return SteeringFactor::RIGHT; + }(); + if (finish_distance > -1.0e-03) { steering_factor_interface_ptr_->updateSteeringFactor( {right_shift.start_pose, right_shift.finish_pose}, {start_distance, finish_distance}, - PlanningBehavior::AVOIDANCE, SteeringFactor::RIGHT, SteeringFactor::TURNING, ""); + PlanningBehavior::AVOIDANCE, steering_factor, SteeringFactor::TURNING, ""); } } } @@ -417,6 +457,10 @@ class AvoidanceModule : public SceneModuleInterface bool safe_{true}; + bool is_recording_{false}; + + bool is_record_necessary_{false}; + std::shared_ptr helper_; std::shared_ptr parameters_; diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index ebe42f73d538b..382398338c94c 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -97,6 +97,34 @@ lanelet::BasicLineString3d toLineString3d(const std::vector & bound) bound.begin(), bound.end(), [&](const auto & p) { ret.emplace_back(p.x, p.y, p.z); }); return ret; } + +bool straddleRoadBound( + const ShiftedPath & path, const lanelet::ConstLanelets & lanes, + const vehicle_info_util::VehicleInfo & vehicle_info) +{ + using boost::geometry::intersects; + using tier4_autoware_utils::pose2transform; + using tier4_autoware_utils::transformVector; + + const auto footprint = vehicle_info.createFootprint(); + + for (const auto & lane : lanes) { + for (const auto & p : path.path.points) { + const auto transform = pose2transform(p.point.pose); + const auto shifted_vehicle_footprint = transformVector(footprint, transform); + + if (intersects(lane.leftBound2d().basicLineString(), shifted_vehicle_footprint)) { + return true; + } + + if (intersects(lane.rightBound2d().basicLineString(), shifted_vehicle_footprint)) { + return true; + } + } + } + + return false; +} } // namespace AvoidanceModule::AvoidanceModule( @@ -894,6 +922,12 @@ BehaviorModuleOutput AvoidanceModule::plan() planner_data_->parameters.ego_nearest_yaw_threshold); } + // check if the ego straddles lane border + if (!is_recording_) { + is_record_necessary_ = straddleRoadBound( + spline_shift_path, data.current_lanelets, planner_data_->parameters.vehicle_info); + } + // sparse resampling for computational cost { spline_shift_path.path = utils::resamplePathWithSpline( From 79b1618b68f1427bf516574caa2222281be47bac Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 19 Mar 2024 18:56:26 +0900 Subject: [PATCH 3/3] fix(avoidance): update logic Signed-off-by: satoshi-ota --- .../behavior_path_avoidance_module/scene.hpp | 50 ------------------- .../src/scene.cpp | 44 +++++++++++++--- 2 files changed, 36 insertions(+), 58 deletions(-) diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp index e3844f7eb43ea..54f8ccf0b9aea 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp @@ -122,31 +122,6 @@ class AvoidanceModule : public SceneModuleInterface calcSignedArcLength(path.points, ego_position, left_shift.finish_pose.position); rtc_interface_ptr_map_.at("left")->updateCooperateStatus( left_shift.uuid, true, start_distance, finish_distance, clock_->now()); - - const auto record_start_time = !is_recording_ && is_record_necessary_ && helper_->isShifted(); - const auto record_end_time = is_recording_ && !helper_->isShifted(); - const auto steering_factor = [&]() { - if (record_start_time) { - is_recording_ = true; - RCLCPP_DEBUG(getLogger(), "start left avoidance maneuver. get time stamp."); - return uint16_t(100); - } - - if (record_end_time) { - is_recording_ = false; - is_record_necessary_ = false; - RCLCPP_DEBUG(getLogger(), "end avoidance maneuver. get time stamp."); - return uint16_t(200); - } - - return SteeringFactor::LEFT; - }(); - - if (finish_distance > -1.0e-03) { - steering_factor_interface_ptr_->updateSteeringFactor( - {left_shift.start_pose, left_shift.finish_pose}, {start_distance, finish_distance}, - PlanningBehavior::AVOIDANCE, steering_factor, SteeringFactor::TURNING, ""); - } } for (const auto & right_shift : right_shift_array_) { @@ -156,31 +131,6 @@ class AvoidanceModule : public SceneModuleInterface calcSignedArcLength(path.points, ego_position, right_shift.finish_pose.position); rtc_interface_ptr_map_.at("right")->updateCooperateStatus( right_shift.uuid, true, start_distance, finish_distance, clock_->now()); - - const auto record_start_time = !is_recording_ && is_record_necessary_ && helper_->isShifted(); - const auto record_end_time = is_recording_ && !helper_->isShifted(); - const auto steering_factor = [&]() { - if (record_start_time) { - is_recording_ = true; - RCLCPP_DEBUG(getLogger(), "start right avoidance maneuver. get time stamp."); - return uint16_t(100); - } - - if (record_end_time) { - is_recording_ = false; - is_record_necessary_ = false; - RCLCPP_DEBUG(getLogger(), "end avoidance maneuver. get time stamp."); - return uint16_t(200); - } - - return SteeringFactor::RIGHT; - }(); - - if (finish_distance > -1.0e-03) { - steering_factor_interface_ptr_->updateSteeringFactor( - {right_shift.start_pose, right_shift.finish_pose}, {start_distance, finish_distance}, - PlanningBehavior::AVOIDANCE, steering_factor, SteeringFactor::TURNING, ""); - } } } diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 382398338c94c..b45d0bf93273e 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -928,6 +928,42 @@ BehaviorModuleOutput AvoidanceModule::plan() spline_shift_path, data.current_lanelets, planner_data_->parameters.vehicle_info); } + if (!path_shifter_.getShiftLines().empty()) { + const auto front_shift_line = path_shifter_.getShiftLines().front(); + const double start_distance = calcSignedArcLength( + data.reference_path.points, getEgoPosition(), front_shift_line.start.position); + const double finish_distance = calcSignedArcLength( + data.reference_path.points, getEgoPosition(), front_shift_line.end.position); + + const auto record_start_time = !is_recording_ && is_record_necessary_ && helper_->isShifted(); + const auto record_end_time = is_recording_ && !helper_->isShifted(); + const auto relative_shift_length = front_shift_line.end_shift_length - helper_->getEgoShift(); + const auto steering_direction = + relative_shift_length > 0.0 ? SteeringFactor::LEFT : SteeringFactor::RIGHT; + const auto steering_status = [&]() { + if (record_start_time) { + is_recording_ = true; + RCLCPP_ERROR(getLogger(), "start right avoidance maneuver. get time stamp."); + return uint16_t(100); + } + + if (record_end_time) { + is_recording_ = false; + is_record_necessary_ = false; + RCLCPP_ERROR(getLogger(), "end avoidance maneuver. get time stamp."); + return uint16_t(200); + } + + return helper_->isShifted() ? SteeringFactor::TURNING : SteeringFactor::APPROACHING; + }(); + + if (finish_distance > -1.0e-03) { + steering_factor_interface_ptr_->updateSteeringFactor( + {front_shift_line.start, front_shift_line.end}, {start_distance, finish_distance}, + PlanningBehavior::AVOIDANCE, steering_direction, steering_status, ""); + } + } + // sparse resampling for computational cost { spline_shift_path.path = utils::resamplePathWithSpline( @@ -1008,14 +1044,6 @@ CandidateOutput AvoidanceModule::planCandidate() const output.start_distance_to_path_change = sl_front.start_longitudinal; output.finish_distance_to_path_change = sl_back.end_longitudinal; - const uint16_t steering_factor_direction = std::invoke([&output]() { - return output.lateral_shift > 0.0 ? SteeringFactor::LEFT : SteeringFactor::RIGHT; - }); - steering_factor_interface_ptr_->updateSteeringFactor( - {sl_front.start, sl_back.end}, - {output.start_distance_to_path_change, output.finish_distance_to_path_change}, - PlanningBehavior::AVOIDANCE, steering_factor_direction, SteeringFactor::APPROACHING, ""); - output.path_candidate = shifted_path.path; return output; }