diff --git a/common/motion_utils/include/motion_utils/marker/marker_helper.hpp b/common/motion_utils/include/motion_utils/marker/marker_helper.hpp index 03d36b24ba37..db63bb1b0052 100644 --- 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 index 3571ba7260f5..b90c86df5f00 100644 --- a/common/motion_utils/src/marker/marker_helper.cpp +++ b/common/motion_utils/src/marker/marker_helper.cpp @@ -17,6 +17,7 @@ #include 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 index 3ef74ffcbcef..78e7c5f9288d 100644 --- 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 index 907092c9d3ca..a24a2501a231 100644 --- 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 index bfc40a655b7a..afddd8349cbb 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -90,6 +90,12 @@ 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; } @@ -97,6 +103,11 @@ Trajectory PlannerInterface::generateStopTrajectory( 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();