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..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,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..3e1f69835ac6f 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,14 @@ void MotionVelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstShar 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"); @@ -589,6 +601,13 @@ TrajectoryPoints MotionVelocitySmootherNode::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; @@ -1156,12 +1175,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;