feat(crosswalk): dynamic timeout for no intention to walk decision (a…

…utowarefoundation#5188) (#902)

feat(crosswalk): dyanmic timeout for no intention to walk decision (autowarefoundation#5188)

* feat(crosswalk): dyanmic timeout for no intention to walk decision



* update config



* update



* update config



---------

Signed-off-by: Takayuki Murooka <[email protected]>

planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml

@@ -54,7 +54,9 @@
         ## param for yielding
         disable_stop_for_yield_cancel: true # for the crosswalk where there is a traffic signal
         disable_yield_for_new_stopped_object: true # for the crosswalk where there is a traffic signal
-        timeout_no_intention_to_walk: 3.0 # [s] if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first.
+        # if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first.
+        distance_map_for_no_intention_to_walk: [1.0, 5.0] # [m] ascending order
+        timeout_map_for_no_intention_to_walk: [3.0, 0.0] # [s]
         timeout_ego_stop_for_yield: 3.0 # [s] the amount of time which ego should be stopping to query whether it yields or not
 
       # param for target object filtering

planning/behavior_velocity_crosswalk_module/src/manager.cpp

@@ -104,8 +104,10 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)
     getOrDeclareParameter<bool>(node, ns + ".pass_judge.disable_stop_for_yield_cancel");
   cp.disable_yield_for_new_stopped_object =
     getOrDeclareParameter<bool>(node, ns + ".pass_judge.disable_yield_for_new_stopped_object");
-  cp.timeout_no_intention_to_walk =
-    getOrDeclareParameter<double>(node, ns + ".pass_judge.timeout_no_intention_to_walk");
+  cp.distance_map_for_no_intention_to_walk = getOrDeclareParameter<std::vector<double>>(
+    node, ns + ".pass_judge.distance_map_for_no_intention_to_walk");
+  cp.timeout_map_for_no_intention_to_walk = getOrDeclareParameter<std::vector<double>>(
+    node, ns + ".pass_judge.timeout_map_for_no_intention_to_walk");
   cp.timeout_ego_stop_for_yield =
     getOrDeclareParameter<double>(node, ns + ".pass_judge.timeout_ego_stop_for_yield");
 

planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

@@ -921,7 +921,7 @@ void CrosswalkModule::updateObjectState(
       getCollisionPoint(sparse_resample_path, object, crosswalk_attention_range, attention_area);
     object_info_manager_.update(
       obj_uuid, obj_pos, std::hypot(obj_vel.x, obj_vel.y), clock_->now(), is_ego_yielding,
-      has_traffic_light, collision_point, planner_param_);
+      has_traffic_light, collision_point, planner_param_, crosswalk_.polygon2d().basicPolygon());
 
     if (collision_point) {
       const auto collision_state = object_info_manager_.getCollisionState(obj_uuid);

planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp

@@ -31,13 +31,15 @@
 #include <boost/assert.hpp>
 #include <boost/assign/list_of.hpp>
 
-#include <lanelet2_core/LaneletMap.h>
-#include <lanelet2_routing/RoutingGraph.h>
-#include <lanelet2_routing/RoutingGraphContainer.h>
+#include <lanelet2_core/geometry/Point.h>
+#include <lanelet2_core/geometry/Polygon.h>
+#include <lanelet2_core/primitives/Lanelet.h>
+#include <lanelet2_core/primitives/LineString.h>
 #include <pcl/common/distances.h>
 #include <pcl/point_types.h>
 #include <pcl_conversions/pcl_conversions.h>
 
+#include <algorithm>
 #include <memory>
 #include <optional>
 #include <string>
@@ -47,6 +49,7 @@
 
 namespace behavior_velocity_planner
 {
+namespace bg = boost::geometry;
 using autoware_auto_perception_msgs::msg::ObjectClassification;
 using autoware_auto_perception_msgs::msg::PredictedObject;
 using autoware_auto_perception_msgs::msg::PredictedObjects;
@@ -73,6 +76,33 @@ double interpolateEgoPassMargin(
   }
   return y_vec.back();
 }
+
+double InterpolateMap(
+  const std::vector<double> & key_map, const std::vector<double> & value_map, const double query)
+{
+  // If the speed is out of range of the key_map, apply zero-order hold.
+  if (query <= key_map.front()) {
+    return value_map.front();
+  }
+  if (query >= key_map.back()) {
+    return value_map.back();
+  }
+
+  // Apply linear interpolation
+  for (size_t i = 0; i < key_map.size() - 1; ++i) {
+    if (key_map.at(i) <= query && query <= key_map.at(i + 1)) {
+      auto ratio = (query - key_map.at(i)) / std::max(key_map.at(i + 1) - key_map.at(i), 1.0e-5);
+      ratio = std::clamp(ratio, 0.0, 1.0);
+      const auto interp = value_map.at(i) + ratio * (value_map.at(i + 1) - value_map.at(i));
+      return interp;
+    }
+  }
+
+  std::cerr << "Crosswalk's InterpolateMap interpolation logic is broken."
+               "Please check the code."
+            << std::endl;
+  return value_map.back();
+}
 }  // namespace
 
 class CrosswalkModule : public SceneModuleInterface
@@ -113,7 +143,8 @@ class CrosswalkModule : public SceneModuleInterface
     double min_object_velocity;
     bool disable_stop_for_yield_cancel;
     bool disable_yield_for_new_stopped_object;
-    double timeout_no_intention_to_walk;
+    std::vector<double> distance_map_for_no_intention_to_walk;
+    std::vector<double> timeout_map_for_no_intention_to_walk;
     double timeout_ego_stop_for_yield;
     // param for input data
     double traffic_light_state_timeout;
@@ -134,9 +165,10 @@ class CrosswalkModule : public SceneModuleInterface
     std::optional<CollisionPoint> collision_point{};
 
     void transitState(
-      const rclcpp::Time & now, const double vel, const bool is_ego_yielding,
-      const bool has_traffic_light, const std::optional<CollisionPoint> & collision_point,
-      const PlannerParam & planner_param)
+      const rclcpp::Time & now, const geometry_msgs::msg::Point & position, const double vel,
+      const bool is_ego_yielding, const bool has_traffic_light,
+      const std::optional<CollisionPoint> & collision_point, const PlannerParam & planner_param,
+      const lanelet::BasicPolygon2d & crosswalk_polygon)
     {
       const bool is_stopped = vel < planner_param.stop_object_velocity;
 
@@ -149,8 +181,13 @@ class CrosswalkModule : public SceneModuleInterface
         if (!time_to_start_stopped) {
           time_to_start_stopped = now;
         }
+        const double distance_to_crosswalk =
+          bg::distance(crosswalk_polygon, lanelet::BasicPoint2d(position.x, position.y));
+        const double timeout_no_intention_to_walk = InterpolateMap(
+          planner_param.distance_map_for_no_intention_to_walk,
+          planner_param.timeout_map_for_no_intention_to_walk, distance_to_crosswalk);
         const bool intent_to_cross =
-          (now - *time_to_start_stopped).seconds() < planner_param.timeout_no_intention_to_walk;
+          (now - *time_to_start_stopped).seconds() < timeout_no_intention_to_walk;
         if (
           (is_ego_yielding || (has_traffic_light && planner_param.disable_stop_for_yield_cancel)) &&
           !intent_to_cross) {
@@ -205,7 +242,8 @@ class CrosswalkModule : public SceneModuleInterface
     void update(
       const std::string & uuid, const geometry_msgs::msg::Point & position, const double vel,
       const rclcpp::Time & now, const bool is_ego_yielding, const bool has_traffic_light,
-      const std::optional<CollisionPoint> & collision_point, const PlannerParam & planner_param)
+      const std::optional<CollisionPoint> & collision_point, const PlannerParam & planner_param,
+      const lanelet::BasicPolygon2d & crosswalk_polygon)
     {
       // update current uuids
       current_uuids_.push_back(uuid);
@@ -223,7 +261,8 @@ class CrosswalkModule : public SceneModuleInterface
 
       // update object state
       objects.at(uuid).transitState(
-        now, vel, is_ego_yielding, has_traffic_light, collision_point, planner_param);
+        now, position, vel, is_ego_yielding, has_traffic_light, collision_point, planner_param,
+        crosswalk_polygon);
       objects.at(uuid).collision_point = collision_point;
       objects.at(uuid).position = position;
     }