add string debug publisher for mode and killswitch

Author: Vortex

mission/operation_mode_manager/include/operation_mode_manager.hpp

@@ -6,6 +6,7 @@
 #include <geometry_msgs/msg/wrench_stamped.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <std_msgs/msg/bool.hpp>
+#include <std_msgs/msg/string.hpp>
 
 #include <vortex/utils/types.hpp>
 #include <vortex_msgs/msg/operation_mode.hpp>
@@ -47,6 +48,8 @@ class OperationModeManager : public rclcpp::Node {
     rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>::SharedPtr wrench_pub_;
     rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr killswitch_pub_;
     rclcpp::Publisher<vortex_msgs::msg::OperationMode>::SharedPtr mode_pub_;
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr killswitch_string_pub_;
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr mode_string_pub_;
 
     rclcpp::Service<vortex_msgs::srv::SetOperationMode>::SharedPtr
         set_operation_mode_service_;

mission/operation_mode_manager/src/operation_mode_manager.cpp

@@ -41,6 +41,8 @@ void OperationModeManager::setup_publishers() {
         this->get_parameter("topics.killswitch").as_string();
     const auto operation_mode_topic =
         this->get_parameter("topics.operation_mode").as_string();
+    const auto killswitch_string_topic = killswitch_topic + "_string";
+    const auto operation_mode_string_topic = operation_mode_topic + "_string";
 
     wrench_pub_ = this->create_publisher<geometry_msgs::msg::WrenchStamped>(
         wrench_input_topic,
@@ -51,6 +53,14 @@ void OperationModeManager::setup_publishers() {
 
     mode_pub_ = this->create_publisher<vortex_msgs::msg::OperationMode>(
         operation_mode_topic, vortex::utils::qos_profiles::reliable_profile(1));
+
+    killswitch_string_pub_ = this->create_publisher<std_msgs::msg::String>(
+        killswitch_string_topic,
+        vortex::utils::qos_profiles::reliable_profile(1));
+
+    mode_string_pub_ = this->create_publisher<std_msgs::msg::String>(
+        operation_mode_string_topic,
+        vortex::utils::qos_profiles::reliable_profile(1));
 }
 
 void OperationModeManager::setup_services() {
@@ -152,6 +162,14 @@ void OperationModeManager::publish_mode() {
     std_msgs::msg::Bool killswitch_msg;
     killswitch_msg.data = killswitch_;
     killswitch_pub_->publish(killswitch_msg);
+
+    std_msgs::msg::String killswitch_string_msg;
+    killswitch_string_msg.data = "killswitch: " + std::string(killswitch_ ? "true" : "false");
+    killswitch_string_pub_->publish(killswitch_string_msg);
+
+    std_msgs::msg::String mode_string_msg;
+    mode_string_msg.data = "mode: " + vortex::utils::types::mode_to_string(mode_);
+    mode_string_pub_->publish(mode_string_msg);
 }
 
 void OperationModeManager::publish_empty_wrench() {