fix(obstacle_cruise_planner): fix stop against objects after backward trajectory terminal

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/node.cpp

@@ -129,11 +129,11 @@
 }
 
 TrajectoryPoint getExtendTrajectoryPoint(
-  const double extend_distance, const TrajectoryPoint & goal_point)
+  const double extend_distance, const TrajectoryPoint & goal_point, const bool is_driving_forward)
 {
   TrajectoryPoint extend_trajectory_point;
-  extend_trajectory_point.pose =
-    tier4_autoware_utils::calcOffsetPose(goal_point.pose, extend_distance, 0.0, 0.0);
+  extend_trajectory_point.pose = tier4_autoware_utils::calcOffsetPose(
+    goal_point.pose, extend_distance * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0);
   extend_trajectory_point.longitudinal_velocity_mps = goal_point.longitudinal_velocity_mps;
   extend_trajectory_point.lateral_velocity_mps = goal_point.lateral_velocity_mps;
   extend_trajectory_point.acceleration_mps2 = goal_point.acceleration_mps2;
@@ -145,6 +145,8 @@
   const double step_length)
 {
   auto output_points = input_points;
+  const auto is_driving_forward_opt = motion_utils::isDrivingForwardWithTwist(input_points);
+  const bool is_driving_forward = is_driving_forward_opt ? *is_driving_forward_opt : true;
 
   if (extend_distance < std::numeric_limits<double>::epsilon()) {
     return output_points;
@@ -154,11 +156,13 @@
 
   double extend_sum = 0.0;
   while (extend_sum <= (extend_distance - step_length)) {
-    const auto extend_trajectory_point = getExtendTrajectoryPoint(extend_sum, goal_point);
+    const auto extend_trajectory_point =
+      getExtendTrajectoryPoint(extend_sum, goal_point, is_driving_forward);
     output_points.push_back(extend_trajectory_point);
     extend_sum += step_length;
   }
-  const auto extend_trajectory_point = getExtendTrajectoryPoint(extend_distance, goal_point);
+  const auto extend_trajectory_point =
+    getExtendTrajectoryPoint(extend_distance, goal_point, is_driving_forward);
   output_points.push_back(extend_trajectory_point);
 
   return output_points;
@@ -995,7 +999,7 @@
       }
       return true;
     }
     // check if entrying slow down
     if (is_lat_dist_low) {
       const int count = slow_down_condition_counter_.increaseCounter(obstacle.uuid);
       if (p.successive_num_to_entry_slow_down_condition <= count) {
@@ -1016,7 +1020,7 @@
   const auto obstacle_poly = obstacle.toPolygon();
 
   // calculate collision points with trajectory with lateral stop margin
   // NOTE: For additional margin, hysteresis is not devided by two.
   const auto traj_polys_with_lat_margin = polygon_utils::createOneStepPolygons(
     traj_points, vehicle_info_,
     p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down);

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