refactor(behavior_path_planner): reorganize debug visualization (#1101)

* refactor(behavior_path_planner): reorganize debug visualization

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* refactor the code for consistency

reuse utilities function

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* refactor const reference string to rvalue reference

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* clear debug marker on exit to remove artifact

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* initialize marker using common createDefaultMarker and add reserve

add reserve to increase performance of push back.

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

planning/behavior_path_planner/CMakeLists.txt

@@ -11,6 +11,7 @@ ament_auto_add_library(behavior_path_planner_node SHARED
   src/behavior_tree_manager.cpp
   src/utilities.cpp
   src/path_utilities.cpp
+  src/debug_utilities.cpp
   src/path_shifter/path_shifter.cpp
   src/turn_signal_decider.cpp
   src/scene_module/scene_module_bt_node_interface.cpp

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -18,7 +18,6 @@
 #include "behavior_path_planner/behavior_tree_manager.hpp"
 #include "behavior_path_planner/data_manager.hpp"
 #include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp"
-#include "behavior_path_planner/scene_module/avoidance/debug.hpp"
 #include "behavior_path_planner/scene_module/lane_change/lane_change_module.hpp"
 #include "behavior_path_planner/scene_module/lane_following/lane_following_module.hpp"
 #include "behavior_path_planner/scene_module/pull_out/pull_out_module.hpp"

planning/behavior_path_planner/include/behavior_path_planner/debug_utilities.hpp

@@ -0,0 +1,83 @@
+// Copyright 2022 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 BEHAVIOR_PATH_PLANNER__DEBUG_UTILITIES_HPP_
+#define BEHAVIOR_PATH_PLANNER__DEBUG_UTILITIES_HPP_
+
+#include "behavior_path_planner/path_shifter/path_shifter.hpp"
+#include "behavior_path_planner/path_utilities.hpp"
+#include "behavior_path_planner/utilities.hpp"
+
+#include <tier4_autoware_utils/ros/marker_helper.hpp>
+
+#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
+#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
+#include <geometry_msgs/msg/polygon.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_routing/RoutingGraph.h>
+#include <tf2/utils.h>
+
+#include <string>
+#include <vector>
+
+namespace marker_utils
+{
+using autoware_auto_perception_msgs::msg::PredictedObjects;
+using autoware_auto_planning_msgs::msg::PathWithLaneId;
+using behavior_path_planner::ShiftPointArray;
+using geometry_msgs::msg::Point;
+using geometry_msgs::msg::Polygon;
+using geometry_msgs::msg::Pose;
+using geometry_msgs::msg::Vector3;
+using std_msgs::msg::ColorRGBA;
+using visualization_msgs::msg::Marker;
+using visualization_msgs::msg::MarkerArray;
+
+inline int64_t bitShift(int64_t original_id) { return original_id << (sizeof(int32_t) * 8 / 2); }
+
+MarkerArray createPoseMarkerArray(
+  const Pose & pose, std::string && ns, const int32_t & id, const float & r, const float & g,
+  const float & b);
+
+MarkerArray createPathMarkerArray(
+  const PathWithLaneId & path, std::string && ns, const int64_t & lane_id, const float & r,
+  const float & g, const float & b);
+
+MarkerArray createShiftPointMarkerArray(
+  const ShiftPointArray & shift_points, const double & base_shift, std::string && ns,
+  const float & r, const float & g, const float & b, const float & w);
+
+MarkerArray createShiftLengthMarkerArray(
+  const std::vector<double> & shift_distance,
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & reference, std::string && ns,
+  const float & r, const float & g, const float & b);
+
+MarkerArray createLaneletsAreaMarkerArray(
+  const std::vector<lanelet::ConstLanelet> & lanelets, std::string && ns, const float & r,
+  const float & g, const float & b);
+
+MarkerArray createFurthestLineStringMarkerArray(const lanelet::ConstLineStrings3d & linestrings);
+
+MarkerArray createPolygonMarkerArray(
+  const Polygon & polygon, std::string && ns, const int64_t & lane_id, const float & r,
+  const float & g, const float & b);
+
+MarkerArray createObjectsMarkerArray(
+  const PredictedObjects & objects, std::string && ns, const int64_t & lane_id, const float & r,
+  const float & g, const float & b);
+
+}  // namespace marker_utils
+
+#endif  // BEHAVIOR_PATH_PLANNER__DEBUG_UTILITIES_HPP_

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp

@@ -15,6 +15,7 @@
 #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__DEBUG_HPP_
 #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__DEBUG_HPP_
 
+#include "behavior_path_planner/debug_utilities.hpp"
 #include "behavior_path_planner/path_shifter/path_shifter.hpp"
 #include "behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp"
 
@@ -31,71 +32,32 @@
 #include <string>
 #include <vector>
 
-namespace marker_utils
+namespace marker_utils::avoidance_marker
 {
 using autoware_auto_perception_msgs::msg::PredictedObjects;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
-using behavior_path_planner::AvoidPoint;
 using behavior_path_planner::AvoidPointArray;
-using behavior_path_planner::ShiftPoint;
 using behavior_path_planner::ShiftPointArray;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Polygon;
 using geometry_msgs::msg::Pose;
 using visualization_msgs::msg::MarkerArray;
 
-MarkerArray createShiftLengthMarkerArray(
-  const std::vector<double> shift_distance, const PathWithLaneId & reference,
-  const std::string & ns, const double r, const double g, const double b);
-
 MarkerArray createAvoidPointMarkerArray(
-  const AvoidPointArray & shift_points, const std::string & ns, const double r, const double g,
-  const double b, const double w);
-
-MarkerArray createShiftPointMarkerArray(
-  const ShiftPointArray & shift_points, const double base_shift, const std::string & ns,
-  const double r, const double g, const double b, const double w);
-
-MarkerArray createLaneletsAreaMarkerArray(
-  const std::vector<lanelet::ConstLanelet> & lanelets, const std::string & ns, const double r,
-  const double g, const double b);
-
-MarkerArray createLaneletPolygonsMarkerArray(
-  const std::vector<lanelet::CompoundPolygon3d> & polygons, const std::string & ns,
-  const int64_t lane_id);
-
-MarkerArray createPolygonMarkerArray(
-  const Polygon & polygon, const std::string & ns, const int64_t lane_id, const double r,
-  const double g, const double b);
-
-MarkerArray createObjectsMarkerArray(
-  const PredictedObjects & objects, const std::string & ns, const int64_t lane_id, const double r,
-  const double g, const double b);
+  const AvoidPointArray & shift_points, std::string && ns, const float & r, const float & g,
+  const float & b, const double & w);
 
 MarkerArray createAvoidanceObjectsMarkerArray(
-  const behavior_path_planner::ObjectDataArray & objects, const std::string & ns);
-
-MarkerArray createPathMarkerArray(
-  const PathWithLaneId & path, const std::string & ns, const int64_t lane_id, const double r,
-  const double g, const double b);
-
-MarkerArray createPoseLineMarkerArray(
-  const Pose & pose, const std::string & ns, const int64_t id, const double r, const double g,
-  const double b);
-
-MarkerArray createPoseMarkerArray(
-  const Pose & pose, const std::string & ns, const int64_t id, const double r, const double g,
-  const double b);
+  const behavior_path_planner::ObjectDataArray & objects, std::string && ns);
 
 MarkerArray makeOverhangToRoadShoulderMarkerArray(
-  const behavior_path_planner::ObjectDataArray & objects);
+  const behavior_path_planner::ObjectDataArray & objects, std::string && ns);
 
 MarkerArray createOverhangFurthestLineStringMarkerArray(
-  const lanelet::ConstLineStrings3d & linestrings, const std::string & ns, const double r,
-  const double g, const double b);
+  const lanelet::ConstLineStrings3d & linestrings, std::string && ns, const float & r,
+  const float & g, const float & b);
 
-MarkerArray createFurthestLineStringMarkerArray(const lanelet::ConstLineStrings3d & linestrings);
-}  // namespace marker_utils
+}  // namespace marker_utils::avoidance_marker
 
 std::string toStrInfo(const behavior_path_planner::ShiftPointArray & sp_arr);
 

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -14,6 +14,7 @@
 
 #include "behavior_path_planner/behavior_path_planner_node.hpp"
 
+#include "behavior_path_planner/debug_utilities.hpp"
 #include "behavior_path_planner/path_utilities.hpp"
 #include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp"
 #include "behavior_path_planner/scene_module/lane_change/lane_change_module.hpp"