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..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,11 +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()); - 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, ""); - } } for (const auto & right_shift : right_shift_array_) { @@ -136,11 +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()); - 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, ""); - } } } @@ -417,6 +407,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..b45d0bf93273e 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,48 @@ 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); + } + + 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( @@ -974,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; } 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);