From da4cc5483d9466a457fd3b49a76708132943ba83 Mon Sep 17 00:00:00 2001
From: Caileigh Fitzgerald <cfitzgerald@whoi.edu>
Date: Wed, 10 Jul 2024 20:41:58 +0000
Subject: [PATCH 1/2] acomms_driver and modem_sim new default minimum
ping_timeout_sec.
---
ros_acomms/src/acomms_driver_node.py | 4 +-
ros_acomms/src/tdma_advanced_node.py | 24 ++-
ros_acomms/src/tdma_node.py | 30 +++
ros_acomms_modeling/src/modem_sim_node.py | 4 +-
ros_acomms_tests/launch/tdma_fast_ping.launch | 173 ++++++++++++++++++
5 files changed, 223 insertions(+), 12 deletions(-)
create mode 100644 ros_acomms_tests/launch/tdma_fast_ping.launch
diff --git a/ros_acomms/src/acomms_driver_node.py b/ros_acomms/src/acomms_driver_node.py
index 4b1d86f6..21b4204b 100755
--- a/ros_acomms/src/acomms_driver_node.py
+++ b/ros_acomms/src/acomms_driver_node.py
@@ -522,8 +522,8 @@ class AcommsDriverNode(object):
"""
rospy.loginfo("Requesting modem send ping")
self.um.send_fdp_ping(request.dest, request.rate, request.cdr, request.hexdata)
- if request.reply_timeout <= 0:
- request.reply_timeout = 5
+ if request.reply_timeout < 1:
+ request.reply_timeout = 1
ping_reply, cst = self.um.wait_for_fdp_ping_reply(
include_cst=True, timeout=request.reply_timeout
)
diff --git a/ros_acomms/src/tdma_advanced_node.py b/ros_acomms/src/tdma_advanced_node.py
index 0f82a1cd..74dfeba7 100755
--- a/ros_acomms/src/tdma_advanced_node.py
+++ b/ros_acomms/src/tdma_advanced_node.py
@@ -48,12 +48,12 @@ class TdmaAdvancedMacNode(TdmaMacNode):
"""
def __init__(self, subclass=False):
if not subclass:
- rospy.init_node('tdma_advanced_mac')
+ rospy.init_node('tdma_advanced_mac') #, log_level=rospy.DEBUG)
self.tdma_status_publisher = rospy.Publisher('~tdma_advanced_status' if rospy.get_param('~publish_private_status', False) else 'tdma_advanced_status', TdmaAdvancedStatus, queue_size=0)
super().__init__(subclass=True)
# take supers active_slots list of ints and generate comms_slot & nav_slots masks
self.nav_slots = self.config_slot_mask(slots=rospy.get_param('~nav_slots', None), allow_empty=True)
- self.comms_slots = self.config_slot_mask(slots=rospy.get_param('~comms_slots', None))
+ self.comms_slots = self.config_slot_mask(slots=rospy.get_param('~comms_slots', None), allow_empty=True)
self.cache_slot_ranges_for_status()
# do advanced stuff
@@ -82,6 +82,11 @@ class TdmaAdvancedMacNode(TdmaMacNode):
# set up subscriber to toggle software_mute without overhead of dynamic_reconfigure
rospy.Subscriber('~toggle_software_mute', Header, self.on_toggle_software_mute)
+ # this method is called everytime the slot changes. We reset modem ping flags here
+ self.sent_modem_pings = False
+ self.sent_transponder_ping = False
+ self.register_on_slot_change_callback(self.on_slot_changed)
+
if not subclass:
# setup dynamic reconf for periodic ping params
self.first_dynamic_reconf = True
@@ -182,6 +187,7 @@ class TdmaAdvancedMacNode(TdmaMacNode):
config['packet_length_seconds'] = self.packet_length_seconds
config['miniframe_rate'] = self.miniframe_rate
config['dataframe_rate'] = self.dataframe_rate
+
rospy.logdebug(f'DEBUG: First dynamic_reconfigure call, syncing config from init')
return config
@@ -307,12 +313,13 @@ class TdmaAdvancedMacNode(TdmaMacNode):
return sent_modem_pings, sent_transponder_ping, update_last_tx
+ def on_slot_changed(self):
+ self.sent_modem_pings, self.sent_transponder_ping = False, False
+
def spin(self):
- rate = rospy.Rate(5)
+ rate = rospy.Rate(15)
last_tx_time = 0
msg = None
- sent_modem_pings = False
- sent_transponder_ping = False
while not rospy.is_shutdown():
msg = self.get_current_slot_info(software_mute=self.software_mute)
@@ -320,16 +327,17 @@ class TdmaAdvancedMacNode(TdmaMacNode):
if self.can_transmit_now(msg=msg, last_tx_time=last_tx_time):
# we are active and have atleast enough time to send another packet
- sent_modem_pings, sent_transponder_ping, update_last_tx = self.handle_queuing_packet(
+ self.sent_modem_pings, self.sent_transponder_ping, update_last_tx = self.handle_queuing_packet(
msg.current_slot,
msg.remaining_active_seconds,
- sent_modem_pings,
- sent_transponder_ping
+ self.sent_modem_pings,
+ self.sent_transponder_ping
)
if update_last_tx:
last_tx_time = rospy.get_time()
else:
# nothing sent, sleep then check again
+ rospy.logdebug(f'DEBUG: Nothing sent during tdma... remaining_active_seconds: {msg.remaining_active_seconds}')
rate.sleep()
continue
else:
diff --git a/ros_acomms/src/tdma_node.py b/ros_acomms/src/tdma_node.py
index 7ca3e8d9..b7c55818 100755
--- a/ros_acomms/src/tdma_node.py
+++ b/ros_acomms/src/tdma_node.py
@@ -11,6 +11,7 @@ import datetime
import dateutil.parser
import json
import logging
+from threading import Thread
from mac_utils import PSK_ROS_FDP_Rates, ManualTransmitRequestQueue # pure python class for non-ros functionality
from mac_utils import validate_slot_range
@@ -56,10 +57,18 @@ class TdmaMacNode(object):
self.manual_transmit_queue.add_manual_transmit_topic(topic='nmea_to_modem', msg_type=String)
self.allow_manual_tx_during_priority_slots = rospy.get_param('~allow_manual_tx_during_priority_slots', False)
+ # mechanism for registering callbacks in child classes on slot change
+ self.on_slot_change_callbacks = []
+ self.on_slot_change_thread = Thread(target=self.on_slot_change, daemon=True)
+ self.on_slot_change_thread.start()
+
if not subclass:
rospy.loginfo("~*+ tdma_mac node running.")
self.spin()
+ def register_on_slot_change_callback(self, callback):
+ self.on_slot_change_callbacks.append(callback)
+
def wait_for_services(self, timeout=120):
rospy.loginfo("INFO: tdma waiting for queue_tx_packet service")
rospy.wait_for_service('queue_tx_packet', timeout=timeout)
@@ -334,6 +343,27 @@ class TdmaMacNode(object):
return True
+ def on_slot_change(self):
+ rate = rospy.Rate(1)
+ last_slot = None
+
+ while not rospy.is_shutdown():
+ rate.sleep()
+
+ current_slot, remaining_slot_seconds = self.get_current_slot()
+ if last_slot is None:
+ last_slot = current_slot
+
+ if last_slot != current_slot:
+ for callback in self.on_slot_change_callbacks:
+ try:
+ callback()
+ except:
+ rospy.logwarn(f'WARNING: Exception thrown during on_slot_change()')
+
+ last_slot = current_slot
+ rospy.sleep(self.slot_duration_seconds / 2)
+
def spin(self):
rate = rospy.Rate(5)
last_tx_time = 0
diff --git a/ros_acomms_modeling/src/modem_sim_node.py b/ros_acomms_modeling/src/modem_sim_node.py
index eefcbcae..8d24b04e 100755
--- a/ros_acomms_modeling/src/modem_sim_node.py
+++ b/ros_acomms_modeling/src/modem_sim_node.py
@@ -378,8 +378,8 @@ class ModemSimNode(object):
# TODO should handle this error
return PingModemResponse(timed_out=True)
- if request.reply_timeout < 5:
- request.reply_timeout = 5
+ if request.reply_timeout < 1:
+ request.reply_timeout = 1
this_transaction, response, sim_packet_reply = None, None, None
try:
diff --git a/ros_acomms_tests/launch/tdma_fast_ping.launch b/ros_acomms_tests/launch/tdma_fast_ping.launch
new file mode 100644
index 00000000..3bd53278
--- /dev/null
+++ b/ros_acomms_tests/launch/tdma_fast_ping.launch
@@ -0,0 +1,173 @@
+<launch>
+ <arg name="num_slots" default="2" doc="" />
+ <arg name="slot_duration_seconds" default="6" doc="" />
+ <arg name="tdma_type" default="tdma_advanced" doc="type of TDMA node to use: tdma_advanced, or tdma_scripted or tdma_slotted_aloha" />
+ <arg name="test_cycling_rates" default="true" doc="" />
+ <arg name="use_hw_modem" default="true" doc="" />
+
+ <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${time:%Y-%m-%dT%H:%M:%S}] [${node}]: ${message}"/>
+
+ <param name="use_sim_time" value="false"/>
+
+ <group ns="modem0">
+ <group if="$(arg use_hw_modem)">
+ <node name="acomms_driver" pkg="ros_acomms" type="acomms_driver_node.py" respawn="true" respawn_delay="10" >
+ <param name="modem_serial_port" value="/dev/ttyUSB2" type="str" />
+ <param name="modem_baud_rate" value="19200" />
+ <param name="set_modem_time" value="true"/>
+ <param name="pwramp_txlevel" value="2" type="int" />
+ <rosparam param="modem_config" subst_value="True">
+ ALL: 0
+ BND: 3
+ FC0: 25000
+ BW0: 5000
+ SST: 1
+ DTP: 50
+ pwramp.txlevel: 2
+ psk.packet.mod_hdr_version: 1
+ SRC: 0
+ </rosparam>
+ </node>
+ </group>
+
+ <group unless="$(arg use_hw_modem)">
+ <param name="random_seed" value="1" />
+
+ <node name="modem_sim_node" pkg="ros_acomms_modeling" type="modem_sim_node.py" output="screen" >
+ <param name="center_frequency_hz" value="10000" type="int" />
+ <param name="bandwidth_hz" value="5000" type="int" />
+ <param name="SRC" value="0" type="int" />
+ </node>
+
+ <node name="modem_location_sim" pkg="ros_acomms_modeling" type="modem_location_sim_node.py" >
+ <param name="simple_path" value="true" />
+ </node>
+
+ </group>
+
+ <node name="acoustic_range_node" pkg="ros_acomms" type="acoustic_range_node.py" output="screen" >
+ <param name="sound_speed_max_age_sec" value="30" type="int" />
+ </node>
+
+ <node name="mac_switcher" pkg="ros_acomms" type="mac_switcher_node.py" output="screen" required="true" respawn="false" clear_params="true" >
+ <rosparam subst_value="True">
+ default_mac_namespace: 'tdma_fast_ping'
+ managed_mac_namespaces:
+ - 'tdma'
+
+ # TODO: require managed mac nodes to continually publish the select message at minimum every managed_mac_heartbeat_timeout_sec
+ # .. of the mac manager will revert to the default (or prior mac if configured that way)
+ # .. mac manager select topic name: mac_switcher/MANAGED_MAC_NAMESPACE/select
+ # .. select topic type: std_msgs/Bool (True for select, False for deselect)
+ managed_mac_heartbeat_timeout_sec: -1 # if set to -1, it will not timeout
+ </rosparam>
+ </node>
+
+ <node name="tdma_fast_ping" pkg="ros_acomms" type="$(arg tdma_type)_node.py" output="screen" required="true" respawn="false">
+ <param name="num_slots" value="$(arg num_slots)"/>
+ <param name="slot_duration_seconds" value="$(arg slot_duration_seconds)" />
+ <param name="active_slots" value="0,1"/>
+ <param name="comms_slots" value=""/>
+ <param name="pings_per_slot" value="1"/>
+ <param name="ping_modem_src" value="1"/>
+ <param name="ping_modem_timeout_sec" value="4" />
+ <param name="packet_length_seconds" value="0"/>
+ <param name="guard_time_seconds" value="0"/>
+ <param name="always_send_test_data" value="true"/>
+ <param name="publish_private_status" value="true"/>
+ <!-- <param name="software_mute" value="true"/> -->
+ </node>
+
+ <node name="tdma" pkg="ros_acomms" type="$(arg tdma_type)_node.py" output="screen" required="true" respawn="false">
+ <param name="num_slots" value="$(arg num_slots)"/>
+ <param name="slot_duration_seconds" value="$(arg slot_duration_seconds)" />
+ <param name="active_slots" value="0,1"/>
+ <param name="packet_length_seconds" value="3"/>
+ <param name="guard_time_seconds" value="2.5"/>
+ <param name="publish_private_status" value="true"/>
+ <param name="software_mute" value="true"/>
+ </node>
+
+ <node name="message_queue_node" pkg="ros_acomms" type="message_queue_node.py" output="screen" >
+ <param name="packet_codec_file" value="$(find ros_acomms_tests)/launch/message_codec_config.yaml"/>
+ </node>
+
+ <node name="packet_dispatch" pkg="ros_acomms" type="packet_dispatch_node.py" output="screen" >
+ <param name="packet_codec_file" value="$(find ros_acomms_tests)/launch/message_codec_config.yaml"/>
+ </node>
+ </group>
+
+ <group ns="modem1">
+ <group if="$(arg use_hw_modem)">
+ <node name="acomms_driver" pkg="ros_acomms" type="acomms_driver_node.py" respawn="true" respawn_delay="10" >
+ <param name="modem_serial_port" value="/dev/ttyUSB3" type="str" />
+ <param name="modem_baud_rate" value="19200" />
+ <param name="set_modem_time" value="true"/>
+ <param name="pwramp_txlevel" value="2" type="int" />
+ <rosparam param="modem_config" subst_value="True">
+ ALL: 0
+ BND: 3
+ FC0: 25000
+ BW0: 5000
+ SST: 1
+ DTP: 50
+ pwramp.txlevel: 2
+ psk.packet.mod_hdr_version: 1
+ SRC: 1
+ </rosparam>
+ </node>
+ </group>
+
+ <group unless="$(arg use_hw_modem)">
+ <param name="random_seed" value="2" />
+
+ <node name="modem_sim_node" pkg="ros_acomms_modeling" type="modem_sim_node.py" output="screen" >
+ <param name="center_frequency_hz" value="10000" type="int" />
+ <param name="bandwidth_hz" value="5000" type="int" />
+ <param name="SRC" value="1" type="int" />
+ </node>
+
+ <node name="modem_location_sim" pkg="ros_acomms_modeling" type="modem_location_sim_node.py" >
+ <param name="simple_path" value="true" />
+ </node>
+
+ </group>
+
+ <node name="acoustic_range_node" pkg="ros_acomms" type="acoustic_range_node.py" output="screen" >
+ <param name="sound_speed_max_age_sec" value="30" type="int" />
+ </node>
+
+ <node name="tdma" pkg="ros_acomms" type="$(arg tdma_type)_node.py" output="screen" required="true" respawn="false">
+ <param name="software_mute" value="true"/>
+ <param name="num_slots" value="$(arg num_slots)"/>
+ <param name="slot_duration_seconds" value="$(arg slot_duration_seconds)" />
+ <param name="active_slots" value="0,1"/>
+ <param name="comms_slots" value=""/>
+ <param name="pings_per_slot" value="1"/>
+ <param name="ping_modem_src" value="0"/>
+ <param name="always_send_test_data" value="True"/>
+ <param name="packet_length_seconds" value="2.5"/>
+ <param name="guard_time_seconds" value="2.5"/>
+ </node>
+
+ <node name="message_queue_node" pkg="ros_acomms" type="message_queue_node.py" output="screen" >
+ <param name="packet_codec_file" value="$(find ros_acomms_tests)/launch/message_codec_config.yaml"/>
+ </node>
+
+ <node name="packet_dispatch" pkg="ros_acomms" type="packet_dispatch_node.py" output="screen" >
+ <param name="packet_codec_file" value="$(find ros_acomms_tests)/launch/message_codec_config.yaml"/>
+ </node>
+ </group>
+
+ <param name="random_seed" value="4" />
+ <param name="multiplier" value="1" />
+ <node name="sim_packet_performance_node" pkg="ros_acomms_modeling" type="sim_packet_performance_node.py" />
+
+ <node name="sim_transmission_loss_node" pkg="ros_acomms_modeling" type="sim_transmission_loss_node.py" >
+ <param name="water_depth" value="20" type="int" />
+ <param name="bellhop_arrivals" value="false" type="bool" />
+ </node>
+
+ <node name="clock_generator" pkg="ros_acomms_modeling" type="clock_generator.py"/>
+
+</launch>
--
GitLab
From 86494088eeb5c92745bfadf2184d2367e51a776b Mon Sep 17 00:00:00 2001
From: Caileigh Fitzgerald <cfitzgerald@whoi.edu>
Date: Tue, 30 Jul 2024 23:22:07 +0000
Subject: [PATCH 2/2] acomms_driver_node and modem_sim_node can now send ROS
msg traffic in FDP ping payloads
---
.gitlab-ci.yml | 32 ++++
ros_acomms/cfg/acomms_driver.cfg | 15 +-
ros_acomms/src/acomms_driver_node.py | 137 ++++++++++++++----
ros_acomms_modeling/src/modem_sim_node.py | 46 +++++-
.../launch/test_use_ping_payload.launch | 115 +++++++++++++++
ros_acomms_tests/src/test_use_ping_payload.py | 108 ++++++++++++++
6 files changed, 415 insertions(+), 38 deletions(-)
create mode 100644 ros_acomms_tests/launch/test_use_ping_payload.launch
create mode 100755 ros_acomms_tests/src/test_use_ping_payload.py
diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 2dba1bba..7f0d67de 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -334,6 +334,37 @@ test-mac_switcher:
reports:
junit: '*.results.xml'
+test-use_ping_payload:
+ stage: test
+ image: ros_acomms-tests:pipeline_$CI_PIPELINE_ID
+ script:
+ - source /ros_entrypoint.sh
+ - mkdir -p ~/catkin_ws/src
+ - cd ../ && cp -r $CI_PROJECT_NAME ~/catkin_ws/src/
+ - cd ~/catkin_ws/src/ros_acomms && pip install -U -r requirements.txt
+ - cd ~/catkin_ws && catkin_make
+ - roscore &
+ - sleep 5
+ - rosparam set /clock_generator/multiplier $CLOCK_MULTIPLIER
+ - rosparam set /clock_generator/publish_rate $CLOCK_PUBLISH_RATE
+ - source devel/setup.bash
+ - TESTS_PATH=$(rospack find ros_acomms_tests)/src
+ - ROS_ACOMMS_PATH=$(rospack find ros_acomms)/src
+ - ROS_ACOMMS_MODELING_PATH=$(rospack find ros_acomms_modeling)/src
+ - cd ${CI_PROJECT_DIR}
+ - pytest --capture=no --junitxml=test_use_ping_payload.results.xml --cov=$ROS_ACOMMS_PATH --cov-report= $TESTS_PATH/test_use_ping_payload.py
+ - mv .coverage test_use_ping_payload.coverage # Note that the next call to pytest will delete anything that starts with .coverage*
+ - coverage combine --keep *.coverage
+ - coverage report
+ - mv .coverage "$CI_JOB_NAME_SLUG.job.coverage"
+ except:
+ - dev/autodocs
+ artifacts:
+ paths:
+ - '*.coverage'
+ reports:
+ junit: '*.results.xml'
+
test-coverage:
image: ros_acomms-tests:pipeline_$CI_PIPELINE_ID
except:
@@ -354,6 +385,7 @@ test-coverage:
- "test-tdma-slotted-aloha"
- "test-tdma-scripted"
- "test-mac_switcher"
+ - "test-use_ping_payload"
script:
- mkdir -p ~/catkin_ws/src
- cp -r ../$CI_PROJECT_NAME ~/catkin_ws/src/
diff --git a/ros_acomms/cfg/acomms_driver.cfg b/ros_acomms/cfg/acomms_driver.cfg
index 475e7e59..e48e2f03 100644
--- a/ros_acomms/cfg/acomms_driver.cfg
+++ b/ros_acomms/cfg/acomms_driver.cfg
@@ -1,6 +1,6 @@
#! /usr/bin/env python
import roslib
-from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, str_t, int_t, bool_t, double_t
+from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, str_t, int_t, bool_t
PACKAGE = 'ros_acomms'
@@ -9,10 +9,19 @@ roslib.load_manifest(PACKAGE)
gen = ParameterGenerator()
# Name, Type, Level, Description, Default, Min, Max
+gen.add("use_ping_payload", bool_t, 0, "Use the payload of an FDP ping for ROS message transport", False)
+gen.add("ping_maximum_miniframe_bytes", int_t, 0, "Ping payload max size", 32, 0, 32) # default size is entire miniframe 32 bytes
gen.add("tx_inhibit", bool_t, 0, "Disable modem transmissions", False)
-gen.add("pwramp_txlevel", int_t, 0, "pwramp.txlevel setting", 3, 0, 3)
+
+pwramp_txlevel_enum = gen.enum([gen.const("most_loud", int_t, 0, "Highest TX volume"),
+ gen.const("more_loud", int_t, 1, "Mid-Upper TX volume"),
+ gen.const("less_loud", int_t, 2, "Mid-Lower TX volume"),
+ gen.const("least_loud", int_t, 3, "Lowest TX volume")],
+ "Modem Transmit Volume")
+
+gen.add("pwramp_txlevel", int_t, 0, "pwramp.txlevel setting", 3, 0, 3, edit_method=pwramp_txlevel_enum)
# The second parameter is the name of a node this could run in (used to generate documentation only),
# the third parameter is a name prefix the generated files will get (e.g. "<name>Config.h" for c++, or "<name>Config.py"
# for python.
-exit(gen.generate(PACKAGE, "acomms_driver", "acomms_driver"))
\ No newline at end of file
+exit(gen.generate(PACKAGE, "acomms_driver", "acomms_driver"))
diff --git a/ros_acomms/src/acomms_driver_node.py b/ros_acomms/src/acomms_driver_node.py
index 21b4204b..a4cd2946 100755
--- a/ros_acomms/src/acomms_driver_node.py
+++ b/ros_acomms/src/acomms_driver_node.py
@@ -40,7 +40,7 @@ from ros_acomms_msgs.msg import (
from ros_acomms_msgs.srv import QueryModemParam, QueryModemParamRequest, QueryModemParamResponse
from ros_acomms_msgs.srv import PingModem, PingModemResponse
from ros_acomms_msgs.srv import PingTransponders, PingTranspondersResponse
-from ros_acomms_msgs.srv import QueueTxPacket, QueueTxPacketResponse
+from ros_acomms_msgs.srv import QueueTxPacket, QueueTxPacketResponse, GetNextPacketData
from ros_acomms_msgs.srv import QueryModemSrcRequest, QueryModemSrcResponse, QueryModemSrc
from ros_acomms.cfg import acomms_driverConfig
from std_msgs.msg import Header, String, Time
@@ -121,7 +121,24 @@ class AcommsDriverNode(object):
modem_serial_port = rospy.get_param("~modem_serial_port", "/dev/ttyS3")
modem_baud_rate = rospy.get_param("~modem_baud_rate", 19200)
set_modem_time = rospy.get_param("~set_modem_time", False)
- modem_config = rospy.get_param("~modem_config", {})
+ self.modem_config = rospy.get_param("~modem_config", {})
+
+ # pull modem_config params used in dynamic_reconfigure (if there, otherwise use default and add to modem_config dict)
+ self.pwramp_txlevel = self.modem_config.get('pwramp.txlevel', None)
+ self.tx_inhibit = self.modem_config.get('xmit.txinhibit', None)
+
+ if self.pwramp_txlevel is None:
+ self.pwramp_txlevel = rospy.get_param("~pwramp_txlevel", 3) # default is lowest volume (checks top level param)
+ self.modem_config['pwramp.txlevel'] = self.pwramp_txlevel
+
+ if self.tx_inhibit is None:
+ self.tx_inhibit = False # default is false
+ self.modem_config['xmit.txinhibit'] = 1 # when 1:False, 2:True. 0:floating
+
+ # set the updated modem_config dict and pwramp_txlevel on the param server
+ rospy.set_param("~modem_config", self.modem_config)
+ rospy.set_param("~pwramp_txlevel", self.pwramp_txlevel)
+
self.publish_partial_packets = rospy.get_param("~publish_partial_packets", True)
port = modem_serial_port
@@ -194,7 +211,7 @@ class AcommsDriverNode(object):
if set_modem_time:
self.um.set_time()
- for p, v in modem_config.items():
+ for p, v in self.modem_config.items():
self.um.set_config(p, v)
# Request all the parameters so they're saved in the log file.
@@ -243,13 +260,61 @@ class AcommsDriverNode(object):
"query_modem_param", QueryModemParam, self.handle_query_modem_param
)
+ # check if we should use the ping payload for message transport
+ self.use_ping_payload = rospy.get_param("~use_ping_payload", False)
+ # the default is the max bytes we can fit in a miniframe
+ self.ping_maximum_miniframe_bytes = rospy.get_param("~ping_maximum_miniframe_bytes", 32)
+
+ # if using the ping payload (or want to use later), configure the service proxy to message_queue
+ rospy.loginfo("acomms_driver_node creating get_next_packet_data service proxy")
+ self.get_next_packet_data = rospy.ServiceProxy('get_next_packet_data', GetNextPacketData)
+
+ # set flag for first callback only. This syncs dynamic reconf with params passed at launch
+ self.first_dynamic_reconf = True
self.reconfigure_server = DynamicReconfigureServer(
acomms_driverConfig, self.reconfigure
)
+
self.publish_nodestatus(True, "acomms_driver_node initialized.")
rospy.loginfo("Acomms driver node initialized.")
+ def reconfigure(self, config, level):
+ if self.first_dynamic_reconf:
+ self.first_dynamic_reconf = False
+ # due to way rosparam of typle list doesn't properly map to param server in launch file
+ # ..meaning the defaults set in the .cfg file will always use those values at first call
+ # ..so to avoid having defaults set (when ros handles this by calling all non-list type ros params from param server),
+ # ..set values from init in config
+ config["tx_inhibit"] = self.tx_inhibit
+ config["pwramp_txlevel"] = self.pwramp_txlevel
+
+ config["use_ping_payload"] = self.use_ping_payload
+ config["ping_maximum_miniframe_bytes"] = self.ping_maximum_miniframe_bytes
+
+ rospy.logdebug(f'DEBUG: First dynamic_reconfigure call, syncing config from init')
+ return config
+
+ rospy.loginfo("Acomms driver reconfigure request received.")
+
+ self.pwramp_txlevel = config["pwramp_txlevel"]
+ self.tx_inhibit = config["tx_inhibit"]
+
+ self.use_ping_payload = config["use_ping_payload"]
+ self.ping_maximum_miniframe_bytes = config["ping_maximum_miniframe_bytes"] # dynamic reconf has a min:0 max:32
+
+ # we always set all the params passed rather than checking state.
+ # .. e.g., If the modem is booted into another slot etc.. we may need to set the same value again
+ rospy.loginfo(f"Acomms driver reconfigure request received. config={config}")
+
+ um_response = self.um.set_config("xmit.txinhibit", 2 if self.tx_inhibit else 1, response_timeout=5)
+ rospy.loginfo(f"Acomms driver reconfigure xmit.txinhibit um_response: {um_response}")
+
+ um_response = self.um.set_config("pwramp.txlevel", self.pwramp_txlevel, response_timeout=5)
+ rospy.loginfo(f"Acomms driver reconfigure pwramp.txlevel um_response: {um_response}")
+
+ return config
+
def verify_modem_connection(self, missed_checkin_factor: int = 5):
secs_since_checkin = rospy.get_rostime().secs - self.carev_checkin.secs
cycle_timeout_factor = self.um.config_data['CTO'] * missed_checkin_factor
@@ -403,9 +468,37 @@ class AcommsDriverNode(object):
if "CATXF" in incoming_string:
txf_msg = Header(stamp=rospy.get_rostime())
self.txf_publisher.publish(txf_msg)
+
+ # if any params we set with dynamic reconfigure flow through, update our value
+ # .. so future calls to dynamic reconfigure set the latest value
+ if "CACFG" in incoming_string:
+ self.handle_config_update(cacfg_nmea=incoming_string)
+
except Exception as e:
rospy.logerr_throttle(1, "Error in NMEA RX handler: {}".format(e))
+ def handle_config_update(self, cacfg_nmea):
+ # any params added to dynamic reconfigure should add a conditional to this method
+ # this methods intent is to capture those values as they are read in a $CACFG
+ is_dynamic_reconf_param = False
+ try:
+ value = cacfg_nmea.split('*')[0].split(',')[-1]
+ if 'pwramp.txlevel' in cacfg_nmea:
+ is_dynamic_reconf_param = True
+ self.pwramp_txlevel = int(value)
+
+ elif 'xmit.txinhibit' in cacfg_nmea:
+ is_dynamic_reconf_param = True
+ if int(value) == 2:
+ self.tx_inhibit = True
+ else:
+ self.tx_inhibit = False
+ except ValueError:
+ rospy.logwarn(f'WARNING: ValueError when updating modem config value managed by dynamic reconfigure! modem msg: {cacfg_nmea}')
+ rospy.logwarn(f'WARNING: Leaving config value as-is. A call to dynamic reconfigure will use the old value.')
+ else:
+ if is_dynamic_reconf_param: rospy.logdebug(f'DEBUG: Got a CACFG for a param used in dynamic_reconfigure! Updating: {cacfg_nmea}')
+
def handle_ping_transponders(self, request):
"""handler for rospy PingTransponders service class
@@ -521,7 +614,20 @@ class AcommsDriverNode(object):
(PingModemResponse): ping response from the modem
"""
rospy.loginfo("Requesting modem send ping")
- self.um.send_fdp_ping(request.dest, request.rate, request.cdr, request.hexdata)
+ ping_payload = request.hexdata
+
+ if self.use_ping_payload and self.get_next_packet_data:
+ rospy.loginfo(f"Attempting to fill ping payload for ROS msg transport. payload max size: {self.ping_maximum_miniframe_bytes} bytes")
+ try:
+ packet_data_response = self.get_next_packet_data(num_miniframe_bytes=self.ping_maximum_miniframe_bytes, num_dataframe_bytes=0)
+ except:
+ rospy.logwarn(f"ERROR: problem with get_next_packet_data() service. Not sending a payload with this ping...")
+ else:
+ if packet_data_response.num_messages > 0:
+ rospy.loginfo(f"Received data to send with ping request length: {len(packet_data_response.miniframe_bytes)}")
+ ping_payload = packet_data_response.miniframe_bytes
+
+ self.um.send_fdp_ping(request.dest, request.rate, request.cdr, ping_payload)
if request.reply_timeout < 1:
request.reply_timeout = 1
ping_reply, cst = self.um.wait_for_fdp_ping_reply(
@@ -605,29 +711,6 @@ class AcommsDriverNode(object):
queue_tx_packet_resp.success = True
return queue_tx_packet_resp
- def reconfigure(self, config, level):
- rospy.loginfo("Acomms driver reconfigure request received.")
- tx_inhibit = config["tx_inhibit"]
- pwramp_txlevel = config["pwramp_txlevel"]
- rospy.loginfo(
- "Acomms driver reconfigure request received. tx_inhibit={}".format(
- tx_inhibit
- )
- )
-
- if tx_inhibit:
- value = 2
- else:
- value = 1
- # Err on the side of sending configs too often for now. TODO: track state and clean this up
- um_response = self.um.set_config("xmit.txinhibit", value, response_timeout=5)
- um_response = self.um.set_config(
- "pwramp.txlevel", pwramp_txlevel, response_timeout=5
- )
- # um_response = self.um.set_config('psk.packet.mod_hdr_version', pwramp_txlevel, response_timeout=5)
- #
- return config
-
def _send_packet(self, packet: Packet):
rospy.loginfo("Sending message via acomms")
diff --git a/ros_acomms_modeling/src/modem_sim_node.py b/ros_acomms_modeling/src/modem_sim_node.py
index 8d24b04e..8e58aa0a 100755
--- a/ros_acomms_modeling/src/modem_sim_node.py
+++ b/ros_acomms_modeling/src/modem_sim_node.py
@@ -5,7 +5,7 @@ from rospy import AnyMsg
from std_msgs.msg import Header, Float32
from ros_acomms_msgs.msg import PingReply, CST, XST, TransmittedPacket, ReceivedPacket, Packet, SST, AcousticRange
from ros_acomms_msgs.srv import PingModem, PingModemResponse, QueueTxPacket, QueueTxPacketResponse, \
- QueryModemSrc, QueryModemSrcRequest, QueryModemSrcResponse
+ QueryModemSrc, QueryModemSrcRequest, QueryModemSrcResponse, GetNextPacketData
from ros_acomms_modeling.msg import SimPacket
from ros_acomms_modeling.srv import SimTransmissionLoss, SimTransmissionLossRequest, SimPacketPerformance, \
SimPacketPerformanceRequest, SimPacketPerformanceResponse, SimTravelTime, SimTravelTimeRequest, SimTravelTimeResponse
@@ -252,6 +252,15 @@ class ModemSimNode(object):
"query_modem_src", QueryModemSrc, self.handle_query_modem_src
)
+ # check if we should use the ping payload for message transport
+ self.use_ping_payload = rospy.get_param("~use_ping_payload", False)
+ # the default is the max bytes we can fit in a miniframe
+ self.ping_maximum_miniframe_bytes = rospy.get_param("~ping_maximum_miniframe_bytes", 32)
+
+ # if using the ping payload (or want to use later), configure the service proxy to message_queue
+ rospy.loginfo("modem_sim_node creating get_next_packet_data service proxy")
+ self.get_next_packet_data = rospy.ServiceProxy('get_next_packet_data', GetNextPacketData)
+
rospy.loginfo("Modem sim node running{}. SRC={}, FC={} Hz, BW={} Hz".format(
" with sim ticks active" if self.use_sim_tick else "", self.src, self.fc, self.bw))
@@ -324,11 +333,8 @@ class ModemSimNode(object):
return tx_packet
- def send_fdp_ping_sim(self, dest, rate=1, cdr=0, hexdata=None):
+ def send_fdp_ping_sim(self, dest, rate=1, cdr=0, ping_payload=bytes()):
queue_tx_packet_resp, sim_packet = None, None
- if not hexdata:
- hexdata = bytes()
-
start_t = rospy.Time.now()
queue_tx_packet_req = self.format_ping_for_tx_queue(dest=dest)
@@ -338,8 +344,9 @@ class ModemSimNode(object):
'ping_reply': self.create_ping_reply_msg(dest=dest),
'ping_type': PingReplyTransaction.PING_TYPE_REQUEST,
'transaction_start_t': start_t,
- })
-
+ 'ping_payload': ping_payload,
+ }
+ )
return queue_tx_packet_resp, sim_packet
def create_ping_transaction(self, sim_packet):
@@ -358,12 +365,28 @@ class ModemSimNode(object):
Equivalent with a real modem would be sending $CCCMD,PNG,...
'''
rospy.logdebug("MODEM {}: Requesting modem send ping".format(self.src))
+ ping_payload = request.hexdata
+
+ # handle case where user wants to fill the ping payload with ros msgs
+ if self.use_ping_payload and self.get_next_packet_data:
+ rospy.loginfo(f"MODEM {self.src}: Attempting to fill ping payload for ROS msg transport. payload max size: {self.ping_maximum_miniframe_bytes} bytes")
+ try:
+ packet_data_response = self.get_next_packet_data(num_miniframe_bytes=self.ping_maximum_miniframe_bytes, num_dataframe_bytes=0)
+ except:
+ rospy.logwarn(f"ERROR: problem with get_next_packet_data() service. Not sending a payload with this ping...")
+ else:
+ if packet_data_response.num_messages > 0:
+ rospy.logdebug(f"Received data to send with ping request length: {len(packet_data_response.miniframe_bytes)}")
+ ping_payload = packet_data_response.miniframe_bytes
+ else:
+ rospy.logdebug(f"Received NO data to send with ping request \npacket_data_response: {packet_data_response}\nlength: {len(packet_data_response.miniframe_bytes)}")
+
try:
# Equivalent with a real modem would be sending $CACMD,PNG,...
queue_tx_packet_resp, sim_packet = self.send_fdp_ping_sim(request.dest,
request.rate,
request.cdr,
- request.hexdata)
+ ping_payload)
except TypeError:
import traceback
rospy.logwarn("MODEM {}: Threw exception\r\n{}".format(
@@ -884,11 +907,18 @@ class ModemSimNode(object):
sim_packet.ping_reply = ping['ping_reply']
sim_packet.ping_type = ping['ping_type']
sim_packet.ping_transaction_start = ping['transaction_start_t']
+ # optionally user can include a payload the size of a miniframe
+ miniframe_bytes = ping.get('ping_payload', None)
else:
sim_packet.ping_type = PingReplyTransaction.PING_TYPE_NONE
+ miniframe_bytes = None # unset, use packet from req
sim_packet.packet = queue_tx_packet_req.packet
sim_packet.packet.src = self.src
+ if miniframe_bytes is not None:
+ # only set if explicit ping_payload key in req
+ sim_packet.packet.miniframe_bytes = miniframe_bytes
+
if queue_tx_packet_req.requested_src_level_db == 0:
queue_tx_packet_req.requested_src_level_db = 180
queue_tx_packet_resp.actual_src_level_db = self.get_actual_src_level_db(
diff --git a/ros_acomms_tests/launch/test_use_ping_payload.launch b/ros_acomms_tests/launch/test_use_ping_payload.launch
new file mode 100644
index 00000000..3a5a455d
--- /dev/null
+++ b/ros_acomms_tests/launch/test_use_ping_payload.launch
@@ -0,0 +1,115 @@
+<launch>
+ <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${time}] [${node}]: ${message}"/>
+
+ <arg name="sim" default="true" doc="Sim or Hardware modems, default: sim" />
+ <arg name="tdma_type" default="tdma_advanced" doc="type of TDMA node to use: tdma_advanced, or tdma_scripted or tdma_slotted_aloha" />
+
+ <!-- <node pkg="rostopic" type="rostopic" name="modem0_talker" args="pub /modem0/test_msg std_msgs/UInt8 1 -r 1" output="screen"/>
+ <node pkg="rostopic" type="rostopic" name="modem1_talker" args="pub /modem1/test_msg std_msgs/UInt8 1 -r 1" output="screen"/> -->
+
+ <group ns="modem0">
+ <node name="acoustic_range_node" pkg="ros_acomms" type="acoustic_range_node.py" output="screen" />
+
+ <node unless="$(arg sim)" name="acomms_driver_node" pkg="ros_acomms" type="acomms_driver_node.py" output="screen" >
+ <param name="modem_serial_port" value="/dev/ttyUSB1" type="str" />
+ <param name="modem_baud_rate" value="19200" type="int" />
+ <param name="pwramp_txlevel" value="3" type="int" />
+ <param name="use_ping_payload" value="false" type="bool" />
+ <rosparam param="modem_config">
+ SRC: 0
+ BND: 0
+ FC0: 25000
+ BW0: 5000
+ pwramp.txlevel: 2
+ psk.packet.mod_hdr_version: 1
+ </rosparam>
+ </node>
+
+ <node if="$(arg sim)" name="modem_sim_node" pkg="ros_acomms_modeling" type="modem_sim_node.py" output="screen" >
+ <param name="center_frequency_hz" value="25000" type="int" />
+ <param name="bandwidth_hz" value="5000" type="int" />
+ <param name="SRC" value="0" type="int" />
+ <param name="use_ping_payload" value="false" type="bool" />
+ </node>
+
+ <node name="tdma" pkg="ros_acomms" type="$(arg tdma_type)_node.py" output="screen" required="true" respawn="false">
+ <rosparam>
+ active_slots: 0
+ num_slots: 2
+ slot_duration_seconds: 10
+ comms_slots: ''
+ guard_time_seconds: 4
+ packet_length_seconds: 5
+ ping_modem: True
+ ping_modem_src: 1
+ pings_per_slot: 1
+ </rosparam>
+ </node>
+
+ <node name="message_queue_node" pkg="ros_acomms" type="message_queue_node.py" output="screen" >
+ <param name="packet_codec_file" value="$(find ros_acomms_tests)/launch/message_codec_config.yaml"/>
+ </node>
+
+ <node name="packet_dispatch" pkg="ros_acomms" type="packet_dispatch_node.py" output="screen" >
+ <param name="packet_codec_file" value="$(find ros_acomms_tests)/launch/message_codec_config.yaml"/>
+ </node>
+
+ <node name="modem_location_sim" pkg="ros_acomms_modeling" type="modem_location_sim_node.py" />
+ </group>
+
+ <group ns="modem1">
+ <node name="acoustic_range_node" pkg="ros_acomms" type="acoustic_range_node.py" output="screen" />
+
+ <node unless="$(arg sim)" name="acomms_driver_node" pkg="ros_acomms" type="acomms_driver_node.py" output="screen" >
+ <param name="modem_serial_port" value="/dev/ttyUSB2" type="str" />
+ <param name="modem_baud_rate" value="19200" type="int" />
+ <param name="pwramp_txlevel" value="3" type="int" />
+ <param name="use_ping_payload" value="true" type="bool" />
+ <rosparam param="modem_config">
+ SRC: 1
+ BND: 0
+ FC0: 25000
+ BW0: 5000
+ psk.packet.mod_hdr_version: 1
+ </rosparam>
+ </node>
+
+ <node if="$(arg sim)" name="modem_sim_node" pkg="ros_acomms_modeling" type="modem_sim_node.py" output="screen" >
+ <param name="center_frequency_hz" value="25000" type="int" />
+ <param name="bandwidth_hz" value="5000" type="int" />
+ <param name="SRC" value="1" type="int" />
+ <param name="use_ping_payload" value="true" type="bool" />
+ </node>
+
+ <node name="tdma" pkg="ros_acomms" type="$(arg tdma_type)_node.py" output="screen" required="true" respawn="false">
+ <rosparam>
+ active_slots: 1
+ num_slots: 2
+ slot_duration_seconds: 10
+ comms_slots: ''
+ guard_time_seconds: 4
+ packet_length_seconds: 5
+ ping_modem: True
+ ping_modem_src: 0
+ pings_per_slot: 1
+ </rosparam>
+ </node>
+
+ <node name="message_queue_node" pkg="ros_acomms" type="message_queue_node.py" output="screen" >
+ <param name="packet_codec_file" value="$(find ros_acomms_tests)/launch/message_codec_config.yaml"/>
+ </node>
+
+ <node name="packet_dispatch" pkg="ros_acomms" type="packet_dispatch_node.py" output="screen" >
+ <param name="packet_codec_file" value="$(find ros_acomms_tests)/launch/message_codec_config.yaml"/>
+ </node>
+
+ <node name="modem_location_sim" pkg="ros_acomms_modeling" type="modem_location_sim_node.py" />
+ </group>
+
+ <node name="sim_packet_performance_node" pkg="ros_acomms_modeling" type="sim_packet_performance_node.py" />
+
+ <node name="sim_transmission_loss_node" pkg="ros_acomms_modeling" type="sim_transmission_loss_node.py" >
+ <param name="water_depth" value="20" type="int" />
+ <param name="bellhop_arrivals" value="false" type="bool" />
+ </node>
+</launch>
diff --git a/ros_acomms_tests/src/test_use_ping_payload.py b/ros_acomms_tests/src/test_use_ping_payload.py
new file mode 100755
index 00000000..0aea4f7f
--- /dev/null
+++ b/ros_acomms_tests/src/test_use_ping_payload.py
@@ -0,0 +1,108 @@
+#! /usr/bin/env python3
+
+'''
+tests for using ping payload for ros msg transport
+'''
+import hashlib
+import os
+import secrets
+import sys
+import time
+import traceback
+import threading
+import logging
+
+import pytest
+import rospy
+import rospkg
+import roslaunch
+
+from std_msgs.msg import Header, Bool, UInt8
+
+class TestUsePingPayload:
+ '''tdma Extended test class
+
+ test cases:
+ '''
+ VALID_TDMA_TYPES = ['tdma_advanced', 'tdma_scripted', 'tdma_slotted_aloha']
+ @classmethod
+ def setup_class(self):
+ """
+ setup function for tests
+ """
+ self.test_counter = 0
+ rospy.init_node("test_use_ping_payload", log_level=rospy.DEBUG)
+ self.logger = logging.getLogger("rosout")
+
+ self.tdma_type = rospy.get_param("/tdma_type", "tdma_advanced")
+ self.sim = rospy.get_param("/sim", True)
+ if self.tdma_type not in TestUsePingPayload.VALID_TDMA_TYPES:
+ raise NotImplementedError(f'This test module does not work with {self.tdma_type}. Valid tdma_types: {TestUsePingPayload.VALID_TDMA_TYPES}')
+
+ self.roslaunch_dict = dict(
+ sim=self.sim,
+ tdma_type=self.tdma_type,
+ )
+ self.rospack = rospkg.RosPack()
+
+ uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
+ roslaunch.configure_logging(uuid)
+
+ launch_file = os.path.join(
+ self.rospack.get_path("ros_acomms_tests"),
+ "launch/test_use_ping_payload.launch",
+ )
+
+ roslaunch_args = [f"{k}:={v}" for k, v in self.roslaunch_dict.items()]
+ full_cmd_list = [launch_file] + roslaunch_args
+ roslaunch_file_param = [(roslaunch.rlutil.resolve_launch_arguments(full_cmd_list)[0], roslaunch_args)]
+
+ self.launch = roslaunch.parent.ROSLaunchParent(uuid, roslaunch_file_param)
+ self.launch.start()
+
+ rospy.sleep(30.0)
+ self.logger.warning('Node: test_use_ping_payload running!')
+
+ def test_modem1_enabled(self):
+ self.logger.warning(f'Testing use ping payload on modem1 which has ~use_ping_payload set to true')
+
+ publisher = rospy.Publisher('/modem1/test_msg', UInt8, queue_size=5)
+ time.sleep(2)
+ msg = UInt8(data=1)
+ publisher.publish(msg)
+
+ try:
+ ret = rospy.wait_for_message(f'/modem0/from_acomms/test_msg', UInt8, timeout=30)
+ except rospy.exceptions.ROSException:
+ self.logger.warning("Timed-out waiting for msg on /modem0/from_acomms/test_msg from modem1")
+ pytest.fail("Timed-out waiting for msg on /modem0/from_acomms/test_msg from modem1")
+ else:
+ if ret.data == 1:
+ self.logger.warning("Success! Got msg on /modem0/from_acomms/test_msg from modem1 which has ~use_ping_payload:True and no comms_slots")
+
+ def test_modem0_disabled(self):
+ self.logger.warning(f'Testing use ping payload on modem0 which has ~use_ping_payload set to false')
+
+ publisher = rospy.Publisher('/modem0/test_msg', UInt8, queue_size=5)
+ time.sleep(2)
+ msg = UInt8(data=1)
+ publisher.publish(msg)
+
+ try:
+ ret = rospy.wait_for_message(f'/modem1/from_acomms/test_msg', UInt8, timeout=30)
+ except rospy.exceptions.ROSException:
+ self.logger.warning("Success! timed out while waiting for /modem1/from_acomms/test_msg")
+ else:
+ self.logger.warning("Got test_msg from modem0 which has ~use_ping_payload:False and no comms_slots.")
+ pytest.fail("Got test_msg from modem0 which has ~use_ping_payload:False and no comms_slots.")
+
+
+ @classmethod
+ def teardown_class(self):
+ """
+ teardown function for tests
+ """
+ self.launch.shutdown()
+
+if __name__ == "__main__":
+ sys.exit(pytest.main(['--capture=no', __file__]))
--
GitLab