From be6cebdb4c5766e8c4f8ab14982017bfd2269013 Mon Sep 17 00:00:00 2001 From: Shumpei Wakabayashi Date: Mon, 13 Jun 2022 14:25:45 +0900 Subject: [PATCH 1/2] feat(behavior_path_planner): add route handler diagnostic Signed-off-by: Shumpei Wakabayashi --- .../behavior_path_planner_node.hpp | 3 +++ .../src/behavior_path_planner_node.cpp | 10 ++++++++++ 2 files changed, 13 insertions(+) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 62779c3d5924d..b9d6d93a2170b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include #include @@ -78,6 +79,7 @@ using tier4_planning_msgs::msg::PathChangeModule; using tier4_planning_msgs::msg::PathChangeModuleArray; using tier4_planning_msgs::msg::Scenario; using visualization_msgs::msg::MarkerArray; +using diagnostic_msgs::msg::DiagnosticStatus; class BehaviorPathPlannerNode : public rclcpp::Node { @@ -174,6 +176,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_path_publisher_; rclcpp::Publisher::SharedPtr debug_avoidance_msg_array_publisher_; rclcpp::Publisher::SharedPtr debug_marker_publisher_; + rclcpp::Publisher::SharedPtr debug_diag_publisher_; void publishDebugMarker(const std::vector & debug_markers); }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index da30b6d2e0d41..4e33259521afd 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -86,6 +86,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod create_publisher("~/drivable_area_boundary", 1); } + debug_diag_publisher_ = create_publisher("~/output/load_route", 1); + // subscriber velocity_subscriber_ = create_subscription( "~/input/odometry", 1, std::bind(&BehaviorPathPlannerNode::onVelocity, this, _1), @@ -742,6 +744,14 @@ void BehaviorPathPlannerNode::onRoute(const HADMapRoute::ConstSharedPtr msg) const bool is_first_time = !(planner_data_->route_handler->isHandlerReady()); planner_data_->route_handler->setRoute(*msg); + const bool is_handler_ready = planner_data_->route_handler->isHandlerReady(); + + diagnostic_msgs::msg::DiagnosticStatus status; + status.name = "route_handler_status"; + status.level = is_handler_ready ? diagnostic_msgs::msg::DiagnosticStatus::OK : + diagnostic_msgs::msg::DiagnosticStatus::ERROR; + status.message = is_handler_ready ? "OK" : "route_handler is not ready"; + debug_diag_publisher_->publish(status); // Reset behavior tree when new route is received, // so that the each modules do not have to care about the "route jump". From 40cd0c95f9c49dba53ffe5d31a5b7c06475be6cb Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 13 Jun 2022 05:41:27 +0000 Subject: [PATCH 2/2] ci(pre-commit): autofix --- .../behavior_path_planner/behavior_path_planner_node.hpp | 4 ++-- .../src/behavior_path_planner_node.cpp | 7 ++++--- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index b9d6d93a2170b..6415d4308c3a0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -36,9 +36,9 @@ #include #include #include +#include #include #include -#include #include #include #include @@ -68,6 +68,7 @@ using autoware_auto_planning_msgs::msg::Path; using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; +using diagnostic_msgs::msg::DiagnosticStatus; using geometry_msgs::msg::TwistStamped; using nav_msgs::msg::OccupancyGrid; using nav_msgs::msg::Odometry; @@ -79,7 +80,6 @@ using tier4_planning_msgs::msg::PathChangeModule; using tier4_planning_msgs::msg::PathChangeModuleArray; using tier4_planning_msgs::msg::Scenario; using visualization_msgs::msg::MarkerArray; -using diagnostic_msgs::msg::DiagnosticStatus; class BehaviorPathPlannerNode : public rclcpp::Node { diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 4e33259521afd..2e513bd9aa079 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -86,7 +86,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod create_publisher("~/drivable_area_boundary", 1); } - debug_diag_publisher_ = create_publisher("~/output/load_route", 1); + debug_diag_publisher_ = + create_publisher("~/output/load_route", 1); // subscriber velocity_subscriber_ = create_subscription( @@ -748,8 +749,8 @@ void BehaviorPathPlannerNode::onRoute(const HADMapRoute::ConstSharedPtr msg) diagnostic_msgs::msg::DiagnosticStatus status; status.name = "route_handler_status"; - status.level = is_handler_ready ? diagnostic_msgs::msg::DiagnosticStatus::OK : - diagnostic_msgs::msg::DiagnosticStatus::ERROR; + status.level = is_handler_ready ? diagnostic_msgs::msg::DiagnosticStatus::OK + : diagnostic_msgs::msg::DiagnosticStatus::ERROR; status.message = is_handler_ready ? "OK" : "route_handler is not ready"; debug_diag_publisher_->publish(status);