feat(rtc_interface): add new field

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

Author: satoshi-ota

planning/rtc_interface/include/rtc_interface/rtc_interface.hpp

@@ -24,6 +24,7 @@
 #include "tier4_rtc_msgs/msg/cooperate_status.hpp"
 #include "tier4_rtc_msgs/msg/cooperate_status_array.hpp"
 #include "tier4_rtc_msgs/msg/module.hpp"
+#include "tier4_rtc_msgs/msg/state.hpp"
 #include "tier4_rtc_msgs/srv/auto_mode.hpp"
 #include "tier4_rtc_msgs/srv/cooperate_commands.hpp"
 #include <unique_identifier_msgs/msg/uuid.hpp>
@@ -41,6 +42,7 @@ using tier4_rtc_msgs::msg::CooperateResponse;
 using tier4_rtc_msgs::msg::CooperateStatus;
 using tier4_rtc_msgs::msg::CooperateStatusArray;
 using tier4_rtc_msgs::msg::Module;
+using tier4_rtc_msgs::msg::State;
 using tier4_rtc_msgs::srv::AutoMode;
 using tier4_rtc_msgs::srv::CooperateCommands;
 using unique_identifier_msgs::msg::UUID;
@@ -51,8 +53,8 @@ class RTCInterface
   RTCInterface(rclcpp::Node * node, const std::string & name, const bool enable_rtc = true);
   void publishCooperateStatus(const rclcpp::Time & stamp);
   void updateCooperateStatus(
-    const UUID & uuid, const bool safe, const double start_distance, const double finish_distance,
-    const rclcpp::Time & stamp);
+    const UUID & uuid, const bool safe, const uint8_t state, const double start_distance,
+    const double finish_distance, const rclcpp::Time & stamp);
   void removeCooperateStatus(const UUID & uuid);
   void clearCooperateStatus();
   bool isActivated(const UUID & uuid) const;

planning/rtc_interface/src/rtc_interface.cpp

@@ -201,8 +201,8 @@ void RTCInterface::onTimer()
 }
 
 void RTCInterface::updateCooperateStatus(
-  const UUID & uuid, const bool safe, const double start_distance, const double finish_distance,
-  const rclcpp::Time & stamp)
+  const UUID & uuid, const bool safe, const uint8_t state, const double start_distance,
+  const double finish_distance, const rclcpp::Time & stamp)
 {
   std::lock_guard<std::mutex> lock(mutex_);
   // Find registered status which has same uuid
@@ -218,6 +218,7 @@ void RTCInterface::updateCooperateStatus(
     status.module = module_;
     status.safe = safe;
     status.command_status.type = Command::DEACTIVATE;
+    status.state.type = State::WAITING_FOR_EXECUTION;
     status.start_distance = start_distance;
     status.finish_distance = finish_distance;
     status.auto_mode = is_auto_mode_enabled_;
@@ -228,6 +229,7 @@ void RTCInterface::updateCooperateStatus(
   // If the registered status is found, update status
   itr->stamp = stamp;
   itr->safe = safe;
+  itr->state.type = state;
   itr->start_distance = start_distance;
   itr->finish_distance = finish_distance;
 }