From 24c8e869423774f598f9e178e5bbd1522947ddec Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Mon, 3 Jun 2024 17:12:07 +0900 Subject: [PATCH 1/5] add feature for force acceleration Signed-off-by: Go Sakayori --- .../motion_velocity_smoother_node.hpp | 18 ++++++ .../smoother/smoother_base.hpp | 6 +- planning/motion_velocity_smoother/package.xml | 1 + .../src/motion_velocity_smoother_node.cpp | 55 +++++++++++++++++++ .../src/smoother/smoother_base.cpp | 15 +++++ 5 files changed, 94 insertions(+), 1 deletion(-) diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp index 868c1ab12cce8..137b1ca8d7aaf 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp +++ b/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::SharedPtr sub_current_trajectory_; rclcpp::Subscription::SharedPtr sub_external_velocity_limit_; rclcpp::Subscription::SharedPtr sub_operation_mode_; + rclcpp::Service::SharedPtr srv_force_acceleration_; + rclcpp::Service::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,9 @@ class MotionVelocitySmootherNode : public rclcpp::Node // parameter handling void initCommonParam(); + void onForceAcceleration(const std::shared_ptr request, std::shared_ptr response); + bool force_acceleration_mode_; + void onSlowDriving(const std::shared_ptr request, std::shared_ptr response); // debug tier4_autoware_utils::StopWatch stop_watch_; diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp index beb571635f70c..9d1be3d53f791 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp @@ -77,9 +77,13 @@ 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/package.xml b/planning/motion_velocity_smoother/package.xml index 9792aa2bdd60b..3462ec68273ef 100644 --- a/planning/motion_velocity_smoother/package.xml +++ b/planning/motion_velocity_smoother/package.xml @@ -28,6 +28,7 @@ osqp_interface planning_test_utils rclcpp + std_srvs tf2 tf2_ros tier4_autoware_utils diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp index eb8592bb99637..e2adfbdda7b8c 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/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,10 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions "~/input/operation_mode_state", 1, [this](const OperationModeState::ConstSharedPtr msg) { operation_mode_ = *msg; }); + srv_force_acceleration_ = create_service("~/adjust_common_param", std::bind(&MotionVelocitySmootherNode::onForceAcceleration, this, _1, _2)); + srv_slow_driving_ = create_service("~/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 +194,12 @@ 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 +319,12 @@ void MotionVelocitySmootherNode::initCommonParam() p.plan_from_ego_speed_on_manual_mode = declare_parameter("plan_from_ego_speed_on_manual_mode"); + + p.force_acceleration_param.max_acceleration = declare_parameter("force_acceleration.max_acc"); + p.force_acceleration_param.max_jerk = declare_parameter("force_acceleration.max_jerk"); + p.force_acceleration_param.max_lateral_acceleration = declare_parameter("force_acceleration.max_lateral_acc"); + p.force_acceleration_param.engage_velocity = declare_parameter("force_acceleration.engage_velocity"); + p.force_acceleration_param.engage_acceleration = declare_parameter("force_acceleration.engage_acceleration"); } void MotionVelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory) const @@ -1100,6 +1117,44 @@ TrajectoryPoint MotionVelocitySmootherNode::calcProjectedTrajectoryPointFromEgo( return calcProjectedTrajectoryPoint(trajectory, current_odometry_ptr_->pose.pose); } +void MotionVelocitySmootherNode::onForceAcceleration(const std::shared_ptr request, std::shared_ptr response) +{ + std::string message = "defualt"; + + if(request->data && !force_acceleration_mode_){ + 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(); + + force_acceleration_mode_ = true; + message = "Trigger force acceleration"; + } + else if(!request->data && force_acceleration_mode_){ + 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(); + + force_acceleration_mode_ = false; + message = "Trigger normal acceleration"; + } + + response->success = true; + +} + +void MotionVelocitySmootherNode::onSlowDriving(const std::shared_ptr request, std::shared_ptr 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" diff --git a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp index bf193b7251382..ee554d60dde6f 100644 --- a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp +++ b/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; From b9f589b6934d79f27f9b7a47bf841c6d03fbf591 Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Tue, 4 Jun 2024 14:58:53 +0900 Subject: [PATCH 2/5] Add log info when force acceleration is activated/deactivated Signed-off-by: Go Sakayori --- .../src/motion_velocity_smoother_node.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp index e2adfbdda7b8c..d94e188915b3d 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -1122,6 +1122,7 @@ void MotionVelocitySmootherNode::onForceAcceleration(const std::shared_ptrdata && !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()); @@ -1132,6 +1133,7 @@ void MotionVelocitySmootherNode::onForceAcceleration(const std::shared_ptrdata && 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()); From 721f15ce9b8e86db4e50b98f6d621b21c20764b2 Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Thu, 6 Jun 2024 23:29:13 +0900 Subject: [PATCH 3/5] fix enagage velocity/acceleration Signed-off-by: Go Sakayori --- .../motion_velocity_smoother_node.hpp | 8 ++-- .../smoother/smoother_base.hpp | 3 +- .../src/motion_velocity_smoother_node.cpp | 46 +++++++++++-------- 3 files changed, 34 insertions(+), 23 deletions(-) diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp index 137b1ca8d7aaf..c1c336782cae4 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp +++ b/planning/motion_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 request, std::shared_ptr response); + void onForceAcceleration( + const std::shared_ptr request, std::shared_ptr response); bool force_acceleration_mode_; - void onSlowDriving(const std::shared_ptr request, std::shared_ptr response); + void onSlowDriving( + const std::shared_ptr request, std::shared_ptr response); // debug tier4_autoware_utils::StopWatch stop_watch_; diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp index 9d1be3d53f791..8b8c920e3f2f7 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp +++ b/planning/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 index d94e188915b3d..add4921aea138 100644 --- 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("~/adjust_common_param", std::bind(&MotionVelocitySmootherNode::onForceAcceleration, this, _1, _2)); - srv_slow_driving_ = create_service("~/slow_driving", std::bind(&MotionVelocitySmootherNode::onSlowDriving, this, _1, _2)); + srv_force_acceleration_ = create_service( + "~/adjust_common_param", + std::bind(&MotionVelocitySmootherNode::onForceAcceleration, this, _1, _2)); + srv_slow_driving_ = create_service( + "~/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("plan_from_ego_speed_on_manual_mode"); - p.force_acceleration_param.max_acceleration = declare_parameter("force_acceleration.max_acc"); + p.force_acceleration_param.max_acceleration = + declare_parameter("force_acceleration.max_acc"); p.force_acceleration_param.max_jerk = declare_parameter("force_acceleration.max_jerk"); - p.force_acceleration_param.max_lateral_acceleration = declare_parameter("force_acceleration.max_lateral_acc"); - p.force_acceleration_param.engage_velocity = declare_parameter("force_acceleration.engage_velocity"); - p.force_acceleration_param.engage_acceleration = declare_parameter("force_acceleration.engage_acceleration"); + p.force_acceleration_param.max_lateral_acceleration = + declare_parameter("force_acceleration.max_lateral_acc"); + p.force_acceleration_param.engage_velocity = + declare_parameter("force_acceleration.engage_velocity"); + p.force_acceleration_param.engage_acceleration = + declare_parameter("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 request, std::shared_ptr response) +void MotionVelocitySmootherNode::onForceAcceleration( + const std::shared_ptr request, std::shared_ptr 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 request, std::shared_ptr response) +void MotionVelocitySmootherNode::onSlowDriving( + const std::shared_ptr request, std::shared_ptr response) { const std::string message = request->data ? "Slow driving" : "Default"; From 3d603d1d79856a99d930be5b12b6f1ab6a00f192 Mon Sep 17 00:00:00 2001 From: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Date: Fri, 7 Jun 2024 18:40:41 +0900 Subject: [PATCH 4/5] Update planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp --- .../src/motion_velocity_smoother_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp index add4921aea138..d66daaae01ec8 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -1143,7 +1143,7 @@ void MotionVelocitySmootherNode::onForceAcceleration( force_acceleration_mode_ = true; message = "Trigger force acceleration"; } else if (!request->data && force_acceleration_mode_) { - RCLCPP_INFO(get_logger(), "Force accelration is deactivated"); + 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()); From 8d185be655820dc14d91a63e07c6afdb5f05fc13 Mon Sep 17 00:00:00 2001 From: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Date: Fri, 7 Jun 2024 18:46:50 +0900 Subject: [PATCH 5/5] Update planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp --- .../src/motion_velocity_smoother_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp index d66daaae01ec8..39f9a8b6e2811 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -1129,7 +1129,7 @@ TrajectoryPoint MotionVelocitySmootherNode::calcProjectedTrajectoryPointFromEgo( void MotionVelocitySmootherNode::onForceAcceleration( const std::shared_ptr request, std::shared_ptr response) { - std::string message = "defualt"; + std::string message = "default"; if (request->data && !force_acceleration_mode_) { RCLCPP_INFO(get_logger(), "Force acceleration is activated");