added no setpoint timeout safety

uri-ocean-robotics · Jun 24, 2024 · b9290af · b9290af
1 parent 75f8108
commit b9290af
Show file tree

Hide file tree

Showing 3 changed files with 30 additions and 11 deletions.
diff --git a/include/mvp_control/dictionary.h b/include/mvp_control/dictionary.h
@@ -75,6 +75,7 @@ namespace ctrl {
     static constexpr const char * CONF_CONTROL_ALLOCATION_MATRIX = "control_allocation_matrix";
     static constexpr const char * CONF_CONTROL_TF = "control_tf";
     static constexpr const char * CONF_CONTROLLER_FREQUENCY = "controller_frequency";
+    static constexpr const char * CONF_NO_SETPOINT_TIMEOUT = "controller_no_setpoint_timeout";
 
 
     static constexpr const char * TOPIC_SAFETY = "safety";

diff --git a/src/mvp_control/mvp_control_ros.cpp b/src/mvp_control/mvp_control_ros.cpp
@@ -81,6 +81,12 @@ MvpControlROS::MvpControlROS()
         10.0
     );
 
+    m_pnh.param<double>(
+        CONF_NO_SETPOINT_TIMEOUT,
+        m_no_setpoint_timeout,
+        10.0
+    );
+
     /**
      * Initialize Subscribers
      */
@@ -798,7 +804,8 @@ bool MvpControlROS::f_compute_process_values() {
 void MvpControlROS::f_control_loop() {
 
     double pt = ros::Time::now().toSec();
-
+    setpoint_timer = ros::Time::now().toSec();
+
     auto r = ros::Rate(m_controller_frequency);
 
     while(ros::ok()) {
@@ -811,21 +818,25 @@ void MvpControlROS::f_control_loop() {
             continue;
         }
 
+
         /**
-         * Check if controller is enabled or not.
+         * Compute the state of the system. Continue on failure. This may
+         * happen when transform tree is not ready.
          */
-        if(!m_enabled) {
-             for(int i = 0 ; i < m_thrusters.size() ; i++) {
-                m_thrusters.at(i)->command(0);
-            }
+        if(not f_compute_process_values()) {
             continue;
         }
 
         /**
-         * Compute the state of the system. Continue on failure. This may
-         * happen when transform tree is not ready.
+         * Check if controller is enabled or not.
          */
-        if(not f_compute_process_values()) {
+        double time_since_last_setpoint = ros::Time::now().toSec() - setpoint_timer;
+        // printf("timeout = %lf, %lf\r\n", time_since_last_setpoint, m_no_setpoint_timeout);
+        // if(!m_enabled) {
+        if(!m_enabled || time_since_last_setpoint > m_no_setpoint_timeout) {
+             for(int i = 0 ; i < m_thrusters.size() ; i++) {
+                m_thrusters.at(i)->command(0);
+            }
             continue;
         }
 
@@ -864,6 +875,7 @@ void MvpControlROS::f_cb_msg_odometry(
 
 void MvpControlROS::f_cb_srv_set_point(
         const mvp_msgs::ControlProcess::ConstPtr &msg) {
+    setpoint_timer = ros::Time::now().toSec();
     f_amend_set_point(*msg);
 }
 
@@ -1492,9 +1504,9 @@ bool MvpControlROS::f_amend_set_point(
                 rpy_world.y(),
                 rpy_world.z()
             );
-            printf("setpoint frame=%s\r\n", set_point.header.frame_id.c_str());
+            // printf("setpoint frame=%s\r\n", set_point.header.frame_id.c_str());
             // std::cout<<"xyz =\n"<<p_world<<std::endl;
-            std::cout<<"rpy =\n"<<rpy_world<<std::endl;
+            // std::cout<<"rpy =\n"<<rpy_world<<std::endl;
         }
         else
         {

diff --git a/src/mvp_control/mvp_control_ros.h b/src/mvp_control/mvp_control_ros.h
@@ -140,6 +140,12 @@ namespace ctrl {
         //! @brief Controller frequency
         double m_controller_frequency;
 
+        //! @brief Controller timeout
+        double m_no_setpoint_timeout;
+
+        //! @brief setpont timer
+        double setpoint_timer;
+
         //! @brief Get control modes ros service server
         ros::ServiceServer m_get_control_modes_server;