From 18985499503912adfa2637263d7e8012667ecb6d Mon Sep 17 00:00:00 2001
From: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
Date: Tue, 27 Aug 2024 16:53:38 +0900
Subject: [PATCH] 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 <kyoichi.sugahara@tier4.jp>
---
 .../mission_planner/mission_planner_base.hpp       |  8 ++++++++
 .../mission_planner/lib/mission_planner_base.cpp   | 14 ++++++++++++++
 planning/mission_planner/package.xml               |  1 +
 3 files changed, 23 insertions(+)

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 <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);
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<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()) {
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 @@
   <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>