fix: actually publish zero thrust (#536)

* fix(script): improve error handling and early termination on failure

* fix: actually publish zero thrust

---------

Co-authored-by: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com>

Author: Andeshog

motion/thrust_allocator_auv/src/thrust_allocator_ros.cpp

@@ -91,10 +91,13 @@ void ThrustAllocator::set_subscriber_and_publisher() {
 void ThrustAllocator::wrench_cb(const geometry_msgs::msg::Wrench& msg) {
     last_msg_time_ = this->now();
     Eigen::Vector6d wrench_vector = wrench_to_vector(msg);
+    Eigen::VectorXd zero_forces = Eigen::VectorXd::Zero(8);
 
     if (!healthy_wrench(wrench_vector)) {
         RCLCPP_ERROR(get_logger(), "Wrench vector invalid, publishing zeros.");
-        thruster_forces_publisher_->publish(vortex_msgs::msg::ThrusterForces());
+        vortex_msgs::msg::ThrusterForces msg_out =
+            array_eigen_to_msg(zero_forces);
+        thruster_forces_publisher_->publish(msg_out);
         return;
     }
 
@@ -104,7 +107,9 @@ void ThrustAllocator::wrench_cb(const geometry_msgs::msg::Wrench& msg) {
     if (is_invalid_matrix(thruster_forces)) {
         RCLCPP_ERROR(get_logger(),
                      "ThrusterForces vector invalid, publishing zeros.");
-        thruster_forces_publisher_->publish(vortex_msgs::msg::ThrusterForces());
+        vortex_msgs::msg::ThrusterForces msg_out =
+            array_eigen_to_msg(zero_forces);
+        thruster_forces_publisher_->publish(msg_out);
         return;
     }
 
@@ -119,10 +124,13 @@ void ThrustAllocator::wrench_cb(const geometry_msgs::msg::Wrench& msg) {
 
 void ThrustAllocator::watchdog_callback() {
     auto now = this->now();
+    Eigen::VectorXd zero_forces = Eigen::VectorXd::Zero(8);
     if ((now - last_msg_time_) >= timeout_treshold_ && !watchdog_triggered_) {
         watchdog_triggered_ = true;
         RCLCPP_WARN(get_logger(), "Watchdog triggered, publishing zeros.");
-        thruster_forces_publisher_->publish(vortex_msgs::msg::ThrusterForces());
+        vortex_msgs::msg::ThrusterForces msg_out =
+            array_eigen_to_msg(zero_forces);
+        thruster_forces_publisher_->publish(msg_out);
     }
 }
 

tests/waypoint_navigation/simulator_test.sh

@@ -1,5 +1,6 @@
 #!/bin/bash
 set -e
+set -o pipefail
 
 # Load ROS 2 environment
 echo "Setting up ROS 2 environment..."
@@ -10,6 +11,14 @@ export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib
 # Get the directory of this script dynamically
 SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
 
+# Function to terminate processes safely on error
+cleanup() {
+    echo "Error detected. Cleaning up..."
+    kill -TERM -"$SIM_PID" -"$ORCA_PID" -"$CONTROLLER_PID" -"$FILTER_PID" || true
+    exit 1
+}
+trap cleanup ERR
+
 # Launch Stonefish Simulator
 setsid ros2 launch stonefish_sim simulation_nogpu.launch.py &
 SIM_PID=$!
@@ -22,6 +31,12 @@ timeout 30s bash -c '
     done || true'
 echo "Simulator started"
 
+# Check for ROS errors in logs
+if journalctl -u ros2 | grep -i "error"; then
+    echo "Error detected in ROS logs. Exiting..."
+    exit 1
+fi
+
 # Wait for odometry data
 echo "Waiting for odom data..."
 timeout 10s ros2 topic echo /orca/odom --once
@@ -36,6 +51,12 @@ echo "Waiting for sim interface to start..."
 timeout 30s bash -c 'until ros2 topic list | grep -q "/orca/pose"; do sleep 1; done'
 echo "Simulator started"
 
+# Check for ROS errors again
+if journalctl -u ros2 | grep -i "error"; then
+    echo "Error detected in ROS logs. Exiting..."
+    exit 1
+fi
+
 # Wait for pose data
 echo "Waiting for pose data..."
 timeout 10s ros2 topic echo /orca/pose --once
@@ -50,6 +71,12 @@ setsid ros2 launch reference_filter_dp reference_filter.launch.py &
 FILTER_PID=$!
 echo "Launched filter with PID: $FILTER_PID"
 
+# Check for ROS errors before continuing
+if journalctl -u ros2 | grep -i "error"; then
+    echo "Error detected in ROS logs. Exiting..."
+    exit 1
+fi
+
 # Set operation mode
 echo "Turning off killswitch and setting operation mode to autonomous mode"
 ros2 topic pub /orca/killswitch std_msgs/msg/Bool "{data: false}" -1
@@ -72,3 +99,5 @@ fi
 
 # Terminate processes
 kill -TERM -"$SIM_PID" -"$ORCA_PID" -"$CONTROLLER_PID" -"$FILTER_PID"
+
+echo "Test completed successfully."