From 85bf85975f497f7dfc2f138c53e94d4fa7ce94d6 Mon Sep 17 00:00:00 2001 From: mingxi Date: Sat, 7 Sep 2024 09:44:47 -0400 Subject: [PATCH] added controller state topic --- include/mvp_control/dictionary.h | 1 + src/mvp_control/mvp_control_ros.cpp | 12 +++++++++++- src/mvp_control/mvp_control_ros.h | 5 +++++ 3 files changed, 17 insertions(+), 1 deletion(-) diff --git a/include/mvp_control/dictionary.h b/include/mvp_control/dictionary.h index 8f5c616..0946b20 100644 --- 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 index 403f9cc..fd7653b 100644 --- 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( + 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 index 30dfc51..8504faa 100644 --- 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;