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
{