Skip to content

Commit 191bd44

Browse files
ignaciotbignaciotb
authored
Merge pull request #187 from ignaciotb/humble
Changes in Sam from Asko
2 parents 1475fca + 1c1b1ce commit 191bd44

File tree

8 files changed

+405
-110
lines changed

8 files changed

+405
-110
lines changed

‎messages/sam_msgs/msg/Topics.msg

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -87,6 +87,7 @@ string LED_CMD_TOPIC = 'core/led_cmd'
8787
string DVL_CMD_TOPIC = 'core/dvl_relay'
8888
string SSS_CMD_TOPIC = 'core/sss_relay'
8989
string TOGGLE_7V_CMD_TOPIC = 'core/enable_7v'
90+
string UTM_ZONE_BAND = 'core/utm_zone_band'
9091

9192
string GET_NODE_INFO_SRV = 'core/uavcan_get_node_info'
9293
string GET_TRANSPORT_STATS_SRV = 'core/uavcan_get_transport_stats'

‎scripts/smarc_bringups/scripts/sam_bringup.sh

Lines changed: 48 additions & 36 deletions
Original file line numberDiff line numberDiff line change
@@ -1,11 +1,15 @@
11
#! /bin/bash
22
# Set LOCAL_ROBOT_NAME, LOCAL_MQTT_BROKER_IP, and LOCAL_MQTT_BROKER_PORT in your bashrc
33
ROBOT_NAME=$LOCAL_ROBOT_NAME
4-
MQTT_BROKER_IP=$LOCAL_MQTT_BROKER_IP
5-
MQTT_BROKER_PORT=$LOCAL_MQTT_BROKER_PORT
4+
# ROBOT_NAME=sam
5+
# MQTT_BROKER_IP=$LOCAL_MQTT_BROKER_IP
6+
MQTT_BROKER_IP=20.240.40.232
7+
# MQTT_BROKER_PORT=$LOCAL_MQTT_BROKER_PORT
8+
MQTT_BROKER_PORT=1884
9+
SSS_SAVE_PATH=/home/orin/sss_auto_save
610

711
SESSION=${ROBOT_NAME}_bringup
8-
USE_SIM_TIME=True
12+
USE_SIM_TIME=False
913

1014
# New variables for wasp_bt.launch and wasp_mqtt_agent.launch
1115
AGENT_TYPE=subsurface
@@ -22,6 +26,8 @@ fi
2226
tmux -2 new-session -d -s $SESSION
2327
if [ $REALSIM = "simulation" ]
2428
then
29+
30+
# TODO: this can be used in both, remove from here
2531
# Mostly static stuff that wont be giving much feedback
2632
# for robot description launch. so we get base_link -> everything else
2733
tmux new-window -t $SESSION:6 -n 'description'
@@ -36,23 +42,29 @@ else
3642
tmux new-window -t $SESSION:0 -n 'core'
3743
tmux select-window -t $SESSION:0
3844
tmux send-keys "ros2 launch sam_drivers sam_core.launch robot_name:=$ROBOT_NAME" C-m
39-
40-
tmux new-window -t $SESSION:7 -n payloads
41-
tmux select-window -t $SESSION:7
42-
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
43-
44-
tmux new-window -t $SESSION:8 -n uwcomms
45+
46+
tmux new-window -t $SESSION:8 -n payloads
4547
tmux select-window -t $SESSION:8
48+
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
49+
50+
tmux new-window -t $SESSION:9 -n uwcomms
51+
tmux select-window -t $SESSION:9
4652
tmux send-keys "ros2 launch sam_drivers sam_uwcomms.launch robot_name:=$ROBOT_NAME use_sim_time:=$USE_SIM_TIME" C-m
4753

54+
# Mostly static stuff that wont be giving much feedback
55+
# for robot description launch. so we get base_link -> everything else
56+
tmux new-window -t $SESSION:10 -n 'description'
57+
tmux select-window -t $SESSION:10
58+
tmux send-keys "ros2 launch sam_description sam_description.launch robot_name:=$ROBOT_NAME" C-m
59+
4860
fi
4961

5062
# state estimation stuff like pressure->depth, imu->tf etc
5163
tmux new-window -t $SESSION:1 -n 'dr'
5264
tmux rename-window "dr"
5365
tmux select-window -t $SESSION:1
54-
#tmux send-keys "ros2 launch sam_dead_reckoning sam_dr_launch.launch robot_name:=$ROBOT_NAME" C-m
55-
tmux send-keys "echo 'Not launching sam_dead_reckoning sam_dr_launch.launch until someone fixes it!'" C-m
66+
tmux send-keys "ros2 launch hydrobatic_localization state_estimator.launch robot_name:=$ROBOT_NAME" C-m
67+
# tmux send-keys "echo 'Not launching sam_dead_reckoning sam_dr_launch.launch until someone fixes it!'" C-m
5668

5769
# BT, action servers etc.
5870
tmux new-window -t $SESSION:2 -n 'bt'
@@ -71,41 +83,41 @@ tmux new-window -t $SESSION:4 -n 'smcp'
7183
tmux select-window -t $SESSION:4
7284
tmux send-keys "ros2 launch sam_smarc_publisher default.launch robot_name:=$ROBOT_NAME" C-m
7385

74-
# utility stuff like dubins planning and lat/lon conversions that other stuff rely on
75-
tmux new-window -t $SESSION:5 -n 'utils'
86+
# for the mqtt bridge.
87+
tmux new-window -t $SESSION:5 -n 'mqtt'
88+
# Set your MQTT Broker IP and Port in your bashrc
7689
tmux select-window -t $SESSION:5
77-
tmux send-keys "ros2 launch smarc_bringups utilities.launch robot_name:=$ROBOT_NAME" C-m
90+
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
7891

79-
# for robot description launch. so we get base_link -> everything else
80-
tmux new-window -t $SESSION:8 -n 'description'
81-
tmux select-window -t $SESSION:8
82-
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
83-
84-
# dummy stuff to temporarily let other stuff work
85-
tmux new-window -t $SESSION:9 -n 'dummies'
86-
tmux new-window -t $SESSION:9 -n 'emergency_action'
87-
tmux select-window -t $SESSION:9
92+
tmux new-window -t $SESSION:6 -n 'emergency'
93+
tmux select-window -t $SESSION:6
8894
# tmux send-keys "ros2 launch smarc_bringups dummies.launch robot_name:=$ROBOT_NAME" C-m
8995
tmux send-keys "ros2 launch sam_emergency_action sam_emergency_action.launch robot_name:=$ROBOT_NAME" C-m
9096

91-
# for the mqtt bridge.
92-
tmux new-window -t $SESSION:10 -n 'mqtt'
93-
# Set your MQTT Broker IP and Port in your bashrc
94-
tmux select-window -t $SESSION:10
95-
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
97+
# utility stuff like dubins planning and lat/lon conversions that other stuff rely on
98+
# tmux new-window -t $SESSION:5 -n 'utils'
99+
# tmux select-window -t $SESSION:5
100+
# tmux send-keys "ros2 launch smarc_bringups utilities.launch robot_name:=$ROBOT_NAME" C-m
101+
102+
# for robot description launch. so we get base_link -> everything else
103+
tmux new-window -t $SESSION:7 -n 'health'
104+
tmux select-window -t $SESSION:7
105+
# 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
106+
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
96107

97108
# Launch the wasp_bt LAST, to give action servers time to start publishing heartbeats
98109
tmux select-window -t $SESSION:2
99110
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
100111

101-
USERNAME=$(whoami)
102-
if [ $USERNAME != "sam" ]
103-
then
104-
echo "You are not the real sam!"
105-
ROS_IP=127.0.0.1
106-
# Maybe launch ros-tcp-bridge here?
107-
fi
112+
#USERNAME=$(whoami)
113+
#if [ $USERNAME != "sam" ]
114+
#then
115+
# echo "You are not the real sam!"
116+
# ROS_IP=127.0.0.1
117+
# # Maybe launch ros-tcp-bridge here?
118+
#fi
119+
108120
# Set default window
109-
tmux select-window -t $SESSION:2
121+
tmux select-window -t $SESSION:0
110122
# attach to the new session
111123
tmux -2 attach-session -t $SESSION

‎vehicles/hardware/sam/sam_health_checker/config/sam_health_limits.yaml

Lines changed: 53 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,53 @@
1+
# Default limits for SAM.
2+
3+
# Copied from LoLo limits, most values are not being used
4+
# PLEASE CHECK VALUES BEFORE USING
5+
6+
# --------- DO NOT MODIFY ------------
7+
# ----- ACTION SERVER LIMITS -------
8+
# Maximum allowed time for LoLo to complete
9+
# an action (e.g. MoveTo, DepthMoveTo, Loiter).
10+
max_timeout_secs: 1800 # [s]
11+
12+
# Maximum allowed distance (in the XY plane)
13+
# between Lolo and the next waypoint (or between
14+
# consecutive waypoints).
15+
max_waypoint_dist: 1000 # [m]
16+
17+
# Goal tolerance radius (on the XY plane).
18+
goal_tolerance_plane: 5 # [m]
19+
20+
# Goal tolerance for depth (Z).
21+
goal_tolerance_depth: 5 # [m]
22+
# ------------------------------------
23+
24+
# ----- GENERAL VEHICLE LIMITS -------
25+
# Absolute maximum depth allowed.
26+
max_depth: 100 # [m]
27+
28+
# Depth threshold for diving
29+
diving_threshold_depth: 0.5 # [m]
30+
31+
# Absolute minimum altitude from the
32+
# seafloor allowed.
33+
min_altitude: 0.5 # [m]
34+
35+
# Absolute maximum RPMs for horizontal
36+
# thrusters allowed.
37+
max_thruster_rpm: 1000 # [rpm]
38+
39+
# Absolute maximum RPMs for vertical
40+
# thrusters allowed.
41+
max_vert_thruster_rpm: 2500 # [rpm]
42+
43+
# Maximum uninterrupted diving time.
44+
max_dive_time: 1800
45+
46+
# Pressure tolerance +/- in [mbar] for the
47+
# pressure vessels' pressure readings.
48+
pressure_tolerance: 100
49+
50+
# Battery voltage range limits in [v].
51+
voltage: [22, 40]
52+
# ----------------------------------------
53+
# --------- DO NOT MODIFY END ------------

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

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -8,11 +8,11 @@
88
<param name="battery_min_voltage" value="20.0" />
99
<param name="battery_min_capacity" value="0.25" />
1010

11-
<param name="initial_timeout_time_sec" value="30.0" />
12-
<param name="timeout_time_sec" value="15.0" />
11+
<param name="initial_timeout_time_sec" value="60.0" />
12+
<param name="timeout_time_sec" value="7.5" />
1313

1414
<param name="verbose" value="True" />
1515

1616
</node>
1717

18-
</launch>
18+
</launch>

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

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,20 @@
1+
<launch>
2+
<!--Launch file for testing the INS -> ODOM, map_odom_initializer_node is used to define odom's fixed transform -->
3+
<arg name="robot_name" default="sam" />
4+
5+
<node pkg="sam_health_checker" exec="sam_rate_health_node" name="sam_rate_health_node" output="screen" namespace="$(var robot_name)">
6+
<!-- <param name="leak_topic" value="..." /> -->
7+
<!-- <param name="battery_topic" value="..." /> -->
8+
<param name="battery_min_voltage" value="20.0" />
9+
<param name="battery_min_capacity" value="0.25" />
10+
11+
<param name="initial_timeout_time_sec" value="30.0" />
12+
<param name="timeout_time_sec" value="2.5" />
13+
14+
<param name="verbose" value="True" />
15+
<param name="testing" value="True" />
16+
17+
18+
</node>
19+
20+
</launch>

‎vehicles/hardware/sam/sam_health_checker/sam_health_checker/helpers/health_helpers.py

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -121,9 +121,10 @@ def determine_fault(self):
121121

122122
# Check for timeout
123123
now = self.node.get_clock().now().nanoseconds/1e9
124-
if now - self.timestamps[topic_name][0] > self.timeout_time_sec:
124+
time_diff = now - self.timestamps[topic_name][0]
125+
if time_diff > self.timeout_time_sec:
125126
self.fault = True
126-
self.node.get_logger().warn(f"Fault: timeout on {topic_name}")
127+
self.node.get_logger().warn(f"Fault: timeout on {topic_name}, time difference: {time_diff:.2f} s > {self.timeout_time_sec:.2f} s")
127128
return True
128129

129130
# Check frequency

0 commit comments

Comments
 (0)