Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

feat(behavior_path_planner): add route handler diagnostic #1081

Closed
wants to merge 2 commits into from
Closed

feat(behavior_path_planner): add route handler diagnostic #1081

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
be6cebd
feat(behavior_path_planner): add route handler diagnostic
shmpwk Jun 13, 2022
40cd0c9
ci(pre-commit): autofix
pre-commit-ci[bot] Jun 13, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
#include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
#include <autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp>
#include <diagnostic_msgs/msg/diagnostic_status.hpp>
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <tier4_planning_msgs/msg/approval.hpp>
Expand Down Expand Up @@ -67,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;
Expand Down Expand Up @@ -174,6 +176,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
rclcpp::Publisher<Path>::SharedPtr debug_path_publisher_;
rclcpp::Publisher<AvoidanceDebugMsgArray>::SharedPtr debug_avoidance_msg_array_publisher_;
rclcpp::Publisher<MarkerArray>::SharedPtr debug_marker_publisher_;
rclcpp::Publisher<DiagnosticStatus>::SharedPtr debug_diag_publisher_;
void publishDebugMarker(const std::vector<MarkerArray> & debug_markers);
};
} // namespace behavior_path_planner
Expand Down
11 changes: 11 additions & 0 deletions planning/behavior_path_planner/src/behavior_path_planner_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -86,6 +86,9 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
create_publisher<MarkerArray>("~/drivable_area_boundary", 1);
}

debug_diag_publisher_ =
create_publisher<diagnostic_msgs::msg::DiagnosticStatus>("~/output/load_route", 1);

// subscriber
velocity_subscriber_ = create_subscription<Odometry>(
"~/input/odometry", 1, std::bind(&BehaviorPathPlannerNode::onVelocity, this, _1),
Expand Down Expand Up @@ -742,6 +745,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".
Expand Down