fix enagage velocity/acceleration

Signed-off-by: Go Sakayori <gsakayori@gmail.com>
tier4 · Jun 6, 2024 · 721f15c · 721f15c
1 parent b9f589b
commit 721f15c
Show file tree

Hide file tree

Showing 3 changed files with 34 additions and 23 deletions.
diff --git a/...tion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp b/...tion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp
@@ -132,7 +132,7 @@ class MotionVelocitySmootherNode : public rclcpp::Node
     NORMAL = 3,
   };
 
-    struct ForceAccelerationParam
+  struct ForceAccelerationParam
   {
     double max_acceleration;
     double max_jerk;
@@ -257,9 +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);
+  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);
+  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_;

diff --git a/...ning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp b/...ning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp
@@ -77,13 +77,12 @@ class SmootherBase
   double getMinDecel() const;
   double getMaxJerk() const;
   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;
 

diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp
@@ -73,8 +73,11 @@ 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));
+  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
@@ -197,9 +200,11 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter
 
     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.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_acceleration.engage_acceleration", p.force_acceleration_param.engage_acceleration);
   }
 
   {
@@ -320,11 +325,15 @@ 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_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_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
@@ -1117,39 +1126,40 @@ 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)
+void MotionVelocitySmootherNode::onForceAcceleration(
+  const std::shared_ptr<SetBool::Request> request, std::shared_ptr<SetBool::Response> response)
 {
   std::string message = "defualt";
 
-  if(request->data && !force_acceleration_mode_){
+  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_.force_acceleration_param.engage_velocity = get_parameter("force_acceleration.engage_velocity").as_double();
-    node_param_.force_acceleration_param.engage_acceleration = get_parameter("force_acceleration.engage_acceleration").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_){
+  } else if (!request->data && force_acceleration_mode_) {
     RCLCPP_INFO(get_logger(), "Force accelration 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_.force_acceleration_param.engage_velocity = get_parameter("engage_velocity").as_double();
-    node_param_.force_acceleration_param.engage_acceleration = get_parameter("engage_acceleration").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)
+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";