From 9b60f4d2aba20219f1f5a8db3a39c227667dcd18 Mon Sep 17 00:00:00 2001 From: taikitanaka3 Date: Fri, 19 Aug 2022 18:02:13 +0900 Subject: [PATCH 1/3] feat(behavior_velocity): publish internal path as debug path Signed-off-by: taikitanaka3 --- .../scene_module/scene_module_interface.hpp | 8 +++++- .../behavior_velocity_analyzer.launch.xml | 28 +++++++++++++++++++ 2 files changed, 35 insertions(+), 1 deletion(-) create mode 100644 planning/planning_debug_tools/launch/behavior_velocity_analyzer.launch.xml diff --git a/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp b/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp index dc129687e9b42..b98a3aaacd8e7 100644 --- a/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp @@ -120,6 +120,8 @@ class SceneModuleManagerInterface { const auto ns = std::string("~/debug/") + module_name; pub_debug_ = node.create_publisher(ns, 20); + pub_debug_path_ = node.create_publisher( + std::string("~/debug/path_with_lane_id/") + module_name, 20); pub_virtual_wall_ = node.create_publisher( std::string("~/virtual_wall/") + module_name, 20); pub_stop_reason_ = @@ -157,7 +159,7 @@ class SceneModuleManagerInterface { StopWatch stop_watch; stop_watch.tic("Total"); - + const bool publish_debug_scene_module_path = true; 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 +201,9 @@ class SceneModuleManagerInterface } pub_infrastructure_commands_->publish(infrastructure_command_array); pub_debug_->publish(debug_marker_array); + if(publish_debug_scene_module_path) { + pub_debug_path_->publish(*path); + } pub_virtual_wall_->publish(virtual_wall_marker_array); processing_time_publisher_->publish( std::string(getModuleName()) + "/processing_time_ms", stop_watch.toc("Total")); @@ -258,6 +263,7 @@ class SceneModuleManagerInterface rclcpp::Logger logger_; rclcpp::Publisher::SharedPtr pub_virtual_wall_; rclcpp::Publisher::SharedPtr pub_debug_; + rclcpp::Publisher::SharedPtr pub_debug_path_; rclcpp::Publisher::SharedPtr pub_stop_reason_; rclcpp::Publisher::SharedPtr pub_infrastructure_commands_; diff --git a/planning/planning_debug_tools/launch/behavior_velocity_analyzer.launch.xml b/planning/planning_debug_tools/launch/behavior_velocity_analyzer.launch.xml new file mode 100644 index 0000000000000..a1f742369e7e4 --- /dev/null +++ b/planning/planning_debug_tools/launch/behavior_velocity_analyzer.launch.xml @@ -0,0 +1,28 @@ + + + + + + + + From 60018411b8290a0b13aeba021733ea57baa1cb3c Mon Sep 17 00:00:00 2001 From: tanaka3 Date: Fri, 19 Aug 2022 18:38:55 +0900 Subject: [PATCH 2/3] feat(behavior_velocity): add debug internal scene module path Signed-off-by: tanaka3 --- .../behavior_velocity_planner.param.yaml | 1 + .../config/behavior_velocity_planner.param.yaml | 1 + .../include/scene_module/scene_module_interface.hpp | 11 +++++++---- .../launch/behavior_velocity_analyzer.launch.xml | 12 ++++-------- 4 files changed, 13 insertions(+), 12 deletions(-) diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml index 5ac74c2015d08..1e0777ecf994a 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml +++ b/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: false # publish all debug path with lane id in each module diff --git a/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml b/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml index 5ac74c2015d08..1e0777ecf994a 100644 --- a/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml +++ b/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 diff --git a/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp b/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp index b98a3aaacd8e7..e23cccbc73679 100644 --- a/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp @@ -120,8 +120,11 @@ class SceneModuleManagerInterface { const auto ns = std::string("~/debug/") + module_name; pub_debug_ = node.create_publisher(ns, 20); - pub_debug_path_ = node.create_publisher( - std::string("~/debug/path_with_lane_id/") + module_name, 20); + bool is_publish_debug_path_ = node.declare_parameter("is_publish_debug_path", false); + if (is_publish_debug_path_) { + pub_debug_path_ = node.create_publisher( + std::string("~/debug/path_with_lane_id/") + module_name, 1); + } pub_virtual_wall_ = node.create_publisher( std::string("~/virtual_wall/") + module_name, 20); pub_stop_reason_ = @@ -159,7 +162,6 @@ class SceneModuleManagerInterface { StopWatch stop_watch; stop_watch.tic("Total"); - const bool publish_debug_scene_module_path = true; visualization_msgs::msg::MarkerArray debug_marker_array; visualization_msgs::msg::MarkerArray virtual_wall_marker_array; tier4_planning_msgs::msg::StopReasonArray stop_reason_array; @@ -201,7 +203,7 @@ class SceneModuleManagerInterface } pub_infrastructure_commands_->publish(infrastructure_command_array); pub_debug_->publish(debug_marker_array); - if(publish_debug_scene_module_path) { + if (is_publish_debug_path_) { pub_debug_path_->publish(*path); } pub_virtual_wall_->publish(virtual_wall_marker_array); @@ -260,6 +262,7 @@ class SceneModuleManagerInterface boost::optional first_stop_path_point_index_; rclcpp::Clock::SharedPtr clock_; // Debug + bool is_publish_debug_path_; // note : this is very heavy debug topic option rclcpp::Logger logger_; rclcpp::Publisher::SharedPtr pub_virtual_wall_; rclcpp::Publisher::SharedPtr pub_debug_; diff --git a/planning/planning_debug_tools/launch/behavior_velocity_analyzer.launch.xml b/planning/planning_debug_tools/launch/behavior_velocity_analyzer.launch.xml index a1f742369e7e4..2ece17ae7c0c5 100644 --- a/planning/planning_debug_tools/launch/behavior_velocity_analyzer.launch.xml +++ b/planning/planning_debug_tools/launch/behavior_velocity_analyzer.launch.xml @@ -1,8 +1,8 @@ - - + From 5f1057dde349b21306d55eb07d7e1a4eb4632a7a Mon Sep 17 00:00:00 2001 From: taikitanaka3 Date: Sun, 21 Aug 2022 21:09:08 +0900 Subject: [PATCH 3/3] feat(behavior_velcoity, planning_debug_tools): add params for debug path Signed-off-by: taikitanaka3 --- .../behavior_velocity_planner.param.yaml | 2 +- .../scene_module/scene_module_interface.hpp | 12 ++++++-- planning/planning_debug_tools/Readme.md | 30 +++++++++++++++++++ .../behavior_velocity_analyzer.launch.xml | 9 ++---- 4 files changed, 42 insertions(+), 11 deletions(-) diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml index 1e0777ecf994a..20886e14b7a0f 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml @@ -16,4 +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 + is_publish_debug_path: true # publish all debug path with lane id in each module diff --git a/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp b/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp index e23cccbc73679..1f196a8cc7701 100644 --- a/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp @@ -120,7 +120,11 @@ class SceneModuleManagerInterface { const auto ns = std::string("~/debug/") + module_name; pub_debug_ = node.create_publisher(ns, 20); - bool is_publish_debug_path_ = node.declare_parameter("is_publish_debug_path", false); + 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( std::string("~/debug/path_with_lane_id/") + module_name, 1); @@ -204,7 +208,9 @@ class SceneModuleManagerInterface pub_infrastructure_commands_->publish(infrastructure_command_array); pub_debug_->publish(debug_marker_array); if (is_publish_debug_path_) { - pub_debug_path_->publish(*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( @@ -262,7 +268,7 @@ class SceneModuleManagerInterface boost::optional first_stop_path_point_index_; rclcpp::Clock::SharedPtr clock_; // Debug - bool is_publish_debug_path_; // note : this is very heavy debug topic option + bool is_publish_debug_path_ = {false}; // note : this is very heavy debug topic option rclcpp::Logger logger_; rclcpp::Publisher::SharedPtr pub_virtual_wall_; rclcpp::Publisher::SharedPtr pub_debug_; diff --git a/planning/planning_debug_tools/Readme.md b/planning/planning_debug_tools/Readme.md index bde36eac19024..a0f5bcd2b128c 100644 --- a/planning/planning_debug_tools/Readme.md +++ b/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) +``` diff --git a/planning/planning_debug_tools/launch/behavior_velocity_analyzer.launch.xml b/planning/planning_debug_tools/launch/behavior_velocity_analyzer.launch.xml index 2ece17ae7c0c5..f08bc8c64326d 100644 --- a/planning/planning_debug_tools/launch/behavior_velocity_analyzer.launch.xml +++ b/planning/planning_debug_tools/launch/behavior_velocity_analyzer.launch.xml @@ -4,21 +4,16 @@ - +