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 d6033b66d5d6f..b0074295589a3 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_debug_msgs/msg/float64_stamped.hpp" // temporary #include "tier4_planning_msgs/msg/stop_speed_exceeded.hpp" // temporary @@ -66,6 +67,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_debug_msgs::msg::Float64Stamped; // temporary using tier4_planning_msgs::msg::StopSpeedExceeded; // temporary @@ -100,6 +102,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_; @@ -137,6 +141,21 @@ class VelocitySmootherNode : public rclcpp::Node NORMAL = 3, }; + enum class ForceSlowDrivingType { + DEACTIVATED = 0, + READY = 1, + ACTIVATED = 2, + }; + + 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; @@ -162,6 +181,9 @@ 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; + double force_slow_driving_velocity; } node_param_{}; struct ExternalVelocityLimit @@ -248,6 +270,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_; std::shared_ptr prev_time_; 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 7206a64ea32eb..893a66f7eb7fa 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 @@ -83,6 +83,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 65684e414e90d..7d5642f85a058 100644 --- a/planning/autoware_velocity_smoother/package.xml +++ b/planning/autoware_velocity_smoother/package.xml @@ -32,6 +32,7 @@ osqp_interface qp_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 a358a1b61c11b..023d04942f939 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; + force_slow_driving_mode_ = ForceSlowDrivingType::DEACTIVATED; + // parameter update set_param_res_ = this->add_on_set_parameters_callback(std::bind(&VelocitySmootherNode::onParameter, this, _1)); @@ -184,6 +192,15 @@ 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); + update_param("force_slow_driving.velocity", p.force_slow_driving_velocity); } { @@ -303,6 +320,18 @@ 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"); + + p.force_slow_driving_velocity = declare_parameter("force_slow_driving.velocity"); } void VelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory) const @@ -473,6 +502,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"); @@ -562,6 +599,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; @@ -1111,6 +1155,56 @@ 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; + response->message = message; +} + +void VelocitySmootherNode::onSlowDriving( + const std::shared_ptr request, std::shared_ptr response) +{ + std::string message = "default"; + if (request->data && force_slow_driving_mode_ == ForceSlowDrivingType::DEACTIVATED) { + force_slow_driving_mode_ = ForceSlowDrivingType::READY; + + message = "Activated force slow driving"; + } else if (!request->data) { + force_slow_driving_mode_ = ForceSlowDrivingType::DEACTIVATED; + message = "Deactivated force slow driving"; + } + + 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;