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 315656ef80f2f..850a70b161580 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(&MotionVelocitySmootherNode::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 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 && 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;