diff --git a/planning/mission_planner/include/mission_planner/mission_planner_base.hpp b/planning/mission_planner/include/mission_planner/mission_planner_base.hpp index c7c56479f3f2f..3aa0b5c39daa2 100644 --- a/planning/mission_planner/include/mission_planner/mission_planner_base.hpp +++ b/planning/mission_planner/include/mission_planner/mission_planner_base.hpp @@ -15,6 +15,9 @@ #ifndef MISSION_PLANNER__MISSION_PLANNER_BASE_HPP_ #define MISSION_PLANNER__MISSION_PLANNER_BASE_HPP_ +#include + +#include #include #include @@ -61,6 +64,11 @@ class MissionPlanner : public rclcpp::Node tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; + rclcpp::Service::SharedPtr alive_service_; + void handleAliveService( + const std::shared_ptr request, + std::shared_ptr response); + bool getEgoVehiclePose(geometry_msgs::msg::PoseStamped * ego_vehicle_pose); void goalPoseCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr goal_msg_ptr); void checkpointCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr checkpoint_msg_ptr); diff --git a/planning/mission_planner/lib/mission_planner_base.cpp b/planning/mission_planner/lib/mission_planner_base.cpp index 5a1551a156784..7a9cf219ecb58 100644 --- a/planning/mission_planner/lib/mission_planner_base.cpp +++ b/planning/mission_planner/lib/mission_planner_base.cpp @@ -47,6 +47,11 @@ MissionPlanner::MissionPlanner( create_publisher("output/route", durable_qos); marker_publisher_ = create_publisher("debug/route_marker", durable_qos); + + alive_service_ = create_service( + "/planning/mission_planner/alive", + std::bind( + &MissionPlanner::handleAliveService, this, std::placeholders::_1, std::placeholders::_2)); } bool MissionPlanner::getEgoVehiclePose(geometry_msgs::msg::PoseStamped * ego_vehicle_pose) @@ -134,6 +139,15 @@ void MissionPlanner::checkpointCallback( publishRoute(route); } +void MissionPlanner::handleAliveService( + const std::shared_ptr request, + std::shared_ptr response) +{ + (void)request; + response->success = true; + response->message = "MissionPlanner is alive and running."; +} + void MissionPlanner::publishRoute(const autoware_auto_planning_msgs::msg::HADMapRoute & route) const { if (!route.segments.empty()) { diff --git a/planning/mission_planner/package.xml b/planning/mission_planner/package.xml index f42875b759788..68d0d6fe1c3e8 100644 --- a/planning/mission_planner/package.xml +++ b/planning/mission_planner/package.xml @@ -19,6 +19,7 @@ route_handler tf2_geometry_msgs tf2_ros + std_srvs ament_lint_auto autoware_lint_common