feat(behavior_velocity): publish internal debug path (autowarefoundat…

…ion#1635)

* feat(behavior_velocity): publish internal path as debug path

Signed-off-by: taikitanaka3 <ttatcoder@outlook.jp>

* feat(behavior_velocity): add debug internal scene module path

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* feat(behavior_velcoity, planning_debug_tools): add params for debug path

Signed-off-by: taikitanaka3 <ttatcoder@outlook.jp>

Signed-off-by: taikitanaka3 <ttatcoder@outlook.jp>
Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml

@@ -16,3 +16,4 @@
     max_jerk: -5.0
     system_delay: 0.5
     delay_response_time: 0.5
+    is_publish_debug_path: true # publish all debug path with lane id in each module

planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml

@@ -16,3 +16,4 @@
     max_jerk: -5.0
     system_delay: 0.5
     delay_response_time: 0.5
+    is_publish_debug_path: false # publish all debug path with lane id in each module

planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp

@@ -120,6 +120,15 @@ class SceneModuleManagerInterface
   {
     const auto ns = std::string("~/debug/") + module_name;
     pub_debug_ = node.create_publisher<visualization_msgs::msg::MarkerArray>(ns, 20);
+    if (!node.has_parameter("is_publish_debug_path")) {
+      is_publish_debug_path_ = node.declare_parameter("is_publish_debug_path", false);
+    } else {
+      is_publish_debug_path_ = node.get_parameter("is_publish_debug_path").as_bool();
+    }
+    if (is_publish_debug_path_) {
+      pub_debug_path_ = node.create_publisher<autoware_auto_planning_msgs::msg::PathWithLaneId>(
+        std::string("~/debug/path_with_lane_id/") + module_name, 1);
+    }
     pub_virtual_wall_ = node.create_publisher<visualization_msgs::msg::MarkerArray>(
       std::string("~/virtual_wall/") + module_name, 20);
     pub_stop_reason_ =
@@ -157,7 +166,6 @@ class SceneModuleManagerInterface
   {
     StopWatch<std::chrono::milliseconds> stop_watch;
     stop_watch.tic("Total");
-
     visualization_msgs::msg::MarkerArray debug_marker_array;
     visualization_msgs::msg::MarkerArray virtual_wall_marker_array;
     tier4_planning_msgs::msg::StopReasonArray stop_reason_array;
@@ -199,6 +207,11 @@ class SceneModuleManagerInterface
     }
     pub_infrastructure_commands_->publish(infrastructure_command_array);
     pub_debug_->publish(debug_marker_array);
+    if (is_publish_debug_path_) {
+      autoware_auto_planning_msgs::msg::PathWithLaneId debug_path;
+      debug_path.points = path->points;
+      pub_debug_path_->publish(debug_path);
+    }
     pub_virtual_wall_->publish(virtual_wall_marker_array);
     processing_time_publisher_->publish<Float64Stamped>(
       std::string(getModuleName()) + "/processing_time_ms", stop_watch.toc("Total"));
@@ -255,9 +268,11 @@ class SceneModuleManagerInterface
   boost::optional<int> first_stop_path_point_index_;
   rclcpp::Clock::SharedPtr clock_;
   // Debug
+  bool is_publish_debug_path_ = {false};  // note : this is very heavy debug topic option
   rclcpp::Logger logger_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_virtual_wall_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_debug_;
+  rclcpp::Publisher<autoware_auto_planning_msgs::msg::PathWithLaneId>::SharedPtr pub_debug_path_;
   rclcpp::Publisher<tier4_planning_msgs::msg::StopReasonArray>::SharedPtr pub_stop_reason_;
   rclcpp::Publisher<tier4_v2x_msgs::msg::InfrastructureCommandArray>::SharedPtr
     pub_infrastructure_commands_;

planning/planning_debug_tools/Readme.md

@@ -87,6 +87,7 @@ function PlotValue(name, path, timestamp, value)
     new_series:push_back(s,k)
     index = index+1
   end
+end
 
 function PlotCurvatureOverArclength(name, path, timestamp)
   PlotValue(name, path, timestamp,"curvature")
@@ -137,3 +138,32 @@ ros2 run planning_debug_tools closest_velocity_checker.py
 ## Trajectory visualizer
 
 The old version of the trajectory analyzer. It is written in Python and more flexible, but very slow.
+
+## For other use case (experimental)
+
+To see behavior velocity planner's internal plath with lane id
+add below example value to behavior velocity analayzer and set `is_publish_debug_path: true`
+
+```lua
+crosswalk ='/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path_with_lane_id/crosswalk/debug_info'
+intersection ='/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path_with_lane_id/intersection/debug_info'
+traffic_light ='/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path_with_lane_id/traffic_light/debug_info'
+merge_from_private ='/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path_with_lane_id/merge_from_private/debug_info'
+occlusion_spot ='/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path_with_lane_id/occlusion_spot/debug_info'
+```
+
+```lua
+PlotVelocityOverArclength('v_crosswalk', crosswalk, tracker_time)
+PlotVelocityOverArclength('v_intersection', intersection, tracker_time)
+PlotVelocityOverArclength('v_merge_from_private', merge_from_private, tracker_time)
+PlotVelocityOverArclength('v_traffic_light', traffic_light, tracker_time)
+PlotVelocityOverArclength('v_occlusion', occlusion_spot, tracker_time)
+
+PlotYawOverArclength('yaw_crosswalk', crosswalk, tracker_time)
+PlotYawOverArclength('yaw_intersection', intersection, tracker_time)
+PlotYawOverArclength('yaw_merge_from_private', merge_from_private, tracker_time)
+PlotYawOverArclength('yaw_traffic_light', traffic_light, tracker_time)
+PlotYawOverArclength('yaw_occlusion', occlusion_spot, tracker_time)
+
+PlotCurrentVelocity('localization_kinematic_state', '/localization/kinematic_state', tracker_time)
+```

planning/planning_debug_tools/launch/behavior_velocity_analyzer.launch.xml

@@ -0,0 +1,19 @@
+<launch>
+  <node pkg="planning_debug_tools" exec="trajectory_analyzer_exe" name="trajectory_analyzer" output="screen">
+    <param name="path_topics" value="[/planning/scenario_planning/lane_driving/behavior_planning/path]"/>
+    <param
+      name="path_wlid_topics"
+      value="[/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id
+              ,/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path_with_lane_id/crosswalk
+              ,/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path_with_lane_id/intersection
+              ,/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path_with_lane_id/merge_from_private
+              ,/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path_with_lane_id/occlusion_spot
+              ,/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path_with_lane_id/stop_line
+              ,/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path_with_lane_id/traffic_light
+              ,/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path_with_lane_id/walkway
+              ]"
+    />
+    <param name="trajectory_topics" value="[/planning/scenario_planning/trajectory]"/>
+    <remap from="ego_kinematics" to="/localization/kinematic_state"/>
+  </node>
+</launch>