From 2a53348eadf259a302fbac960fabb1464a3df698 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 14 Nov 2023 21:37:23 +0900 Subject: [PATCH] feat(motion_velocity_smoother): add motion_velocity_smoother's virtual wall such as MRM (#5555) * feat: add motion_velocity_smoother's virtual wall Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../src/external_velocity_limit_selector_node.cpp | 1 + .../motion_velocity_smoother_node.hpp | 5 +++++ .../src/motion_velocity_smoother_node.cpp | 14 ++++++++++++++ 3 files changed, 20 insertions(+) diff --git a/planning/external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp b/planning/external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp index 2ba0a4c1d46f3..ccca5e97d2b38 100644 --- a/planning/external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp +++ b/planning/external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp @@ -46,6 +46,7 @@ VelocityLimit getHardestLimit( // find hardest max velocity if (max_velocity < hardest_max_velocity) { hardest_limit.stamp = limit.second.stamp; + hardest_limit.sender = limit.first; hardest_limit.max_velocity = max_velocity; hardest_max_velocity = max_velocity; } 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 347d19f631588..7be91d67cb945 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 @@ -42,6 +42,7 @@ #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 +#include "visualization_msgs/msg/marker_array.hpp" #include #include @@ -63,6 +64,7 @@ using nav_msgs::msg::Odometry; using tier4_debug_msgs::msg::Float32Stamped; // temporary using tier4_planning_msgs::msg::StopSpeedExceeded; // temporary using tier4_planning_msgs::msg::VelocityLimit; // temporary +using visualization_msgs::msg::MarkerArray; struct Motion { @@ -80,6 +82,7 @@ class MotionVelocitySmootherNode : public rclcpp::Node private: rclcpp::Publisher::SharedPtr pub_trajectory_; + rclcpp::Publisher::SharedPtr pub_virtual_wall_; rclcpp::Publisher::SharedPtr pub_over_stop_velocity_; rclcpp::Subscription::SharedPtr sub_current_odometry_; rclcpp::Subscription::SharedPtr sub_current_acceleration_; @@ -95,6 +98,7 @@ class MotionVelocitySmootherNode : public rclcpp::Node double max_velocity_with_deceleration_; // maximum velocity with deceleration // for external velocity limit double wheelbase_; // wheelbase + double base_link2front_; // base_link to front TrajectoryPoints prev_output_; // previously published trajectory @@ -153,6 +157,7 @@ class MotionVelocitySmootherNode : public rclcpp::Node { double velocity{0.0}; // current external_velocity_limit double dist{0.0}; // distance to set external velocity limit + std::string sender{""}; }; ExternalVelocityLimit external_velocity_limit_; // velocity and distance constraint of external velocity limit 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 6b5da01b9cb09..a27e02c15710f 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -14,6 +14,7 @@ #include "motion_velocity_smoother/motion_velocity_smoother_node.hpp" +#include "motion_utils/marker/marker_helper.hpp" #include "motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp" #include "motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" #include "motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" @@ -41,6 +42,7 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions // set common params const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); wheelbase_ = vehicle_info.wheel_base_m; + base_link2front_ = vehicle_info.max_longitudinal_offset_m; initCommonParam(); over_stop_velocity_warn_thr_ = declare_parameter("over_stop_velocity_warn_thr"); @@ -49,6 +51,7 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions // publishers, subscribers pub_trajectory_ = create_publisher("~/output/trajectory", 1); + pub_virtual_wall_ = create_publisher("~/virtual_wall", 1); pub_velocity_limit_ = create_publisher( "~/output/current_velocity_limit_mps", rclcpp::QoS{1}.transient_local()); pub_dist_to_stopline_ = create_publisher("~/distance_to_stopline", 1); @@ -329,6 +332,9 @@ void MotionVelocitySmootherNode::calcExternalVelocityLimit() return; } + // sender + external_velocity_limit_.sender = external_velocity_limit_ptr_->sender; + // on the first time, apply directly if (prev_output_.empty() || !current_closest_point_from_prev_output_) { external_velocity_limit_.velocity = external_velocity_limit_ptr_->max_velocity; @@ -890,6 +896,14 @@ void MotionVelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & t trajectory_utils::applyMaximumVelocityLimit( *inserted_index, traj.size(), external_velocity_limit_.velocity, traj); + // create virtual wall + if (std::abs(external_velocity_limit_.velocity) < 1e-3) { + const auto virtual_wall_marker = motion_utils::createStopVirtualWallMarker( + traj.at(*inserted_index).pose, external_velocity_limit_.sender, this->now(), 0, + base_link2front_); + pub_virtual_wall_->publish(virtual_wall_marker); + } + RCLCPP_DEBUG( get_logger(), "externalVelocityLimit : limit_vel = %.3f", external_velocity_limit_.velocity); }