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

* add force slow driving function Signed-off-by: Go Sakayori <[email protected]> * fix state Signed-off-by: Go Sakayori <[email protected]> * erase log info Signed-off-by: Go Sakayori <[email protected]> --------- Signed-off-by: Go Sakayori <[email protected]> Signed-off-by: Go Sakayori <[email protected]>
tier4 · Dec 13, 2024 · 52b0f71 · 52b0f71
1 parent 478a0d5
commit 52b0f71
Show file tree

Hide file tree

Showing 2 changed files with 57 additions and 0 deletions.
diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp
@@ -43,6 +43,7 @@
 #include "autoware_planning_msgs/msg/trajectory_point.hpp"
 #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
 #include "nav_msgs/msg/odometry.hpp"
+#include "std_srvs/srv/set_bool.hpp"
 #include "tier4_debug_msgs/msg/float32_stamped.hpp"         // temporary
 #include "tier4_debug_msgs/msg/float64_stamped.hpp"         // temporary
 #include "tier4_planning_msgs/msg/stop_speed_exceeded.hpp"  // temporary
@@ -66,6 +67,7 @@ using geometry_msgs::msg::AccelWithCovarianceStamped;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::PoseStamped;
 using nav_msgs::msg::Odometry;
+using std_srvs::srv::SetBool;
 using tier4_debug_msgs::msg::Float32Stamped;        // temporary
 using tier4_debug_msgs::msg::Float64Stamped;        // temporary
 using tier4_planning_msgs::msg::StopSpeedExceeded;  // temporary
@@ -100,6 +102,7 @@ class VelocitySmootherNode : public rclcpp::Node
     sub_external_velocity_limit_{this, "~/input/external_velocity_limit_mps"};
   autoware::universe_utils::InterProcessPollingSubscriber<OperationModeState> sub_operation_mode_{
     this, "~/input/operation_mode_state"};
+  rclcpp::Service<SetBool>::SharedPtr srv_slow_driving_;
 
   Odometry::ConstSharedPtr current_odometry_ptr_;  // current odometry
   AccelWithCovarianceStamped::ConstSharedPtr current_acceleration_ptr_;
@@ -137,6 +140,12 @@ class VelocitySmootherNode : public rclcpp::Node
     NORMAL = 3,
   };
 
+  enum class ForceSlowDrivingType {
+    DEACTIVATED = 0,
+    READY = 1,
+    ACTIVATED = 2,
+  };
+
   struct Param
   {
     bool enable_lateral_acc_limit;
@@ -162,6 +171,8 @@ class VelocitySmootherNode : public rclcpp::Node
     AlgorithmType algorithm_type;  // Option : JerkFiltered, Linf, L2
 
     bool plan_from_ego_speed_on_manual_mode = true;
+
+    double force_slow_driving_velocity;
   } node_param_{};
 
   struct AccelerationRequest
@@ -255,6 +266,11 @@ class VelocitySmootherNode : public rclcpp::Node
   // parameter handling
   void initCommonParam();
 
+  // 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_;
   std::shared_ptr<rclcpp::Time> prev_time_;

diff --git a/planning/autoware_velocity_smoother/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp
@@ -39,6 +39,7 @@ VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_opti
 : Node("velocity_smoother", node_options)
 {
   using std::placeholders::_1;
+  using std::placeholders::_2;
 
   // set common params
   const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo();
@@ -67,6 +68,10 @@ VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_opti
   sub_current_trajectory_ = create_subscription<Trajectory>(
     "~/input/trajectory", 1, std::bind(&VelocitySmootherNode::onCurrentTrajectory, this, _1));
 
+  srv_slow_driving_ = create_service<SetBool>(
+    "~/slow_driving", std::bind(&VelocitySmootherNode::onSlowDriving, this, _1, _2));
+  force_slow_driving_mode_ = ForceSlowDrivingType::DEACTIVATED;
+
   // parameter update
   set_param_res_ =
     this->add_on_set_parameters_callback(std::bind(&VelocitySmootherNode::onParameter, this, _1));
@@ -185,6 +190,8 @@ rcl_interfaces::msg::SetParametersResult VelocitySmootherNode::onParameter(
     update_param("ego_nearest_dist_threshold", p.ego_nearest_dist_threshold);
     update_param("ego_nearest_yaw_threshold", p.ego_nearest_yaw_threshold);
     update_param_bool("plan_from_ego_speed_on_manual_mode", p.plan_from_ego_speed_on_manual_mode);
+
+    update_param("force_slow_driving.velocity", p.force_slow_driving_velocity);
   }
 
   {
@@ -304,6 +311,8 @@ void VelocitySmootherNode::initCommonParam()
 
   p.plan_from_ego_speed_on_manual_mode =
     declare_parameter<bool>("plan_from_ego_speed_on_manual_mode");
+
+  p.force_slow_driving_velocity = declare_parameter<double>("force_slow_driving.velocity");
 }
 
 void VelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory) const
@@ -489,6 +498,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");
@@ -578,6 +595,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;
@@ -1142,6 +1166,23 @@ TrajectoryPoint VelocitySmootherNode::calcProjectedTrajectoryPointFromEgo(
   return calcProjectedTrajectoryPoint(trajectory, current_odometry_ptr_->pose.pose);
 }
 
+void VelocitySmootherNode::onSlowDriving(
+  const std::shared_ptr<SetBool::Request> request, std::shared_ptr<SetBool::Response> response)
+{
+  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;
+}
+
 }  // namespace autoware::velocity_smoother
 
 #include "rclcpp_components/register_node_macro.hpp"