feat: Add alive service to mission planner

The code changes add an alive service to the mission planner. This service allows checking if the mission planner is alive and running. The service handler sets the response success flag to true and provides a message indicating that the mission planner is alive.

Signed-off-by: kyoichi-sugahara <[email protected]>

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 <std_srvs/srv/trigger.hpp>
+
+#include <memory>
 #include <string>
 #include <vector>
 
@@ -61,6 +64,11 @@ class MissionPlanner : public rclcpp::Node
   tf2_ros::Buffer tf_buffer_;
   tf2_ros::TransformListener tf_listener_;
 
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr alive_service_;
+  void handleAliveService(
+    const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
+    std::shared_ptr<std_srvs::srv::Trigger::Response> 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);

planning/mission_planner/lib/mission_planner_base.cpp

@@ -47,6 +47,11 @@ MissionPlanner::MissionPlanner(
     create_publisher<autoware_auto_planning_msgs::msg::HADMapRoute>("output/route", durable_qos);
   marker_publisher_ =
     create_publisher<visualization_msgs::msg::MarkerArray>("debug/route_marker", durable_qos);
+
+  alive_service_ = create_service<std_srvs::srv::Trigger>(
+    "/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<std_srvs::srv::Trigger::Request> request,
+  std::shared_ptr<std_srvs::srv::Trigger::Response> 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()) {

planning/mission_planner/package.xml

@@ -19,6 +19,7 @@
   <depend>route_handler</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
+  <depend>std_srvs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>