diff --git a/ros_acomms/src/acomms_driver_node.py b/ros_acomms/src/acomms_driver_node.py
index 4b1d86f64c2e54474f802926a53e85067f10925a..21b4204b33478320b306b7ab1088ef1236b4c684 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 0f82a1cddee6afc9b0d3528a5024906b67345159..74dfeba7857c76240dec83c8ccb2fb08e2934a78 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 7ca3e8d938b6443951e278482ce64a2c8369a96a..b7c55818db88f37f170d1e5efc7b4d90ca8fb399 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 eefcbcaeceaee18abdbf41a9f1fce2ba29c2c1e7..8d24b04efef25911218cf504157bece32401b3b7 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 0000000000000000000000000000000000000000..3bd532789947fc6c523fa7288a51eefce9aad320
--- /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>