From 34ea4a0635e8d09799e14f5f4972650748670c86 Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Fri, 7 Jun 2024 18:49:14 +0900 Subject: [PATCH 1/3] feat(motion_velocity_smoother)!: added force acceleration (#1329) * add feature for force acceleration Signed-off-by: Go Sakayori * Add log info when force acceleration is activated/deactivated Signed-off-by: Go Sakayori * fix enagage velocity/acceleration Signed-off-by: Go Sakayori * 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 Signed-off-by: Go Sakayori Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> --- .../autoware/velocity_smoother/node.hpp | 20 ++++++ .../smoother/smoother_base.hpp | 3 + .../autoware_velocity_smoother/package.xml | 1 + .../autoware_velocity_smoother/src/node.cpp | 67 +++++++++++++++++++ .../src/smoother/smoother_base.cpp | 15 +++++ 5 files changed, 106 insertions(+) diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp index 26918cce24549..6584de73d2c84 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp +++ b/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_planning_msgs/msg/stop_speed_exceeded.hpp" // temporary #include "tier4_planning_msgs/msg/velocity_limit.hpp" // temporary @@ -65,6 +66,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 @@ -97,6 +99,8 @@ class VelocitySmootherNode : public rclcpp::Node sub_external_velocity_limit_{this, "~/input/external_velocity_limit_mps"}; autoware::universe_utils::InterProcessPollingSubscriber sub_operation_mode_{ this, "~/input/operation_mode_state"}; + rclcpp::Service::SharedPtr srv_force_acceleration_; + rclcpp::Service::SharedPtr srv_slow_driving_; Odometry::ConstSharedPtr current_odometry_ptr_; // current odometry AccelWithCovarianceStamped::ConstSharedPtr current_acceleration_ptr_; @@ -134,6 +138,15 @@ class VelocitySmootherNode : 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; @@ -159,6 +172,8 @@ 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; } node_param_{}; struct ExternalVelocityLimit @@ -244,6 +259,11 @@ class VelocitySmootherNode : 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 autoware::universe_utils::StopWatch stop_watch_; diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp index 6671eaa3eabe7..5efb003db183e 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp @@ -82,6 +82,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; diff --git a/planning/autoware_velocity_smoother/package.xml b/planning/autoware_velocity_smoother/package.xml index 997f7b16e5652..37420b534938c 100644 --- a/planning/autoware_velocity_smoother/package.xml +++ b/planning/autoware_velocity_smoother/package.xml @@ -30,6 +30,7 @@ nav_msgs osqp_interface rclcpp + std_srvs tf2 tf2_ros tier4_debug_msgs diff --git a/planning/autoware_velocity_smoother/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp index 3640c93d2d807..20b98e8f72ea7 100644 --- a/planning/autoware_velocity_smoother/src/node.cpp +++ b/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( "~/input/trajectory", 1, std::bind(&VelocitySmootherNode::onCurrentTrajectory, this, _1)); + srv_force_acceleration_ = create_service( + "~/adjust_common_param", + std::bind(&VelocitySmootherNode::onForceAcceleration, this, _1, _2)); + srv_slow_driving_ = create_service( + "~/slow_driving", std::bind(&VelocitySmootherNode::onSlowDriving, this, _1, _2)); + force_acceleration_mode_ = false; + // parameter update set_param_res_ = this->add_on_set_parameters_callback(std::bind(&VelocitySmootherNode::onParameter, this, _1)); @@ -184,6 +192,14 @@ 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); } { @@ -303,6 +319,16 @@ void VelocitySmootherNode::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 VelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory) const @@ -1116,6 +1142,47 @@ TrajectoryPoint VelocitySmootherNode::calcProjectedTrajectoryPointFromEgo( return calcProjectedTrajectoryPoint(trajectory, current_odometry_ptr_->pose.pose); } +void VelocitySmootherNode::onForceAcceleration( + const std::shared_ptr request, std::shared_ptr 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 VelocitySmootherNode::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 autoware::velocity_smoother #include "rclcpp_components/register_node_macro.hpp" diff --git a/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp index 46faf10fe4a62..131327ee9ad0b 100644 --- a/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp +++ b/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; From e3c1699c97fb31dc610ccb4eb145ae82d1440a04 Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Thu, 11 Jul 2024 13:07:15 +0900 Subject: [PATCH 2/3] feat(motion_velocity_smoother): force slow driving (#1409) * add force slow driving function Signed-off-by: Go Sakayori * fix state Signed-off-by: Go Sakayori * erase log info Signed-off-by: Go Sakayori --------- Signed-off-by: Go Sakayori Signed-off-by: Go Sakayori --- .../autoware/velocity_smoother/node.hpp | 12 ++++++++ .../autoware_velocity_smoother/src/node.cpp | 30 ++++++++++++++++++- 2 files changed, 41 insertions(+), 1 deletion(-) diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp index 6584de73d2c84..bcc9338422c9f 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp @@ -138,6 +138,12 @@ class VelocitySmootherNode : public rclcpp::Node NORMAL = 3, }; + enum class ForceSlowDrivingType { + DEACTIVATED = 0, + READY = 1, + ACTIVATED = 2, + }; + struct ForceAccelerationParam { double max_acceleration; @@ -174,6 +180,7 @@ class VelocitySmootherNode : public rclcpp::Node bool plan_from_ego_speed_on_manual_mode = true; ForceAccelerationParam force_acceleration_param; + double force_slow_driving_velocity; } node_param_{}; struct ExternalVelocityLimit @@ -259,11 +266,16 @@ class VelocitySmootherNode : public rclcpp::Node // parameter handling void initCommonParam(); + + // Related to force acceleration void onForceAcceleration( const std::shared_ptr request, std::shared_ptr response); bool force_acceleration_mode_; + + // Related to force slow driving void onSlowDriving( const std::shared_ptr request, std::shared_ptr response); + ForceSlowDrivingType force_slow_driving_mode_; // debug autoware::universe_utils::StopWatch stop_watch_; diff --git a/planning/autoware_velocity_smoother/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp index 20b98e8f72ea7..eeebc96f1516d 100644 --- a/planning/autoware_velocity_smoother/src/node.cpp +++ b/planning/autoware_velocity_smoother/src/node.cpp @@ -73,6 +73,7 @@ VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_opti srv_slow_driving_ = create_service( "~/slow_driving", std::bind(&VelocitySmootherNode::onSlowDriving, this, _1, _2)); force_acceleration_mode_ = false; + force_slow_driving_mode_ = ForceSlowDrivingType::DEACTIVATED; // parameter update set_param_res_ = @@ -200,6 +201,7 @@ rcl_interfaces::msg::SetParametersResult VelocitySmootherNode::onParameter( 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); } { @@ -329,6 +331,8 @@ void VelocitySmootherNode::initCommonParam() declare_parameter("force_acceleration.engage_velocity"); p.force_acceleration_param.engage_acceleration = declare_parameter("force_acceleration.engage_acceleration"); + + p.force_slow_driving_velocity = declare_parameter("force_slow_driving.velocity"); } void VelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory) const @@ -502,6 +506,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"); @@ -591,6 +603,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; @@ -1172,12 +1191,21 @@ void VelocitySmootherNode::onForceAcceleration( } response->success = true; + response->message = message; } void VelocitySmootherNode::onSlowDriving( const std::shared_ptr request, std::shared_ptr response) { - const std::string message = request->data ? "Slow driving" : "Default"; + std::string message = "default"; + if (request->data && force_slow_driving_mode_ == ForceSlowDrivingType::DEACTIVATED) { + force_slow_driving_mode_ = ForceSlowDrivingType::READY; + + message = "Activated force slow drving"; + } else if (!request->data) { + force_slow_driving_mode_ = ForceSlowDrivingType::DEACTIVATED; + message = "Deactivated force slow driving"; + } response->success = true; response->message = message; From def5cadc81177563c515fb82dbacd63cb3552853 Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Wed, 7 Aug 2024 11:35:15 +0900 Subject: [PATCH 3/3] fix spelling mistake Signed-off-by: Go Sakayori --- planning/autoware_velocity_smoother/src/node.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/planning/autoware_velocity_smoother/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp index eeebc96f1516d..ca0fdc9ca7117 100644 --- a/planning/autoware_velocity_smoother/src/node.cpp +++ b/planning/autoware_velocity_smoother/src/node.cpp @@ -68,8 +68,7 @@ VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_opti "~/input/trajectory", 1, std::bind(&VelocitySmootherNode::onCurrentTrajectory, this, _1)); srv_force_acceleration_ = create_service( - "~/adjust_common_param", - std::bind(&VelocitySmootherNode::onForceAcceleration, this, _1, _2)); + "~/adjust_common_param", std::bind(&VelocitySmootherNode::onForceAcceleration, this, _1, _2)); srv_slow_driving_ = create_service( "~/slow_driving", std::bind(&VelocitySmootherNode::onSlowDriving, this, _1, _2)); force_acceleration_mode_ = false; @@ -1201,7 +1200,7 @@ void VelocitySmootherNode::onSlowDriving( if (request->data && force_slow_driving_mode_ == ForceSlowDrivingType::DEACTIVATED) { force_slow_driving_mode_ = ForceSlowDrivingType::READY; - message = "Activated force slow drving"; + message = "Activated force slow driving"; } else if (!request->data) { force_slow_driving_mode_ = ForceSlowDrivingType::DEACTIVATED; message = "Deactivated force slow driving";