improve algorithm

Signed-off-by: Yuki Takagi <[email protected]>

planning/behavior_velocity_crosswalk_module/src/manager.cpp

@@ -68,8 +68,8 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)
     getOrDeclareParameter<double>(node, ns + ".stuck_vehicle.stuck_vehicle_velocity");
   cp.max_stuck_vehicle_lateral_offset =
     getOrDeclareParameter<double>(node, ns + ".stuck_vehicle.max_stuck_vehicle_lateral_offset");
-  cp.stuck_vehicle_attention_range =
-    getOrDeclareParameter<double>(node, ns + ".stuck_vehicle.stuck_vehicle_attention_range");
+  cp.required_clearance =
+    getOrDeclareParameter<double>(node, ns + ".stuck_vehicle.required_clearance");
   cp.min_acc_for_stuck_vehicle = getOrDeclareParameter<double>(node, ns + ".stuck_vehicle.min_acc");
   cp.max_jerk_for_stuck_vehicle =
     getOrDeclareParameter<double>(node, ns + ".stuck_vehicle.max_jerk");

planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

@@ -896,6 +896,8 @@ std::optional<StopFactor> CrosswalkModule::checkStopForStuckVehicles(
     }
   }
 
+  const auto & ego_pos = planner_data_->current_odometry->pose.position;
+
   for (const auto & object : objects) {
     if (!isVehicle(object)) {
       continue;
@@ -906,34 +908,35 @@ std::optional<StopFactor> CrosswalkModule::checkStopForStuckVehicles(
       continue;
     }
 
-    const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position;
-    const auto lateral_offset = calcLateralOffset(ego_path.points, obj_pos);
+    const auto & obj_pose = object.kinematics.initial_pose_with_covariance.pose;
+    const auto lateral_offset = calcLateralOffset(ego_path.points, obj_pose.position);
     if (p.max_stuck_vehicle_lateral_offset < std::abs(lateral_offset)) {
       continue;
     }
 
-    const auto & ego_pos = planner_data_->current_odometry->pose.position;
-    const auto ego_vel = planner_data_->current_velocity->twist.linear.x;
-    const auto ego_acc = planner_data_->current_acceleration->accel.accel.linear.x;
-
-    const double near_attention_range =
-      calcSignedArcLength(ego_path.points, ego_pos, path_intersects.back());
-    const double far_attention_range = near_attention_range + p.stuck_vehicle_attention_range;
-
-    const auto dist_ego2obj = calcSignedArcLength(ego_path.points, ego_pos, obj_pos);
-
-    if (near_attention_range < dist_ego2obj && dist_ego2obj < far_attention_range) {
+    // check if STOP is required
+    const double crosswalk_front_to_obj_rear =
+      calcSignedArcLength(ego_path.points, path_intersects.front(), obj_pose.position) -
+      object.shape.dimensions.x / 2.0;
+    const double crosswalk_back_to_obj_rear =
+      calcSignedArcLength(ego_path.points, path_intersects.back(), obj_pose.position) -
+      object.shape.dimensions.x / 2.0;
+    const double required_space_length =
+      planner_data_->vehicle_info_.vehicle_length_m + planner_param_.required_clearance;
+
+    if (crosswalk_front_to_obj_rear > 0.0 && crosswalk_back_to_obj_rear < required_space_length) {
       // Plan STOP considering min_acc, max_jerk and min_jerk.
-      const auto min_feasible_dist_ego2stop = calcDecelDistWithJerkAndAccConstraints(
-        ego_vel, 0.0, ego_acc, p.min_acc_for_stuck_vehicle, p.max_jerk_for_stuck_vehicle,
-        p.min_jerk_for_stuck_vehicle);
-      if (!min_feasible_dist_ego2stop) {
+      const auto braking_distance = calcDecelDistWithJerkAndAccConstraints(
+        planner_data_->current_velocity->twist.linear.x, 0.0,
+        planner_data_->current_acceleration->accel.accel.linear.x, p.min_acc_for_stuck_vehicle,
+        p.max_jerk_for_stuck_vehicle, p.min_jerk_for_stuck_vehicle);
+      if (!braking_distance) {
         continue;
       }
 
       const double dist_ego2stop =
         calcSignedArcLength(ego_path.points, ego_pos, stop_pose->position);
-      const double feasible_dist_ego2stop = std::max(*min_feasible_dist_ego2stop, dist_ego2stop);
+      const double feasible_dist_ego2stop = std::max(*braking_distance, dist_ego2stop);
       const double dist_to_ego =
         calcSignedArcLength(ego_path.points, ego_path.points.front().point.pose.position, ego_pos);
 
@@ -943,11 +946,8 @@ std::optional<StopFactor> CrosswalkModule::checkStopForStuckVehicles(
         continue;
       }
 
-      setObjectsOfInterestData(
-        object.kinematics.initial_pose_with_covariance.pose, object.shape, ColorName::RED);
-
-      // early return may not appropriate because the nearest in range object should be handled
-      return createStopFactor(*feasible_stop_pose, {obj_pos});
+      setObjectsOfInterestData(obj_pose, object.shape, ColorName::RED);
+      return createStopFactor(*feasible_stop_pose, {obj_pose.position});
     }
   }
 

planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp

@@ -124,7 +124,7 @@ class CrosswalkModule : public SceneModuleInterface
     bool enable_stuck_check_in_intersection{false};
     double stuck_vehicle_velocity;
     double max_stuck_vehicle_lateral_offset;
-    double stuck_vehicle_attention_range;
+    double required_clearance;
     double min_acc_for_stuck_vehicle;
     double max_jerk_for_stuck_vehicle;
     double min_jerk_for_stuck_vehicle;