From 24c8e869423774f598f9e178e5bbd1522947ddec Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Mon, 3 Jun 2024 17:12:07 +0900 Subject: [PATCH] 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;