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..315656ef80f2f 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(&MotionVelocitySmootherNode::onForceAcceleration, this, _1, _2)); + srv_slow_driving_ = create_service( + "~/slow_driving", std::bind(&MotionVelocitySmootherNode::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 MotionVelocitySmootherNode::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 MotionVelocitySmootherNode::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;