feat(tier4_autoware_utils, obstacle_cruise_planner): add function to …

…create deleted marker (#1513) * feat(tier4_autoware_utils): add function to create deleted marker Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * apply fix to obstacle_cruise_planner Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix bug Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
autowarefoundation · Aug 5, 2022 · bbecb7f · bbecb7f
1 parent 49d91ff
commit bbecb7f
Show file tree

Hide file tree

Showing 5 changed files with 81 additions and 0 deletions.
diff --git a/common/motion_utils/include/motion_utils/marker/marker_helper.hpp b/common/motion_utils/include/motion_utils/marker/marker_helper.hpp
@@ -32,6 +32,15 @@ visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker(
 visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker(
   const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now,
   const int32_t id);
+
+visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker(
+  const rclcpp::Time & now, const int32_t id);
+
+visualization_msgs::msg::MarkerArray createDeletedSlowDownVirtualWallMarker(
+  const rclcpp::Time & now, const int32_t id);
+
+visualization_msgs::msg::MarkerArray createDeletedDeadLineVirtualWallMarker(
+  const rclcpp::Time & now, const int32_t id);
 }  // namespace motion_utils
 
 #endif  // MOTION_UTILS__MARKER__MARKER_HELPER_HPP_
diff --git a/common/motion_utils/src/marker/marker_helper.cpp b/common/motion_utils/src/marker/marker_helper.cpp
@@ -17,6 +17,7 @@
 #include <string>
 
 using tier4_autoware_utils::createDefaultMarker;
+using tier4_autoware_utils::createDeletedDefaultMarker;
 using tier4_autoware_utils::createMarkerColor;
 using tier4_autoware_utils::createMarkerScale;
 
@@ -57,6 +58,25 @@ inline visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray(
   return marker_array;
 }
 
+inline visualization_msgs::msg::MarkerArray createDeletedVirtualWallMarkerArray(
+  const std::string & ns_prefix, const rclcpp::Time & now, const int32_t id)
+{
+  visualization_msgs::msg::MarkerArray marker_array;
+
+  // Virtual Wall
+  {
+    auto marker = createDeletedDefaultMarker(now, ns_prefix + "virtual_wall", id);
+    marker_array.markers.push_back(marker);
+  }
+
+  // Factor Text
+  {
+    auto marker = createDeletedDefaultMarker(now, ns_prefix + "factor_text", id);
+    marker_array.markers.push_back(marker);
+  }
+
+  return marker_array;
+}
 }  // namespace
 
 namespace motion_utils
@@ -84,4 +104,22 @@ visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker(
   return createVirtualWallMarkerArray(
     pose, module_name, "dead_line_", now, id, createMarkerColor(0.0, 1.0, 0.0, 0.5));
 }
+
+visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker(
+  const rclcpp::Time & now, const int32_t id)
+{
+  return createDeletedVirtualWallMarkerArray("stop_", now, id);
+}
+
+visualization_msgs::msg::MarkerArray createDeletedSlowDownVirtualWallMarker(
+  const rclcpp::Time & now, const int32_t id)
+{
+  return createDeletedVirtualWallMarkerArray("slow_down_", now, id);
+}
+
+visualization_msgs::msg::MarkerArray createDeletedDeadLineVirtualWallMarker(
+  const rclcpp::Time & now, const int32_t id)
+{
+  return createDeletedVirtualWallMarkerArray("dead_line_", now, id);
+}
 }  // namespace motion_utils
diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/marker_helper.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/marker_helper.hpp
@@ -88,6 +88,19 @@ inline visualization_msgs::msg::Marker createDefaultMarker(
   return marker;
 }
 
+inline visualization_msgs::msg::Marker createDeletedDefaultMarker(
+  const rclcpp::Time & now, const std::string & ns, const int32_t id)
+{
+  visualization_msgs::msg::Marker marker;
+
+  marker.header.stamp = now;
+  marker.ns = ns;
+  marker.id = id;
+  marker.action = visualization_msgs::msg::Marker::DELETE;
+
+  return marker;
+}
+
 inline void appendMarkerArray(
   const visualization_msgs::msg::MarkerArray & additional_marker_array,
   visualization_msgs::msg::MarkerArray * marker_array,

diff --git a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp
@@ -193,6 +193,11 @@ void PIDBasedPlanner::planCruise(
     // reset previous target velocity if adaptive cruise is not enabled
     prev_target_vel_ = {};
     lpf_cruise_ptr_->reset();
+
+    // delete marker
+    const auto markers =
+      motion_utils::createDeletedSlowDownVirtualWallMarker(planner_data.current_time, 0);
+    tier4_autoware_utils::appendMarkerArray(markers, &debug_data.cruise_wall_marker);
   }
 }
 

diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp
@@ -90,13 +90,24 @@ Trajectory PlannerInterface::generateStopTrajectory(
 
   if (planner_data.target_obstacles.empty()) {
     stop_reasons_pub_->publish(makeEmptyStopReasonArray(planner_data.current_time));
+
+    // delete marker
+    const auto markers =
+      motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0);
+    tier4_autoware_utils::appendMarkerArray(markers, &debug_data.stop_wall_marker);
+
     return planner_data.traj;
   }
 
   // Get Closest Stop Obstacle
   const auto closest_stop_obstacle =
     obstacle_cruise_utils::getClosestStopObstacle(planner_data.traj, planner_data.target_obstacles);
   if (!closest_stop_obstacle) {
+    // delete marker
+    const auto markers =
+      motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0);
+    tier4_autoware_utils::appendMarkerArray(markers, &debug_data.stop_wall_marker);
+
     return planner_data.traj;
   }
 
@@ -108,6 +119,11 @@ Trajectory PlannerInterface::generateStopTrajectory(
     planner_data.traj.points, planner_data.current_pose, 0, nearest_dist_deviation_threshold_,
     nearest_yaw_deviation_threshold_);
   if (!negative_dist_to_ego) {
+    // delete marker
+    const auto markers =
+      motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0);
+    tier4_autoware_utils::appendMarkerArray(markers, &debug_data.stop_wall_marker);
+
     return planner_data.traj;
   }
   const double dist_to_ego = -negative_dist_to_ego.get();