From 8b86dd22e979784851a5dabdca258ddf1f01e0d2 Mon Sep 17 00:00:00 2001 From: Ryan Govostes Date: Thu, 5 Jun 2025 01:25:01 -0700 Subject: [PATCH 1/7] Add SeaTrac node and network comms bridge --- configs/seatrac.yaml | 11 +++++++++++ deps/deps.rosinstall | 4 ++++ src/phyto_arm/launch/seatrac.launch | 12 ++++++++++++ src/phyto_arm/package.xml | 1 + 4 files changed, 28 insertions(+) create mode 100644 configs/seatrac.yaml create mode 100644 src/phyto_arm/launch/seatrac.launch diff --git a/configs/seatrac.yaml b/configs/seatrac.yaml new file mode 100644 index 0000000..f42953a --- /dev/null +++ b/configs/seatrac.yaml @@ -0,0 +1,11 @@ +seatrac: + comms: + connection: + type: "UDP" + udp_rx: 62001 + udp_tx: 62001 + udp_address: "127.0.0.1" + + outlets: + - name: "starlink" + outlet: 15 diff --git a/deps/deps.rosinstall b/deps/deps.rosinstall index fc8aca3..271c8b9 100644 --- a/deps/deps.rosinstall +++ b/deps/deps.rosinstall @@ -15,3 +15,7 @@ - git: local-name: ros-triton-classifier uri: https://github.com/WHOIGit/ros-triton-classifier.git + +- git: + local-name: seatrac-api + uri: https://github.com/WHOIGit/seatrac-api.git diff --git a/src/phyto_arm/launch/seatrac.launch b/src/phyto_arm/launch/seatrac.launch new file mode 100644 index 0000000..7f81eb5 --- /dev/null +++ b/src/phyto_arm/launch/seatrac.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + diff --git a/src/phyto_arm/package.xml b/src/phyto_arm/package.xml index da981d9..174ace9 100644 --- a/src/phyto_arm/package.xml +++ b/src/phyto_arm/package.xml @@ -34,6 +34,7 @@ ifcb jvl_motor rbr_maestro3_ctd + seatrac triton_classifier From 92b70d88a6ca2013f8b776fd2ea364efa21d95ac Mon Sep 17 00:00:00 2001 From: Ryan Govostes Date: Tue, 10 Jun 2025 19:30:31 -0700 Subject: [PATCH 2/7] Use frontseat IP address for outbound packets --- configs/seatrac.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/configs/seatrac.yaml b/configs/seatrac.yaml index f42953a..fd8f40c 100644 --- a/configs/seatrac.yaml +++ b/configs/seatrac.yaml @@ -4,7 +4,7 @@ seatrac: type: "UDP" udp_rx: 62001 udp_tx: 62001 - udp_address: "127.0.0.1" + udp_address: "10.1.20.88" outlets: - name: "starlink" From 56c4f1dec59c5be44ac9cae664f977b60889c050 Mon Sep 17 00:00:00 2001 From: Ryan Govostes Date: Tue, 10 Jun 2025 22:34:46 -0700 Subject: [PATCH 3/7] Initial progress towards Starlink manager --- configs/seatrac.yaml | 6 + src/phyto_arm/launch/seatrac.launch | 3 + src/phyto_arm/src/seatrac_starlink_manager.py | 116 ++++++++++++++++++ 3 files changed, 125 insertions(+) create mode 100755 src/phyto_arm/src/seatrac_starlink_manager.py diff --git a/configs/seatrac.yaml b/configs/seatrac.yaml index fd8f40c..24d4d5a 100644 --- a/configs/seatrac.yaml +++ b/configs/seatrac.yaml @@ -9,3 +9,9 @@ seatrac: outlets: - name: "starlink" outlet: 15 + +seatrac_starlink_manager: + shutdown_reminder: 900 # seconds + soc_force_threshold: 95 # percent (auto-enable until manual off) + soc_threshold: 80 # percent (auto-enable if last off > off_delay) + off_delay: 86400 # seconds (24h since manual off) diff --git a/src/phyto_arm/launch/seatrac.launch b/src/phyto_arm/launch/seatrac.launch index 7f81eb5..ed110cb 100644 --- a/src/phyto_arm/launch/seatrac.launch +++ b/src/phyto_arm/launch/seatrac.launch @@ -9,4 +9,7 @@ + + diff --git a/src/phyto_arm/src/seatrac_starlink_manager.py b/src/phyto_arm/src/seatrac_starlink_manager.py new file mode 100755 index 0000000..cffc225 --- /dev/null +++ b/src/phyto_arm/src/seatrac_starlink_manager.py @@ -0,0 +1,116 @@ +#!/usr/bin/env python3 +import json +import urllib.request + +import rospy + +from seatrac.msg import PowerLevel +from std_msgs.msg import Bool as StdBool + +from phyto_arm.msg import OutletStatus + + +def send_alerts(alert_config, message): + for alert in alert_config: + assert alert['type'] == 'slack' and alert['url'] + urllib.request.urlopen( + alert['url'], + json.dumps({'text': message}).encode() + ) + + +def main(): + rospy.init_node('seatrac_starlink_manager') + shutdown_reminder = rospy.get_param('~shutdown_reminder', None) + force_on_threshold = rospy.get_param('~soc_force_threshold', 95) + on_threshold = rospy.get_param('~soc_threshold', 80) + off_delay = rospy.get_param('~off_delay', 86400) + alerts = rospy.get_param('/alerts', []) + + outlet_pub = rospy.Publisher('/seatrac/outlet/starlink/control', StdBool, + queue_size=1) + + manual_shutdown = False + last_off = None + nag_timer = None + + enable_sent = False + last_state = None + soc = 0.0 + + def nag_operator() -> None: + nonlocal alerts, nag_timer + nag_timer = None + send_alerts(alerts, 'Starlink has been active for a while') + + def outlet_status_cb(msg: OutletStatus) -> None: + nonlocal manual_shutdown, last_off, nag_timer, last_state + now = rospy.Time.now().to_sec() + if last_state is None: + if not msg.is_active: + manual_shutdown = True + last_off = now + elif last_state and not msg.is_active: + manual_shutdown = True + last_off = now + elif manual_shutdown and msg.is_active: + manual_shutdown = False + last_state = msg.is_active + + if shutdown_reminder is not None and last_state and nag_timer is None: + nag_timer = rospy.Timer( + rospy.Duration(shutdown_reminder), + nag_operator, + oneshot=True + ) + elif not msg.is_active and nag_timer: + nag_timer.shutdown() + nag_timer = None + + def power_level_cb(msg: PowerLevel) -> None: + nonlocal enable_sent, last_state, soc, manual_shutdown, last_off + if last_state is None: + return + + soc = msg.soc_percentage + is_charging = msg.pack_current < 0 + now = rospy.Time.now().to_sec() + + if ( + soc >= force_on_threshold + and is_charging + and not enable_sent + and not manual_shutdown + ): + rospy.loginfo( + f'Enabling Starlink: SOC {soc}% > {force_on_threshold} and charging' + ) + outlet_pub.publish(StdBool(True)) + enable_sent = True + elif ( + soc >= on_threshold + and not enable_sent + and not manual_shutdown + and (last_off is None or now - last_off > off_delay) + ): + rospy.loginfo( + f'Enabling Starlink: SOC {soc}% > {on_threshold}' + f' and last_off {int(now-last_off)}s > {off_delay}' + ) + outlet_pub.publish(StdBool(True)) + enable_sent = True + elif soc < on_threshold: + enable_sent = False + + rospy.Subscriber('/seatrac/outlet/starlink/status', OutletStatus, + outlet_status_cb) + rospy.Subscriber('/seatrac/power', PowerLevel, power_level_cb) + + rospy.spin() + + +if __name__ == '__main__': + try: + main() + except rospy.ROSInterruptException: + pass From 917e38fcd433d2228c2362d8c6d2b379dfaa95d5 Mon Sep 17 00:00:00 2001 From: Ryan Govostes Date: Sun, 15 Jun 2025 11:56:34 -0700 Subject: [PATCH 4/7] Update logic of Starlink manager --- configs/seatrac.yaml | 9 +- src/phyto_arm/src/seatrac_starlink_manager.py | 156 +++++++++++------- 2 files changed, 104 insertions(+), 61 deletions(-) diff --git a/configs/seatrac.yaml b/configs/seatrac.yaml index 24d4d5a..b9eabc2 100644 --- a/configs/seatrac.yaml +++ b/configs/seatrac.yaml @@ -11,7 +11,8 @@ seatrac: outlet: 15 seatrac_starlink_manager: - shutdown_reminder: 900 # seconds - soc_force_threshold: 95 # percent (auto-enable until manual off) - soc_threshold: 80 # percent (auto-enable if last off > off_delay) - off_delay: 86400 # seconds (24h since manual off) + scheduled_on: "12:30" # HH:MM, UTC timezone + shutdown_reminder: 1200 # seconds + soc_threshold: + high: 95 # percent (minimum threshold for automatic activation) + medium: 80 # percent (minimum threshold for daily activation) diff --git a/src/phyto_arm/src/seatrac_starlink_manager.py b/src/phyto_arm/src/seatrac_starlink_manager.py index cffc225..2aee095 100755 --- a/src/phyto_arm/src/seatrac_starlink_manager.py +++ b/src/phyto_arm/src/seatrac_starlink_manager.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +import datetime import json import urllib.request @@ -21,45 +22,79 @@ def send_alerts(alert_config, message): def main(): rospy.init_node('seatrac_starlink_manager') - shutdown_reminder = rospy.get_param('~shutdown_reminder', None) - force_on_threshold = rospy.get_param('~soc_force_threshold', 95) - on_threshold = rospy.get_param('~soc_threshold', 80) - off_delay = rospy.get_param('~off_delay', 86400) + + # Read parameters alerts = rospy.get_param('/alerts', []) + high_threshold = rospy.get_param('~soc_threshold', {}).get('high', 95) + medium_threshold = rospy.get_param('~soc_threshold', {}).get('medium', 80) + + shutdown_reminder = rospy.get_param('~shutdown_reminder', None) + if shutdown_reminder is not None: + shutdown_reminder = rospy.Duration(shutdown_reminder) + + scheduled_on = rospy.get_param('~scheduled_on', None) + if scheduled_on is not None: + scheduled_on = datetime.datetime.strptime(scheduled_on, '%H:%M').time() + + # Create a publisher for controlling the Starlink outlet outlet_pub = rospy.Publisher('/seatrac/outlet/starlink/control', StdBool, queue_size=1) - manual_shutdown = False - last_off = None - nag_timer = None - enable_sent = False + # Subscribe to power level messages and record the state of charge, and + # turn Starlink on automatically if the SOC is high enough. + charging = False + + already_sent = False last_state = None soc = 0.0 + def power_level_cb(msg: PowerLevel) -> None: + nonlocal already_sent, charging, soc + if last_state is None: + return + + soc = msg.soc_percentage + charging = msg.pack_current < 0 + + if soc >= high_threshold and charging and \ + not last_state and not already_sent: + rospy.loginfo( + f'Enabling Starlink: SOC {soc}% >= {high_threshold}% and charging' + ) + outlet_pub.publish(StdBool(True)) + already_sent = True + elif soc < medium_threshold: + already_sent = False + + rospy.Subscriber('/seatrac/power', PowerLevel, power_level_cb) + + + # Nag the operators if Starlink has been active for too long + nag_timer = None + def nag_operator() -> None: - nonlocal alerts, nag_timer + nonlocal nag_timer nag_timer = None send_alerts(alerts, 'Starlink has been active for a while') def outlet_status_cb(msg: OutletStatus) -> None: - nonlocal manual_shutdown, last_off, nag_timer, last_state - now = rospy.Time.now().to_sec() - if last_state is None: - if not msg.is_active: - manual_shutdown = True - last_off = now - elif last_state and not msg.is_active: - manual_shutdown = True - last_off = now - elif manual_shutdown and msg.is_active: - manual_shutdown = False + nonlocal nag_timer, last_state last_state = msg.is_active - if shutdown_reminder is not None and last_state and nag_timer is None: + # Set up the nag timer if Starlink is active (except if we are at a + # high state of charge and charging). When the outlet is off, cancel + # the nag timer. + if shutdown_reminder is None: + return + + if msg.is_active and nag_timer is None and \ + not (soc >= high_threshold and charging): + # Assume we'll be getting outlet status messages regularly, so we + # can do a one-shot timer. nag_timer = rospy.Timer( - rospy.Duration(shutdown_reminder), + shutdown_reminder, nag_operator, oneshot=True ) @@ -67,50 +102,57 @@ def outlet_status_cb(msg: OutletStatus) -> None: nag_timer.shutdown() nag_timer = None - def power_level_cb(msg: PowerLevel) -> None: - nonlocal enable_sent, last_state, soc, manual_shutdown, last_off - if last_state is None: + rospy.Subscriber('/seatrac/outlet/starlink/status', OutletStatus, + outlet_status_cb) + + + # Schedule Starlink to turn on at a specific time every day + daily_timer = None + + def schedule_daily_timer(): + nonlocal daily_timer + now = datetime.datetime.now(datetime.timezone.utc) + target = now.replace(hour=scheduled_on.hour, + minute=scheduled_on.minute, + second=0, microsecond=0) + if target <= now: + target += datetime.timedelta(days=1) + + daily_timer = rospy.Timer( + rospy.Duration((target - now).total_seconds()), + daily_timer_cb, + oneshot=True + ) + + def daily_timer_cb(event): + nonlocal already_sent + + # Starlink is already enabled, no need to enable it again. We don't + # check already_sent because the operator may have manually turned it + # off yesterday, and we want to turn it on today. + if last_state: return - soc = msg.soc_percentage - is_charging = msg.pack_current < 0 - now = rospy.Time.now().to_sec() - - if ( - soc >= force_on_threshold - and is_charging - and not enable_sent - and not manual_shutdown - ): + if soc >= medium_threshold: rospy.loginfo( - f'Enabling Starlink: SOC {soc}% > {force_on_threshold} and charging' + f'Enabling Starlink: SOC {soc:.1f}% >= {medium_threshold}% ' + 'at scheduled time' ) outlet_pub.publish(StdBool(True)) - enable_sent = True - elif ( - soc >= on_threshold - and not enable_sent - and not manual_shutdown - and (last_off is None or now - last_off > off_delay) - ): - rospy.loginfo( - f'Enabling Starlink: SOC {soc}% > {on_threshold}' - f' and last_off {int(now-last_off)}s > {off_delay}' + already_sent = True + elif soc < medium_threshold: + send_alerts( + alerts, + 'Not enabling Starlink at scheduled time because ' + f'SOC {soc:.1f}% below medium threshold {medium_threshold}%' ) - outlet_pub.publish(StdBool(True)) - enable_sent = True - elif soc < on_threshold: - enable_sent = False + schedule_daily_timer() - rospy.Subscriber('/seatrac/outlet/starlink/status', OutletStatus, - outlet_status_cb) - rospy.Subscriber('/seatrac/power', PowerLevel, power_level_cb) + if scheduled_on is not None: + schedule_daily_timer() rospy.spin() if __name__ == '__main__': - try: - main() - except rospy.ROSInterruptException: - pass + main() From 46937e69bd91dc36432b4c5383ef2db996d69f31 Mon Sep 17 00:00:00 2001 From: Ryan Govostes Date: Tue, 15 Jul 2025 12:19:18 -0700 Subject: [PATCH 5/7] Break container cache of seatrac-api package --- deps/deps.rosinstall | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/deps/deps.rosinstall b/deps/deps.rosinstall index 271c8b9..8a53203 100644 --- a/deps/deps.rosinstall +++ b/deps/deps.rosinstall @@ -1,4 +1,4 @@ -# Cache buster: 1 -- increment to force rebuild of the container image layer +# Cache buster: 2 -- increment to force rebuild of the container image layer - git: local-name: ds_base From 75db0b8ce5928d26b62bdb6cea927d54a3f0a708 Mon Sep 17 00:00:00 2001 From: Nathan Figueroa Date: Tue, 12 Aug 2025 13:05:50 -0400 Subject: [PATCH 6/7] Fixed OutletStatus import --- src/phyto_arm/src/seatrac_starlink_manager.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/phyto_arm/src/seatrac_starlink_manager.py b/src/phyto_arm/src/seatrac_starlink_manager.py index 2aee095..349dd49 100755 --- a/src/phyto_arm/src/seatrac_starlink_manager.py +++ b/src/phyto_arm/src/seatrac_starlink_manager.py @@ -5,11 +5,9 @@ import rospy -from seatrac.msg import PowerLevel +from seatrac.msg import OutletStatus, PowerLevel from std_msgs.msg import Bool as StdBool -from phyto_arm.msg import OutletStatus - def send_alerts(alert_config, message): for alert in alert_config: From e148789a0b6ab9c42d51c884522abc74696fbebe Mon Sep 17 00:00:00 2001 From: Ryan Govostes Date: Tue, 2 Sep 2025 14:48:28 -0700 Subject: [PATCH 7/7] Add full launch file for SeaTrac vehicles --- configs/example.yaml | 21 ++++++++++++++++- configs/seatrac.yaml | 18 --------------- src/phyto_arm/launch/seatrac.launch | 36 +++++++++++++++++++++-------- 3 files changed, 47 insertions(+), 28 deletions(-) delete mode 100644 configs/seatrac.yaml diff --git a/configs/example.yaml b/configs/example.yaml index f73e74c..3d9dbea 100644 --- a/configs/example.yaml +++ b/configs/example.yaml @@ -324,4 +324,23 @@ digital_logger: #optional - name: "winch" outlet: 6 - name: "starlink" - outlet: 7 \ No newline at end of file + outlet: 7 + +seatrac: #optional + comms: + connection: + type: "UDP" + udp_rx: 62001 + udp_tx: 62001 + udp_address: "10.1.20.88" + + outlets: + - name: "starlink" + outlet: 15 + +seatrac_starlink_manager: #optional + scheduled_on: "12:30" # HH:MM, UTC timezone + shutdown_reminder: 1200 # seconds + soc_threshold: + high: 95 # percent (minimum threshold for automatic activation) + medium: 80 # percent (minimum threshold for daily activation) diff --git a/configs/seatrac.yaml b/configs/seatrac.yaml deleted file mode 100644 index b9eabc2..0000000 --- a/configs/seatrac.yaml +++ /dev/null @@ -1,18 +0,0 @@ -seatrac: - comms: - connection: - type: "UDP" - udp_rx: 62001 - udp_tx: 62001 - udp_address: "10.1.20.88" - - outlets: - - name: "starlink" - outlet: 15 - -seatrac_starlink_manager: - scheduled_on: "12:30" # HH:MM, UTC timezone - shutdown_reminder: 1200 # seconds - soc_threshold: - high: 95 # percent (minimum threshold for automatic activation) - medium: 80 # percent (minimum threshold for daily activation) diff --git a/src/phyto_arm/launch/seatrac.launch b/src/phyto_arm/launch/seatrac.launch index ed110cb..1db3bd8 100644 --- a/src/phyto_arm/launch/seatrac.launch +++ b/src/phyto_arm/launch/seatrac.launch @@ -1,15 +1,33 @@ - + - - - - + - + - + + + + + + + + + + + + + + + + + + + + + + +