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 da31ffc8bfd7b..dc8ff89f5be81 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 @@ -150,6 +154,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 837e6b1720a91..87fea8101c499 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 = 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); @@ -323,6 +326,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; @@ -879,6 +885,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); }