diff --git a/messages/sam_msgs/msg/Topics.msg b/messages/sam_msgs/msg/Topics.msg
index 52716c56..8c7b3c2b 100644
--- a/messages/sam_msgs/msg/Topics.msg
+++ b/messages/sam_msgs/msg/Topics.msg
@@ -87,6 +87,7 @@ string LED_CMD_TOPIC = 'core/led_cmd'
string DVL_CMD_TOPIC = 'core/dvl_relay'
string SSS_CMD_TOPIC = 'core/sss_relay'
string TOGGLE_7V_CMD_TOPIC = 'core/enable_7v'
+string UTM_ZONE_BAND = 'core/utm_zone_band'
string GET_NODE_INFO_SRV = 'core/uavcan_get_node_info'
string GET_TRANSPORT_STATS_SRV = 'core/uavcan_get_transport_stats'
diff --git a/scripts/smarc_bringups/scripts/sam_bringup.sh b/scripts/smarc_bringups/scripts/sam_bringup.sh
index 4c2071c6..d2e519a6 100755
--- a/scripts/smarc_bringups/scripts/sam_bringup.sh
+++ b/scripts/smarc_bringups/scripts/sam_bringup.sh
@@ -1,11 +1,15 @@
#! /bin/bash
# Set LOCAL_ROBOT_NAME, LOCAL_MQTT_BROKER_IP, and LOCAL_MQTT_BROKER_PORT in your bashrc
ROBOT_NAME=$LOCAL_ROBOT_NAME
-MQTT_BROKER_IP=$LOCAL_MQTT_BROKER_IP
-MQTT_BROKER_PORT=$LOCAL_MQTT_BROKER_PORT
+# ROBOT_NAME=sam
+# MQTT_BROKER_IP=$LOCAL_MQTT_BROKER_IP
+MQTT_BROKER_IP=20.240.40.232
+# MQTT_BROKER_PORT=$LOCAL_MQTT_BROKER_PORT
+MQTT_BROKER_PORT=1884
+SSS_SAVE_PATH=/home/orin/sss_auto_save
SESSION=${ROBOT_NAME}_bringup
-USE_SIM_TIME=True
+USE_SIM_TIME=False
# New variables for wasp_bt.launch and wasp_mqtt_agent.launch
AGENT_TYPE=subsurface
@@ -22,6 +26,8 @@ fi
tmux -2 new-session -d -s $SESSION
if [ $REALSIM = "simulation" ]
then
+
+ # TODO: this can be used in both, remove from here
# Mostly static stuff that wont be giving much feedback
# for robot description launch. so we get base_link -> everything else
tmux new-window -t $SESSION:6 -n 'description'
@@ -36,23 +42,29 @@ else
tmux new-window -t $SESSION:0 -n 'core'
tmux select-window -t $SESSION:0
tmux send-keys "ros2 launch sam_drivers sam_core.launch robot_name:=$ROBOT_NAME" C-m
-
- tmux new-window -t $SESSION:7 -n payloads
- tmux select-window -t $SESSION:7
- tmux send-keys "ros2 launch sam_drivers sam_payloads.launch sss_out_file:=$SSS_SAVE_PATH/ high_freq:=true range:=40 robot_name:=$ROBOT_NAME" C-m
-
- tmux new-window -t $SESSION:8 -n uwcomms
+
+ tmux new-window -t $SESSION:8 -n payloads
tmux select-window -t $SESSION:8
+ tmux send-keys "ros2 launch sam_drivers sam_payloads.launch sss_out_file:=$SSS_SAVE_PATH/ high_freq:=true robot_name:=$ROBOT_NAME use_sim_time:=$USE_SIM_TIME" C-m
+
+ tmux new-window -t $SESSION:9 -n uwcomms
+ tmux select-window -t $SESSION:9
tmux send-keys "ros2 launch sam_drivers sam_uwcomms.launch robot_name:=$ROBOT_NAME use_sim_time:=$USE_SIM_TIME" C-m
+ # Mostly static stuff that wont be giving much feedback
+ # for robot description launch. so we get base_link -> everything else
+ tmux new-window -t $SESSION:10 -n 'description'
+ tmux select-window -t $SESSION:10
+ tmux send-keys "ros2 launch sam_description sam_description.launch robot_name:=$ROBOT_NAME" C-m
+
fi
# state estimation stuff like pressure->depth, imu->tf etc
tmux new-window -t $SESSION:1 -n 'dr'
tmux rename-window "dr"
tmux select-window -t $SESSION:1
-#tmux send-keys "ros2 launch sam_dead_reckoning sam_dr_launch.launch robot_name:=$ROBOT_NAME" C-m
-tmux send-keys "echo 'Not launching sam_dead_reckoning sam_dr_launch.launch until someone fixes it!'" C-m
+tmux send-keys "ros2 launch hydrobatic_localization state_estimator.launch robot_name:=$ROBOT_NAME" C-m
+# tmux send-keys "echo 'Not launching sam_dead_reckoning sam_dr_launch.launch until someone fixes it!'" C-m
# BT, action servers etc.
tmux new-window -t $SESSION:2 -n 'bt'
@@ -71,41 +83,41 @@ tmux new-window -t $SESSION:4 -n 'smcp'
tmux select-window -t $SESSION:4
tmux send-keys "ros2 launch sam_smarc_publisher default.launch robot_name:=$ROBOT_NAME" C-m
-# utility stuff like dubins planning and lat/lon conversions that other stuff rely on
-tmux new-window -t $SESSION:5 -n 'utils'
+# for the mqtt bridge.
+tmux new-window -t $SESSION:5 -n 'mqtt'
+# Set your MQTT Broker IP and Port in your bashrc
tmux select-window -t $SESSION:5
-tmux send-keys "ros2 launch smarc_bringups utilities.launch robot_name:=$ROBOT_NAME" C-m
+tmux send-keys "ros2 launch str_json_mqtt_bridge waraps_bridge.launch broker_addr:=$MQTT_BROKER_IP broker_port:=$MQTT_BROKER_PORT robot_name:=$ROBOT_NAME" C-m
-# for robot description launch. so we get base_link -> everything else
-tmux new-window -t $SESSION:8 -n 'description'
-tmux select-window -t $SESSION:8
-tmux send-keys "ros2 topic pub -r 1 /$ROBOT_NAME/smarc/vehicle_health std_msgs/msg/Int8 '{data: 0}' " # don't use C-m here, we want to keep the command in the window
-
-# dummy stuff to temporarily let other stuff work
-tmux new-window -t $SESSION:9 -n 'dummies'
-tmux new-window -t $SESSION:9 -n 'emergency_action'
-tmux select-window -t $SESSION:9
+tmux new-window -t $SESSION:6 -n 'emergency'
+tmux select-window -t $SESSION:6
# tmux send-keys "ros2 launch smarc_bringups dummies.launch robot_name:=$ROBOT_NAME" C-m
tmux send-keys "ros2 launch sam_emergency_action sam_emergency_action.launch robot_name:=$ROBOT_NAME" C-m
-# for the mqtt bridge.
-tmux new-window -t $SESSION:10 -n 'mqtt'
-# Set your MQTT Broker IP and Port in your bashrc
-tmux select-window -t $SESSION:10
-tmux send-keys "ros2 launch str_json_mqtt_bridge waraps_bridge.launch broker_addr:=$MQTT_BROKER_IP broker_port:=$MQTT_BROKER_PORT robot_name:=$ROBOT_NAME" C-m
+# utility stuff like dubins planning and lat/lon conversions that other stuff rely on
+# tmux new-window -t $SESSION:5 -n 'utils'
+# tmux select-window -t $SESSION:5
+# tmux send-keys "ros2 launch smarc_bringups utilities.launch robot_name:=$ROBOT_NAME" C-m
+
+# for robot description launch. so we get base_link -> everything else
+tmux new-window -t $SESSION:7 -n 'health'
+tmux select-window -t $SESSION:7
+# tmux send-keys "ros2 topic pub -r 1 /$ROBOT_NAME/smarc/vehicle_health std_msgs/msg/Int8 '{data: 0}' " # don't use C-m here, we want to keep the command in the window
+tmux send-keys "ros2 launch sam_health_checker sam_rate_health_checker.launch robot_name:=$ROBOT_NAME use_sim_time:=$USE_SIM_TIME" C-m
# Launch the wasp_bt LAST, to give action servers time to start publishing heartbeats
tmux select-window -t $SESSION:2
tmux send-keys "ros2 launch wasp_bt wasp_bt.launch robot_name:=$ROBOT_NAME agent_type:=$AGENT_TYPE pulse_rate:=$PULSE_RATE use_sim_time:=$USE_SIM_TIME" C-m
-USERNAME=$(whoami)
-if [ $USERNAME != "sam" ]
-then
- echo "You are not the real sam!"
- ROS_IP=127.0.0.1
- # Maybe launch ros-tcp-bridge here?
-fi
+#USERNAME=$(whoami)
+#if [ $USERNAME != "sam" ]
+#then
+# echo "You are not the real sam!"
+# ROS_IP=127.0.0.1
+# # Maybe launch ros-tcp-bridge here?
+#fi
+
# Set default window
-tmux select-window -t $SESSION:2
+tmux select-window -t $SESSION:0
# attach to the new session
tmux -2 attach-session -t $SESSION
diff --git a/vehicles/hardware/sam/sam_health_checker/config/sam_health_limits.yaml b/vehicles/hardware/sam/sam_health_checker/config/sam_health_limits.yaml
new file mode 100644
index 00000000..a99b9bf7
--- /dev/null
+++ b/vehicles/hardware/sam/sam_health_checker/config/sam_health_limits.yaml
@@ -0,0 +1,53 @@
+# Default limits for SAM.
+
+# Copied from LoLo limits, most values are not being used
+# PLEASE CHECK VALUES BEFORE USING
+
+# --------- DO NOT MODIFY ------------
+# ----- ACTION SERVER LIMITS -------
+# Maximum allowed time for LoLo to complete
+# an action (e.g. MoveTo, DepthMoveTo, Loiter).
+max_timeout_secs: 1800 # [s]
+
+# Maximum allowed distance (in the XY plane)
+# between Lolo and the next waypoint (or between
+# consecutive waypoints).
+max_waypoint_dist: 1000 # [m]
+
+# Goal tolerance radius (on the XY plane).
+goal_tolerance_plane: 5 # [m]
+
+# Goal tolerance for depth (Z).
+goal_tolerance_depth: 5 # [m]
+# ------------------------------------
+
+# ----- GENERAL VEHICLE LIMITS -------
+# Absolute maximum depth allowed.
+max_depth: 100 # [m]
+
+# Depth threshold for diving
+diving_threshold_depth: 0.5 # [m]
+
+# Absolute minimum altitude from the
+# seafloor allowed.
+min_altitude: 0.5 # [m]
+
+# Absolute maximum RPMs for horizontal
+# thrusters allowed.
+max_thruster_rpm: 1000 # [rpm]
+
+# Absolute maximum RPMs for vertical
+# thrusters allowed.
+max_vert_thruster_rpm: 2500 # [rpm]
+
+# Maximum uninterrupted diving time.
+max_dive_time: 1800
+
+# Pressure tolerance +/- in [mbar] for the
+# pressure vessels' pressure readings.
+pressure_tolerance: 100
+
+# Battery voltage range limits in [v].
+voltage: [22, 40]
+# ----------------------------------------
+# --------- DO NOT MODIFY END ------------
diff --git a/vehicles/hardware/sam/sam_health_checker/launch/sam_rate_health_checker.launch b/vehicles/hardware/sam/sam_health_checker/launch/sam_rate_health_checker.launch
index f517c11b..bd761e22 100644
--- a/vehicles/hardware/sam/sam_health_checker/launch/sam_rate_health_checker.launch
+++ b/vehicles/hardware/sam/sam_health_checker/launch/sam_rate_health_checker.launch
@@ -8,11 +8,11 @@
-
-
+
+
-
\ No newline at end of file
+
diff --git a/vehicles/hardware/sam/sam_health_checker/launch/testing_sam_health.launch b/vehicles/hardware/sam/sam_health_checker/launch/testing_sam_health.launch
new file mode 100644
index 00000000..9a42ec4d
--- /dev/null
+++ b/vehicles/hardware/sam/sam_health_checker/launch/testing_sam_health.launch
@@ -0,0 +1,20 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/vehicles/hardware/sam/sam_health_checker/sam_health_checker/helpers/health_helpers.py b/vehicles/hardware/sam/sam_health_checker/sam_health_checker/helpers/health_helpers.py
index 468da491..6013ca20 100644
--- a/vehicles/hardware/sam/sam_health_checker/sam_health_checker/helpers/health_helpers.py
+++ b/vehicles/hardware/sam/sam_health_checker/sam_health_checker/helpers/health_helpers.py
@@ -121,9 +121,10 @@ def determine_fault(self):
# Check for timeout
now = self.node.get_clock().now().nanoseconds/1e9
- if now - self.timestamps[topic_name][0] > self.timeout_time_sec:
+ time_diff = now - self.timestamps[topic_name][0]
+ if time_diff > self.timeout_time_sec:
self.fault = True
- self.node.get_logger().warn(f"Fault: timeout on {topic_name}")
+ self.node.get_logger().warn(f"Fault: timeout on {topic_name}, time difference: {time_diff:.2f} s > {self.timeout_time_sec:.2f} s")
return True
# Check frequency
diff --git a/vehicles/hardware/sam/sam_health_checker/sam_health_checker/sam_rate_health_node.py b/vehicles/hardware/sam/sam_health_checker/sam_health_checker/sam_rate_health_node.py
index 55f7795d..950beb8b 100644
--- a/vehicles/hardware/sam/sam_health_checker/sam_health_checker/sam_rate_health_node.py
+++ b/vehicles/hardware/sam/sam_health_checker/sam_health_checker/sam_rate_health_node.py
@@ -1,11 +1,14 @@
+import os
+import yaml
from dataclasses import dataclass, field
import rclpy
from rclpy.node import Node
+from ament_index_python import get_package_share_directory
# General messages
from sensor_msgs.msg import Imu
-from std_msgs.msg import Int8, Float64
+from std_msgs.msg import Int8, Float32, String
from sensor_msgs.msg import BatteryState, Imu
# SMaRC messages
@@ -38,14 +41,17 @@ def __init__(self, namespace=None):
self.namespace = namespace
# === Load Parameters ===
- self.declare_parameter('verbose', False)
- self.verbose = self.get_parameter("verbose").value
+
+ # Limits
+ self.declare_parameter("limits_filename", "sam_health_limits.yaml")
+ self.limits_filename = self.get_parameter("limits_filename").value
+ self.limits = self.read_limits()
self.declare_parameter("output_rate", 1.0)
self.output_rate = self.get_parameter("output_rate").value
- # Leak and batter parameters
- self.declare_parameter("leak_topic", SamTopics.LEAK_TOPIC_FB)
+ # Leak and battery parameters
+ self.declare_parameter("leak_topic", SamTopics.LEAK_TOPIC)
self.leak_topic = self.get_parameter("leak_topic").value
self.declare_parameter("battery_topic", SamTopics.BATTERY_STATUS_TOPIC)
@@ -57,6 +63,14 @@ def __init__(self, namespace=None):
self.declare_parameter("battery_min_capacity", 0.25)
self.battery_min_capacity = self.get_parameter("battery_min_capacity").value
+ # Depth and Altitude parameters
+ # The values from these topics are compared to the values read from limits
+ self.declare_parameter("depth_topic", SmarcTopics.DEPTH_TOPIC)
+ self.depth_topic = self.get_parameter("depth_topic").value
+
+ self.declare_parameter("altitude_topic", SmarcTopics.ALTITUDE_TOPIC)
+ self.altitude_topic = self.get_parameter("altitude_topic").value
+
# Topic rate monitor parameters
# time since start if no message is received
self.declare_parameter("initial_timeout_time_sec", 30.0)
@@ -69,24 +83,44 @@ def __init__(self, namespace=None):
self.declare_parameter("report_topic", SmarcTopics.VEHICLE_HEALTH_TOPIC)
self.report_topic = self.get_parameter("report_topic").value
+ self.declare_parameter('verbose', False)
+ self.verbose = self.get_parameter("verbose").value
if self.verbose:
self.log_all_parameters()
+ self.declare_parameter('testing', False)
+ self.testing = self.get_parameter('testing').value
+
# === Hardcoded topics, message types, and rates ===
- monitored_topics = {
- '/diagnostics': [DiagnosticArray, .5],
- SamTopics.DVL_TOPIC: [DVL, 1.0],
+ # Essential topics - Will trigger fault if no detected within initial timeout time
+ self.essential_topics = {
SamTopics.STIM_IMU_TOPIC: [Imu, 20.0]
}
+ # Optional topics - Will NOT trigger fault based on initial timeout time
+ self.optional_topics = {
+ SamTopics.DVL_TOPIC: [DVL, 1.0],
+ }
+
self.start_time = self.get_clock().now().nanoseconds/1e9
+
self.current_leak = None
self.current_leak_time = None
- self.current_leak_status = StatusReport()
+ self.current_leak_status = StatusReport(ready=True,fault=False) # No waiting, messages only when a fault is detected
self.current_battery = None
self.current_battery_time = None
self.current_battery_status = StatusReport()
+ self.current_depth = None
+ self.current_depth_time = None
+ self.current_depth_status = StatusReport()
+ self.current_altitude = None
+ self.current_altitude_time = None
+ self.current_altitude_status = StatusReport()
+
+ self.fault_report = None
+ self.fault_logged = False
+
# === Set up subs, pubs, and timers ===
self.leak_sub = self.create_subscription(msg_type=Leak,
topic=self.leak_topic,
@@ -98,13 +132,33 @@ def __init__(self, namespace=None):
callback=self.battery_callback,
qos_profile=10)
- self.get_logger().info(f"topic rate monitor instantiated")
- self.monitor = TopicRateMonitor(self, monitored_topics, timeout_time_sec=self.timeout_time_sec,
- verbose=self.verbose)
+ self.depth_sub = self.create_subscription(msg_type=Float32,
+ topic=self.depth_topic,
+ callback=self.depth_callback,
+ qos_profile=10)
- self.report_pub = self.create_publisher(msg_type=Int8, topic=self.report_topic, qos_profile=10)
+ self.altitude_sub = self.create_subscription(msg_type=Float32,
+ topic=self.altitude_topic,
+ callback=self.altitude_callback,
+ qos_profile=10)
+
+ self.get_logger().info(f"topic rate monitor(s) instantiated")
+ self.essential_monitor = TopicRateMonitor(self, self.essential_topics, timeout_time_sec=self.timeout_time_sec,
+ verbose=self.verbose)
+
+ self.optional_monitor = TopicRateMonitor(self, self.optional_topics, timeout_time_sec=self.timeout_time_sec,
+ verbose=self.verbose)
+
+ if self.testing:
+ self.report_pub = self.create_publisher(msg_type=Int8, topic='health_testing', qos_profile=10)
+ else:
+ self.report_pub = self.create_publisher(msg_type=Int8, topic=self.report_topic, qos_profile=10)
+
+ self.report_string_pub = self.create_publisher(msg_type=String, topic=f"{self.report_topic}/report", qos_profile=10)
+
self.report_timer = self.create_timer(timer_period_sec=float(1.0 / self.output_rate),
callback=self.output_callback)
+
def leak_callback(self, msg):
self.current_leak = msg
@@ -114,64 +168,145 @@ def battery_callback(self, msg):
self.current_battery = msg
self.current_battery_time = self.get_clock().now().nanoseconds/1e9
- def leak_check(self):
+ def depth_callback(self, msg):
+ self.current_depth = msg
+ self.current_depth_time = self.get_clock().now().nanoseconds / 1e9
+
+ def altitude_callback(self, msg):
+ self.current_altitude = msg
+ self.current_altitude_time = self.get_clock().now().nanoseconds / 1e9
+ def leak_check(self):
if self.current_leak_status.fault:
return self.current_leak_status
-
- check_time = self.get_clock().now().nanoseconds/1e9
-
+
if self.current_leak is None:
- if check_time - self.start_time > self.initial_timeout_time_sec:
- self.get_logger().warn(f"Fault detected: Leak initial time out!")
- self.current_leak_status.fault = True
- else:
- self.current_leak_status.ready = True
-
- if self.current_leak.value:
- self.get_logger().warn(f"Fault detected: Leak!")
- self.current_leak_status.fault = True
-
- if (check_time - self.current_leak_time) > self.timeout_time_sec:
- self.get_logger().warn(f"Fault detected: Leak time out!")
- self.current_leak_status.fault = True
+ return
+
+ self.current_leak_status.ready = True
- if self.verbose:
- self.get_logger().info(f"Leak: {self.current_leak_status}")
+ if self.current_leak.value:
+ current_fault_report = f"Fault detected: Leak!"
+ if self.fault_report is None:
+ self.fault_report = current_fault_report
+ self.get_logger().warn(f"Fault detected: Leak!")
+ self.current_leak_status.fault = True
return self.current_leak_status
def battery_check(self):
-
+
+ # TODO the batter is not required to move into ready
if self.current_battery_status.fault:
return self.current_battery_status
- check_time = self.get_clock().now().nanoseconds/1e9
+ # check_time = self.get_clock().now().nanoseconds/1e9
if self.current_battery is None:
- if check_time - self.start_time > self.initial_timeout_time_sec:
- self.get_logger().warn(f"Fault detected: battery initial time out!")
- self.current_battery_status.fault = True
- else:
self.current_battery_status.ready = True
+ return self.current_battery_status
- if self.current_battery.voltage < self.battery_min_voltage:
- self.get_logger().warn(f"Fault detected: Battery low voltage!")
- self.current_battery_status.fault = True
+ # Battery status received
+ self.current_battery_status.ready = True
- if self.current_battery.percentage < self.battery_min_capacity:
- self.get_logger().warn(f"Fault detected: Battery low capacity!")
- self.current_battery_status.fault = True
+ if self.current_battery.voltage < self.battery_min_voltage:
+ current_fault_report = f"Fault detected: Battery low voltage! Current voltage: {self.current_battery.voltage}, Min voltage: {self.battery_min_voltage}"
+ if self.fault_report is None:
+ self.fault_report = current_fault_report
+
+ self.get_logger().warn(current_fault_report)
+ self.current_battery_status.fault = True
- if (check_time - self.current_battery_time) > self.timeout_time_sec:
- self.get_logger().info(f"Fault detected: Leak time out!")
- self.current_battery_status.fault = True
+ if self.current_battery.percentage < self.battery_min_capacity:
+ current_fault_report = f"Fault detected: Battery low capacity! Current capacity: {self.current_battery.percentage}, Min capacity: {self.battery_min_capacity}"
+ if self.fault_report is None:
+ self.fault_report = current_fault_report
+ self.get_logger().warn(self.fault_report)
+ self.current_battery_status.fault = True
- if self.verbose:
- self.get_logger().info(f"Battery: {self.current_battery_status}")
+ # if (check_time - self.current_battery_time) > self.timeout_time_sec:
+ # self.get_logger().info(f"Fault detected: Leak time out!")
+ # self.current_battery_status.fault = True
return self.current_battery_status
+ def depth_check(self):
+
+ if self.current_depth_status.fault:
+ return self.current_depth_status
+
+ check_time = self.get_clock().now().nanoseconds/1e9
+
+ if self.current_depth is None:
+ if self.check_initial_timeout(check_time):
+ current_fault_report = f"Fault detected: depth initial time out!"
+ if self.fault_report is None:
+ self.fault_report = current_fault_report
+ self.get_logger().warn(current_fault_report)
+ self.current_depth_status.fault = True
+ else:
+ self.current_depth_status.ready = True
+
+ # Check that 'max_depth' is defined in limits
+ if 'max_depth' not in self.limits:
+ return self.current_depth_status
+
+ if self.current_depth.data > self.limits['max_depth']:
+ current_fault_report = f"Fault detected: Max depth exceeded! Current depth: {self.current_depth.data:.2f}, Max depth: {self.limits['max_depth']}"
+ if self.fault_report is None:
+ self.fault_report = current_fault_report
+ self.get_logger().warn(current_fault_report)
+ self.current_depth_status.fault = True
+
+ time_diff = check_time - self.current_depth_time
+ if time_diff > self.timeout_time_sec:
+ current_fault_report = f"Fault detected: depth time out! Time diff = {time_diff:.2f} s > {self.timeout_time_sec:.2f} s"
+ if self.fault_report is None:
+ self.fault_report = current_fault_report
+ self.get_logger().info(current_fault_report)
+ self.current_depth_status.fault = True
+
+ return self.current_depth_status
+
+ def altitude_check(self):
+ """
+ REMOVED INITIAL
+ """
+
+ if self.current_altitude_status.fault:
+ return self.current_altitude_status
+
+ check_time = self.get_clock().now().nanoseconds/1e9
+
+ if self.current_altitude is None:
+ return self.current_altitude_status
+
+ self.current_altitude_status.ready = True
+
+ # Check that 'max_depth' is defined in limits
+ if 'min_altitude' not in self.limits:
+ return self.current_altitude_status
+
+ if self.current_altitude.data == -1:
+ return self.current_altitude_status
+
+ if self.current_altitude.data < self.limits['min_altitude']:
+ current_fault_report = f"Fault detected: low altitude! Current altitude: {self.current_altitude.data:.2f}, Min altitude: {self.limits['min_altitude']}"
+ if self.fault_report is None:
+ self.fault_report = current_fault_report
+ self.get_logger().warn(current_fault_report)
+ self.current_altitude_status.fault = True
+
+ time_diff = check_time - self.current_altitude_time
+ if time_diff > self.timeout_time_sec:
+ current_fault_report = f"Fault detected: altitude time out! Time diff = {time_diff:.2f} s > {self.timeout_time_sec:.2f} s"
+ if self.fault_report is None:
+ self.fault_report = current_fault_report
+ self.get_logger().info(current_fault_report)
+ self.current_altitude_status.fault = True
+
+ return self.current_altitude_status
+
def output_callback(self):
"""
Perform
@@ -179,17 +314,45 @@ def output_callback(self):
self.leak_check()
self.battery_check()
+ self.altitude_check()
+ self.depth_check()
+
+ if self.verbose and not self.fault_logged:
+ self.get_logger().info(f"Leak: {self.current_leak_status}")
+ self.get_logger().info(f"Battery: {self.current_battery_status}")
+ self.get_logger().info(f"Altitude: {self.current_altitude_status}")
+ self.get_logger().info(f"Depth: {self.current_depth_status}")
+
+
# Self.monitor updates on it's own
- faults = [self.monitor.fault, self.current_leak_status.fault, self.current_battery_status.fault]
- readys = [self.monitor.ready, self.current_leak_status.ready, self.current_battery_status.ready]
+ faults = [
+ self.essential_monitor.fault, self.optional_monitor.fault,
+ self.current_leak_status.fault, self.current_battery_status.fault,
+ self.current_altitude_status.fault, self.current_depth_status.fault
+ ]
+
+ # These are subject to the initial timeout
+ readys = [
+ self.essential_monitor.ready,
+ self.current_leak_status.ready, self.current_battery_status.ready,
+ # self.current_altitude_status.ready, self.current_depth_status.ready
+ ]
+
+ # These are not subject to the initial timeout
+ optional_readys = [
+ self.optional_monitor.ready,
+ self.current_altitude_status.ready, self.current_depth_status.ready
+ ]
if True in faults:
- self.get_logger().warn(f"Fault detected [rate, leak, battery]: {faults}")
- fault_msg = Int8()
- fault_msg.data = SmarcTopics.VEHICLE_HEALTH_ERROR
- self.report_pub.publish(fault_msg)
- elif all(readys):
+ if not self.fault_logged:
+ self.get_logger().warn(f"Fault detected [essential, optional, leak, battery, altitude, depth]: {faults}")
+ self.fault_logged = True
+ self.publish_fault()
+ return
+
+ elif all(readys) and all(optional_readys):
ready_msg = Int8()
ready_msg.data = SmarcTopics.VEHICLE_HEALTH_READY
self.report_pub.publish(ready_msg)
@@ -201,15 +364,54 @@ def output_callback(self):
if self.verbose:
self.get_logger().info(f"Waiting, Elapsed time: {elapsed_time}")
- if elapsed_time > self.initial_timeout_time_sec:
- fault_msg = Int8()
- fault_msg.data = SmarcTopics.VEHICLE_HEALTH_ERROR
- self.report_pub.publish(fault_msg)
+ if self.check_initial_timeout(current_time) and False in readys:
+ self.publish_fault()
+ return
+
else:
waiting_msg = Int8()
waiting_msg.data = SmarcTopics.VEHICLE_HEALTH_WAITING
self.report_pub.publish(waiting_msg)
+ def publish_fault(self):
+
+ fault_msg = Int8()
+ fault_msg.data = SmarcTopics.VEHICLE_HEALTH_ERROR
+ self.report_pub.publish(fault_msg)
+
+ if self.fault_report is not None:
+ fault_report_msg = String()
+ fault_report_msg.data = str(self.fault_report)
+ self.report_string_pub.publish(fault_report_msg)
+
+ def read_limits(self):
+ """
+ Read YAML file with Lolo's limits.
+ Returns a dictionary with the values.
+ """
+ if not self.limits_filename:
+ self.limits_filename = "sam_health_limits.yaml"
+
+ path_to_pkg = get_package_share_directory('sam_health_checker')
+ yaml_path = os.path.join(path_to_pkg, "config", self.limits_filename)
+
+ with open(yaml_path, 'r') as file:
+ limits = yaml.safe_load(file)
+ self.get_logger().info(f"SAM limits have been configured with filename {self.limits_filename}")
+ [self.get_logger().info(f"{key}:{value}") for key, value in limits.items()]
+
+ return limits
+
+ def check_initial_timeout(self, time):
+ if self.initial_timeout_time_sec < 0:
+ return False
+
+ if (time - self.start_time) > self.initial_timeout_time_sec:
+ return True
+ else:
+ return False
+
+
def log_all_parameters(self):
param_names = self._parameters.keys()
self.get_logger().info("Declared Parameters and their values:")
diff --git a/vehicles/hardware/sam/sam_smarc_publisher/sam_smarc_publisher/sam_smarc_publisher.py b/vehicles/hardware/sam/sam_smarc_publisher/sam_smarc_publisher/sam_smarc_publisher.py
index 288446cc..53a98101 100644
--- a/vehicles/hardware/sam/sam_smarc_publisher/sam_smarc_publisher/sam_smarc_publisher.py
+++ b/vehicles/hardware/sam/sam_smarc_publisher/sam_smarc_publisher/sam_smarc_publisher.py
@@ -5,7 +5,7 @@
from geometry_msgs.msg import PoseStamped
from geographic_msgs.msg import GeoPoint
from nav_msgs.msg import Odometry
-from std_msgs.msg import Empty, Int8, Float32, Float64
+from std_msgs.msg import Empty, Int8, Float32, Float64, String
from smarc_msgs.msg import Topics as SmarcTopics
from smarc_msgs.msg import DVL
@@ -27,23 +27,24 @@ def __init__(self):
self.get_logger().info('SAM SMaRC Publisher Node has been started.')
self.declare_parameter('robot_name', 'sam')
self.robot_name = self.get_parameter('robot_name').get_parameter_value().string_value
- self._create_tf_listener()
+ self.utm_frame = None
+ self.create_subscription(String, SamTopics.UTM_ZONE_BAND, self._utm_callback, 10)
self._create_abort_pubsub()
self._create_bt_heartbeat_pubsub()
self._create_altitude_pubsub()
self._create_battery_status_pubsub()
+ self._create_tf_listener()
self._create_odom_pubsub()
- def _create_tf_listener(self):
- self.declare_parameter('utm_zone', '34')
- self.utm_zone = self.get_parameter('utm_zone').get_parameter_value().string_value
- self.declare_parameter('utm_band', 'V')
- self.utm_band = self.get_parameter('utm_band').get_parameter_value().string_value
- self.utm_frame = f'utm_{self.utm_zone}_{self.utm_band}'
+ def _utm_callback(self, msg):
+ if self.utm_frame is not None:
+ return
+ self.utm_frame = msg.data
self.get_logger().info(f'Using UTM frame: {self.utm_frame}')
+ def _create_tf_listener(self):
self.odom_frame = f'{self.robot_name}/odom'
self.get_logger().info(f'Using odom frame: {self.odom_frame}')
self.tf_buffer = Buffer()
@@ -97,6 +98,7 @@ def dvl_callback(self, msg):
"""
Callback for the DVL topic. It publishes the altitude to the SMaRC topic.
"""
+
self.altitude_pub.publish(Float32(data=msg.altitude))
def _create_odom_pubsub(self):
@@ -125,6 +127,10 @@ def odom_callback(self, msg):
self.odom_pub.publish(msg)
self.depth_pub.publish(Float32(data=msg.pose.pose.position.z))
+ if not self.utm_frame:
+ self.get_logger().warn('UTM frame not set, will not attempt to transform.')
+ return
+
try:
timestamp = msg.header.stamp
utm_transform = self.tf_buffer.lookup_transform(self.utm_frame, self.odom_frame, timestamp)
@@ -168,4 +174,4 @@ def main(args=None):
finally:
node.get_logger().info('Shutting down SAM SMaRC Publisher Node.')
node.destroy_node()
- rclpy.shutdown()
\ No newline at end of file
+ rclpy.shutdown()