diff --git a/ros_acomms/src/tdma_advanced_node.py b/ros_acomms/src/tdma_advanced_node.py
index 5f449ae2600683acb608533d455f7aaf2e4f9ee4..a11f10f3d17fd0ff91700d6f2304bdfb8a8cd159 100755
--- a/ros_acomms/src/tdma_advanced_node.py
+++ b/ros_acomms/src/tdma_advanced_node.py
@@ -1,15 +1,16 @@
#!/usr/bin/env python3
import rospy
-from ros_acomms_msgs.srv import PingModem, PingModemRequest, PingTransponders, PingTranspondersRequest
+from ros_acomms_msgs.srv import PingModem, PingModemRequest, PingModemResponse, PingTransponders, PingTranspondersRequest
from ros_acomms_msgs.msg import TdmaAdvancedStatus
from tdma_node import TdmaMacNode
-from std_msgs.msg import Header
+from std_msgs.msg import Header, UInt8
from ros_acomms.cfg import tdma_advancedConfig
from dynamic_reconfigure.server import Server as DynamicReconfigureServer
import datetime
+import traceback
from mac_utils import PSK_ROS_FDP_Rates, expanded_slots_as_ranges # pure python class for non-ros functionality
@@ -78,9 +79,15 @@ class TdmaAdvancedMacNode(TdmaMacNode):
self.transponder_reply_timeout_sec = rospy.get_param('~transponder_reply_timeout_sec', 5)
self.wait_for_ping_services(timeout=rospy.get_param('~wait_for_services_timeout_sec', None))
+ # setup our own ping_modem endpoint for sending pings in tdma
+ self.queued_ping_request = None
+ self.queued_ping_response = None
+ rospy.Service("~ping_modem", PingModem, self.handle_ping_modem)
# set up subscriber to toggle software_mute without overhead of dynamic_reconfigure
rospy.Subscriber('~toggle_software_mute', Header, self.on_toggle_software_mute)
+ # set up subscriber for setting auto ping modem src id (can also be set with dynamic reconfigure)
+ rospy.Subscriber('~change_auto_ping_modem_src', UInt8, self.on_change_auto_ping_modem_src)
# this method is called everytime the slot changes. We reset modem ping flags here
self.sent_modem_pings = False
@@ -106,12 +113,26 @@ class TdmaAdvancedMacNode(TdmaMacNode):
rospy.loginfo("~*+^ tdma_advanced_mac node running.")
self.spin()
+ def invalidate_dynamic_reconfigure_cache(self):
+ # hacky way of invalidating dynamic reconfigure cache.
+ # this will confuse a user of rqt by seemingly ignoring the next config it passes
+ # .. but, it will be updated with the values from the class
+ self.first_dynamic_reconf = True
+
def on_toggle_software_mute(self, msg):
+ # self.invalidate_dynamic_reconfigure_cache() # TODO we should probably add here... need to do more testing first
# will flip whatever the bool value is
self.software_mute ^= True
rospy.set_param('~software_mute', self.software_mute)
rospy.logwarn(f'NOTICE: on_toggle_software_mute() software_mute: {self.software_mute}')
+ def on_change_auto_ping_modem_src(self, msg):
+ if self.ping_modem_src != int(msg.data):
+ self.invalidate_dynamic_reconfigure_cache()
+ self.ping_modem_src = int(msg.data)
+ rospy.set_param('~ping_modem_src', self.ping_modem_src)
+ rospy.logwarn(f'NOTICE: on_change_auto_ping_modem_src() ping_modem_src: {self.ping_modem_src}')
+
def cache_slot_ranges_for_status(self):
self._active_slots_range = expanded_slots_as_ranges(slots=self.active_slots)
self._comms_slots_range = expanded_slots_as_ranges(slots=self.comms_slots)
@@ -143,6 +164,40 @@ class TdmaAdvancedMacNode(TdmaMacNode):
rospy.logwarn(f'WARNING: Timed out waiting for transponder ping service. This will throw an error when user tries to ping a transponder.')
self.send_transponder_ping = rospy.ServiceProxy('ping_transponders', PingTransponders)
+ def handle_ping_modem(self, req):
+ if self.queued_ping_request is not None:
+ rospy.logwarn(f'WARNING: Received a ping request while handling another! Returning as if we timed-out of this request!')
+ return PingModemResponse(timed_out=True)
+
+ try:
+ self.queued_ping_response = None
+ self.queued_ping_request = req
+ cycle_duration = self.slot_duration_seconds * self.num_slots
+ if cycle_duration < req.reply_timeout:
+ rospy.logwarn(f'WARNING: ping_modem request timeout is longer than cycle_duration! Using req.reply_timeout: {req.reply_timeout}')
+ max_timeout = req.reply_timeout
+ else:
+ max_timeout = cycle_duration
+ # now wait until we have a self.queued_ping_response
+ start_t = rospy.get_time()
+ while not rospy.is_shutdown():
+ if rospy.get_time() - start_t > (max_timeout):
+ rospy.logerr(f'ERROR: we have waited max_timeout: {max_timeout} and have not gotten a ping_response. Timing out..')
+ return PingModemResponse(timed_out=True)
+
+ if self.queued_ping_response is None:
+ rospy.sleep(0.1)
+ else:
+ break
+ except:
+ rospy.logerr(f'ERROR: threw exception in tdma::handle_ping_modem: {traceback.format_exc()}')
+ return PingModemResponse(timed_out=True)
+ else:
+ rospy.loginfo(f'INFO: Received a ping_response from ping_modem via tdma: {self.queued_ping_response}')
+ return self.queued_ping_response
+ finally:
+ self.queued_ping_request = None
+
def config_slot_mask(self, slots=None, allow_empty=False):
if slots is not None:
slots = self.parse_slots_value(slots=slots)
@@ -188,7 +243,7 @@ class TdmaAdvancedMacNode(TdmaMacNode):
config['miniframe_rate'] = self.miniframe_rate
config['dataframe_rate'] = self.dataframe_rate
- rospy.logdebug(f'DEBUG: First dynamic_reconfigure call, syncing config from init')
+ rospy.logdebug(f'DEBUG: First dynamic_reconfigure call, syncing config from init OR post invalidate cache')
return config
rospy.logdebug("DEBUG: tdma_advanced_mac dynamic reconfigure request received.")
@@ -347,7 +402,17 @@ class TdmaAdvancedMacNode(TdmaMacNode):
def send_nav_ping(self, remaining_active_seconds, sent_modem_pings, sent_transponder_ping):
nav_ping_duration_sec = 0
- if self.ping_modems and not sent_modem_pings:
+ if self.queued_ping_request is not None:
+ start_t = rospy.get_time()
+ # handle in tdma ping_request
+ req = self.queued_ping_request
+ rospy.loginfo(f'Sending TWTT ping to modem_src: {req.dest}')
+ self.queued_ping_response = self.send_modem_ping(req)
+ self.queued_ping_request = None # for good measure so we don't send another ping with this request
+
+ nav_ping_duration_sec = rospy.get_time() - start_t
+
+ elif self.ping_modems and not sent_modem_pings:
# fire off the modem pings
start_t = rospy.get_time()
for i in range(self.pings_per_slot):
@@ -361,13 +426,16 @@ class TdmaAdvancedMacNode(TdmaMacNode):
req = PingModemRequest(dest=self.ping_modem_src, rate=self.miniframe_rate, cdr=self.ping_cdr, hexdata=bytes(), reply_timeout=self.ping_modem_timeout_sec)
rospy.logdebug(f'[{i + 1}/{self.pings_per_slot}]: Sending TWTT ping to modem_src: {self.ping_modem_src}, request:\n{req}')
if self.roslog_level < rospy.DEBUG: rospy.loginfo(f'[{i + 1}/{self.pings_per_slot}]: Sending TWTT ping to modem_src: {self.ping_modem_src}')
+
resp = self.send_modem_ping(req)
+
rospy.logdebug(f'DEBUG: Response from send_modem_ping:\n{resp}')
if resp.timed_out:
if self.roslog_level < rospy.DEBUG: rospy.loginfo(f'INFO: Response from send_modem_ping TIMED OUT')
sent_modem_pings = True
nav_ping_duration_sec = rospy.get_time() - start_t
+
elif self.ping_transponders and not sent_transponder_ping:
# fire off transponder ping
start_t = rospy.get_time()
diff --git a/ros_acomms_tests/launch/test_use_ping_payload.launch b/ros_acomms_tests/launch/test_use_ping_payload.launch
index 3a5a455d415f643e1f82f8ebb15f76c85d4d71cb..3281bd00fa13ed4bf3281861efc3270fa730f04c 100644
--- a/ros_acomms_tests/launch/test_use_ping_payload.launch
+++ b/ros_acomms_tests/launch/test_use_ping_payload.launch
@@ -106,6 +106,31 @@
<node name="modem_location_sim" pkg="ros_acomms_modeling" type="modem_location_sim_node.py" />
</group>
+ <group ns="modem2">
+ <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: 2
+ 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="2" type="int" />
+ <param name="use_ping_payload" value="true" type="bool" />
+ </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" >
diff --git a/ros_acomms_tests/src/test_use_ping_payload.py b/ros_acomms_tests/src/test_use_ping_payload.py
index 0aea4f7fed48ffb4a7d07aaf1e73541c35af1298..1c9716046a215b37bb10b02c9af752f7f785dc46 100755
--- a/ros_acomms_tests/src/test_use_ping_payload.py
+++ b/ros_acomms_tests/src/test_use_ping_payload.py
@@ -18,9 +18,11 @@ import rospkg
import roslaunch
from std_msgs.msg import Header, Bool, UInt8
+from ros_acomms_msgs.srv import PingModem, PingModemRequest, PingModemResponse
+from ros_acomms_msgs.msg import PingReply
class TestUsePingPayload:
- '''tdma Extended test class
+ '''tdma and modem_driver ping and auto ping test class
test cases:
'''
@@ -60,13 +62,29 @@ class TestUsePingPayload:
self.launch = roslaunch.parent.ROSLaunchParent(uuid, roslaunch_file_param)
self.launch.start()
- rospy.sleep(30.0)
+ rospy.sleep(5.0)
self.logger.warning('Node: test_use_ping_payload running!')
+ 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, latch=True)
+ 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.")
+
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)
+ publisher = rospy.Publisher('/modem1/test_msg', UInt8, queue_size=5, latch=True)
time.sleep(2)
msg = UInt8(data=1)
publisher.publish(msg)
@@ -80,22 +98,78 @@ class TestUsePingPayload:
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')
+ def test_modem0_ping_modem(self):
+ self.logger.warning(f'Testing ping_modem service endpoint in modem_driver')
- publisher = rospy.Publisher('/modem0/test_msg', UInt8, queue_size=5)
- time.sleep(2)
- msg = UInt8(data=1)
- publisher.publish(msg)
+ rospy.wait_for_service('/modem0/ping_modem', timeout=100)
+ send_modem_ping = rospy.ServiceProxy('/modem0/ping_modem', PingModem)
try:
- ret = rospy.wait_for_message(f'/modem1/from_acomms/test_msg', UInt8, timeout=30)
+ req = PingModemRequest(dest=2, rate=1, reply_timeout=1000.0)
+ resp = send_modem_ping(req)
except rospy.exceptions.ROSException:
- self.logger.warning("Success! timed out while waiting for /modem1/from_acomms/test_msg")
+ err = "timed-out during request?"
+ self.logger.warning(err)
+ pytest.fail(err)
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.")
+ if resp.timed_out:
+ err = "response timed out!"
+ self.logger.warning(err)
+ pytest.fail(err)
+ self.logger.warning(f"Success! we got a ping_reply via the modem_driver service endpoint: resp: {resp}")
+
+ def test_modem0_tdma_ping_modem(self):
+ self.logger.warning(f'Testing ping_modem service endpoint in tdma')
+
+ rospy.wait_for_service('/modem0/tdma/ping_modem', timeout=100)
+ send_modem_ping = rospy.ServiceProxy('/modem0/tdma/ping_modem', PingModem)
+
+ try:
+ req = PingModemRequest(dest=2, rate=1, reply_timeout=1000.0)
+ resp = send_modem_ping(req)
+ except rospy.exceptions.ROSException:
+ err = "timed-out during request?"
+ self.logger.warning(err)
+ pytest.fail(err)
+ else:
+ if resp.timed_out:
+ err = "response timed out!"
+ self.logger.warning(err)
+ pytest.fail(err)
+
+ self.logger.warning(f"Success! we got a ping_reply via the tdma service endpoint: resp: {resp}")
+
+ def test_modem1_change_auto_ping_src(self):
+ self.logger.warning(f'Testing change_auto_ping_src')
+
+ publisher = rospy.Publisher('/modem1/tdma/change_auto_ping_modem_src', UInt8, queue_size=5, latch=True)
+ msg = UInt8(data=2)
+ publisher.publish(msg)
+ time.sleep(2.0)
+ self.logger.warning(f'Published to /modem1/tdma/change_auto_ping_modem_src, msg: {msg}')
+ ping_modem_src = rospy.get_param('/modem1/tdma/ping_modem_src')
+ self.logger.warning(f'Query param server, ping_modem_src: {ping_modem_src}')
+ if ping_modem_src != 2:
+ err = f"Failed setting auto ping_modem_src to 2!"
+ self.logger.warning(err)
+ pytest.fail(err)
+
+ try:
+ ret = rospy.wait_for_message(f'/modem1/ping_reply', PingReply, timeout=1000)
+ # incase we got the last ping_reply from our prior auto ping src
+ ret = rospy.wait_for_message(f'/modem1/ping_reply', PingReply, timeout=1000)
+ except rospy.exceptions.ROSException:
+ err = f"Failed! Timed-out waiting for ping_reply on modem1 from modem2"
+ self.logger.warning(err)
+ pytest.fail(err)
+ else:
+ if ret.src == 2:
+ self.logger.warning("Success! got ping_reply for modem1 from modem2")
+ else:
+ err = f"Failed! Got ping_reply from ret.src: {ret.src} not modem2"
+ self.logger.warning(err)
+ pytest.fail(err)
@classmethod
def teardown_class(self):