added controller state topic

uri-ocean-robotics · Sep 7, 2024 · 85bf859 · 85bf859
1 parent f12b092
commit 85bf859
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 1 deletion.
diff --git a/include/mvp_control/dictionary.h b/include/mvp_control/dictionary.h
@@ -83,6 +83,7 @@ namespace ctrl {
     static constexpr const char * TOPIC_CONTROL_PROCESS_VALUE = "controller/process/value";
     static constexpr const char * TOPIC_CONTROL_PROCESS_SET_POINT = "controller/process/set_point";
     static constexpr const char * TOPIC_CONTROL_PROCESS_ERROR = "controller/process/error";
+    static constexpr const char * TOPIC_CONTROLLER_STATE = "controller/state";
 
     static constexpr const char * SERVICE_CONTROL_ENABLE = "controller/enable";
     static constexpr const char * SERVICE_CONTROL_DISABLE = "controller/disable";

diff --git a/src/mvp_control/mvp_control_ros.cpp b/src/mvp_control/mvp_control_ros.cpp
@@ -117,6 +117,11 @@ MvpControlROS::MvpControlROS()
         100
     );
 
+    m_controller_state_publisher = m_nh.advertise<std_msgs::Bool>(
+        TOPIC_CONTROLLER_STATE,
+        100
+    );
+
     /**
      * Initialize services
      */
@@ -1276,7 +1281,10 @@ bool MvpControlROS::f_cb_srv_enable(
 
     // std::cout << "m_i:\n" << m_i << std::endl;
     m_enabled = true;
+    std_msgs::Bool controller_state;
+    controller_state.data=m_enabled;
 
+    m_controller_state_publisher.publish(controller_state);
     return true;
 }
 
@@ -1286,7 +1294,9 @@ bool MvpControlROS::f_cb_srv_disable(
 
     ROS_INFO("Controller disabled!");
     m_enabled = false;
-
+    std_msgs::Bool controller_state;
+    controller_state.data=m_enabled;
+    m_controller_state_publisher.publish(controller_state);
     return true;
 }
 

diff --git a/src/mvp_control/mvp_control_ros.h b/src/mvp_control/mvp_control_ros.h
@@ -43,6 +43,7 @@
 #include "std_msgs/Float32.h"
 #include "std_srvs/Empty.h"
 #include "std_srvs/Trigger.h"
+#include "std_msgs/Bool.h"
 #include "nav_msgs/Odometry.h"
 #include "dynamic_reconfigure/server.h"
 #include "geometry_msgs/PoseStamped.h"
@@ -179,6 +180,10 @@ namespace ctrl {
         //! @brief Publishes process error publisher
         ros::Publisher m_process_error_publisher;
 
+
+        //! @brief Controller state publisher
+        ros::Publisher m_controller_state_publisher;
+
         //! @brief Holder for latest odometry msg
         nav_msgs::Odometry m_odometry_msg;