diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp index 434bb6b9a0696..e9542feaccfd1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp @@ -114,7 +114,7 @@ class BlindSpotModule : public SceneModuleInterface * @brief plan go-stop velocity at traffic crossing with collision check between reference path * and object predicted path */ - bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + bool modifyPathVelocity(PathWithLaneId * path) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; std::vector createVirtualWalls() override; @@ -133,7 +133,7 @@ class BlindSpotModule : public SceneModuleInterface // Parameter void initializeRTCStatus(); - BlindSpotDecision modifyPathVelocityDetail(PathWithLaneId * path, StopReason * stop_reason); + BlindSpotDecision modifyPathVelocityDetail(PathWithLaneId * path); // setSafe(), setDistance() void setRTCStatus( const BlindSpotDecision & decision, const tier4_planning_msgs::msg::PathWithLaneId & path); @@ -141,12 +141,10 @@ class BlindSpotModule : public SceneModuleInterface void setRTCStatusByDecision( const Decision & decision, const tier4_planning_msgs::msg::PathWithLaneId & path); // stop/GO - void reactRTCApproval( - const BlindSpotDecision & decision, PathWithLaneId * path, StopReason * stop_reason); + void reactRTCApproval(const BlindSpotDecision & decision, PathWithLaneId * path); template void reactRTCApprovalByDecision( - const Decision & decision, tier4_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason); + const Decision & decision, tier4_planning_msgs::msg::PathWithLaneId * path); std::optional generateInterpolatedPathInfo( const tier4_planning_msgs::msg::PathWithLaneId & input_path) const; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp index 6a3d542600b3c..64e1435167a89 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp @@ -33,8 +33,7 @@ void BlindSpotModule::setRTCStatusByDecision( template void BlindSpotModule::reactRTCApprovalByDecision( [[maybe_unused]] const T & decision, - [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, - [[maybe_unused]] StopReason * stop_reason) + [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path) { static_assert("Unsupported type passed to reactRTCApprovalByDecision"); } @@ -53,8 +52,7 @@ void BlindSpotModule::setRTCStatusByDecision( template <> void BlindSpotModule::reactRTCApprovalByDecision( [[maybe_unused]] const InternalError & decision, - [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, - [[maybe_unused]] StopReason * stop_reason) + [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path) { return; } @@ -73,8 +71,7 @@ void BlindSpotModule::setRTCStatusByDecision( template <> void BlindSpotModule::reactRTCApprovalByDecision( [[maybe_unused]] const OverPassJudge & decision, - [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, - [[maybe_unused]] StopReason * stop_reason) + [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path) { return; } @@ -95,8 +92,7 @@ void BlindSpotModule::setRTCStatusByDecision( template <> void BlindSpotModule::reactRTCApprovalByDecision( - const Unsafe & decision, tier4_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason) + const Unsafe & decision, tier4_planning_msgs::msg::PathWithLaneId * path) { if (!isActivated()) { constexpr double stop_vel = 0.0; @@ -104,11 +100,7 @@ void BlindSpotModule::reactRTCApprovalByDecision( debug_data_.virtual_wall_pose = planning_utils::getAheadPose( decision.stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); - tier4_planning_msgs::msg::StopFactor stop_factor; const auto stop_pose = path->points.at(decision.stop_line_idx).point.pose; - stop_factor.stop_pose = stop_pose; - stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data_.conflicting_targets); - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor_.set( path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); } @@ -131,7 +123,7 @@ void BlindSpotModule::setRTCStatusByDecision( template <> void BlindSpotModule::reactRTCApprovalByDecision( - const Safe & decision, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason) + const Safe & decision, tier4_planning_msgs::msg::PathWithLaneId * path) { if (!isActivated()) { constexpr double stop_vel = 0.0; @@ -139,11 +131,7 @@ void BlindSpotModule::reactRTCApprovalByDecision( debug_data_.virtual_wall_pose = planning_utils::getAheadPose( decision.stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); - tier4_planning_msgs::msg::StopFactor stop_factor; const auto stop_pose = path->points.at(decision.stop_line_idx).point.pose; - stop_factor.stop_pose = stop_pose; - stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data_.conflicting_targets); - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor_.set( path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp index c6cc8cf4a3db8..b5cbbfdf354bb 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp @@ -64,8 +64,7 @@ void BlindSpotModule::initializeRTCStatus() setDistance(std::numeric_limits::lowest()); } -BlindSpotDecision BlindSpotModule::modifyPathVelocityDetail( - PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) +BlindSpotDecision BlindSpotModule::modifyPathVelocityDetail(PathWithLaneId * path) { if (planner_param_.use_pass_judge_line && is_over_pass_judge_line_) { return OverPassJudge{"already over the pass judge line. no plan needed."}; @@ -160,26 +159,23 @@ void BlindSpotModule::setRTCStatus( decision); } -void BlindSpotModule::reactRTCApproval( - const BlindSpotDecision & decision, PathWithLaneId * path, StopReason * stop_reason) +void BlindSpotModule::reactRTCApproval(const BlindSpotDecision & decision, PathWithLaneId * path) { std::visit( - VisitorSwitch{[&](const auto & sub_decision) { - reactRTCApprovalByDecision(sub_decision, path, stop_reason); - }}, + VisitorSwitch{ + [&](const auto & sub_decision) { reactRTCApprovalByDecision(sub_decision, path); }}, decision); } -bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path) { debug_data_ = DebugData(); - *stop_reason = planning_utils::initializeStopReason(StopReason::BLIND_SPOT); initializeRTCStatus(); - const auto decision = modifyPathVelocityDetail(path, stop_reason); + const auto decision = modifyPathVelocityDetail(path); const auto & input_path = *path; setRTCStatus(decision, input_path); - reactRTCApproval(decision, path, stop_reason); + reactRTCApproval(decision, path); return true; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 51a0b4aa23fde..b0ea73a2781d4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -201,7 +201,7 @@ CrosswalkModule::CrosswalkModule( node.create_publisher("~/debug/collision_info", 1); } -bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path) { if (path->points.size() < 2) { RCLCPP_DEBUG(logger_, "Do not interpolate because path size is less than 2."); @@ -213,7 +213,6 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto logger_, planner_param_.show_processing_time, "=========== module_id: %ld ===========", module_id_); - *stop_reason = planning_utils::initializeStopReason(StopReason::CROSSWALK); const auto & ego_pos = planner_data_->current_odometry->pose.position; const auto objects_ptr = planner_data_->predicted_objects; @@ -274,7 +273,7 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto if (isActivated()) { planGo(*path, nearest_stop_factor); } else { - planStop(*path, nearest_stop_factor, default_stop_pose, stop_reason); + planStop(*path, nearest_stop_factor, default_stop_pose); } recordTime(4); @@ -1353,7 +1352,7 @@ void CrosswalkModule::planGo( void CrosswalkModule::planStop( PathWithLaneId & ego_path, const std::optional & nearest_stop_factor, - const std::optional & default_stop_pose, StopReason * stop_reason) + const std::optional & default_stop_pose) { // Calculate stop factor auto stop_factor = [&]() -> std::optional { @@ -1376,7 +1375,6 @@ void CrosswalkModule::planStop( // 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); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index de696aa670ad3..d27aebe42d459 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -336,7 +336,7 @@ class CrosswalkModule : public SceneModuleInterface const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); - bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + bool modifyPathVelocity(PathWithLaneId * path) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; autoware::motion_utils::VirtualWalls createVirtualWalls() override; @@ -399,7 +399,7 @@ class CrosswalkModule : public SceneModuleInterface void planStop( PathWithLaneId & ego_path, const std::optional & nearest_stop_factor, - const std::optional & default_stop_pose, StopReason * stop_reason); + const std::optional & default_stop_pose); // minor functions std::pair getAttentionRange( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp index abbb818f2b042..61b3b185999d8 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp @@ -47,7 +47,7 @@ DetectionAreaModule::DetectionAreaModule( velocity_factor_.init(PlanningBehavior::USER_DEFINED_DETECTION_AREA); } -bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path) { // Store original path const auto original_path = *path; @@ -55,7 +55,6 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // Reset data debug_data_ = DebugData(); debug_data_.base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; - *stop_reason = planning_utils::initializeStopReason(StopReason::DETECTION_AREA); // Find obstacles in detection area const auto obstacle_points = detection_area::get_obstacle_points( @@ -184,28 +183,11 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // Create StopReason { - StopFactor stop_factor{}; - stop_factor.stop_pose = stop_point->second; - stop_factor.stop_factor_points = obstacle_points; - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor_.set( path->points, planner_data_->current_odometry->pose, stop_point->second, VelocityFactor::UNKNOWN); } - // Create legacy StopReason - { - const double stop_path_point_distance = autoware::motion_utils::calcSignedArcLength( - path->points, 0, stop_pose.position, stop_point->first); - - if ( - !first_stop_path_point_distance_ || - stop_path_point_distance < first_stop_path_point_distance_.value()) { - debug_data_.first_stop_pose = stop_point->second; - first_stop_path_point_distance_ = stop_path_point_distance; - } - } - return true; } } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp index 46bb46bcdeb61..9224cf4624687 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp @@ -70,7 +70,7 @@ class DetectionAreaModule : public SceneModuleInterface const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); - bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + bool modifyPathVelocity(PathWithLaneId * path) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; autoware::motion_utils::VirtualWalls createVirtualWalls() override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp index 8115f52437a04..cedc3c41e8165 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -91,14 +91,13 @@ IntersectionModule::IntersectionModule( "~/debug/intersection/object_ttc", 1); } -bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path) { debug_data_ = DebugData(); - *stop_reason = planning_utils::initializeStopReason(StopReason::INTERSECTION); initializeRTCStatus(); - const auto decision_result = modifyPathVelocityDetail(path, stop_reason); + const auto decision_result = modifyPathVelocityDetail(path); prev_decision_result_ = decision_result; { @@ -110,7 +109,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * prepareRTCStatus(decision_result, *path); - reactRTCApproval(decision_result, path, stop_reason); + reactRTCApproval(decision_result, path); return true; } @@ -145,8 +144,7 @@ static std::string formatOcclusionType(const IntersectionModule::OcclusionType & return "RTCOccluded"; } -DecisionResult IntersectionModule::modifyPathVelocityDetail( - PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) +DecisionResult IntersectionModule::modifyPathVelocityDetail(PathWithLaneId * path) { const auto prepare_data = prepareIntersectionData(path); if (!prepare_data) { @@ -683,7 +681,6 @@ void prepareRTCByDecisionResult( *occlusion_safety = true; *occlusion_distance = autoware::motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); - return; } void IntersectionModule::prepareRTCStatus( @@ -711,7 +708,6 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, - [[maybe_unused]] StopReason * stop_reason, [[maybe_unused]] VelocityFactorInterface * velocity_factor, [[maybe_unused]] IntersectionModule::DebugData * debug_data) { @@ -727,7 +723,6 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, - [[maybe_unused]] StopReason * stop_reason, [[maybe_unused]] VelocityFactorInterface * velocity_factor, [[maybe_unused]] IntersectionModule::DebugData * debug_data) { @@ -742,7 +737,6 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, - [[maybe_unused]] StopReason * stop_reason, [[maybe_unused]] VelocityFactorInterface * velocity_factor, [[maybe_unused]] IntersectionModule::DebugData * debug_data) { @@ -755,8 +749,7 @@ void reactRTCApprovalByDecisionResult( const StuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, - IntersectionModule::DebugData * debug_data) + VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -770,10 +763,6 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data->unsafe_targets); - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(closest_idx).point.pose, path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); @@ -785,9 +774,6 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(occlusion_stopline_idx, baselink2front, *path); { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(occlusion_stopline_idx).point.pose; - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(closest_idx).point.pose, path->points.at(occlusion_stopline_idx).point.pose, VelocityFactor::UNKNOWN); @@ -802,8 +788,7 @@ void reactRTCApprovalByDecisionResult( const YieldStuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, - IntersectionModule::DebugData * debug_data) + VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -817,10 +802,6 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data->unsafe_targets); - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(closest_idx).point.pose, path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); @@ -835,8 +816,7 @@ void reactRTCApprovalByDecisionResult( const NonOccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, - IntersectionModule::DebugData * debug_data) + VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -848,9 +828,6 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); @@ -862,9 +839,6 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); @@ -878,8 +852,8 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const FirstWaitBeforeOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, - tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + tier4_planning_msgs::msg::PathWithLaneId * path, VelocityFactorInterface * velocity_factor, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -891,9 +865,6 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_first_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); @@ -913,9 +884,6 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); @@ -929,8 +897,8 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const PeekingTowardOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, - tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + tier4_planning_msgs::msg::PathWithLaneId * path, VelocityFactorInterface * velocity_factor, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -955,9 +923,6 @@ void reactRTCApprovalByDecisionResult( debug_data->static_occlusion_with_traffic_light_timeout = decision_result.static_occlusion_timeout; { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(occlusion_peeking_stopline).point.pose; - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(occlusion_peeking_stopline).point.pose, VelocityFactor::UNKNOWN); @@ -969,9 +934,6 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); @@ -986,8 +948,7 @@ void reactRTCApprovalByDecisionResult( const OccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, - IntersectionModule::DebugData * debug_data) + VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -999,9 +960,6 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); @@ -1017,9 +975,6 @@ void reactRTCApprovalByDecisionResult( debug_data->static_occlusion_with_traffic_light_timeout = decision_result.static_occlusion_timeout; { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); @@ -1034,8 +989,7 @@ void reactRTCApprovalByDecisionResult( const OccludedAbsenceTrafficLight & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, - IntersectionModule::DebugData * debug_data) + VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -1047,9 +1001,6 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); @@ -1061,9 +1012,6 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); @@ -1087,8 +1035,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const Safe & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, - IntersectionModule::DebugData * debug_data) + VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -1099,9 +1046,6 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); @@ -1113,9 +1057,6 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); @@ -1130,8 +1071,7 @@ void reactRTCApprovalByDecisionResult( const FullyPrioritized & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, - IntersectionModule::DebugData * debug_data) + VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -1143,9 +1083,6 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); @@ -1157,9 +1094,6 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); @@ -1169,15 +1103,14 @@ void reactRTCApprovalByDecisionResult( } void IntersectionModule::reactRTCApproval( - const DecisionResult & decision_result, tier4_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason) + const DecisionResult & decision_result, tier4_planning_msgs::msg::PathWithLaneId * path) { const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; std::visit( VisitorSwitch{[&](const auto & decision) { reactRTCApprovalByDecisionResult( activated_, occlusion_activated_, decision, planner_param_, baselink2front, path, - stop_reason, &velocity_factor_, &debug_data_); + &velocity_factor_, &debug_data_); }}, decision_result); return; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp index 8ffb4792f55db..b718dd84d33af 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -321,7 +321,7 @@ class IntersectionModule : public SceneModuleInterface * INTERSECTION_OCCLUSION. * @{ */ - bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + bool modifyPathVelocity(PathWithLaneId * path) override; /** @}*/ visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; @@ -504,7 +504,7 @@ class IntersectionModule : public SceneModuleInterface /** * @brief analyze traffic_light/occupancy/objects context and return DecisionResult */ - DecisionResult modifyPathVelocityDetail(PathWithLaneId * path, StopReason * stop_reason); + DecisionResult modifyPathVelocityDetail(PathWithLaneId * path); /** * @brief set RTC value according to calculated DecisionResult @@ -516,8 +516,7 @@ class IntersectionModule : public SceneModuleInterface * @brief act based on current RTC approval */ void reactRTCApproval( - const DecisionResult & decision_result, tier4_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason); + const DecisionResult & decision_result, tier4_planning_msgs::msg::PathWithLaneId * path); /** @}*/ private: diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index d5cd9ca587716..478576a5c6676 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -76,10 +76,9 @@ static std::optional getFirstConflictingLanelet( return std::nullopt; } -bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path) { debug_data_ = DebugData(); - *stop_reason = planning_utils::initializeStopReason(StopReason::MERGE_FROM_PRIVATE_ROAD); const auto input_path = *path; @@ -152,9 +151,6 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR planning_utils::setVelocityFromIndex(stopline_idx, v, path); /* get stop point and stop factor */ - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = debug_data_.stop_point_pose; - planning_utils::appendStopReason(stop_factor, stop_reason); const auto & stop_pose = path->points.at(stopline_idx).point.pose; velocity_factor_.set( path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp index a69297adea881..dc8bf1a96a7a2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp @@ -67,7 +67,7 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface * @brief plan go-stop velocity at traffic crossing with collision check between reference path * and object predicted path */ - bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + bool modifyPathVelocity(PathWithLaneId * path) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; autoware::motion_utils::VirtualWalls createVirtualWalls() override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp index c3af04bbd1d1f..8365251904b18 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp @@ -37,14 +37,12 @@ NoDrivableLaneModule::NoDrivableLaneModule( velocity_factor_.init(PlanningBehavior::NO_DRIVABLE_LANE); } -bool NoDrivableLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +bool NoDrivableLaneModule::modifyPathVelocity(PathWithLaneId * path) { if (path->points.empty()) { return false; } - *stop_reason = planning_utils::initializeStopReason(StopReason::NO_DRIVABLE_LANE); - const auto & ego_pos = planner_data_->current_odometry->pose.position; const auto & lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); const auto & no_drivable_lane = lanelet_map_ptr->laneletLayer.get(lane_id_); @@ -82,7 +80,7 @@ bool NoDrivableLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason RCLCPP_INFO(logger_, "Approaching "); } - handle_approaching_state(path, stop_reason); + handle_approaching_state(path); break; } @@ -92,7 +90,7 @@ bool NoDrivableLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason RCLCPP_INFO(logger_, "INSIDE_NO_DRIVABLE_LANE"); } - handle_inside_no_drivable_lane_state(path, stop_reason); + handle_inside_no_drivable_lane_state(path); break; } @@ -102,7 +100,7 @@ bool NoDrivableLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason RCLCPP_INFO(logger_, "STOPPED"); } - handle_stopped_state(path, stop_reason); + handle_stopped_state(path); break; } @@ -131,7 +129,7 @@ void NoDrivableLaneModule::handle_init_state() } } -void NoDrivableLaneModule::handle_approaching_state(PathWithLaneId * path, StopReason * stop_reason) +void NoDrivableLaneModule::handle_approaching_state(PathWithLaneId * path) { const double longitudinal_offset = -1.0 * (planner_param_.stop_margin + planner_data_->vehicle_info_.max_longitudinal_offset_m); @@ -163,11 +161,7 @@ void NoDrivableLaneModule::handle_approaching_state(PathWithLaneId * path, StopR // Get stop point and stop factor { - tier4_planning_msgs::msg::StopFactor stop_factor; const auto & stop_pose = op_stop_pose.value(); - stop_factor.stop_pose = stop_pose; - stop_factor.stop_factor_points.push_back(stop_point); - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor_.set( path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::APPROACHING); @@ -205,8 +199,7 @@ void NoDrivableLaneModule::handle_approaching_state(PathWithLaneId * path, StopR } } -void NoDrivableLaneModule::handle_inside_no_drivable_lane_state( - PathWithLaneId * path, StopReason * stop_reason) +void NoDrivableLaneModule::handle_inside_no_drivable_lane_state(PathWithLaneId * path) { const auto & current_point = planner_data_->current_odometry->pose.position; const size_t current_seg_idx = findEgoSegmentIndex(path->points); @@ -216,11 +209,7 @@ void NoDrivableLaneModule::handle_inside_no_drivable_lane_state( // Get stop point and stop factor { - tier4_planning_msgs::msg::StopFactor stop_factor; const auto & stop_pose = autoware::universe_utils::getPose(path->points.at(0)); - stop_factor.stop_pose = stop_pose; - stop_factor.stop_factor_points.push_back(current_point); - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor_.set( path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::APPROACHING); @@ -239,7 +228,7 @@ void NoDrivableLaneModule::handle_inside_no_drivable_lane_state( } } -void NoDrivableLaneModule::handle_stopped_state(PathWithLaneId * path, StopReason * stop_reason) +void NoDrivableLaneModule::handle_stopped_state(PathWithLaneId * path) { const auto & stopped_pose = autoware::motion_utils::calcLongitudinalOffsetPose( path->points, planner_data_->current_odometry->pose.position, 0.0); @@ -258,11 +247,7 @@ void NoDrivableLaneModule::handle_stopped_state(PathWithLaneId * path, StopReaso // Get stop point and stop factor { - tier4_planning_msgs::msg::StopFactor stop_factor; const auto & stop_pose = ego_pos_on_path.pose; - stop_factor.stop_pose = stop_pose; - stop_factor.stop_factor_points.push_back(stop_pose.position); - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor_.set( path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::STOPPED); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp index d9e6121a8ae51..d8c5e3e0425d1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp @@ -57,7 +57,7 @@ class NoDrivableLaneModule : public SceneModuleInterface const int64_t module_id, const int64_t lane_id, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); - bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + bool modifyPathVelocity(PathWithLaneId * path) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; autoware::motion_utils::VirtualWalls createVirtualWalls() override; @@ -79,9 +79,9 @@ class NoDrivableLaneModule : public SceneModuleInterface double distance_ego_first_intersection{}; void handle_init_state(); - void handle_approaching_state(PathWithLaneId * path, StopReason * stop_reason); - void handle_inside_no_drivable_lane_state(PathWithLaneId * path, StopReason * stop_reason); - void handle_stopped_state(PathWithLaneId * path, StopReason * stop_reason); + void handle_approaching_state(PathWithLaneId * path); + void handle_inside_no_drivable_lane_state(PathWithLaneId * path); + void handle_stopped_state(PathWithLaneId * path); void initialize_debug_data( const lanelet::Lanelet & no_drivable_lane, const geometry_msgs::msg::Point & ego_pos); }; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index bac91d6ca92fd..3769aed71a1ec 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -52,7 +52,7 @@ NoStoppingAreaModule::NoStoppingAreaModule( state_machine_.setMarginTime(planner_param_.state_clear_time); } -bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path) { // Store original path const auto original_path = *path; @@ -64,7 +64,6 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason // Reset data debug_data_ = no_stopping_area::DebugData(); debug_data_.base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; - *stop_reason = planning_utils::initializeStopReason(StopReason::NO_STOPPING_AREA); const no_stopping_area::EgoData ego_data(*planner_data_); @@ -142,27 +141,11 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason // Create StopReason { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = stop_point->second; - stop_factor.stop_factor_points = debug_data_.stuck_points; - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor_.set( path->points, planner_data_->current_odometry->pose, stop_point->second, VelocityFactor::UNKNOWN); } - // Create legacy StopReason - { - const double stop_path_point_distance = autoware::motion_utils::calcSignedArcLength( - path->points, 0, stop_pose.position, stop_point->first); - - if ( - !first_stop_path_point_distance_ || - stop_path_point_distance < first_stop_path_point_distance_.value()) { - debug_data_.first_stop_pose = stop_point->second; - first_stop_path_point_distance_ = stop_path_point_distance; - } - } } else if (state_machine_.getState() == StateMachine::State::GO) { // reset pass judge if current state is go pass_judge_.is_stoppable = true; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp index 886c8f04dc702..51f3a0d261ebd 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp @@ -59,7 +59,7 @@ class NoStoppingAreaModule : public SceneModuleInterface const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); - bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + bool modifyPathVelocity(PathWithLaneId * path) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; autoware::motion_utils::VirtualWalls createVirtualWalls() override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp index 24b9d10e09830..dfc2dca7afc21 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp @@ -82,8 +82,7 @@ OcclusionSpotModule::OcclusionSpotModule( } } -bool OcclusionSpotModule::modifyPathVelocity( - PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) +bool OcclusionSpotModule::modifyPathVelocity(PathWithLaneId * path) { if (param_.is_show_processing_time) stop_watch_.tic("total_processing_time"); debug_data_.resetData(); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp index b83051fb6b6ec..35c409a9c459b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp @@ -53,7 +53,7 @@ class OcclusionSpotModule : public SceneModuleInterface /** * @brief plan occlusion spot velocity at unknown area in occupancy grid */ - bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + bool modifyPathVelocity(PathWithLaneId * path) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; autoware::motion_utils::VirtualWalls createVirtualWalls() override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp index 5f4d0606abfc6..2b921028c499a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp @@ -79,8 +79,6 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio // Publishers path_pub_ = this->create_publisher("~/output/path", 1); - stop_reason_diag_pub_ = - this->create_publisher("~/output/stop_reason", 1); debug_viz_pub_ = this->create_publisher("~/debug/path", 1); // Parameters @@ -327,7 +325,6 @@ void BehaviorVelocityPlannerNode::onTrigger( path_pub_->publish(output_path_msg); published_time_publisher_->publish_if_subscribed(path_pub_, output_path_msg.header.stamp); - stop_reason_diag_pub_->publish(planner_manager_.getStopReasonDiag()); if (debug_viz_pub_->get_subscription_count() > 0) { publishDebugMarker(output_path_msg); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp index d72b90d41e8f4..e260f582aae60 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp @@ -113,7 +113,6 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node // publisher rclcpp::Publisher::SharedPtr path_pub_; - rclcpp::Publisher::SharedPtr stop_reason_diag_pub_; rclcpp::Publisher::SharedPtr debug_viz_pub_; void publishDebugMarker(const autoware_planning_msgs::msg::Path & path); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp index 05aa4868249f3..4820c340058ff 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp @@ -25,34 +25,6 @@ namespace autoware::behavior_velocity_planner { -namespace -{ -std::string jsonDumpsPose(const geometry_msgs::msg::Pose & pose) -{ - const std::string json_dumps_pose = - (boost::format( - R"({"position":{"x":%lf,"y":%lf,"z":%lf},"orientation":{"w":%lf,"x":%lf,"y":%lf,"z":%lf}})") % - pose.position.x % pose.position.y % pose.position.z % pose.orientation.w % pose.orientation.x % - pose.orientation.y % pose.orientation.z) - .str(); - return json_dumps_pose; -} - -diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( - const std::string & stop_reason, const geometry_msgs::msg::Pose & stop_pose) -{ - diagnostic_msgs::msg::DiagnosticStatus stop_reason_diag; - diagnostic_msgs::msg::KeyValue stop_reason_diag_kv; - stop_reason_diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - stop_reason_diag.name = "stop_reason"; - stop_reason_diag.message = stop_reason; - stop_reason_diag_kv.key = "stop_pose"; - stop_reason_diag_kv.value = jsonDumpsPose(stop_pose); - stop_reason_diag.values.push_back(stop_reason_diag_kv); - return stop_reason_diag; -} -} // namespace - BehaviorVelocityPlannerManager::BehaviorVelocityPlannerManager() : plugin_loader_( "autoware_behavior_velocity_planner", "autoware::behavior_velocity_planner::PluginInterface") @@ -107,34 +79,12 @@ tier4_planning_msgs::msg::PathWithLaneId BehaviorVelocityPlannerManager::planPat { tier4_planning_msgs::msg::PathWithLaneId output_path_msg = input_path_msg; - double first_stop_path_point_distance = - autoware::motion_utils::calcArcLength(output_path_msg.points); - std::string stop_reason_msg("path_end"); - for (const auto & plugin : scene_manager_plugins_) { plugin->updateSceneModuleInstances(planner_data, input_path_msg); plugin->plan(&output_path_msg); - const std::optional firstStopPathPointDistance = - plugin->getFirstStopPathPointDistance(); - - if (firstStopPathPointDistance.has_value()) { - if (firstStopPathPointDistance.value() < first_stop_path_point_distance) { - first_stop_path_point_distance = firstStopPathPointDistance.value(); - stop_reason_msg = plugin->getModuleName(); - } - } } - const geometry_msgs::msg::Pose stop_pose = autoware::motion_utils::calcInterpolatedPose( - output_path_msg.points, first_stop_path_point_distance); - - stop_reason_diag_ = makeStopReasonDiag(stop_reason_msg, stop_pose); - return output_path_msg; } -diagnostic_msgs::msg::DiagnosticStatus BehaviorVelocityPlannerManager::getStopReasonDiag() const -{ - return stop_reason_diag_; -} } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.hpp index abeab16dd71cb..fddd658cef06e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.hpp @@ -46,14 +46,12 @@ class BehaviorVelocityPlannerManager void launchScenePlugin(rclcpp::Node & node, const std::string & name); void removeScenePlugin(rclcpp::Node & node, const std::string & name); + // cppcheck-suppress functionConst tier4_planning_msgs::msg::PathWithLaneId planPathVelocity( const std::shared_ptr & planner_data, const tier4_planning_msgs::msg::PathWithLaneId & input_path_msg); - diagnostic_msgs::msg::DiagnosticStatus getStopReasonDiag() const; - private: - diagnostic_msgs::msg::DiagnosticStatus stop_reason_diag_; pluginlib::ClassLoader plugin_loader_; std::vector> scene_manager_plugins_; }; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_interface.hpp index 5f6549f1e4492..4867b14cbb806 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_interface.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_interface.hpp @@ -34,7 +34,6 @@ class PluginInterface virtual void updateSceneModuleInstances( const std::shared_ptr & planner_data, const tier4_planning_msgs::msg::PathWithLaneId & path) = 0; - virtual std::optional getFirstStopPathPointDistance() = 0; virtual const char * getModuleName() = 0; }; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_wrapper.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_wrapper.hpp index 0392bf24d3a36..44a00072a0be4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_wrapper.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_wrapper.hpp @@ -38,10 +38,6 @@ class PluginWrapper : public PluginInterface { scene_manager_->updateSceneModuleInstances(planner_data, path); } - std::optional getFirstStopPathPointDistance() override - { - return scene_manager_->getFirstStopPathPointDistance(); - } const char * getModuleName() override { return scene_manager_->getModuleName(); } private: diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp index e39cfb3a5144d..5725ebe51b4a5 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp @@ -30,8 +30,6 @@ #include #include #include -#include -#include #include #include #include @@ -61,8 +59,6 @@ using autoware::universe_utils::getOrDeclareParameter; using builtin_interfaces::msg::Time; using tier4_debug_msgs::msg::Float64Stamped; using tier4_planning_msgs::msg::PathWithLaneId; -using tier4_planning_msgs::msg::StopFactor; -using tier4_planning_msgs::msg::StopReason; using tier4_rtc_msgs::msg::Module; using tier4_rtc_msgs::msg::State; using unique_identifier_msgs::msg::UUID; @@ -87,7 +83,7 @@ class SceneModuleInterface const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock); virtual ~SceneModuleInterface() = default; - virtual bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) = 0; + virtual bool modifyPathVelocity(PathWithLaneId * path) = 0; virtual visualization_msgs::msg::MarkerArray createDebugMarkerArray() = 0; virtual std::vector createVirtualWalls() = 0; @@ -116,8 +112,6 @@ class SceneModuleInterface std::shared_ptr getTimeKeeper() { return time_keeper_; } - std::optional getFirstStopPathPointDistance() { return first_stop_path_point_distance_; } - void setActivation(const bool activated) { activated_ = activated; } void setRTCEnabled(const bool enable_rtc) { rtc_enabled_ = enable_rtc; } bool isActivated() const { return activated_; } @@ -139,7 +133,6 @@ class SceneModuleInterface rclcpp::Clock::SharedPtr clock_; std::shared_ptr planner_data_; std::optional infrastructure_command_; - std::optional first_stop_path_point_distance_; autoware::motion_utils::VelocityFactorInterface velocity_factor_; std::vector objects_of_interest_; mutable std::shared_ptr time_keeper_; @@ -174,8 +167,6 @@ class SceneModuleManagerInterface virtual const char * getModuleName() = 0; - std::optional getFirstStopPathPointDistance() { return first_stop_path_point_distance_; } - void updateSceneModuleInstances( const std::shared_ptr & planner_data, const tier4_planning_msgs::msg::PathWithLaneId & path); @@ -208,7 +199,6 @@ class SceneModuleManagerInterface std::shared_ptr planner_data_; autoware::motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator_; - std::optional first_stop_path_point_distance_; rclcpp::Node & node_; rclcpp::Clock::SharedPtr clock_; // Debug @@ -217,7 +207,6 @@ class SceneModuleManagerInterface rclcpp::Publisher::SharedPtr pub_virtual_wall_; rclcpp::Publisher::SharedPtr pub_debug_; rclcpp::Publisher::SharedPtr pub_debug_path_; - rclcpp::Publisher::SharedPtr pub_stop_reason_; rclcpp::Publisher::SharedPtr pub_velocity_factor_; rclcpp::Publisher::SharedPtr diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp index 5f1c17ea1b815..4cbf820268df5 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp @@ -21,7 +21,6 @@ #include #include #include -#include #include #include @@ -65,8 +64,6 @@ using Polygons2d = std::vector; using autoware_perception_msgs::msg::PredictedObjects; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; -using tier4_planning_msgs::msg::StopFactor; -using tier4_planning_msgs::msg::StopReason; namespace planning_utils { @@ -114,10 +111,6 @@ double findReachTime( const double jerk, const double accel, const double velocity, const double distance, const double t_min, const double t_max); -StopReason initializeStopReason(const std::string & stop_reason); - -void appendStopReason(const StopFactor stop_factor, StopReason * stop_reason); - std::vector toRosPoints(const PredictedObjects & object); LineString2d extendLine( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp index 2d014d4bbf81d..fb19c4955c12a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp @@ -67,8 +67,6 @@ SceneModuleManagerInterface::SceneModuleManagerInterface( std::string("~/virtual_wall/") + module_name, 5); pub_velocity_factor_ = node.create_publisher( std::string("/planning/velocity_factors/") + module_name, 1); - pub_stop_reason_ = - node.create_publisher("~/output/stop_reasons", 1); pub_infrastructure_commands_ = node.create_publisher( "~/output/infrastructure_commands", 1); @@ -107,40 +105,28 @@ void SceneModuleManagerInterface::modifyPathVelocity( StopWatch stop_watch; stop_watch.tic("Total"); visualization_msgs::msg::MarkerArray debug_marker_array; - tier4_planning_msgs::msg::StopReasonArray stop_reason_array; autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array; - stop_reason_array.header.frame_id = "map"; - stop_reason_array.header.stamp = clock_->now(); velocity_factor_array.header.frame_id = "map"; velocity_factor_array.header.stamp = clock_->now(); tier4_v2x_msgs::msg::InfrastructureCommandArray infrastructure_command_array; infrastructure_command_array.stamp = clock_->now(); - first_stop_path_point_distance_ = autoware::motion_utils::calcArcLength(path->points); for (const auto & scene_module : scene_modules_) { - tier4_planning_msgs::msg::StopReason stop_reason; scene_module->resetVelocityFactor(); scene_module->setPlannerData(planner_data_); - scene_module->modifyPathVelocity(path, &stop_reason); + scene_module->modifyPathVelocity(path); // The velocity factor must be called after modifyPathVelocity. const auto velocity_factor = scene_module->getVelocityFactor(); if (velocity_factor.behavior != PlanningBehavior::UNKNOWN) { velocity_factor_array.factors.emplace_back(velocity_factor); } - if (stop_reason.reason != "") { - stop_reason_array.stop_reasons.emplace_back(stop_reason); - } if (const auto command = scene_module->getInfrastructureCommand()) { infrastructure_command_array.commands.push_back(*command); } - if (scene_module->getFirstStopPathPointDistance() < first_stop_path_point_distance_) { - first_stop_path_point_distance_ = scene_module->getFirstStopPathPointDistance(); - } - for (const auto & marker : scene_module->createDebugMarkerArray().markers) { debug_marker_array.markers.push_back(marker); } @@ -148,9 +134,6 @@ void SceneModuleManagerInterface::modifyPathVelocity( virtual_wall_marker_creator_.add_virtual_walls(scene_module->createVirtualWalls()); } - if (!stop_reason_array.stop_reasons.empty()) { - pub_stop_reason_->publish(stop_reason_array); - } pub_velocity_factor_->publish(velocity_factor_array); pub_infrastructure_commands_->publish(infrastructure_command_array); pub_debug_->publish(debug_marker_array); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp index ccb2660dbf3c6..88186a5b904b0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp @@ -490,18 +490,6 @@ double calcDecelerationVelocityFromDistanceToTarget( return current_velocity; } -StopReason initializeStopReason(const std::string & stop_reason) -{ - StopReason stop_reason_msg; - stop_reason_msg.reason = stop_reason; - return stop_reason_msg; -} - -void appendStopReason(const StopFactor stop_factor, StopReason * stop_reason) -{ - stop_reason->stop_factors.emplace_back(stop_factor); -} - std::vector toRosPoints(const PredictedObjects & object) { std::vector points; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp index c3f91d0506388..32bf81ed63598 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp @@ -63,8 +63,7 @@ void RunOutModule::setPlannerParam(const PlannerParam & planner_param) planner_param_ = planner_param; } -bool RunOutModule::modifyPathVelocity( - PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) +bool RunOutModule::modifyPathVelocity(PathWithLaneId * path) { // timer starts const auto t_start = std::chrono::system_clock::now(); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp index 4f326aef298be..3673a215bb749 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp @@ -49,7 +49,7 @@ class RunOutModule : public SceneModuleInterface std::unique_ptr dynamic_obstacle_creator, const std::shared_ptr & debug_ptr, const rclcpp::Clock::SharedPtr clock); - bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + bool modifyPathVelocity(PathWithLaneId * path) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; autoware::motion_utils::VirtualWalls createVirtualWalls() override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp index dfe5d4de85300..2ac8f1c477620 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp @@ -70,8 +70,7 @@ SpeedBumpModule::SpeedBumpModule( } } -bool SpeedBumpModule::modifyPathVelocity( - PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) +bool SpeedBumpModule::modifyPathVelocity(PathWithLaneId * path) { if (path->points.empty()) { return false; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp index b909dc80689d5..176a01d5112c5 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp @@ -56,7 +56,7 @@ class SpeedBumpModule : public SceneModuleInterface const lanelet::autoware::SpeedBump & speed_bump_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); - bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + bool modifyPathVelocity(PathWithLaneId * path) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; autoware::motion_utils::VirtualWalls createVirtualWalls() override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp index b83a274f99513..8a06b9ba5f82d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp @@ -46,7 +46,7 @@ StopLineModule::StopLineModule( velocity_factor_.init(PlanningBehavior::STOP_SIGN); } -bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +bool StopLineModule::modifyPathVelocity(PathWithLaneId * path) { auto trajectory = trajectory::Trajectory::Builder{}.build( @@ -74,8 +74,6 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop geometry_msgs::msg::Pose stop_pose = trajectory->compute(*stop_point).point.pose; - updateStopReason(stop_reason, stop_pose); - updateDebugData(&debug_data_, stop_pose, state_); return true; @@ -177,15 +175,6 @@ void StopLineModule::updateVelocityFactor( } } -void StopLineModule::updateStopReason( - StopReason * stop_reason, const geometry_msgs::msg::Pose & stop_pose) const -{ - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = stop_pose; - stop_factor.stop_factor_points.push_back(getCenterOfStopLine(stop_line_)); - planning_utils::appendStopReason(stop_factor, stop_reason); -} - void StopLineModule::updateDebugData( DebugData * debug_data, const geometry_msgs::msg::Pose & stop_pose, const State & state) const { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp index cb5bf339f8846..9ac1463da8618 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp @@ -70,7 +70,7 @@ class StopLineModule : public SceneModuleInterface const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); - bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + bool modifyPathVelocity(PathWithLaneId * path) override; /** * @brief Calculate ego position and stop point. @@ -99,8 +99,6 @@ class StopLineModule : public SceneModuleInterface autoware::motion_utils::VelocityFactorInterface * velocity_factor, const State & state, const double & distance_to_stop_point); - void updateStopReason(StopReason * stop_reason, const geometry_msgs::msg::Pose & stop_pose) const; - void updateDebugData( DebugData * debug_data, const geometry_msgs::msg::Pose & stop_pose, const State & state) const; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp index 0394b0c9e381f..3ce8502baaf63 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp @@ -42,8 +42,7 @@ autoware::motion_utils::VirtualWalls TemplateModule::createVirtualWalls() return vw; } -bool TemplateModule::modifyPathVelocity( - [[maybe_unused]] PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) +bool TemplateModule::modifyPathVelocity([[maybe_unused]] PathWithLaneId * path) { RCLCPP_INFO_ONCE(logger_, "Template Module is executing!"); return false; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp index 9789ee37aa669..70cd5cb1235e7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp @@ -38,10 +38,9 @@ class TemplateModule : public SceneModuleInterface * specific criteria. * * @param path A pointer to the path containing points to be modified. - * @param stop_reason A pointer to the stop reason data. * @return [bool] wether the path velocity was modified or not. */ - bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + bool modifyPathVelocity(PathWithLaneId * path) override; /** * @brief Create a visualization of debug markers. diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp index 0d7c77d5e1b59..b6747724ba6f7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp @@ -57,34 +57,20 @@ void TrafficLightModuleManager::modifyPathVelocity(tier4_planning_msgs::msg::Pat velocity_factor_array.header.frame_id = "map"; velocity_factor_array.header.stamp = clock_->now(); - tier4_planning_msgs::msg::StopReasonArray stop_reason_array; - stop_reason_array.header.frame_id = "map"; - stop_reason_array.header.stamp = path->header.stamp; - - first_stop_path_point_distance_ = autoware::motion_utils::calcArcLength(path->points); nearest_ref_stop_path_point_index_ = static_cast(path->points.size() - 1); for (const auto & scene_module : scene_modules_) { - tier4_planning_msgs::msg::StopReason stop_reason; std::shared_ptr traffic_light_scene_module( std::dynamic_pointer_cast(scene_module)); traffic_light_scene_module->resetVelocityFactor(); traffic_light_scene_module->setPlannerData(planner_data_); - traffic_light_scene_module->modifyPathVelocity(path, &stop_reason); + traffic_light_scene_module->modifyPathVelocity(path); // The velocity factor must be called after modifyPathVelocity. const auto velocity_factor = traffic_light_scene_module->getVelocityFactor(); if (velocity_factor.behavior != PlanningBehavior::UNKNOWN) { velocity_factor_array.factors.emplace_back(velocity_factor); } - if (stop_reason.reason != "") { - stop_reason_array.stop_reasons.emplace_back(stop_reason); - } - if ( - traffic_light_scene_module->getFirstStopPathPointDistance() < - first_stop_path_point_distance_) { - first_stop_path_point_distance_ = traffic_light_scene_module->getFirstStopPathPointDistance(); - } if ( traffic_light_scene_module->getFirstRefStopPathPointIndex() < nearest_ref_stop_path_point_index_) { @@ -102,9 +88,6 @@ void TrafficLightModuleManager::modifyPathVelocity(tier4_planning_msgs::msg::Pat virtual_wall_marker_creator_.add_virtual_walls( traffic_light_scene_module->createVirtualWalls()); } - if (!stop_reason_array.stop_reasons.empty()) { - pub_stop_reason_->publish(stop_reason_array); - } pub_velocity_factor_->publish(velocity_factor_array); pub_debug_->publish(debug_marker_array); pub_virtual_wall_->publish(virtual_wall_marker_creator_.create_markers(clock_->now())); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp index c59071f2043bb..76f1a19bfc69a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp @@ -54,13 +54,11 @@ TrafficLightModule::TrafficLightModule( planner_param_ = planner_param; } -bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path) { debug_data_ = DebugData(); debug_data_.base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; - first_stop_path_point_distance_ = autoware::motion_utils::calcArcLength(path->points); first_ref_stop_path_point_index_ = static_cast(path->points.size()) - 1; - *stop_reason = planning_utils::initializeStopReason(StopReason::TRAFFIC_LIGHT); const auto input_path = *path; @@ -133,8 +131,7 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // Decide whether to stop or pass even if a stop signal is received. if (!isPassthrough(signed_arc_length_to_stop_point)) { - *path = - insertStopPose(input_path, stop_line.value().first, stop_line.value().second, stop_reason); + *path = insertStopPose(input_path, stop_line.value().first, stop_line.value().second); is_prev_state_stop_ = true; } return true; @@ -275,7 +272,7 @@ bool TrafficLightModule::isTrafficSignalTimedOut() const tier4_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopPose( const tier4_planning_msgs::msg::PathWithLaneId & input, const size_t & insert_target_point_idx, - const Eigen::Vector2d & target_point, tier4_planning_msgs::msg::StopReason * stop_reason) + const Eigen::Vector2d & target_point) { tier4_planning_msgs::msg::PathWithLaneId modified_path; modified_path = input; @@ -292,24 +289,9 @@ tier4_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopPose( size_t insert_index = insert_target_point_idx; planning_utils::insertVelocity(modified_path, target_point_with_lane_id, 0.0, insert_index); - const double target_velocity_point_distance = autoware::motion_utils::calcArcLength(std::vector( - modified_path.points.begin(), modified_path.points.begin() + target_velocity_point_idx)); - if (target_velocity_point_distance < first_stop_path_point_distance_) { - first_stop_path_point_distance_ = target_velocity_point_distance; - debug_data_.first_stop_pose = target_point_with_lane_id.point.pose; - } - - // Get stop point and stop factor - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = debug_data_.first_stop_pose; - if (debug_data_.highest_confidence_traffic_light_point != std::nullopt) { - stop_factor.stop_factor_points = std::vector{ - debug_data_.highest_confidence_traffic_light_point.value()}; - } velocity_factor_.set( modified_path.points, planner_data_->current_odometry->pose, target_point_with_lane_id.point.pose, VelocityFactor::UNKNOWN); - planning_utils::appendStopReason(stop_factor, stop_reason); return modified_path; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp index e35d9f86fbe46..8221bb3740273 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp @@ -71,7 +71,7 @@ class TrafficLightModule : public SceneModuleInterface lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); - bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + bool modifyPathVelocity(PathWithLaneId * path) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; autoware::motion_utils::VirtualWalls createVirtualWalls() override; @@ -90,7 +90,7 @@ class TrafficLightModule : public SceneModuleInterface tier4_planning_msgs::msg::PathWithLaneId insertStopPose( const tier4_planning_msgs::msg::PathWithLaneId & input, const size_t & insert_target_point_idx, - const Eigen::Vector2d & target_point, tier4_planning_msgs::msg::StopReason * stop_reason); + const Eigen::Vector2d & target_point); bool isPassthrough(const double & signed_arc_length) const; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp index e60e2a42e3a47..206fb14b6d41c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp @@ -97,11 +97,10 @@ VirtualTrafficLightModule::VirtualTrafficLightModule( logger_ = logger_.get_child((map_data_.instrument_type + "_" + map_data_.instrument_id).c_str()); } -bool VirtualTrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +bool VirtualTrafficLightModule::modifyPathVelocity(PathWithLaneId * path) { // Initialize setInfrastructureCommand({}); - *stop_reason = planning_utils::initializeStopReason(StopReason::VIRTUAL_TRAFFIC_LIGHT); module_data_ = {}; @@ -150,7 +149,7 @@ bool VirtualTrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopRe // Stop at stop_line if no message received if (!virtual_traffic_light_state) { RCLCPP_DEBUG(logger_, "no message received"); - insertStopVelocityAtStopLine(path, stop_reason, end_line_idx); + insertStopVelocityAtStopLine(path, end_line_idx); updateInfrastructureCommand(); return true; } @@ -158,7 +157,7 @@ bool VirtualTrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopRe // Stop at stop_line if no right is given if (!hasRightOfWay(*virtual_traffic_light_state)) { RCLCPP_DEBUG(logger_, "no right is given"); - insertStopVelocityAtStopLine(path, stop_reason, end_line_idx); + insertStopVelocityAtStopLine(path, end_line_idx); updateInfrastructureCommand(); return true; } @@ -167,7 +166,7 @@ bool VirtualTrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopRe if (isBeforeStopLine(end_line_idx)) { if (isStateTimeout(*virtual_traffic_light_state)) { RCLCPP_DEBUG(logger_, "state is timeout before stop line"); - insertStopVelocityAtStopLine(path, stop_reason, end_line_idx); + insertStopVelocityAtStopLine(path, end_line_idx); } updateInfrastructureCommand(); @@ -181,7 +180,7 @@ bool VirtualTrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopRe if ( planner_param_.check_timeout_after_stop_line && isStateTimeout(*virtual_traffic_light_state)) { RCLCPP_DEBUG(logger_, "state is timeout after stop line"); - insertStopVelocityAtStopLine(path, stop_reason, end_line_idx); + insertStopVelocityAtStopLine(path, end_line_idx); updateInfrastructureCommand(); return true; } @@ -189,7 +188,7 @@ bool VirtualTrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopRe // Stop at stop_line if finalization isn't completed if (!virtual_traffic_light_state->is_finalized) { RCLCPP_DEBUG(logger_, "finalization isn't completed"); - insertStopVelocityAtEndLine(path, stop_reason, end_line_idx); + insertStopVelocityAtEndLine(path, end_line_idx); if (isNearAnyEndLine(end_line_idx) && planner_data_->isVehicleStopped()) { state_ = State::FINALIZING; @@ -207,15 +206,6 @@ void VirtualTrafficLightModule::updateInfrastructureCommand() setInfrastructureCommand(command_); } -void VirtualTrafficLightModule::setStopReason( - const geometry_msgs::msg::Pose & stop_pose, tier4_planning_msgs::msg::StopReason * stop_reason) -{ - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = stop_pose; - stop_factor.stop_factor_points.push_back(toMsg(map_data_.instrument_center)); - planning_utils::appendStopReason(stop_factor, stop_reason); -} - std::optional VirtualTrafficLightModule::getPathIndexOfFirstEndLine() { std::optional min_seg_idx; @@ -376,8 +366,7 @@ bool VirtualTrafficLightModule::hasRightOfWay( } void VirtualTrafficLightModule::insertStopVelocityAtStopLine( - tier4_planning_msgs::msg::PathWithLaneId * path, - tier4_planning_msgs::msg::StopReason * stop_reason, const size_t end_line_idx) + tier4_planning_msgs::msg::PathWithLaneId * path, const size_t end_line_idx) { const auto collision = findLastCollisionBeforeEndLine(path->points, *map_data_.stop_line, end_line_idx); @@ -428,7 +417,6 @@ void VirtualTrafficLightModule::insertStopVelocityAtStopLine( } // Set StopReason - setStopReason(stop_pose, stop_reason); velocity_factor_.set( path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN, command_.type); @@ -439,8 +427,7 @@ void VirtualTrafficLightModule::insertStopVelocityAtStopLine( } void VirtualTrafficLightModule::insertStopVelocityAtEndLine( - tier4_planning_msgs::msg::PathWithLaneId * path, - tier4_planning_msgs::msg::StopReason * stop_reason, const size_t end_line_idx) + tier4_planning_msgs::msg::PathWithLaneId * path, const size_t end_line_idx) { const auto collision = findLastCollisionBeforeEndLine(path->points, map_data_.end_lines, end_line_idx); @@ -463,7 +450,6 @@ void VirtualTrafficLightModule::insertStopVelocityAtEndLine( } // Set StopReason - setStopReason(stop_pose, stop_reason); velocity_factor_.set( path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp index e4e58e4288354..b031ba5b2bb2b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp @@ -79,7 +79,7 @@ class VirtualTrafficLightModule : public SceneModuleInterface const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); - bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + bool modifyPathVelocity(PathWithLaneId * path) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; autoware::motion_utils::VirtualWalls createVirtualWalls() override; @@ -96,8 +96,6 @@ class VirtualTrafficLightModule : public SceneModuleInterface void updateInfrastructureCommand(); - void setStopReason(const Pose & stop_pose, StopReason * stop_reason); - void setVelocityFactor( const geometry_msgs::msg::Pose & stop_pose, autoware_adapi_v1_msgs::msg::VelocityFactor * velocity_factor); @@ -119,12 +117,10 @@ class VirtualTrafficLightModule : public SceneModuleInterface bool hasRightOfWay(const tier4_v2x_msgs::msg::VirtualTrafficLightState & state); void insertStopVelocityAtStopLine( - tier4_planning_msgs::msg::PathWithLaneId * path, - tier4_planning_msgs::msg::StopReason * stop_reason, const size_t end_line_idx); + tier4_planning_msgs::msg::PathWithLaneId * path, const size_t end_line_idx); void insertStopVelocityAtEndLine( - tier4_planning_msgs::msg::PathWithLaneId * path, - tier4_planning_msgs::msg::StopReason * stop_reason, const size_t end_line_idx); + tier4_planning_msgs::msg::PathWithLaneId * path, const size_t end_line_idx); }; } // namespace autoware::behavior_velocity_planner #endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp index 765732969951d..42a235a67edd1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp @@ -81,12 +81,11 @@ std::pair WalkwayModule::getStopLine( return std::make_pair(dist_ego_to_stop, p_stop_line); } -bool WalkwayModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +bool WalkwayModule::modifyPathVelocity(PathWithLaneId * path) { const auto & base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; debug_data_ = DebugData(planner_data_); - *stop_reason = planning_utils::initializeStopReason(StopReason::WALKWAY); const auto input = *path; @@ -119,10 +118,6 @@ bool WalkwayModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_ } /* get stop point and stop factor */ - StopFactor stop_factor; - stop_factor.stop_pose = stop_pose.value(); - stop_factor.stop_factor_points.push_back(path_end_points_on_walkway->first); - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor_.set( path->points, planner_data_->current_odometry->pose, stop_pose.value(), VelocityFactor::UNKNOWN); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp index a400f57451d2e..df54eafd11cc2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp @@ -46,7 +46,7 @@ class WalkwayModule : public SceneModuleInterface const PlannerParam & planner_param, const bool use_regulatory_element, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); - bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + bool modifyPathVelocity(PathWithLaneId * path) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; autoware::motion_utils::VirtualWalls createVirtualWalls() override;