fix(obstacle_cruise_planner): fix backward virtual wall

Signed-off-by: kosuke55 <[email protected]>

common/motion_utils/include/motion_utils/marker/marker_helper.hpp

@@ -28,15 +28,18 @@ using geometry_msgs::msg::Pose;
 
 visualization_msgs::msg::MarkerArray createStopVirtualWallMarker(
   const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id,
-  const double longitudinal_offset = 0.0, const std::string & ns_prefix = "");
+  const double longitudinal_offset = 0.0, const std::string & ns_prefix = "",
+  const bool is_driving_forward = true);
 
 visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker(
   const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id,
-  const double longitudinal_offset = 0.0, const std::string & ns_prefix = "");
+  const double longitudinal_offset = 0.0, const std::string & ns_prefix = "",
+  const bool is_driving_forward = true);
 
 visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker(
   const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id,
-  const double longitudinal_offset = 0.0, const std::string & ns_prefix = "");
+  const double longitudinal_offset = 0.0, const std::string & ns_prefix = "",
+  const bool is_driving_forward = true);
 
 visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker(
   const rclcpp::Time & now, const int32_t id);

common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp

@@ -38,6 +38,7 @@ struct VirtualWall
   std::string ns{};
   VirtualWallType style = stop;
   double longitudinal_offset{};
+  bool is_driving_forward{true};
 };
 typedef std::vector<VirtualWall> VirtualWalls;
 
@@ -54,7 +55,7 @@ class VirtualWallMarkerCreator
   using create_wall_function = std::function<visualization_msgs::msg::MarkerArray(
     const geometry_msgs::msg::Pose & pose, const std::string & module_name,
     const rclcpp::Time & now, const int32_t id, const double longitudinal_offset,
-    const std::string & ns_prefix)>;
+    const std::string & ns_prefix, const bool is_driving_forward)>;
 
   VirtualWalls virtual_walls;
   std::unordered_map<std::string, MarkerCount> marker_count_per_namespace;

common/motion_utils/src/marker/marker_helper.cpp

@@ -86,32 +86,35 @@ namespace motion_utils
 {
 visualization_msgs::msg::MarkerArray createStopVirtualWallMarker(
   const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now,
-  const int32_t id, const double longitudinal_offset, const std::string & ns_prefix)
+  const int32_t id, const double longitudinal_offset, const std::string & ns_prefix,
+  const bool is_driving_forward)
 {
-  const auto pose_with_offset =
-    tier4_autoware_utils::calcOffsetPose(pose, longitudinal_offset, 0.0, 0.0);
+  const auto pose_with_offset = tier4_autoware_utils::calcOffsetPose(
+    pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0);
   return createVirtualWallMarkerArray(
     pose_with_offset, module_name, ns_prefix + "stop_", now, id,
     createMarkerColor(1.0, 0.0, 0.0, 0.5));
 }
 
 visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker(
   const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now,
-  const int32_t id, const double longitudinal_offset, const std::string & ns_prefix)
+  const int32_t id, const double longitudinal_offset, const std::string & ns_prefix,
+  const bool is_driving_forward)
 {
-  const auto pose_with_offset =
-    tier4_autoware_utils::calcOffsetPose(pose, longitudinal_offset, 0.0, 0.0);
+  const auto pose_with_offset = tier4_autoware_utils::calcOffsetPose(
+    pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0);
   return createVirtualWallMarkerArray(
     pose_with_offset, module_name, ns_prefix + "slow_down_", now, id,
     createMarkerColor(1.0, 1.0, 0.0, 0.5));
 }
 
 visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker(
   const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now,
-  const int32_t id, const double longitudinal_offset, const std::string & ns_prefix)
+  const int32_t id, const double longitudinal_offset, const std::string & ns_prefix,
+  const bool is_driving_forward)
 {
-  const auto pose_with_offset =
-    tier4_autoware_utils::calcOffsetPose(pose, longitudinal_offset, 0.0, 0.0);
+  const auto pose_with_offset = tier4_autoware_utils::calcOffsetPose(
+    pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0);
   return createVirtualWallMarkerArray(
     pose_with_offset, module_name, ns_prefix + "dead_line_", now, id,
     createMarkerColor(0.0, 1.0, 0.0, 0.5));

common/motion_utils/src/marker/virtual_wall_marker_creator.cpp

@@ -64,7 +64,7 @@ visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::create_markers(
     }
     auto markers = create_fn(
       virtual_wall.pose, virtual_wall.text, now, 0, virtual_wall.longitudinal_offset,
-      virtual_wall.ns);
+      virtual_wall.ns, virtual_wall.is_driving_forward);
     for (auto & marker : markers.markers) {
       marker.id = marker_count_per_namespace[marker.ns].current++;
       marker_array.markers.push_back(marker);

planning/obstacle_cruise_planner/src/planner_interface.cpp

@@ -323,7 +323,7 @@ std::vector<TrajectoryPoint> PlannerInterface::generateStopTrajectory(
     // virtual wall marker for stop obstacle
     const auto markers = motion_utils::createStopVirtualWallMarker(
       output_traj_points.at(*zero_vel_idx).pose, "obstacle stop", planner_data.current_time, 0,
-      abs_ego_offset);
+      abs_ego_offset, "", planner_data.is_driving_forward);
     tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker);
     debug_data_ptr_->obstacles_to_stop.push_back(*closest_stop_obstacle);
 
@@ -479,22 +479,22 @@ std::vector<TrajectoryPoint> PlannerInterface::generateSlowDownTrajectory(
 
       const auto markers = motion_utils::createSlowDownVirtualWallMarker(
         slow_down_traj_points.at(slow_down_wall_idx).pose, "obstacle slow down",
-        planner_data.current_time, i, abs_ego_offset);
+        planner_data.current_time, i, abs_ego_offset, "", planner_data.is_driving_forward);
       tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->slow_down_wall_marker);
     }
 
     // add debug virtual wall
     if (slow_down_start_idx) {
       const auto markers = motion_utils::createSlowDownVirtualWallMarker(
         slow_down_traj_points.at(*slow_down_start_idx).pose, "obstacle slow down start",
-        planner_data.current_time, i * 2, abs_ego_offset);
+        planner_data.current_time, i * 2, abs_ego_offset, "", planner_data.is_driving_forward);
       tier4_autoware_utils::appendMarkerArray(
         markers, &debug_data_ptr_->slow_down_debug_wall_marker);
     }
     if (slow_down_end_idx) {
       const auto markers = motion_utils::createSlowDownVirtualWallMarker(
         slow_down_traj_points.at(*slow_down_end_idx).pose, "obstacle slow down end",
-        planner_data.current_time, i * 2 + 1, abs_ego_offset);
+        planner_data.current_time, i * 2 + 1, abs_ego_offset, "", planner_data.is_driving_forward);
       tier4_autoware_utils::appendMarkerArray(
         markers, &debug_data_ptr_->slow_down_debug_wall_marker);
     }