feat(intersection,crosswalk): add creep functionality

planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp

@@ -23,10 +23,13 @@
 #include "tier4_rtc_msgs/msg/cooperate_response.hpp"
 #include "tier4_rtc_msgs/msg/cooperate_status.hpp"
 #include "tier4_rtc_msgs/msg/cooperate_status_array.hpp"
+#include "tier4_rtc_msgs/msg/creep_command.hpp"
+#include "tier4_rtc_msgs/msg/creep_response.hpp"
 #include "tier4_rtc_msgs/msg/module.hpp"
 #include "tier4_rtc_msgs/msg/state.hpp"
 #include "tier4_rtc_msgs/srv/auto_mode.hpp"
 #include "tier4_rtc_msgs/srv/cooperate_commands.hpp"
+#include "tier4_rtc_msgs/srv/creep_commands.hpp"
 #include <unique_identifier_msgs/msg/uuid.hpp>
 
 #include <mutex>
@@ -41,16 +44,21 @@ using tier4_rtc_msgs::msg::CooperateCommand;
 using tier4_rtc_msgs::msg::CooperateResponse;
 using tier4_rtc_msgs::msg::CooperateStatus;
 using tier4_rtc_msgs::msg::CooperateStatusArray;
+using tier4_rtc_msgs::msg::CreepCommand;
+using tier4_rtc_msgs::msg::CreepResponse;
 using tier4_rtc_msgs::msg::Module;
 using tier4_rtc_msgs::msg::State;
 using tier4_rtc_msgs::srv::AutoMode;
 using tier4_rtc_msgs::srv::CooperateCommands;
+using tier4_rtc_msgs::srv::CreepCommands;
 using unique_identifier_msgs::msg::UUID;
 
 class RTCInterface
 {
 public:
-  RTCInterface(rclcpp::Node * node, const std::string & name, const bool enable_rtc = true);
+  RTCInterface(
+    rclcpp::Node * node, const std::string & name, const bool enable_rtc = true,
+    const bool creep_supported = false);
   void publishCooperateStatus(const rclcpp::Time & stamp);
   /// @brief update the cooperate status of the module identified by the given UUID
   /// @param[in] uuid unique ID of the module
@@ -75,6 +83,8 @@ class RTCInterface
   bool isRegistered(const UUID & uuid) const;
   bool isRTCEnabled(const UUID & uuid) const;
   bool isTerminated(const UUID & uuid) const;
+  bool isCreepTriggered(const UUID & uuid) const;
+  bool isCreepSupported() const;
   void lockCommandUpdate();
   void unlockCommandUpdate();
   void print() const;
@@ -85,6 +95,9 @@ class RTCInterface
     const CooperateCommands::Response::SharedPtr responses);
   void onAutoModeService(
     const AutoMode::Request::SharedPtr request, const AutoMode::Response::SharedPtr response);
+  void onCreepCommandService(
+    const CreepCommands::Request::SharedPtr request,
+    const CreepCommands::Response::SharedPtr responses);
   void onTimer();
   std::vector<CooperateResponse> validateCooperateCommands(
     const std::vector<CooperateCommand> & commands);
@@ -97,6 +110,7 @@ class RTCInterface
   rclcpp::Publisher<AutoModeStatus>::SharedPtr pub_auto_mode_status_;
   rclcpp::Service<CooperateCommands>::SharedPtr srv_commands_;
   rclcpp::Service<AutoMode>::SharedPtr srv_auto_mode_;
+  rclcpp::Service<CreepCommands>::SharedPtr srv_creep_commands_;
   rclcpp::CallbackGroup::SharedPtr callback_group_;
   rclcpp::TimerBase::SharedPtr timer_;
   rclcpp::Clock::SharedPtr clock_;
@@ -107,11 +121,13 @@ class RTCInterface
   std::vector<CooperateCommand> stored_commands_;
   bool is_auto_mode_enabled_;
   bool is_locked_;
+  bool is_creep_supported_;
 
   std::string cooperate_status_namespace_ = "/planning/cooperate_status";
   std::string auto_mode_status_namespace_ = "/planning/auto_mode_status";
   std::string cooperate_commands_namespace_ = "/planning/cooperate_commands";
   std::string enable_auto_mode_namespace_ = "/planning/enable_auto_mode";
+  std::string creep_commands_namespace_ = "/planning/creep_commands";
 
   mutable std::mutex mutex_;
 

planning/autoware_rtc_interface/src/rtc_interface.cpp

@@ -116,11 +116,13 @@ Module getModuleType(const std::string & module_name)
 
 namespace autoware::rtc_interface
 {
-RTCInterface::RTCInterface(rclcpp::Node * node, const std::string & name, const bool enable_rtc)
+RTCInterface::RTCInterface(
+  rclcpp::Node * node, const std::string & name, const bool enable_rtc, const bool creep_supported)
 : clock_{node->get_clock()},
   logger_{node->get_logger().get_child("RTCInterface[" + name + "]")},
   is_auto_mode_enabled_{!enable_rtc},
-  is_locked_{false}
+  is_locked_{false},
+  is_creep_supported_{creep_supported}
 {
   using std::placeholders::_1;
   using std::placeholders::_2;
@@ -147,6 +149,10 @@ RTCInterface::RTCInterface(rclcpp::Node * node, const std::string & name, const
     enable_auto_mode_namespace_ + "/" + name,
     std::bind(&RTCInterface::onAutoModeService, this, _1, _2), rmw_qos_profile_services_default,
     callback_group_);
+  srv_creep_commands_ = node->create_service<CreepCommands>(
+    creep_commands_namespace_ + "/" + name,
+    std::bind(&RTCInterface::onCreepCommandService, this, _1, _2), rmw_qos_profile_services_default,
+    callback_group_);
 
   // Module
   module_ = getModuleType(name);
@@ -239,6 +245,51 @@ void RTCInterface::onAutoModeService(
   response->success = true;
 }
 
+void RTCInterface::onCreepCommandService(
+  const CreepCommands::Request::SharedPtr request,
+  const CreepCommands::Response::SharedPtr responses)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  for (const auto & command : request->commands) {
+    CreepResponse response;
+    response.uuid = command.uuid;
+    response.module = command.module;
+
+    const auto itr = std::find_if(
+      registered_status_.statuses.begin(), registered_status_.statuses.end(),
+      [command](const auto & s) { return s.uuid == command.uuid; });
+
+    if (!is_creep_supported_) {
+      response.success = false;
+      RCLCPP_WARN_STREAM(
+        getLogger(), "[onCreepCommandService] creep is not supported." << std::endl);
+      responses->responses.push_back(response);
+      continue;
+    }
+
+    if (itr != registered_status_.statuses.end()) {
+      if (itr->state.type == State::WAITING_FOR_EXECUTION || itr->state.type == State::RUNNING) {
+        itr->creep_triggered = command.creep_enable;
+        response.success = true;
+      } else {
+        RCLCPP_WARN_STREAM(
+          getLogger(), "[onCreepCommandService] uuid : "
+                         << uuid_to_string(command.uuid)
+                         << " state is not WAITING_FOR_EXECUTION or RUNNING. state : "
+                         << itr->state.type << std::endl);
+        response.success = false;
+      }
+    } else {
+      RCLCPP_WARN_STREAM(
+        getLogger(), "[onCreepCommandService] uuid : " << uuid_to_string(command.uuid)
+                                                       << " is not found." << std::endl);
+      response.success = false;
+    }
+    responses->responses.push_back(response);
+  }
+}
+
 void RTCInterface::onTimer()
 {
   AutoModeStatus auto_mode_status;
@@ -268,6 +319,8 @@ void RTCInterface::updateCooperateStatus(
     status.safe = safe;
     status.requested = requested;
     status.command_status.type = Command::DEACTIVATE;
+    status.creep_supported = is_creep_supported_;
+    status.creep_triggered = false;
     status.state.type = State::WAITING_FOR_EXECUTION;
     status.start_distance = start_distance;
     status.finish_distance = finish_distance;
@@ -474,6 +527,34 @@ bool RTCInterface::isTerminated(const UUID & uuid) const
   return is_auto_mode_enabled_;
 }
 
+bool RTCInterface::isCreepTriggered(const UUID & uuid) const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if (!is_creep_supported_) {
+    return false;
+  }
+  const auto itr = std::find_if(
+    registered_status_.statuses.begin(), registered_status_.statuses.end(),
+    [uuid](const auto & s) { return s.uuid == uuid; });
+
+  if (itr != registered_status_.statuses.end()) {
+    if (itr->state.type != State::WAITING_FOR_EXECUTION && itr->state.type != State::RUNNING) {
+      return false;
+    }
+    return itr->creep_triggered;
+  }
+
+  RCLCPP_WARN_STREAM(
+    getLogger(),
+    "[isCreepTriggered] uuid : " << uuid_to_string(uuid) << " is not found." << std::endl);
+  return false;
+}
+
+bool RTCInterface::isCreepSupported() const
+{
+  return is_creep_supported_;
+}
+
 void RTCInterface::lockCommandUpdate()
 {
   is_locked_ = true;

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp

@@ -32,7 +32,8 @@ using lanelet::autoware::Crosswalk;
 
 CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)
 : SceneModuleManagerInterfaceWithRTC(
-    node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".common.enable_rtc"))
+    node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".common.enable_rtc"),
+    true)
 {
   const std::string ns(CrosswalkModuleManager::getModuleName());
 

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

@@ -257,6 +257,7 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path)
 
   // Calculate stop point with margin
   const auto default_stop_pose = getDefaultStopPose(*path, first_path_point_on_crosswalk);
+  const auto deadline_stop_pose = getDeadlineStopPose(*path, first_path_point_on_crosswalk);
 
   // Resample path sparsely for less computation cost
   constexpr double resample_interval = 4.0;
@@ -289,11 +290,25 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path)
   setDistanceToStop(*path, default_stop_pose, nearest_stop_factor);
   set_previous_stop_pose(nearest_stop_factor);
 
+  const bool is_creep_activated = isCreepTriggered();
+
+  // RCLCPP_INFO(
+  //   logger_, "Crosswalk Module[%ld] creep activated: %s, deadline_stop_pose.has_value(): %s",
+  //   getModuleId(), is_creep_activated ? "Yes" : "No",
+  //   deadline_stop_pose.has_value() ? "Yes" : "No");
+
   // plan Go/Stop
   if (isActivated()) {
     planGo(*path, nearest_stop_factor);
   } else {
-    planStop(*path, nearest_stop_factor, default_stop_pose, reason);
+    if (is_creep_activated) {
+      if (deadline_stop_pose.has_value()) {
+        planCreeping(*path, nearest_stop_factor, deadline_stop_pose.value());
+      }
+      // If no deadline stop pose is available, already passed the stop line, so do nothing.
+    } else {
+      planStop(*path, nearest_stop_factor, default_stop_pose, reason);
+    }
   }
   recordTime(4);
 
@@ -327,6 +342,17 @@ std::optional<geometry_msgs::msg::Pose> CrosswalkModule::getDefaultStopPose(
     -planner_param_.stop_distance_from_crosswalk - base_link2front);
 }
 
+std::optional<geometry_msgs::msg::Pose> CrosswalkModule::getDeadlineStopPose(
+  const PathWithLaneId & ego_path,
+  const geometry_msgs::msg::Point & first_path_point_on_crosswalk) const
+{
+  // const auto & ego_pos = planner_data_->current_odometry->pose.position;
+  const auto & base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
+  return calcLongitudinalOffsetPose(
+    ego_path.points, first_path_point_on_crosswalk,
+    -planner_param_.stop_distance_from_crosswalk_limit - base_link2front);
+}
+
 std::optional<StopPoseWithObjectUuids> CrosswalkModule::checkStopForCrosswalkUsers(
   const PathWithLaneId & ego_path, const PathWithLaneId & sparse_resample_path,
   const geometry_msgs::msg::Point & first_path_point_on_crosswalk,
@@ -507,7 +533,14 @@ std::optional<geometry_msgs::msg::Pose> CrosswalkModule::calcStopPose(
         p.min_jerk_for_no_stop_decision);
     return strong_brake_dist_opt ? strong_brake_dist_opt.value() : 0.0;
   }();
-  if (p.enable_no_stop_decision && std::max(selected_stop.dist, 0.1) < strong_brake_dist) {
+
+  const bool is_abandon_to_stop =
+    (p.enable_no_stop_decision && std::max(selected_stop.dist, 0.1) < strong_brake_dist);
+  const bool is_abandon_to_stop_for_creeping =
+    (std::max(selected_stop.dist, 0.1) < strong_brake_dist && !previous_stop_pose_.has_value());
+  if (
+    (!isCreepTriggered() && is_abandon_to_stop) ||
+    (isCreepTriggered() && is_abandon_to_stop_for_creeping)) {
     RCLCPP_INFO_THROTTLE(
       logger_, *clock_, 1000,
       "Abandon to stop. "
@@ -1525,6 +1558,87 @@ void CrosswalkModule::setDistanceToStop(
   }
 }
 
+void CrosswalkModule::planCreeping(
+  PathWithLaneId & ego_path, const std::optional<StopPoseWithObjectUuids> & nearest_stop_factor,
+  geometry_msgs::msg::Pose deadline_stop_pose)
+{
+  if (!nearest_stop_factor.has_value()) {
+    return;
+  }
+
+  const double creep_velocity = 3.0 / 3.6;  // 3km/h
+
+  const bool has_moving_object = [&]() {
+    const double moving_threshold = planner_param_.stop_object_velocity;
+    const auto & objects = planner_data_->predicted_objects->objects;
+    for (const auto & uuid : nearest_stop_factor->target_object_ids) {
+      for (const auto & object : objects) {
+        if (object.object_id != uuid) {
+          continue;
+        }
+        const auto & twist = object.kinematics.initial_twist_with_covariance.twist.linear;
+        if (std::hypot(twist.x, twist.y) > moving_threshold) {
+          return true;
+        }
+        break;
+      }
+    }
+    return false;
+  }();
+  if (has_moving_object) {
+    yield_stuck_ = true;
+  }
+
+  if (!yield_stuck_) {
+    const auto & ego_pos = planner_data_->current_odometry->pose.position;
+    const auto ego_vel = std::max(0.0, planner_data_->current_velocity->twist.linear.x);
+    max_ego_speed_from_creep_triggered_ = std::max(max_ego_speed_from_creep_triggered_, ego_vel);
+
+    // calculate distance to deadline stop pose
+    const auto dist_to_deadline_stop =
+      calcSignedArcLength(ego_path.points, ego_pos, deadline_stop_pose.position);
+
+    // calculate braking distance
+    const auto & p = planner_param_;
+    const auto braking_distance = calcDecelDistWithJerkAndAccConstraints(
+      max_ego_speed_from_creep_triggered_, 0.0, 0.0, p.min_acc_preferred, 10.0,
+      p.min_jerk_preferred);
+
+    RCLCPP_DEBUG(
+      logger_, "dist_to_deadline_stop: %.2f m, braking_distance: %.2f m", dist_to_deadline_stop,
+      braking_distance.has_value() ? braking_distance.value() : -1.0);
+
+    // if braking distance is greater than distance to deadline stop pose, can't stop anymore
+    const auto allow_creep_pass = !rtc_enabled_;
+
+    // NOTE(odashima): add a margin to prevent deceleration near the pass judge point
+    // caused by latency, discretized path interval, etc.
+    const double creep_pass_margin = 0.2;  // [m]
+    if (
+      allow_creep_pass && braking_distance.has_value() &&
+      braking_distance.value() + creep_pass_margin > dist_to_deadline_stop) {
+      passed_pass_judge_ = true;
+      RCLCPP_DEBUG(logger_, "creep pass judge passed");
+    }
+  }
+
+  if (passed_pass_judge_) {
+    return;
+  } else {
+    insertDecelPointWithDebugInfo(
+      nearest_stop_factor->stop_pose.position, creep_velocity, ego_path);
+    insertDecelPointWithDebugInfo(deadline_stop_pose.position, 0.0, ego_path);
+
+    // Add planning factor
+    const SafetyFactorArray safety_factors = createSafetyFactorArray(nearest_stop_factor);
+    planning_factor_interface_->add(
+      ego_path.points, planner_data_->current_odometry->pose, deadline_stop_pose,
+      autoware_internal_planning_msgs::msg::PlanningFactor::STOP, safety_factors,
+      true /*is_driving_forward*/, 0.0 /*velocity*/, 0.0 /*shift distance*/,
+      yield_stuck_ ? "yield" : "creeping");
+  }
+}
+
 void CrosswalkModule::planGo(
   PathWithLaneId & ego_path, const std::optional<StopPoseWithObjectUuids> & stop_factor) const
 {