diff --git a/planning/behavior_velocity_planner/crosswalk-design.md b/planning/behavior_velocity_planner/crosswalk-design.md index e31d1ee6cb1f3..a24312d836f97 100644 --- a/planning/behavior_velocity_planner/crosswalk-design.md +++ b/planning/behavior_velocity_planner/crosswalk-design.md @@ -197,6 +197,15 @@ If there are objects stop within a radius of `min_object_velocity * ego_pass_lat
dead lock situation
+#### Safety Slow Down Behavior + +In current autoware implementation if there are no target objects around a crosswalk, ego vehicle +will not slow down for the crosswalk. However, if ego vehicle to slow down to a certain speed in +such cases is wanted then it is possible by adding some tags to the related crosswalk definition as +it is instructed +in [lanelet2_format_extension.md](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md) +document. + ### Limitations When multiple crosswalks are nearby (such as intersection), this module may make a stop decision even at crosswalks where the object has no intention of crossing. diff --git a/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp b/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp index 165fd6e4993e8..d9cd92fb443f2 100644 --- a/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp @@ -45,7 +45,6 @@ namespace behavior_velocity_planner { - using autoware_auto_perception_msgs::msg::ObjectClassification; using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; @@ -134,6 +133,8 @@ class CrosswalkModule : public SceneModuleInterface CollisionPointState getCollisionPointState(const double ttc, const double ttv) const; + void applySafetySlowDownSpeed(PathWithLaneId & output); + float calcTargetVelocity( const geometry_msgs::msg::Point & stop_point, const PathWithLaneId & ego_path) const; @@ -169,6 +170,9 @@ class CrosswalkModule : public SceneModuleInterface // Stop watch StopWatch stop_watch_; + + // whether ego passed safety_slow_point + bool passed_safety_slow_point_; }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp index 1d46c90b14f50..f924d0a0f7798 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp @@ -23,7 +23,6 @@ namespace behavior_velocity_planner { - namespace bg = boost::geometry; using Point = bg::model::d2::point_xy; using Polygon = bg::model::polygon; @@ -137,6 +136,7 @@ CrosswalkModule::CrosswalkModule( crosswalk_(std::move(crosswalk)), planner_param_(planner_param) { + passed_safety_slow_point_ = false; } bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) @@ -150,7 +150,7 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto debug_data_.base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; *stop_reason = planning_utils::initializeStopReason(StopReason::CROSSWALK); - const auto ego_path = *path; + auto ego_path = *path; RCLCPP_INFO_EXPRESSION( logger_, planner_param_.show_processing_time, "- step1: %f ms", @@ -170,6 +170,12 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto debug_data_.crosswalk_polygon.push_back(createPoint(p.x(), p.y(), ego_pos.z)); } + if (crosswalk_.hasAttribute("safety_slow_down_speed")) { + // Safety slow down is on + applySafetySlowDownSpeed(*path); + ego_path = *path; + } + RCLCPP_INFO_EXPRESSION( logger_, planner_param_.show_processing_time, "- step2: %f ms", stop_watch_.toc("total_processing_time", false)); @@ -725,6 +731,49 @@ CollisionPointState CrosswalkModule::getCollisionPointState( return CollisionPointState::YIELD; } +void CrosswalkModule::applySafetySlowDownSpeed(PathWithLaneId & output) +{ + const auto & ego_pos = planner_data_->current_pose.pose.position; + const auto ego_path = output; + + // Safety slow down speed in [m/s] + const auto & safety_slow_down_speed = + static_cast(crosswalk_.attribute("safety_slow_down_speed").asDouble().get()); + + if (!passed_safety_slow_point_) { + // Safety slow down distance [m] + double safety_slow_down_distance = crosswalk_.attributeOr("safety_slow_down_distance", 2.0); + + // the range until to the point where ego will have a const safety slow down speed + auto safety_slow_point_range = + calcSignedArcLength(ego_path.points, ego_pos, path_intersects_.front()); + const auto & safety_slow_margin = + planner_data_->vehicle_info_.max_longitudinal_offset_m + safety_slow_down_distance; + safety_slow_point_range -= safety_slow_margin; + + const auto & p_safety_slow = + calcLongitudinalOffsetPoint(ego_path.points, ego_pos, safety_slow_point_range); + + insertDecelPointWithDebugInfo(p_safety_slow.get(), safety_slow_down_speed, output); + + if (safety_slow_point_range < 0.0) { + passed_safety_slow_point_ = true; + } + } else { + // the range until to the point where ego will start accelerate + auto safety_slow_end_point_range = + calcSignedArcLength(ego_path.points, ego_pos, path_intersects_.back()); + + if (safety_slow_end_point_range > 0) { + // insert constant ego speed until the end of the crosswalk + for (auto & p : output.points) { + const auto & original_velocity = p.point.longitudinal_velocity_mps; + p.point.longitudinal_velocity_mps = std::min(original_velocity, safety_slow_down_speed); + } + } + } +} + bool CrosswalkModule::isStuckVehicle( const PathWithLaneId & ego_path, const PredictedObject & object) const {