feat: watchdog timer for wrench (#532)

* feat: add watchdog timer for wrench message * remove commented code
vortexntnu · Feb 26, 2025 · a8ed3cf · a8ed3cf
1 parent af111b0
commit a8ed3cf
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 3 deletions.
diff --git a/auv_setup/config/robots/orca.yaml b/auv_setup/config/robots/orca.yaml
@@ -86,6 +86,7 @@
           max: 1 # Maximum rate of change in newton per second for a thruster
 
         thrust_update_rate: 100.0 # [Hz]
+        watchdog_timeout: 1.0 # [s]
 
         thruster_to_pin_mapping: [0, 1, 2, 3, 4, 6, 5, 7] # I.e. if thruster_to_pin = [ 7,  6,  5,  4,  3,  2,  1,  0 ] then thruster 0 is pin 1 etc..
         thruster_direction: [1, -1, 1, -1, 1, 1, 1, -1] # Disclose during thruster mapping (+/- 1)

diff --git a/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_ros.hpp b/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_ros.hpp
@@ -80,6 +80,9 @@ class ThrustAllocator : public rclcpp::Node {
 
     Eigen::Vector6d body_frame_forces_;
     PseudoinverseAllocator pseudoinverse_allocator_;
+
+    rclcpp::Time last_msg_time_;
+    rclcpp::Duration timeout_treshold_ = std::chrono::seconds(1);
 };
 
 #endif  // VORTEX_ALLOCATOR_ROS_HPP
diff --git a/motion/thrust_allocator_auv/src/thrust_allocator_ros.cpp b/motion/thrust_allocator_auv/src/thrust_allocator_ros.cpp
@@ -25,6 +25,7 @@ ThrustAllocator::ThrustAllocator()
         std::bind(&ThrustAllocator::calculate_thrust_timer_cb, this));
 
     body_frame_forces_.setZero();
+    last_msg_time_ = this->now();
 }
 
 void ThrustAllocator::extract_parameters() {
@@ -38,6 +39,7 @@ void ThrustAllocator::extract_parameters() {
         "propulsion.thrusters.thruster_force_direction");
     this->declare_parameter<std::vector<double>>(
         "propulsion.thrusters.thruster_position");
+    this->declare_parameter<double>("propulsion.thrusters.watchdog_timeout");
 
     center_of_mass_ = double_array_to_eigen_vector3d(
         this->get_parameter("physical.center_of_mass").as_double_array());
@@ -48,6 +50,11 @@ void ThrustAllocator::extract_parameters() {
     thrust_update_rate_ =
         this->get_parameter("propulsion.thrusters.thrust_update_rate")
             .as_double();
+    double timout_treshold_param =
+        this->get_parameter("propulsion.thrusters.watchdog_timeout")
+            .as_double();
+    timeout_treshold_ = std::chrono::duration_cast<std::chrono::seconds>(
+        std::chrono::duration<double>(timout_treshold_param));
 
     this->declare_parameter<std::string>("topics.wrench_input");
     this->declare_parameter<std::string>("topics.thruster_forces");
@@ -87,6 +94,9 @@ void ThrustAllocator::set_subscriber_and_publisher() {
 }
 
 void ThrustAllocator::calculate_thrust_timer_cb() {
+    if ((this->now() - last_msg_time_) > timeout_treshold_) {
+        body_frame_forces_.setZero();
+    }
     Eigen::VectorXd thruster_forces =
         pseudoinverse_allocator_.calculate_allocated_thrust(body_frame_forces_);
 
@@ -97,8 +107,7 @@ void ThrustAllocator::calculate_thrust_timer_cb() {
     }
 
     if (!saturate_vector_values(thruster_forces, min_thrust_, max_thrust_)) {
-        RCLCPP_WARN(get_logger(),
-                    "Thruster forces vector required saturation.");
+        RCLCPP_WARN(get_logger(), "ThrusterForces vector required saturation.");
     }
 
     vortex_msgs::msg::ThrusterForces msg_out;
@@ -107,10 +116,11 @@ void ThrustAllocator::calculate_thrust_timer_cb() {
 }
 
 void ThrustAllocator::wrench_cb(const geometry_msgs::msg::Wrench& msg) {
+    last_msg_time_ = this->now();
     Eigen::Vector6d msg_vector = wrench_to_vector(msg);
 
     if (!healthy_wrench(msg_vector)) {
-        RCLCPP_ERROR(get_logger(), "ASV wrench vector invalid, ignoring.");
+        RCLCPP_ERROR(get_logger(), "Wrench vector invalid, ignoring.");
         body_frame_forces_.setZero();
         return;
     }