diff --git a/.github/CODEOWNERS b/.github/_CODEOWNERS similarity index 100% rename from .github/CODEOWNERS rename to .github/_CODEOWNERS 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 069363d2f65e0..cb49d979fc9d3 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,7 @@ 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_slow_driving_; Odometry::ConstSharedPtr current_odometry_ptr_; // current odometry AccelWithCovarianceStamped::ConstSharedPtr current_acceleration_ptr_; @@ -137,6 +140,12 @@ class VelocitySmootherNode : public rclcpp::Node NORMAL = 3, }; + enum class ForceSlowDrivingType { + DEACTIVATED = 0, + READY = 1, + ACTIVATED = 2, + }; + struct Param { bool enable_lateral_acc_limit; @@ -162,6 +171,8 @@ class VelocitySmootherNode : public rclcpp::Node AlgorithmType algorithm_type; // Option : JerkFiltered, Linf, L2 bool plan_from_ego_speed_on_manual_mode = true; + + double force_slow_driving_velocity; } node_param_{}; struct AccelerationRequest @@ -255,6 +266,11 @@ class VelocitySmootherNode : public rclcpp::Node // parameter handling void initCommonParam(); + // 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/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp index 1e0f6119ac21e..31dd10c04e357 100644 --- a/planning/autoware_velocity_smoother/src/node.cpp +++ b/planning/autoware_velocity_smoother/src/node.cpp @@ -39,6 +39,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(); @@ -67,6 +68,10 @@ VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_opti sub_current_trajectory_ = create_subscription( "~/input/trajectory", 1, std::bind(&VelocitySmootherNode::onCurrentTrajectory, this, _1)); + srv_slow_driving_ = create_service( + "~/slow_driving", std::bind(&VelocitySmootherNode::onSlowDriving, this, _1, _2)); + force_slow_driving_mode_ = ForceSlowDrivingType::DEACTIVATED; + // parameter update set_param_res_ = this->add_on_set_parameters_callback(std::bind(&VelocitySmootherNode::onParameter, this, _1)); @@ -185,6 +190,8 @@ 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_slow_driving.velocity", p.force_slow_driving_velocity); } { @@ -304,6 +311,8 @@ void VelocitySmootherNode::initCommonParam() p.plan_from_ego_speed_on_manual_mode = declare_parameter("plan_from_ego_speed_on_manual_mode"); + + p.force_slow_driving_velocity = declare_parameter("force_slow_driving.velocity"); } void VelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory) const @@ -489,6 +498,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"); @@ -578,6 +595,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; @@ -1142,6 +1166,23 @@ TrajectoryPoint VelocitySmootherNode::calcProjectedTrajectoryPointFromEgo( return calcProjectedTrajectoryPoint(trajectory, current_odometry_ptr_->pose.pose); } +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 drving"; + } 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"