feat(behavior_velocity_planner): add safety slow down feature for spe…

…cific crosswalks (tier4#1528) * feat: add safety slow down Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> feat: implement safety slow down and maintain slow speed Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> feat: rearrange insertDecelPoint function Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> chore: update comments Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> docs: add safety slow down behavior info in crosswalk design doc Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> chore: create a separate function Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> fix: revert add_debug flag Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> fix: change applySafetySlowDownSpeed function call scope Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> ci(pre-commit): autofix * fix conflicts & add attributeOr Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com>
boyali · Sep 28, 2022 · 889916d · 889916d
1 parent 0a0feb9
commit 889916d
Show file tree

Hide file tree

Showing 3 changed files with 65 additions and 3 deletions.
diff --git 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
   <figcaption>dead lock situation</figcaption>
 </figure>
 
+#### 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
@@ -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<std::chrono::milliseconds> 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
@@ -23,7 +23,6 @@
 
 namespace behavior_velocity_planner
 {
-
 namespace bg = boost::geometry;
 using Point = bg::model::d2::point_xy<double>;
 using Polygon = bg::model::polygon<Point>;
@@ -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<float>(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
 {