Changes in Sam from Asko

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'

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

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 ------------

vehicles/hardware/sam/sam_health_checker/launch/sam_rate_health_checker.launch

@@ -8,11 +8,11 @@
     <param name="battery_min_voltage" value="20.0" />
     <param name="battery_min_capacity" value="0.25" />
 
-    <param name="initial_timeout_time_sec" value="30.0" />
-    <param name="timeout_time_sec" value="15.0" />
+    <param name="initial_timeout_time_sec" value="60.0" />
+    <param name="timeout_time_sec" value="7.5" />
 
     <param name="verbose" value="True" />
 
   </node>
 
-</launch>
+</launch>

vehicles/hardware/sam/sam_health_checker/launch/testing_sam_health.launch

@@ -0,0 +1,20 @@
+<launch>
+  <!--Launch file for testing the INS -> ODOM, map_odom_initializer_node is used to define odom's fixed transform -->
+  <arg name="robot_name" default="sam" />
+
+  <node pkg="sam_health_checker" exec="sam_rate_health_node" name="sam_rate_health_node" output="screen" namespace="$(var robot_name)">
+<!--     <param name="leak_topic" value="..." /> -->
+<!--     <param name="battery_topic" value="..." /> -->
+    <param name="battery_min_voltage" value="20.0" />
+    <param name="battery_min_capacity" value="0.25" />
+
+    <param name="initial_timeout_time_sec" value="30.0" />
+    <param name="timeout_time_sec" value="2.5" />
+
+    <param name="verbose" value="True" />
+    <param name="testing" value="True" />
+
+
+  </node>
+
+</launch>

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