[TMP] debug launch prefix + svg output

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py

@@ -23,6 +23,7 @@
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
 from launch.substitutions import PythonExpression
+from launch_ros.actions import Node
 from launch_ros.actions import ComposableNodeContainer
 from launch_ros.descriptions import ComposableNode
 from launch_ros.substitutions import FindPackageShare
@@ -64,9 +65,10 @@ def launch_setup(context, *args, **kwargs):
     with open(LaunchConfiguration("behavior_path_planner_param_path").perform(context), "r") as f:
         behavior_path_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"]
 
-    behavior_path_planner_component = ComposableNode(
+    behavior_path_planner_component = Node(
         package="behavior_path_planner",
-        plugin="behavior_path_planner::BehaviorPathPlannerNode",
+        # plugin="behavior_path_planner::BehaviorPathPlannerNode",
+        executable="behavior_path_planner",
         name="behavior_path_planner",
         namespace="",
         remappings=[
@@ -117,7 +119,8 @@ def launch_setup(context, *args, **kwargs):
                 "bt_tree_config_path": LaunchConfiguration("behavior_path_planner_tree_param_path"),
             },
         ],
-        extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+        prefix="konsole -e gdb -ex run --args",
+        # extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
 
     # smoother param
@@ -209,7 +212,6 @@ def launch_setup(context, *args, **kwargs):
         package="rclcpp_components",
         executable=LaunchConfiguration("container_executable"),
         composable_node_descriptions=[
-            behavior_path_planner_component,
             behavior_velocity_planner_component,
         ],
         output="screen",
@@ -258,6 +260,7 @@ def launch_setup(context, *args, **kwargs):
     group = GroupAction(
         [
             container,
+            behavior_path_planner_component,
             load_compare_map,
             load_vector_map_inside_area_filter,
         ]

planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp

@@ -25,6 +25,15 @@
 
 #include <boost/geometry.hpp>
 
+// for writing the svg file
+#include <iostream>
+#include <fstream>
+// for the geometry types
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+// for the svg mapper
+#include <boost/geometry/io/svg/svg_mapper.hpp>
+#include <boost/geometry/io/svg/write.hpp>
+
 namespace drivable_area_expansion
 {
 
@@ -49,7 +58,27 @@ void expandDrivableArea(
       : createExpansionPolygons(
           path, path_footprints, predicted_paths, uncrossable_lines_in_range, params);
   const auto expanded_drivable_area = createExpandedDrivableAreaPolygon(path, expansion_polygons);
+
+  // Declare a stream and an SVG mapper
+  std::ofstream svg("/home/mclement/Pictures/debug.svg");
+  boost::geometry::svg_mapper<tier4_autoware_utils::Point2d> mapper(svg, 400, 400);
+  linestring_t path_ls;
+  for(const auto & p : path.points) path_ls.emplace_back(p.point.pose.position.x, p.point.pose.position.y);
+  mapper.add(path_ls);
+  mapper.add(expanded_drivable_area);
+  mapper.add(path_footprints);
+  mapper.map(path_footprints, "fill-opacity:0.3;fill:red;stroke:red;stroke-width:2");
+  mapper.map(path_ls, "fill-opacity:0.3;fill:black;stroke:black;stroke-width:2");
+  mapper.map(expanded_drivable_area, "fill-opacity:0.3;fill:blue;stroke:blue;stroke-width:2");
+  mapper.map(expansion_polygons, "fill-opacity:0.3;fill:black;stroke:black;stroke-width:2");
+  std::cout << "EXPANSION" << std::endl;
   updateDrivableAreaBounds(path, expanded_drivable_area);
+  linestring_t left_ls;
+  linestring_t right_ls;
+  for(const auto & p : path.left_bound) left_ls.emplace_back(p.x, p.y);
+  for(const auto & p : path.right_bound) right_ls.emplace_back(p.x, p.y);
+  mapper.map(left_ls, "fill-opacity:0.3;fill:red;stroke:red;stroke-width:2");
+  mapper.map(right_ls, "fill-opacity:0.3;fill:red;stroke:red;stroke-width:2");
 }
 
 point_t convert_point(const Point & p)