feat(pid_longitudinal_controller): add virtual wall for dry steering and emergency (autowarefoundation#9685)

* feat(pid_longitudinal_controller): add virtual wall for dry steering and emergency

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

Author: takayuki5168

control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp

@@ -50,11 +50,8 @@
 
 namespace autoware::motion::control::pid_longitudinal_controller
 {
-using autoware::universe_utils::createDefaultMarker;
-using autoware::universe_utils::createMarkerColor;
-using autoware::universe_utils::createMarkerScale;
 using autoware_adapi_v1_msgs::msg::OperationModeState;
-using visualization_msgs::msg::Marker;
+using visualization_msgs::msg::MarkerArray;
 
 namespace trajectory_follower = ::autoware::motion::control::trajectory_follower;
 
@@ -103,7 +100,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
   // ros variables
   rclcpp::Publisher<tier4_debug_msgs::msg::Float32MultiArrayStamped>::SharedPtr m_pub_slope;
   rclcpp::Publisher<tier4_debug_msgs::msg::Float32MultiArrayStamped>::SharedPtr m_pub_debug;
-  rclcpp::Publisher<Marker>::SharedPtr m_pub_stop_reason_marker;
+  rclcpp::Publisher<MarkerArray>::SharedPtr m_pub_virtual_wall_marker;
 
   rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr m_set_param_res;
   rcl_interfaces::msg::SetParametersResult paramCallback(

control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp

@@ -14,6 +14,7 @@
 
 #include "autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp"
 
+#include "autoware/motion_utils/marker/marker_helper.hpp"
 #include "autoware/motion_utils/trajectory/trajectory.hpp"
 #include "autoware/universe_utils/geometry/geometry.hpp"
 #include "autoware/universe_utils/math/normalization.hpp"
@@ -215,7 +216,7 @@ PidLongitudinalController::PidLongitudinalController(
     "~/output/slope_angle", rclcpp::QoS{1});
   m_pub_debug = node.create_publisher<tier4_debug_msgs::msg::Float32MultiArrayStamped>(
     "~/output/longitudinal_diagnostic", rclcpp::QoS{1});
-  m_pub_stop_reason_marker = node.create_publisher<Marker>("~/output/stop_reason", rclcpp::QoS{1});
+  m_pub_virtual_wall_marker = node.create_publisher<MarkerArray>("~/virtual_wall", 1);
 
   // set parameter callback
   m_set_param_res = node.add_on_set_parameters_callback(
@@ -521,8 +522,8 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData
 
   // distance to stopline
   control_data.stop_dist = longitudinal_utils::calcStopDistance(
-    control_data.interpolated_traj.points.at(control_data.nearest_idx).pose,
-    control_data.interpolated_traj, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold);
+    current_pose, control_data.interpolated_traj, m_ego_nearest_dist_threshold,
+    m_ego_nearest_yaw_threshold);
 
   // pitch
   // NOTE: getPitchByTraj() calculates the pitch angle as defined in
@@ -576,6 +577,11 @@ PidLongitudinalController::Motion PidLongitudinalController::calcEmergencyCtrlCm
     longitudinal_utils::applyDiffLimitFilter(raw_ctrl_cmd.acc, m_prev_raw_ctrl_cmd.acc, dt, p.jerk);
   m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_JERK_LIMITED, raw_ctrl_cmd.acc);
 
+  const auto virtual_wall_marker = autoware::motion_utils::createStopVirtualWallMarker(
+    m_current_kinematic_state.pose.pose, "velocity control\n (emergency)", clock_->now(), 0,
+    m_wheel_base + m_front_overhang);
+  m_pub_virtual_wall_marker->publish(virtual_wall_marker);
+
   return raw_ctrl_cmd;
 }
 
@@ -740,14 +746,12 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
         }
 
         // publish debug marker
-        auto marker = createDefaultMarker(
-          "map", clock_->now(), "stop_reason", 0, Marker::TEXT_VIEW_FACING,
-          createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999));
-        marker.pose = autoware::universe_utils::calcOffsetPose(
-          m_current_kinematic_state.pose.pose, m_wheel_base + m_front_overhang,
-          m_vehicle_width / 2 + 2.0, 1.5);
-        marker.text = "steering not\nconverged";
-        m_pub_stop_reason_marker->publish(marker);
+        if (is_under_control) {
+          const auto virtual_wall_marker = autoware::motion_utils::createStopVirtualWallMarker(
+            m_current_kinematic_state.pose.pose, "velocity control\n(steering not converged)",
+            clock_->now(), 0, m_wheel_base + m_front_overhang);
+          m_pub_virtual_wall_marker->publish(virtual_wall_marker);
+        }
 
         // keep STOPPED
         return;