feat(behavior_velocity_planner): make debug.cpp (#1773)

* feat(behavior_velocity_planner): make debug.cpp

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

* fix

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

* Update planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp

Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com>

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com>

planning/behavior_velocity_planner/CMakeLists.txt

@@ -34,6 +34,7 @@ ament_auto_add_library(behavior_velocity_planner SHARED
   src/planner_manager.cpp
   src/utilization/path_utilization.cpp
   src/utilization/util.cpp
+  src/utilization/debug.cpp
   ${scene_modules_src}
 )
 

planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp

@@ -52,8 +52,8 @@ class BlindSpotModule : public SceneModuleInterface
     geometry_msgs::msg::Pose virtual_wall_pose;
     geometry_msgs::msg::Pose stop_point_pose;
     geometry_msgs::msg::Pose judge_point_pose;
-    lanelet::CompoundPolygon3d conflict_area_for_blind_spot;
-    lanelet::CompoundPolygon3d detection_area_for_blind_spot;
+    geometry_msgs::msg::Polygon conflict_area_for_blind_spot;
+    geometry_msgs::msg::Polygon detection_area_for_blind_spot;
     autoware_auto_planning_msgs::msg::PathWithLaneId spline_path;
     autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets;
   };

planning/behavior_velocity_planner/include/utilization/debug.hpp

@@ -0,0 +1,47 @@
+// Copyright 2020 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef UTILIZATION__DEBUG_HPP_
+#define UTILIZATION__DEBUG_HPP_
+
+#include <utilization/util.hpp>
+
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include <string>
+#include <vector>
+
+namespace behavior_velocity_planner
+{
+namespace debug
+{
+visualization_msgs::msg::MarkerArray createPolygonMarkerArray(
+  const geometry_msgs::msg::Polygon & polygon, const std::string & ns, const int64_t module_id,
+  const rclcpp::Time & now, const double x, const double y, const double z, const double r,
+  const double g, const double b);
+visualization_msgs::msg::MarkerArray createPathMarkerArray(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::string & ns,
+  const int64_t lane_id, const rclcpp::Time & now, const double x, const double y, const double z,
+  const double r, const double g, const double b);
+visualization_msgs::msg::MarkerArray createObjectsMarkerArray(
+  const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const std::string & ns,
+  const int64_t module_id, const rclcpp::Time & now, const double r, const double g,
+  const double b);
+visualization_msgs::msg::MarkerArray createPointsMarkerArray(
+  const std::vector<geometry_msgs::msg::Point> & points, const std::string & ns,
+  const int64_t module_id, const rclcpp::Time & now, const double x, const double y, const double z,
+  const double r, const double g, const double b);
+}  // namespace debug
+}  // namespace behavior_velocity_planner
+#endif  // UTILIZATION__DEBUG_HPP_

planning/behavior_velocity_planner/include/utilization/path_utilization.hpp

@@ -35,7 +35,6 @@ autoware_auto_planning_msgs::msg::Path filterLitterPathPoint(
   const autoware_auto_planning_msgs::msg::Path & path);
 autoware_auto_planning_msgs::msg::Path filterStopPathPoint(
   const autoware_auto_planning_msgs::msg::Path & path);
-std::vector<double> calcEuclidDist(const std::vector<double> & x, const std::vector<double> & y);
 }  // namespace behavior_velocity_planner
 
 #endif  // UTILIZATION__PATH_UTILIZATION_HPP_

planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp

@@ -14,6 +14,7 @@
 
 #include <motion_utils/motion_utils.hpp>
 #include <scene_module/blind_spot/scene.hpp>
+#include <utilization/debug.hpp>
 #include <utilization/util.hpp>
 
 #include <string>
@@ -28,91 +29,6 @@ using tier4_autoware_utils::createMarkerScale;
 namespace
 {
 
-visualization_msgs::msg::MarkerArray createPolygonMarkerArray(
-  const lanelet::CompoundPolygon3d & polygon, const std::string & ns, const int64_t lane_id,
-  const double r, const double g, const double b)
-{
-  visualization_msgs::msg::MarkerArray msg;
-
-  int32_t uid = planning_utils::bitShift(lane_id);
-  visualization_msgs::msg::Marker marker{};
-  marker.header.frame_id = "map";
-
-  marker.ns = ns;
-  marker.id = uid;
-  marker.lifetime = rclcpp::Duration::from_seconds(0.3);
-  marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
-  marker.action = visualization_msgs::msg::Marker::ADD;
-  marker.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0);
-  marker.scale = createMarkerScale(0.3, 0.0, 0.0);
-  marker.color = createMarkerColor(r, g, b, 0.8);
-  for (const auto & p : polygon) {
-    geometry_msgs::msg::Point point;
-    point.x = p.x();
-    point.y = p.y();
-    point.z = p.z();
-    marker.points.push_back(point);
-  }
-  if (!marker.points.empty()) {
-    marker.points.push_back(marker.points.front());
-    msg.markers.push_back(marker);
-  }
-
-  return msg;
-}
-
-visualization_msgs::msg::MarkerArray createObjectsMarkerArray(
-  const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const std::string & ns,
-  const int64_t lane_id, const double r, const double g, const double b)
-{
-  visualization_msgs::msg::MarkerArray msg;
-
-  visualization_msgs::msg::Marker marker{};
-  marker.header.frame_id = "map";
-  marker.ns = ns;
-
-  int32_t uid = planning_utils::bitShift(lane_id);
-  int32_t i = 0;
-  for (const auto & object : objects.objects) {
-    marker.id = uid + i++;
-    marker.lifetime = rclcpp::Duration::from_seconds(1.0);
-    marker.type = visualization_msgs::msg::Marker::SPHERE;
-    marker.action = visualization_msgs::msg::Marker::ADD;
-    marker.pose = object.kinematics.initial_pose_with_covariance.pose;
-    marker.scale = createMarkerScale(1.0, 1.0, 1.0);
-    marker.color = createMarkerColor(r, g, b, 0.8);
-    msg.markers.push_back(marker);
-  }
-
-  return msg;
-}
-
-visualization_msgs::msg::MarkerArray createPathMarkerArray(
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::string & ns,
-  const int64_t lane_id, const double r, const double g, const double b)
-{
-  visualization_msgs::msg::MarkerArray msg;
-
-  visualization_msgs::msg::Marker marker{};
-  marker.header.frame_id = "map";
-  marker.ns = ns;
-  marker.id = lane_id;
-  marker.lifetime = rclcpp::Duration::from_seconds(0.3);
-  marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
-  marker.action = visualization_msgs::msg::Marker::ADD;
-  marker.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0);
-  marker.scale = createMarkerScale(0.3, 0.0, 0.0);
-  marker.color = createMarkerColor(r, g, b, 0.999);
-
-  for (const auto & p : path.points) {
-    marker.points.push_back(p.point.pose.position);
-  }
-
-  msg.markers.push_back(marker);
-
-  return msg;
-}
-
 visualization_msgs::msg::MarkerArray createPoseMarkerArray(
   const geometry_msgs::msg::Pose & pose, const StateMachine::State & state, const std::string & ns,
   const int64_t id, const double r, const double g, const double b)
@@ -163,7 +79,7 @@ visualization_msgs::msg::MarkerArray BlindSpotModule::createVirtualWallMarkerArr
   if (!isActivated() && !is_over_pass_judge_line_) {
     appendMarkerArray(
       motion_utils::createStopVirtualWallMarker(
-        debug_data_.virtual_wall_pose, "blind_spot", now, lane_id_),
+        debug_data_.virtual_wall_pose, "blind_spot", now, module_id_),
       &wall_marker, now);
   }
   return wall_marker;
@@ -174,42 +90,44 @@ visualization_msgs::msg::MarkerArray BlindSpotModule::createDebugMarkerArray()
   visualization_msgs::msg::MarkerArray debug_marker_array;
 
   const auto state = state_machine_.getState();
-  const auto current_time = this->clock_->now();
+  const auto now = this->clock_->now();
 
   appendMarkerArray(
-    createPathMarkerArray(debug_data_.path_raw, "path_raw", lane_id_, 0.0, 1.0, 1.0),
-    &debug_marker_array, current_time);
+    debug::createPathMarkerArray(
+      debug_data_.path_raw, "path_raw", lane_id_, now, 0.6, 0.3, 0.3, 0.0, 1.0, 1.0),
+    &debug_marker_array, now);
 
   appendMarkerArray(
     createPoseMarkerArray(
-      debug_data_.stop_point_pose, state, "stop_point_pose", lane_id_, 1.0, 0.0, 0.0),
-    &debug_marker_array, current_time);
+      debug_data_.stop_point_pose, state, "stop_point_pose", module_id_, 1.0, 0.0, 0.0),
+    &debug_marker_array, now);
 
   appendMarkerArray(
     createPoseMarkerArray(
-      debug_data_.judge_point_pose, state, "judge_point_pose", lane_id_, 1.0, 1.0, 0.5),
-    &debug_marker_array, current_time);
+      debug_data_.judge_point_pose, state, "judge_point_pose", module_id_, 1.0, 1.0, 0.5),
+    &debug_marker_array, now);
 
   appendMarkerArray(
-    createPolygonMarkerArray(
-      debug_data_.conflict_area_for_blind_spot, "conflict_area_for_blind_spot", lane_id_, 0.0, 0.5,
-      0.5),
-    &debug_marker_array, current_time);
+    debug::createPolygonMarkerArray(
+      debug_data_.conflict_area_for_blind_spot, "conflict_area_for_blind_spot", module_id_, now,
+      0.3, 0.0, 0.0, 0.0, 0.5, 0.5),
+    &debug_marker_array, now);
 
   appendMarkerArray(
-    createPolygonMarkerArray(
-      debug_data_.detection_area_for_blind_spot, "detection_area_for_blind_spot", lane_id_, 0.0,
-      0.5, 0.5),
-    &debug_marker_array, current_time);
+    debug::createPolygonMarkerArray(
+      debug_data_.detection_area_for_blind_spot, "detection_area_for_blind_spot", module_id_, now,
+      0.3, 0.0, 0.0, 0.0, 0.5, 0.5),
+    &debug_marker_array, now);
 
   appendMarkerArray(
-    createObjectsMarkerArray(
-      debug_data_.conflicting_targets, "conflicting_targets", lane_id_, 0.99, 0.4, 0.0),
-    &debug_marker_array, current_time);
+    debug::createObjectsMarkerArray(
+      debug_data_.conflicting_targets, "conflicting_targets", module_id_, now, 0.99, 0.4, 0.0),
+    &debug_marker_array, now);
 
   appendMarkerArray(
-    createPathMarkerArray(debug_data_.spline_path, "spline", lane_id_, 0.5, 0.5, 0.5),
-    &debug_marker_array, current_time);
+    debug::createPathMarkerArray(
+      debug_data_.spline_path, "spline", lane_id_, now, 0.3, 0.1, 0.1, 0.5, 0.5, 0.5),
+    &debug_marker_array, now);
 
   return debug_marker_array;
 }

planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp

@@ -36,6 +36,24 @@ namespace behavior_velocity_planner
 {
 namespace bg = boost::geometry;
 
+namespace
+{
+geometry_msgs::msg::Polygon toGeomPoly(const lanelet::CompoundPolygon3d & poly)
+{
+  geometry_msgs::msg::Polygon geom_poly;
+
+  for (const auto & p : poly) {
+    geometry_msgs::msg::Point32 geom_point;
+    geom_point.x = p.x();
+    geom_point.y = p.y();
+    geom_point.z = p.z();
+    geom_poly.points.push_back(geom_point);
+  }
+
+  return geom_poly;
+}
+}  // namespace
+
 BlindSpotModule::BlindSpotModule(
   const int64_t module_id, const int64_t lane_id, std::shared_ptr<const PlannerData> planner_data,
   const PlannerParam & planner_param, const rclcpp::Logger logger,
@@ -356,8 +374,8 @@ bool BlindSpotModule::checkObstacleInBlindSpot(
   const auto areas_opt = generateBlindSpotPolygons(
     lanelet_map_ptr, routing_graph_ptr, path, closest_idx, stop_line_pose);
   if (!!areas_opt) {
-    debug_data_.detection_area_for_blind_spot = areas_opt.get().detection_area;
-    debug_data_.conflict_area_for_blind_spot = areas_opt.get().conflict_area;
+    debug_data_.detection_area_for_blind_spot = toGeomPoly(areas_opt.get().detection_area);
+    debug_data_.conflict_area_for_blind_spot = toGeomPoly(areas_opt.get().conflict_area);
 
     autoware_auto_perception_msgs::msg::PredictedObjects objects = *objects_ptr;
     cutPredictPathWithDuration(&objects, planner_param_.max_future_movement_time);

planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp

@@ -14,6 +14,7 @@
 
 #include <motion_utils/motion_utils.hpp>
 #include <scene_module/detection_area/scene.hpp>
+#include <utilization/debug.hpp>
 #include <utilization/util.hpp>
 
 #ifdef ROS_DISTRO_GALACTIC
@@ -129,44 +130,21 @@ visualization_msgs::msg::MarkerArray createCorrespondenceMarkerArray(
 
   return msg;
 }
-
-visualization_msgs::msg::MarkerArray createObstacleMarkerArray(
-  const std::vector<geometry_msgs::msg::Point> & obstacle_points, const rclcpp::Time & now)
-{
-  visualization_msgs::msg::MarkerArray msg;
-
-  {
-    auto marker = createDefaultMarker(
-      "map", now, "obstacles", 0, visualization_msgs::msg::Marker::SPHERE,
-      createMarkerScale(0.6, 0.6, 0.6), createMarkerColor(1.0, 0.0, 0.0, 0.999));
-    marker.lifetime = rclcpp::Duration::from_seconds(0.5);
-
-    for (size_t i = 0; i < obstacle_points.size(); ++i) {
-      marker.id = i;
-      marker.pose.position = obstacle_points.at(i);
-
-      msg.markers.push_back(marker);
-    }
-  }
-
-  return msg;
-}
-
 }  // namespace
 
 visualization_msgs::msg::MarkerArray DetectionAreaModule::createDebugMarkerArray()
 {
   visualization_msgs::msg::MarkerArray wall_marker;
-  const rclcpp::Time current_time = clock_->now();
+  const rclcpp::Time now = clock_->now();
 
   if (!debug_data_.stop_poses.empty()) {
     appendMarkerArray(
-      createCorrespondenceMarkerArray(detection_area_reg_elem_, current_time), &wall_marker,
-      current_time);
+      createCorrespondenceMarkerArray(detection_area_reg_elem_, now), &wall_marker, now);
 
     appendMarkerArray(
-      createObstacleMarkerArray(debug_data_.obstacle_points, current_time), &wall_marker,
-      current_time);
+      debug::createPointsMarkerArray(
+        debug_data_.obstacle_points, "obstalces", module_id_, now, 0.6, 0.6, 0.6, 1.0, 0.0, 0.0),
+      &wall_marker, now);
   }
 
   return wall_marker;