From be42248cb50da5133330a2d9449a61f186228754 Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Mon, 24 Jun 2024 10:38:01 +0900 Subject: [PATCH 1/3] add force slow driving function Signed-off-by: Go Sakayori --- .../motion_velocity_smoother_node.hpp | 12 +++++++ .../src/motion_velocity_smoother_node.cpp | 32 ++++++++++++++++++- 2 files changed, 43 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 c1c336782cae4..d44b0036fac89 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,6 +132,12 @@ class MotionVelocitySmootherNode : public rclcpp::Node NORMAL = 3, }; + enum class ForceSlowDrivingType{ + DEACTIVATED = 0, + READY = 1, + ACTIVATED = 2, + }; + struct ForceAccelerationParam { double max_acceleration; @@ -168,6 +174,7 @@ class MotionVelocitySmootherNode : 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 @@ -257,11 +264,16 @@ class MotionVelocitySmootherNode : 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 tier4_autoware_utils::StopWatch stop_watch_; 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 39f9a8b6e2811..ad81040cc54dd 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -79,6 +79,7 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions srv_slow_driving_ = create_service( "~/slow_driving", std::bind(&MotionVelocitySmootherNode::onSlowDriving, this, _1, _2)); force_acceleration_mode_ = false; + force_slow_driving_mode_ = ForceSlowDrivingType::DEACTIVATED; // parameter update set_param_res_ = this->add_on_set_parameters_callback( @@ -205,6 +206,7 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::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); } { @@ -334,6 +336,8 @@ void MotionVelocitySmootherNode::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 MotionVelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory) const @@ -504,6 +508,11 @@ void MotionVelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstShar flipVelocity(input_points); } + double vel_threshold = get_parameter("force_slow_driving.velocity").as_double(); + if (force_slow_driving_mode_ == ForceSlowDrivingType::READY && current_odometry_ptr_->twist.twist.linear.x < 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"); @@ -589,6 +598,14 @@ TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity( // Apply velocity to approach stop point applyStopApproachingVelocity(traj_extracted); + // Apply force slow driving if activated + double vel_threshold = get_parameter("force_slow_driving.velocity").as_double(); + if(force_slow_driving_mode_ == ForceSlowDrivingType::ACTIVATED){ + for (auto & tp : traj_extracted) { + tp.longitudinal_velocity_mps = vel_threshold; + } + } + // Debug if (publish_debug_trajs_) { auto tmp = traj_extracted; @@ -1156,12 +1173,25 @@ void MotionVelocitySmootherNode::onForceAcceleration( } response->success = true; + response->message = message; } void MotionVelocitySmootherNode::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) { + RCLCPP_INFO(get_logger(), "Force slow driving is activated"); + + if(force_slow_driving_mode_ == ForceSlowDrivingType::DEACTIVATED) + force_slow_driving_mode_ = ForceSlowDrivingType::READY; + + message = "Activated force slow drving"; + } else { + RCLCPP_INFO(get_logger(), "Force slow driving is deactivated"); + force_slow_driving_mode_ = ForceSlowDrivingType::DEACTIVATED; + message = "Deactivated force slow driving"; + } response->success = true; response->message = message; From bc841a928814bb14209dc5a84c02f928b5140f1c Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Mon, 24 Jun 2024 11:44:23 +0900 Subject: [PATCH 2/3] fix state Signed-off-by: Go Sakayori --- .../src/motion_velocity_smoother_node.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) 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 ad81040cc54dd..6abddcaf000e3 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -1180,14 +1180,13 @@ void MotionVelocitySmootherNode::onSlowDriving( const std::shared_ptr request, std::shared_ptr response) { std::string message = "default"; - if (request->data) { + if (request->data && force_slow_driving_mode_ == ForceSlowDrivingType::DEACTIVATED) { RCLCPP_INFO(get_logger(), "Force slow driving is activated"); - if(force_slow_driving_mode_ == ForceSlowDrivingType::DEACTIVATED) - force_slow_driving_mode_ = ForceSlowDrivingType::READY; + force_slow_driving_mode_ = ForceSlowDrivingType::READY; message = "Activated force slow drving"; - } else { + } else if(!request->data) { RCLCPP_INFO(get_logger(), "Force slow driving is deactivated"); force_slow_driving_mode_ = ForceSlowDrivingType::DEACTIVATED; message = "Deactivated force slow driving"; From e372ab6879edb8961fa564d3438c2ca19b4c9063 Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Thu, 11 Jul 2024 11:39:47 +0900 Subject: [PATCH 3/3] erase log info Signed-off-by: Go Sakayori --- .../motion_velocity_smoother_node.hpp | 2 +- .../src/motion_velocity_smoother_node.cpp | 17 ++++++++--------- 2 files changed, 9 insertions(+), 10 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 d44b0036fac89..bab02f954b24f 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, }; - enum class ForceSlowDrivingType{ + enum class ForceSlowDrivingType { DEACTIVATED = 0, READY = 1, ACTIVATED = 2, 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 6abddcaf000e3..3e1f69835ac6f 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -508,8 +508,11 @@ void MotionVelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstShar flipVelocity(input_points); } - double vel_threshold = get_parameter("force_slow_driving.velocity").as_double(); - if (force_slow_driving_mode_ == ForceSlowDrivingType::READY && current_odometry_ptr_->twist.twist.linear.x < vel_threshold) { + // 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; } @@ -599,10 +602,9 @@ TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity( applyStopApproachingVelocity(traj_extracted); // Apply force slow driving if activated - double vel_threshold = get_parameter("force_slow_driving.velocity").as_double(); - if(force_slow_driving_mode_ == ForceSlowDrivingType::ACTIVATED){ + if (force_slow_driving_mode_ == ForceSlowDrivingType::ACTIVATED) { for (auto & tp : traj_extracted) { - tp.longitudinal_velocity_mps = vel_threshold; + tp.longitudinal_velocity_mps = get_parameter("force_slow_driving.velocity").as_double(); } } @@ -1181,13 +1183,10 @@ void MotionVelocitySmootherNode::onSlowDriving( { std::string message = "default"; if (request->data && force_slow_driving_mode_ == ForceSlowDrivingType::DEACTIVATED) { - RCLCPP_INFO(get_logger(), "Force slow driving is activated"); - force_slow_driving_mode_ = ForceSlowDrivingType::READY; message = "Activated force slow drving"; - } else if(!request->data) { - RCLCPP_INFO(get_logger(), "Force slow driving is deactivated"); + } else if (!request->data) { force_slow_driving_mode_ = ForceSlowDrivingType::DEACTIVATED; message = "Deactivated force slow driving"; }