feat(motion_velocity_smoother): add motion_velocity_smoother's virtua…

…l wall (#1024)

feat(motion_velocity_smoother): add motion_velocity_smoother's virtual wall such as MRM (autowarefoundation#5555)

* feat: add motion_velocity_smoother's virtual wall



* fix



---------

Signed-off-by: Takayuki Murooka <[email protected]>
Co-authored-by: Takayuki Murooka <[email protected]>

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;
     }

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 <iostream>
 #include <memory>
@@ -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<Trajectory>::SharedPtr pub_trajectory_;
+  rclcpp::Publisher<MarkerArray>::SharedPtr pub_virtual_wall_;
   rclcpp::Publisher<StopSpeedExceeded>::SharedPtr pub_over_stop_velocity_;
   rclcpp::Subscription<Odometry>::SharedPtr sub_current_odometry_;
   rclcpp::Subscription<AccelWithCovarianceStamped>::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

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<double>("over_stop_velocity_warn_thr");
 
@@ -49,6 +51,7 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions
 
   // publishers, subscribers
   pub_trajectory_ = create_publisher<Trajectory>("~/output/trajectory", 1);
+  pub_virtual_wall_ = create_publisher<MarkerArray>("~/virtual_wall", 1);
   pub_velocity_limit_ = create_publisher<VelocityLimit>(
     "~/output/current_velocity_limit_mps", rclcpp::QoS{1}.transient_local());
   pub_dist_to_stopline_ = create_publisher<Float32Stamped>("~/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);
 }