feat(motion_velocity_smoother)!: added force acceleration (#1329)

* add feature for force acceleration

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

* Add log info when force acceleration is activated/deactivated

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

* fix enagage velocity/acceleration

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

* Update planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp

* Update planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp

---------

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>
Signed-off-by: Go Sakayori <gsakayori@gmail.com>
Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com>

Author: go-sakayori

planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp

@@ -41,6 +41,7 @@
 #include "autoware_auto_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_planning_msgs/msg/stop_speed_exceeded.hpp"  // temporary
 #include "tier4_planning_msgs/msg/velocity_limit.hpp"       // temporary
@@ -63,6 +64,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_planning_msgs::msg::StopSpeedExceeded;  // temporary
 using tier4_planning_msgs::msg::VelocityLimit;      // temporary
@@ -91,6 +93,8 @@ class MotionVelocitySmootherNode : public rclcpp::Node
   rclcpp::Subscription<Trajectory>::SharedPtr sub_current_trajectory_;
   rclcpp::Subscription<VelocityLimit>::SharedPtr sub_external_velocity_limit_;
   rclcpp::Subscription<OperationModeState>::SharedPtr sub_operation_mode_;
+  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_;
@@ -128,6 +132,15 @@ class MotionVelocitySmootherNode : public rclcpp::Node
     NORMAL = 3,
   };
 
+  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;
@@ -153,6 +166,8 @@ class MotionVelocitySmootherNode : public rclcpp::Node
     AlgorithmType algorithm_type;  // Option : JerkFiltered, Linf, L2
 
     bool plan_from_ego_speed_on_manual_mode = true;
+
+    ForceAccelerationParam force_acceleration_param;
   } node_param_{};
 
   struct ExternalVelocityLimit
@@ -242,6 +257,11 @@ class MotionVelocitySmootherNode : public rclcpp::Node
 
   // parameter handling
   void initCommonParam();
+  void onForceAcceleration(
+    const std::shared_ptr<SetBool::Request> request, std::shared_ptr<SetBool::Response> response);
+  bool force_acceleration_mode_;
+  void onSlowDriving(
+    const std::shared_ptr<SetBool::Request> request, std::shared_ptr<SetBool::Response> response);
 
   // debug
   tier4_autoware_utils::StopWatch<std::chrono::milliseconds> stop_watch_;

planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp

@@ -79,6 +79,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/motion_velocity_smoother/package.xml

@@ -28,6 +28,7 @@
   <depend>osqp_interface</depend>
   <depend>planning_test_utils</depend>
   <depend>rclcpp</depend>
+  <depend>std_srvs</depend>
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
   <depend>tier4_autoware_utils</depend>

planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp

@@ -38,6 +38,7 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions
 : Node("motion_velocity_smoother", node_options)
 {
   using std::placeholders::_1;
+  using std::placeholders::_2;
 
   // set common params
   const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo();
@@ -72,6 +73,13 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions
     "~/input/operation_mode_state", 1,
     [this](const OperationModeState::ConstSharedPtr msg) { operation_mode_ = *msg; });
 
+  srv_force_acceleration_ = create_service<SetBool>(
+    "~/adjust_common_param",
+    std::bind(&MotionVelocitySmootherNode::onForceAcceleration, this, _1, _2));
+  srv_slow_driving_ = create_service<SetBool>(
+    "~/slow_driving", std::bind(&MotionVelocitySmootherNode::onSlowDriving, this, _1, _2));
+  force_acceleration_mode_ = false;
+
   // parameter update
   set_param_res_ = this->add_on_set_parameters_callback(
     std::bind(&MotionVelocitySmootherNode::onParameter, this, _1));
@@ -189,6 +197,14 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::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);
   }
 
   {
@@ -308,6 +324,16 @@ void MotionVelocitySmootherNode::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");
 }
 
 void MotionVelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory) const
@@ -1100,6 +1126,47 @@ TrajectoryPoint MotionVelocitySmootherNode::calcProjectedTrajectoryPointFromEgo(
   return calcProjectedTrajectoryPoint(trajectory, current_odometry_ptr_->pose.pose);
 }
 
+void MotionVelocitySmootherNode::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;
+}
+
+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";
+
+  response->success = true;
+  response->message = message;
+}
+
 }  // namespace motion_velocity_smoother
 
 #include "rclcpp_components/register_node_macro.hpp"

planning/motion_velocity_smoother/src/smoother/smoother_base.cpp

@@ -95,6 +95,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;