diff --git a/planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp b/planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp index cfc07b294cab9..c74e1eacaea2e 100644 --- a/planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp +++ b/planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp @@ -23,10 +23,13 @@ #include "tier4_rtc_msgs/msg/cooperate_response.hpp" #include "tier4_rtc_msgs/msg/cooperate_status.hpp" #include "tier4_rtc_msgs/msg/cooperate_status_array.hpp" +#include "tier4_rtc_msgs/msg/creep_command.hpp" +#include "tier4_rtc_msgs/msg/creep_response.hpp" #include "tier4_rtc_msgs/msg/module.hpp" #include "tier4_rtc_msgs/msg/state.hpp" #include "tier4_rtc_msgs/srv/auto_mode.hpp" #include "tier4_rtc_msgs/srv/cooperate_commands.hpp" +#include "tier4_rtc_msgs/srv/creep_commands.hpp" #include #include @@ -41,16 +44,21 @@ using tier4_rtc_msgs::msg::CooperateCommand; using tier4_rtc_msgs::msg::CooperateResponse; using tier4_rtc_msgs::msg::CooperateStatus; using tier4_rtc_msgs::msg::CooperateStatusArray; +using tier4_rtc_msgs::msg::CreepCommand; +using tier4_rtc_msgs::msg::CreepResponse; using tier4_rtc_msgs::msg::Module; using tier4_rtc_msgs::msg::State; using tier4_rtc_msgs::srv::AutoMode; using tier4_rtc_msgs::srv::CooperateCommands; +using tier4_rtc_msgs::srv::CreepCommands; using unique_identifier_msgs::msg::UUID; class RTCInterface { public: - RTCInterface(rclcpp::Node * node, const std::string & name, const bool enable_rtc = true); + RTCInterface( + rclcpp::Node * node, const std::string & name, const bool enable_rtc = true, + const bool creep_supported = false); void publishCooperateStatus(const rclcpp::Time & stamp); /// @brief update the cooperate status of the module identified by the given UUID /// @param[in] uuid unique ID of the module @@ -75,6 +83,8 @@ class RTCInterface bool isRegistered(const UUID & uuid) const; bool isRTCEnabled(const UUID & uuid) const; bool isTerminated(const UUID & uuid) const; + bool isCreepTriggered(const UUID & uuid) const; + bool isCreepSupported() const; void lockCommandUpdate(); void unlockCommandUpdate(); void print() const; @@ -85,6 +95,9 @@ class RTCInterface const CooperateCommands::Response::SharedPtr responses); void onAutoModeService( const AutoMode::Request::SharedPtr request, const AutoMode::Response::SharedPtr response); + void onCreepCommandService( + const CreepCommands::Request::SharedPtr request, + const CreepCommands::Response::SharedPtr responses); void onTimer(); std::vector validateCooperateCommands( const std::vector & commands); @@ -97,6 +110,7 @@ class RTCInterface rclcpp::Publisher::SharedPtr pub_auto_mode_status_; rclcpp::Service::SharedPtr srv_commands_; rclcpp::Service::SharedPtr srv_auto_mode_; + rclcpp::Service::SharedPtr srv_creep_commands_; rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::TimerBase::SharedPtr timer_; rclcpp::Clock::SharedPtr clock_; @@ -107,11 +121,13 @@ class RTCInterface std::vector stored_commands_; bool is_auto_mode_enabled_; bool is_locked_; + bool is_creep_supported_; std::string cooperate_status_namespace_ = "/planning/cooperate_status"; std::string auto_mode_status_namespace_ = "/planning/auto_mode_status"; std::string cooperate_commands_namespace_ = "/planning/cooperate_commands"; std::string enable_auto_mode_namespace_ = "/planning/enable_auto_mode"; + std::string creep_commands_namespace_ = "/planning/creep_commands"; mutable std::mutex mutex_; diff --git a/planning/autoware_rtc_interface/src/rtc_interface.cpp b/planning/autoware_rtc_interface/src/rtc_interface.cpp index 151f37aa3323a..d7a835de89a4e 100644 --- a/planning/autoware_rtc_interface/src/rtc_interface.cpp +++ b/planning/autoware_rtc_interface/src/rtc_interface.cpp @@ -116,11 +116,13 @@ Module getModuleType(const std::string & module_name) namespace autoware::rtc_interface { -RTCInterface::RTCInterface(rclcpp::Node * node, const std::string & name, const bool enable_rtc) +RTCInterface::RTCInterface( + rclcpp::Node * node, const std::string & name, const bool enable_rtc, const bool creep_supported) : clock_{node->get_clock()}, logger_{node->get_logger().get_child("RTCInterface[" + name + "]")}, is_auto_mode_enabled_{!enable_rtc}, - is_locked_{false} + is_locked_{false}, + is_creep_supported_{creep_supported} { using std::placeholders::_1; using std::placeholders::_2; @@ -147,6 +149,10 @@ RTCInterface::RTCInterface(rclcpp::Node * node, const std::string & name, const enable_auto_mode_namespace_ + "/" + name, std::bind(&RTCInterface::onAutoModeService, this, _1, _2), rmw_qos_profile_services_default, callback_group_); + srv_creep_commands_ = node->create_service( + creep_commands_namespace_ + "/" + name, + std::bind(&RTCInterface::onCreepCommandService, this, _1, _2), rmw_qos_profile_services_default, + callback_group_); // Module module_ = getModuleType(name); @@ -239,6 +245,51 @@ void RTCInterface::onAutoModeService( response->success = true; } +void RTCInterface::onCreepCommandService( + const CreepCommands::Request::SharedPtr request, + const CreepCommands::Response::SharedPtr responses) +{ + std::lock_guard lock(mutex_); + + for (const auto & command : request->commands) { + CreepResponse response; + response.uuid = command.uuid; + response.module = command.module; + + const auto itr = std::find_if( + registered_status_.statuses.begin(), registered_status_.statuses.end(), + [command](const auto & s) { return s.uuid == command.uuid; }); + + if (!is_creep_supported_) { + response.success = false; + RCLCPP_WARN_STREAM( + getLogger(), "[onCreepCommandService] creep is not supported." << std::endl); + responses->responses.push_back(response); + continue; + } + + if (itr != registered_status_.statuses.end()) { + if (itr->state.type == State::WAITING_FOR_EXECUTION || itr->state.type == State::RUNNING) { + itr->creep_triggered = command.creep_enable; + response.success = true; + } else { + RCLCPP_WARN_STREAM( + getLogger(), "[onCreepCommandService] uuid : " + << uuid_to_string(command.uuid) + << " state is not WAITING_FOR_EXECUTION or RUNNING. state : " + << itr->state.type << std::endl); + response.success = false; + } + } else { + RCLCPP_WARN_STREAM( + getLogger(), "[onCreepCommandService] uuid : " << uuid_to_string(command.uuid) + << " is not found." << std::endl); + response.success = false; + } + responses->responses.push_back(response); + } +} + void RTCInterface::onTimer() { AutoModeStatus auto_mode_status; @@ -268,6 +319,8 @@ void RTCInterface::updateCooperateStatus( status.safe = safe; status.requested = requested; status.command_status.type = Command::DEACTIVATE; + status.creep_supported = is_creep_supported_; + status.creep_triggered = false; status.state.type = State::WAITING_FOR_EXECUTION; status.start_distance = start_distance; status.finish_distance = finish_distance; @@ -474,6 +527,34 @@ bool RTCInterface::isTerminated(const UUID & uuid) const return is_auto_mode_enabled_; } +bool RTCInterface::isCreepTriggered(const UUID & uuid) const +{ + std::lock_guard lock(mutex_); + if (!is_creep_supported_) { + return false; + } + const auto itr = std::find_if( + registered_status_.statuses.begin(), registered_status_.statuses.end(), + [uuid](const auto & s) { return s.uuid == uuid; }); + + if (itr != registered_status_.statuses.end()) { + if (itr->state.type != State::WAITING_FOR_EXECUTION && itr->state.type != State::RUNNING) { + return false; + } + return itr->creep_triggered; + } + + RCLCPP_WARN_STREAM( + getLogger(), + "[isCreepTriggered] uuid : " << uuid_to_string(uuid) << " is not found." << std::endl); + return false; +} + +bool RTCInterface::isCreepSupported() const +{ + return is_creep_supported_; +} + void RTCInterface::lockCommandUpdate() { is_locked_ = true; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp index c38a5eb76d3f5..7c927b8be22fc 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp @@ -32,7 +32,8 @@ using lanelet::autoware::Crosswalk; CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( - node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".common.enable_rtc")) + node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".common.enable_rtc"), + true) { const std::string ns(CrosswalkModuleManager::getModuleName()); 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 c942c6b96ea9d..f46e048a4e11b 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 @@ -257,6 +257,7 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path) // Calculate stop point with margin const auto default_stop_pose = getDefaultStopPose(*path, first_path_point_on_crosswalk); + const auto deadline_stop_pose = getDeadlineStopPose(*path, first_path_point_on_crosswalk); // Resample path sparsely for less computation cost constexpr double resample_interval = 4.0; @@ -289,11 +290,25 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path) setDistanceToStop(*path, default_stop_pose, nearest_stop_factor); set_previous_stop_pose(nearest_stop_factor); + const bool is_creep_activated = isCreepTriggered(); + + // RCLCPP_INFO( + // logger_, "Crosswalk Module[%ld] creep activated: %s, deadline_stop_pose.has_value(): %s", + // getModuleId(), is_creep_activated ? "Yes" : "No", + // deadline_stop_pose.has_value() ? "Yes" : "No"); + // plan Go/Stop if (isActivated()) { planGo(*path, nearest_stop_factor); } else { - planStop(*path, nearest_stop_factor, default_stop_pose, reason); + if (is_creep_activated) { + if (deadline_stop_pose.has_value()) { + planCreeping(*path, nearest_stop_factor, deadline_stop_pose.value()); + } + // If no deadline stop pose is available, already passed the stop line, so do nothing. + } else { + planStop(*path, nearest_stop_factor, default_stop_pose, reason); + } } recordTime(4); @@ -327,6 +342,17 @@ std::optional CrosswalkModule::getDefaultStopPose( -planner_param_.stop_distance_from_crosswalk - base_link2front); } +std::optional CrosswalkModule::getDeadlineStopPose( + const PathWithLaneId & ego_path, + const geometry_msgs::msg::Point & first_path_point_on_crosswalk) const +{ + // const auto & ego_pos = planner_data_->current_odometry->pose.position; + const auto & base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + return calcLongitudinalOffsetPose( + ego_path.points, first_path_point_on_crosswalk, + -planner_param_.stop_distance_from_crosswalk_limit - base_link2front); +} + std::optional CrosswalkModule::checkStopForCrosswalkUsers( const PathWithLaneId & ego_path, const PathWithLaneId & sparse_resample_path, const geometry_msgs::msg::Point & first_path_point_on_crosswalk, @@ -507,7 +533,14 @@ std::optional CrosswalkModule::calcStopPose( p.min_jerk_for_no_stop_decision); return strong_brake_dist_opt ? strong_brake_dist_opt.value() : 0.0; }(); - if (p.enable_no_stop_decision && std::max(selected_stop.dist, 0.1) < strong_brake_dist) { + + const bool is_abandon_to_stop = + (p.enable_no_stop_decision && std::max(selected_stop.dist, 0.1) < strong_brake_dist); + const bool is_abandon_to_stop_for_creeping = + (std::max(selected_stop.dist, 0.1) < strong_brake_dist && !previous_stop_pose_.has_value()); + if ( + (!isCreepTriggered() && is_abandon_to_stop) || + (isCreepTriggered() && is_abandon_to_stop_for_creeping)) { RCLCPP_INFO_THROTTLE( logger_, *clock_, 1000, "Abandon to stop. " @@ -1525,6 +1558,87 @@ void CrosswalkModule::setDistanceToStop( } } +void CrosswalkModule::planCreeping( + PathWithLaneId & ego_path, const std::optional & nearest_stop_factor, + geometry_msgs::msg::Pose deadline_stop_pose) +{ + if (!nearest_stop_factor.has_value()) { + return; + } + + const double creep_velocity = 3.0 / 3.6; // 3km/h + + const bool has_moving_object = [&]() { + const double moving_threshold = planner_param_.stop_object_velocity; + const auto & objects = planner_data_->predicted_objects->objects; + for (const auto & uuid : nearest_stop_factor->target_object_ids) { + for (const auto & object : objects) { + if (object.object_id != uuid) { + continue; + } + const auto & twist = object.kinematics.initial_twist_with_covariance.twist.linear; + if (std::hypot(twist.x, twist.y) > moving_threshold) { + return true; + } + break; + } + } + return false; + }(); + if (has_moving_object) { + yield_stuck_ = true; + } + + if (!yield_stuck_) { + const auto & ego_pos = planner_data_->current_odometry->pose.position; + const auto ego_vel = std::max(0.0, planner_data_->current_velocity->twist.linear.x); + max_ego_speed_from_creep_triggered_ = std::max(max_ego_speed_from_creep_triggered_, ego_vel); + + // calculate distance to deadline stop pose + const auto dist_to_deadline_stop = + calcSignedArcLength(ego_path.points, ego_pos, deadline_stop_pose.position); + + // calculate braking distance + const auto & p = planner_param_; + const auto braking_distance = calcDecelDistWithJerkAndAccConstraints( + max_ego_speed_from_creep_triggered_, 0.0, 0.0, p.min_acc_preferred, 10.0, + p.min_jerk_preferred); + + RCLCPP_DEBUG( + logger_, "dist_to_deadline_stop: %.2f m, braking_distance: %.2f m", dist_to_deadline_stop, + braking_distance.has_value() ? braking_distance.value() : -1.0); + + // if braking distance is greater than distance to deadline stop pose, can't stop anymore + const auto allow_creep_pass = !rtc_enabled_; + + // NOTE(odashima): add a margin to prevent deceleration near the pass judge point + // caused by latency, discretized path interval, etc. + const double creep_pass_margin = 0.2; // [m] + if ( + allow_creep_pass && braking_distance.has_value() && + braking_distance.value() + creep_pass_margin > dist_to_deadline_stop) { + passed_pass_judge_ = true; + RCLCPP_DEBUG(logger_, "creep pass judge passed"); + } + } + + if (passed_pass_judge_) { + return; + } else { + insertDecelPointWithDebugInfo( + nearest_stop_factor->stop_pose.position, creep_velocity, ego_path); + insertDecelPointWithDebugInfo(deadline_stop_pose.position, 0.0, ego_path); + + // Add planning factor + const SafetyFactorArray safety_factors = createSafetyFactorArray(nearest_stop_factor); + planning_factor_interface_->add( + ego_path.points, planner_data_->current_odometry->pose, deadline_stop_pose, + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, safety_factors, + true /*is_driving_forward*/, 0.0 /*velocity*/, 0.0 /*shift distance*/, + yield_stuck_ ? "yield" : "creeping"); + } +} + void CrosswalkModule::planGo( PathWithLaneId & ego_path, const std::optional & stop_factor) const { 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 4e016a2e960ad..aa12ca717c43d 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 @@ -408,6 +408,9 @@ class CrosswalkModule : public SceneModuleInterfaceWithRTC std::optional getDefaultStopPose( const PathWithLaneId & ego_path, const geometry_msgs::msg::Point & first_path_point_on_crosswalk) const; + std::optional getDeadlineStopPose( + const PathWithLaneId & ego_path, + const geometry_msgs::msg::Point & first_path_point_on_crosswalk) const; std::optional calcStopPose( const PathWithLaneId & ego_path, double dist_nearest_cp, @@ -451,6 +454,10 @@ class CrosswalkModule : public SceneModuleInterfaceWithRTC const std::optional & default_stop_pose, const std::optional & stop_factor); + void planCreeping( + PathWithLaneId & ego_path, const std::optional & nearest_stop_factor, + geometry_msgs::msg::Pose deadline_stop_pose); + void planGo( PathWithLaneId & ego_path, const std::optional & stop_factor) const; @@ -520,7 +527,7 @@ class CrosswalkModule : public SceneModuleInterfaceWithRTC void set_previous_stop_pose(const std::optional & current_stop_pose) { - if (!current_stop_pose) { + if (!current_stop_pose && !isCreepTriggered()) { previous_stop_pose_.reset(); return; } @@ -561,6 +568,10 @@ class CrosswalkModule : public SceneModuleInterfaceWithRTC std::optional current_initial_occlusion_time_; std::optional most_recent_occlusion_time_; + double max_ego_speed_from_creep_triggered_{0.0}; + bool yield_stuck_{false}; + bool passed_pass_judge_{false}; + std::optional previous_stop_pose_; struct diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/config/intersection.param.yaml index a32e7c809952f..7251640f29049 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/config/intersection.param.yaml @@ -13,6 +13,7 @@ max_accel: -2.8 max_jerk: -5.0 delay_response_time: 0.5 + creep_stopline_margin: 1.0 stuck_vehicle: target_type: diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_stoplines.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_stoplines.hpp index e49c23b620cf0..e98f50f567fda 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_stoplines.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_stoplines.hpp @@ -59,6 +59,8 @@ struct IntersectionStopLines */ size_t first_attention_stopline{0}; + size_t creep_stopline{0}; + /** * occlusion_peeking_stopline is * - after `first_attention_stopline` by `peeking_offset` if it is feasible (E) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp index d257b679c6e8e..e431b9e468de4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp @@ -36,10 +36,11 @@ using autoware_utils::get_or_declare_parameter; IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( node, getModuleName(), - getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc.intersection")), + getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc.intersection"), true), occlusion_rtc_interface_( &node, "intersection_occlusion", - getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc.intersection_to_occlusion")) + getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc.intersection_to_occlusion"), + true) { const std::string ns(IntersectionModuleManager::getModuleName()); auto & ip = intersection_param_; @@ -66,6 +67,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) ip.common.max_jerk = get_or_declare_parameter(node, ns + ".common.max_jerk"); ip.common.delay_response_time = get_or_declare_parameter(node, ns + ".common.delay_response_time"); + ip.common.creep_stopline_margin = + get_or_declare_parameter(node, ns + ".common.creep_stopline_margin"); } // stuck @@ -379,6 +382,7 @@ void IntersectionModuleManager::launchNewModules( occlusion_uuid, true, State::WAITING_FOR_EXECUTION, std::numeric_limits::lowest(), std::numeric_limits::lowest(), clock_->now(), false, override_occlusion_rtc_auto_mode); + registerModule(std::move(new_module)); } } @@ -478,6 +482,10 @@ void IntersectionModuleManager::setActivation() scene_module->setActivation(rtc_interface_.isActivated(getUUID(scene_module->getModuleId()))); intersection_module->setOcclusionActivation( occlusion_rtc_interface_.isActivated(occlusion_uuid)); + intersection_module->setIntersectionCreepActivation( + rtc_interface_.isCreepTriggered(getUUID(scene_module->getModuleId()))); + intersection_module->setOcclusionCreepActivation( + occlusion_rtc_interface_.isCreepTriggered(occlusion_uuid)); scene_module->setRTCEnabled(rtc_interface_.isRTCEnabled(getUUID(scene_module->getModuleId()))); } } 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 8b9315c054b44..5466924a3898e 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 @@ -207,10 +207,13 @@ DecisionResult IntersectionModule::modifyPathVelocityDetail(PathWithLaneId * pat return InternalError{"attention area is empty"}; } const auto first_attention_area = intersection_lanelets.first_attention_area().value(); - const auto default_stopline_idx_opt = intersection_stoplines.default_stopline; + auto default_stopline_idx_opt = intersection_stoplines.default_stopline; if (!default_stopline_idx_opt) { return InternalError{"default stop line is null"}; } + if (intersection_creep_activated_) { + default_stopline_idx_opt = intersection_stoplines.creep_stopline; + } const auto default_stopline_idx = default_stopline_idx_opt.value(); const auto first_attention_stopline_idx = intersection_stoplines.first_attention_stopline; @@ -218,13 +221,21 @@ DecisionResult IntersectionModule::modifyPathVelocityDetail(PathWithLaneId * pat if (!collision_stopline_idx_opt) { return InternalError{"collision stop line is null"}; } - const auto collision_stopline_idx = collision_stopline_idx_opt.value(); + auto collision_stopline_idx = collision_stopline_idx_opt.value(); + + if (intersection_creep_activated_) { + collision_stopline_idx = intersection_stoplines.creep_stopline; + } const auto occlusion_peeking_stopline_idx_opt = intersection_stoplines.occlusion_peeking_stopline; if (!occlusion_peeking_stopline_idx_opt) { return InternalError{"occlusion stop line is null"}; } - const auto occlusion_stopline_idx = occlusion_peeking_stopline_idx_opt.value(); + auto occlusion_stopline_idx = occlusion_peeking_stopline_idx_opt.value(); + + if (occlusion_creep_activated_) { + occlusion_stopline_idx = intersection_stoplines.creep_stopline; + } // ========================================================================================== // classify the objects to attention_area/intersection_area and update their position, velocity, @@ -360,7 +371,7 @@ DecisionResult IntersectionModule::modifyPathVelocityDetail(PathWithLaneId * pat // ========================================================================================== const auto yield_stuck_status = isYieldStuckStatus(*path, interpolated_path_info, intersection_stoplines); - if (yield_stuck_status) { + if (yield_stuck_status && !intersection_creep_activated_) { if (can_smoothly_stop_at(*path, closest_idx, yield_stuck_status->stuck_stopline_idx)) { return yield_stuck_status.value(); } 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 6fceb2d7a646e..9ab1cc59b82f7 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 @@ -64,6 +64,7 @@ class IntersectionModule : public SceneModuleInterfaceWithRTC double max_accel; double max_jerk; double delay_response_time; + double creep_stopline_margin; } common; struct TurnDirection @@ -332,6 +333,15 @@ class IntersectionModule : public SceneModuleInterfaceWithRTC bool isOcclusionFirstStopRequired() const { return occlusion_first_stop_required_; } InternalDebugData & getInternalDebugData() const { return internal_debug_data_; } + void setIntersectionCreepActivation(const bool activation) + { + intersection_creep_activated_ = activation; + } + void setOcclusionCreepActivation(const bool activation) + { + occlusion_creep_activated_ = activation; + } + private: /** *********************************************************** @@ -346,7 +356,6 @@ class IntersectionModule : public SceneModuleInterfaceWithRTC const std::shared_ptr planning_factor_interface_for_occlusion_; - /** @}*/ private: /** @@ -505,6 +514,9 @@ class IntersectionModule : public SceneModuleInterfaceWithRTC double occlusion_stop_distance_{0.0}; bool occlusion_activated_{true}; bool occlusion_first_stop_required_{false}; + + bool intersection_creep_activated_{false}; + bool occlusion_creep_activated_{false}; /** @}*/ private: diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp index 07bc77f9487d3..14a2889690a0c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp @@ -433,6 +433,10 @@ std::optional IntersectionModule::generateIntersectionSto } }(); + const auto creep_stopline_ip = static_cast(std::max( + 0, static_cast(first_attention_stopline_ip) - + static_cast(std::ceil(planner_param_.common.creep_stopline_margin / ds)))); + // (2) pass judge line position on interpolated path const double braking_dist = planning_utils::calcJudgeLineDistWithJerkLimit( planner_data_->current_velocity->twist.linear.x, @@ -628,6 +632,7 @@ std::optional IntersectionModule::generateIntersectionSto size_t default_stopline{0}; size_t collision_stopline{0}; size_t first_attention_stopline{0}; + size_t creep_stopline{0}; size_t occlusion_peeking_stopline{0}; size_t pass_judge_line{0}; size_t most_footprint_overshoot_line{0}; @@ -640,6 +645,7 @@ std::optional IntersectionModule::generateIntersectionSto {&default_stopline_ip, &intersection_stoplines_temp.default_stopline}, {&collision_stopline_ip, &intersection_stoplines_temp.collision_stopline}, {&first_attention_stopline_ip, &intersection_stoplines_temp.first_attention_stopline}, + {&creep_stopline_ip, &intersection_stoplines_temp.creep_stopline}, {&occlusion_peeking_line_ip, &intersection_stoplines_temp.occlusion_peeking_stopline}, {&first_pass_judge_line_ip, &intersection_stoplines_temp.pass_judge_line}, {&maximum_footprint_overshoot_line_ip, @@ -670,6 +676,7 @@ std::optional IntersectionModule::generateIntersectionSto } intersection_stoplines.first_attention_stopline = intersection_stoplines_temp.first_attention_stopline; + intersection_stoplines.creep_stopline = intersection_stoplines_temp.creep_stopline; if (occlusion_peeking_line_valid) { intersection_stoplines.occlusion_peeking_stopline = intersection_stoplines_temp.occlusion_peeking_stopline; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp index 2c84fbad1babc..583c9fe0b60fa 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp @@ -60,13 +60,16 @@ class SceneModuleInterfaceWithRTC : public SceneModuleInterface virtual ~SceneModuleInterfaceWithRTC() = default; void setActivation(const bool activated) { activated_ = activated; } + void setCreepTriggered(const bool creep_triggered) { creep_triggered_ = creep_triggered; } void setRTCEnabled(const bool enable_rtc) { rtc_enabled_ = enable_rtc; } bool isActivated() const { return activated_; } + bool isCreepTriggered() const { return creep_triggered_; } bool isSafe() const { return safe_; } double getDistance() const { return distance_; } protected: bool activated_; + bool creep_triggered_; bool safe_; bool rtc_enabled_; double distance_; @@ -87,7 +90,8 @@ class SceneModuleManagerInterfaceWithRTC { public: SceneModuleManagerInterfaceWithRTC( - rclcpp::Node & node, const char * module_name, const bool enable_rtc = true); + rclcpp::Node & node, const char * module_name, const bool enable_rtc = true, + const bool creep_supported = false); void plan(autoware_internal_planning_msgs::msg::PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp index fc980eb4ec800..1e448446dbd7f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp @@ -39,9 +39,9 @@ SceneModuleInterfaceWithRTC::SceneModuleInterfaceWithRTC( } SceneModuleManagerInterfaceWithRTC::SceneModuleManagerInterfaceWithRTC( - rclcpp::Node & node, const char * module_name, const bool enable_rtc) + rclcpp::Node & node, const char * module_name, const bool enable_rtc, const bool creep_supported) : SceneModuleManagerInterface(node, module_name), - rtc_interface_(&node, module_name, enable_rtc), + rtc_interface_(&node, module_name, enable_rtc, creep_supported), objects_of_interest_marker_interface_(&node, module_name) { } @@ -73,6 +73,7 @@ void SceneModuleManagerInterfaceWithRTC::setActivation() const UUID uuid = getUUID(scene_module->getModuleId()); scene_module->setActivation(rtc_interface_.isActivated(uuid)); scene_module->setRTCEnabled(rtc_interface_.isRTCEnabled(uuid)); + scene_module->setCreepTriggered(rtc_interface_.isCreepTriggered(uuid)); } }