feat(motion_velocity_smoother): force slow driving (#1409)

* add force slow driving function

Signed-off-by: Go Sakayori <gsakayori@gmail.com>

* fix state

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

* erase log info

Signed-off-by: Go Sakayori <gsakayori@gmail.com>

---------

Signed-off-by: Go Sakayori <gsakayori@gmail.com>
Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

Author: go-sakayori

planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp

@@ -138,6 +138,12 @@ class VelocitySmootherNode : public rclcpp::Node
     NORMAL = 3,
   };
 
+  enum class ForceSlowDrivingType {
+    DEACTIVATED = 0,
+    READY = 1,
+    ACTIVATED = 2,
+  };
+
   struct ForceAccelerationParam
   {
     double max_acceleration;
@@ -174,6 +180,7 @@ class VelocitySmootherNode : public rclcpp::Node
     bool plan_from_ego_speed_on_manual_mode = true;
 
     ForceAccelerationParam force_acceleration_param;
+    double force_slow_driving_velocity;
   } node_param_{};
 
   struct ExternalVelocityLimit
@@ -259,11 +266,16 @@ class VelocitySmootherNode : public rclcpp::Node
 
   // parameter handling
   void initCommonParam();
+
+  // Related to force acceleration
   void onForceAcceleration(
     const std::shared_ptr<SetBool::Request> request, std::shared_ptr<SetBool::Response> response);
   bool force_acceleration_mode_;
+
+  // Related to force slow driving
   void onSlowDriving(
     const std::shared_ptr<SetBool::Request> request, std::shared_ptr<SetBool::Response> response);
+  ForceSlowDrivingType force_slow_driving_mode_;
 
   // debug
   autoware::universe_utils::StopWatch<std::chrono::milliseconds> stop_watch_;

planning/autoware_velocity_smoother/src/node.cpp

@@ -73,6 +73,7 @@ VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_opti
   srv_slow_driving_ = create_service<SetBool>(
     "~/slow_driving", std::bind(&MotionVelocitySmootherNode::onSlowDriving, this, _1, _2));
   force_acceleration_mode_ = false;
+  force_slow_driving_mode_ = ForceSlowDrivingType::DEACTIVATED;
 
   // parameter update
   set_param_res_ =
@@ -200,6 +201,7 @@ rcl_interfaces::msg::SetParametersResult VelocitySmootherNode::onParameter(
     update_param("force_acceleration.engage_velocity", p.force_acceleration_param.engage_velocity);
     update_param(
       "force_acceleration.engage_acceleration", p.force_acceleration_param.engage_acceleration);
+    update_param("force_slow_driving.velocity", p.force_slow_driving_velocity);
   }
 
   {
@@ -329,6 +331,8 @@ void VelocitySmootherNode::initCommonParam()
     declare_parameter<double>("force_acceleration.engage_velocity");
   p.force_acceleration_param.engage_acceleration =
     declare_parameter<double>("force_acceleration.engage_acceleration");
+
+  p.force_slow_driving_velocity = declare_parameter<double>("force_slow_driving.velocity");
 }
 
 void VelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory) const
@@ -502,6 +506,14 @@ void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr
     flipVelocity(input_points);
   }
 
+  // Only activate slow driving when velocity is below threshold
+  double slow_driving_vel_threshold = get_parameter("force_slow_driving.velocity").as_double();
+  if (
+    force_slow_driving_mode_ == ForceSlowDrivingType::READY &&
+    current_odometry_ptr_->twist.twist.linear.x < slow_driving_vel_threshold) {
+    force_slow_driving_mode_ = ForceSlowDrivingType::ACTIVATED;
+  }
+
   const auto output = calcTrajectoryVelocity(input_points);
   if (output.empty()) {
     RCLCPP_WARN(get_logger(), "Output Point is empty");
@@ -591,6 +603,13 @@ TrajectoryPoints VelocitySmootherNode::calcTrajectoryVelocity(
   // Apply velocity to approach stop point
   applyStopApproachingVelocity(traj_extracted);
 
+  // Apply force slow driving if activated
+  if (force_slow_driving_mode_ == ForceSlowDrivingType::ACTIVATED) {
+    for (auto & tp : traj_extracted) {
+      tp.longitudinal_velocity_mps = get_parameter("force_slow_driving.velocity").as_double();
+    }
+  }
+
   // Debug
   if (publish_debug_trajs_) {
     auto tmp = traj_extracted;
@@ -1172,12 +1191,21 @@ void MotionVelocitySmootherNode::onForceAcceleration(
   }
 
   response->success = true;
+  response->message = message;
 }
 
 void MotionVelocitySmootherNode::onSlowDriving(
   const std::shared_ptr<SetBool::Request> request, std::shared_ptr<SetBool::Response> response)
 {
-  const std::string message = request->data ? "Slow driving" : "Default";
+  std::string message = "default";
+  if (request->data && force_slow_driving_mode_ == ForceSlowDrivingType::DEACTIVATED) {
+    force_slow_driving_mode_ = ForceSlowDrivingType::READY;
+
+    message = "Activated force slow drving";
+  } else if (!request->data) {
+    force_slow_driving_mode_ = ForceSlowDrivingType::DEACTIVATED;
+    message = "Deactivated force slow driving";
+  }
 
   response->success = true;
   response->message = message;