Merge pull request #1584 from tier4/hotfix/v0.36/force_acceleration_a…

…nd_slow

feat(motion_velocity_smoother): cherry pick force acceleration and slow driving

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,8 @@ 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_force_acceleration_;
+  rclcpp::Service<SetBool>::SharedPtr srv_slow_driving_;
 
   Odometry::ConstSharedPtr current_odometry_ptr_;  // current odometry
   AccelWithCovarianceStamped::ConstSharedPtr current_acceleration_ptr_;
@@ -137,6 +141,21 @@ class VelocitySmootherNode : public rclcpp::Node
     NORMAL = 3,
   };
 
+  enum class ForceSlowDrivingType {
+    DEACTIVATED = 0,
+    READY = 1,
+    ACTIVATED = 2,
+  };
+
+  struct ForceAccelerationParam
+  {
+    double max_acceleration;
+    double max_jerk;
+    double max_lateral_acceleration;
+    double engage_velocity;
+    double engage_acceleration;
+  };
+
   struct Param
   {
     bool enable_lateral_acc_limit;
@@ -162,6 +181,9 @@ class VelocitySmootherNode : public rclcpp::Node
     AlgorithmType algorithm_type;  // Option : JerkFiltered, Linf, L2
 
     bool plan_from_ego_speed_on_manual_mode = true;
+
+    ForceAccelerationParam force_acceleration_param;
+    double force_slow_driving_velocity;
   } node_param_{};
 
   struct ExternalVelocityLimit
@@ -248,6 +270,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_;
   std::shared_ptr<rclcpp::Time> prev_time_;

planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp

@@ -83,6 +83,9 @@ class SmootherBase
   double getMinJerk() const;
 
   void setWheelBase(const double wheel_base);
+  void setMaxAccel(const double max_accel);
+  void setMaxJerk(const double max_jerk);
+  void setMaxLatAccel(const double max_accel);
 
   void setParam(const BaseParam & param);
   BaseParam getBaseParam() const;

planning/autoware_velocity_smoother/package.xml

@@ -32,6 +32,7 @@
   <depend>osqp_interface</depend>
   <depend>qp_interface</depend>
   <depend>rclcpp</depend>
+  <depend>std_srvs</depend>
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
   <depend>tier4_debug_msgs</depend>

planning/autoware_velocity_smoother/src/node.cpp

@@ -38,6 +38,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();
@@ -66,6 +67,13 @@ VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_opti
   sub_current_trajectory_ = create_subscription<Trajectory>(
     "~/input/trajectory", 1, std::bind(&VelocitySmootherNode::onCurrentTrajectory, this, _1));
 
+  srv_force_acceleration_ = create_service<SetBool>(
+    "~/adjust_common_param", std::bind(&VelocitySmootherNode::onForceAcceleration, this, _1, _2));
+  srv_slow_driving_ = create_service<SetBool>(
+    "~/slow_driving", std::bind(&VelocitySmootherNode::onSlowDriving, this, _1, _2));
+  force_acceleration_mode_ = false;
+  force_slow_driving_mode_ = ForceSlowDrivingType::DEACTIVATED;
+
   // parameter update
   set_param_res_ =
     this->add_on_set_parameters_callback(std::bind(&VelocitySmootherNode::onParameter, this, _1));
@@ -184,6 +192,15 @@ 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_acceleration.max_acc", p.force_acceleration_param.max_acceleration);
+    update_param("force_acceleration.max_jerk", p.force_acceleration_param.max_jerk);
+    update_param(
+      "force_acceleration.max_lateral_acc", p.force_acceleration_param.max_lateral_acceleration);
+    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);
   }
 
   {
@@ -303,6 +320,18 @@ void VelocitySmootherNode::initCommonParam()
 
   p.plan_from_ego_speed_on_manual_mode =
     declare_parameter<bool>("plan_from_ego_speed_on_manual_mode");
+
+  p.force_acceleration_param.max_acceleration =
+    declare_parameter<double>("force_acceleration.max_acc");
+  p.force_acceleration_param.max_jerk = declare_parameter<double>("force_acceleration.max_jerk");
+  p.force_acceleration_param.max_lateral_acceleration =
+    declare_parameter<double>("force_acceleration.max_lateral_acc");
+  p.force_acceleration_param.engage_velocity =
+    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
@@ -473,6 +502,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");
@@ -562,6 +599,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;
@@ -1111,6 +1155,56 @@ TrajectoryPoint VelocitySmootherNode::calcProjectedTrajectoryPointFromEgo(
   return calcProjectedTrajectoryPoint(trajectory, current_odometry_ptr_->pose.pose);
 }
 
+void VelocitySmootherNode::onForceAcceleration(
+  const std::shared_ptr<SetBool::Request> request, std::shared_ptr<SetBool::Response> response)
+{
+  std::string message = "default";
+
+  if (request->data && !force_acceleration_mode_) {
+    RCLCPP_INFO(get_logger(), "Force acceleration is activated");
+    smoother_->setMaxAccel(get_parameter("force_acceleration.max_acc").as_double());
+    smoother_->setMaxJerk(get_parameter("force_acceleration.max_jerk").as_double());
+    smoother_->setMaxLatAccel(get_parameter("force_acceleration.max_lateral_acc").as_double());
+    node_param_.engage_velocity = get_parameter("force_acceleration.engage_velocity").as_double();
+    node_param_.engage_acceleration =
+      get_parameter("force_acceleration.engage_acceleration").as_double();
+
+    force_acceleration_mode_ = true;
+    message = "Trigger force acceleration";
+  } else if (!request->data && force_acceleration_mode_) {
+    RCLCPP_INFO(get_logger(), "Force acceleration is deactivated");
+    smoother_->setMaxAccel(get_parameter("normal.max_acc").as_double());
+    smoother_->setMaxJerk(get_parameter("normal.max_jerk").as_double());
+    smoother_->setMaxLatAccel(get_parameter("max_lateral_accel").as_double());
+
+    node_param_.engage_velocity = get_parameter("engage_velocity").as_double();
+    node_param_.engage_acceleration = get_parameter("engage_acceleration").as_double();
+
+    force_acceleration_mode_ = false;
+    message = "Trigger normal acceleration";
+  }
+
+  response->success = true;
+  response->message = message;
+}
+
+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 driving";
+  } 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"

planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp

@@ -97,6 +97,21 @@ void SmootherBase::setWheelBase(const double wheel_base)
   base_param_.wheel_base = wheel_base;
 }
 
+void SmootherBase::setMaxAccel(const double max_accel)
+{
+  base_param_.max_accel = max_accel;
+}
+
+void SmootherBase::setMaxJerk(const double max_jerk)
+{
+  base_param_.max_jerk = max_jerk;
+}
+
+void SmootherBase::setMaxLatAccel(const double max_accel)
+{
+  base_param_.max_lateral_accel = max_accel;
+}
+
 void SmootherBase::setParam(const BaseParam & param)
 {
   base_param_ = param;