diff --git a/ros_acomms/src/acoustic_range_node.py b/ros_acomms/src/acoustic_range_node.py
new file mode 100755
index 0000000000000000000000000000000000000000..b99b85e172e06edf558e9352109cbd21017e88f0
--- /dev/null
+++ b/ros_acomms/src/acoustic_range_node.py
@@ -0,0 +1,144 @@
+#!/usr/bin/env python3
+
+import rospy
+from ros_acomms_msgs.msg import PingReply, PingTranspondersReply, CST, AcousticRange
+import roslib.message
+from rospy import AnyMsg
+from std_msgs.msg import Header, Float32
+
+def deserialze_anymsg(msg_data: AnyMsg):
+ topic_type = msg_data._connection_header['type']
+ topic_class = roslib.message.get_message_class(topic_type)
+ msg = topic_class()
+ msg.deserialize(msg_data._buff)
+ return msg
+
+class AcousticRangeNode(object):
+ """
+ """
+ def __init__(self):
+ rospy.init_node('acoustic_range_node') #, log_level=rospy.DEBUG)
+ # starting sound_speed in m/s. When AnyMsg is published on the ~sound_speed_topic,
+ # .. we update this value. If the msg on ~sound_speed_topic does not have sound_speed,
+ # .. this doesn't change and we'll print an error
+ self.sound_speed = rospy.get_param('~sound_speed', default=1500)
+ # while None, AcousticRange.speed_of_sound_source == SPEED_OF_SOUND_SOURCE_STATIC
+ # as soon as we get an update with a sound_speed, we get the time
+ self.last_sound_speed_update = None
+ self.sound_speed_max_age_sec = rospy.get_param('~sound_speed_max_age_sec', default=500)
+
+ # publisher for acoustic ranges
+ self.acoustic_range_publisher = rospy.Publisher(
+ rospy.get_param('~acoustic_range_topic', default='acoustic_range'),
+ AcousticRange,
+ queue_size=2,
+ latch=True
+ )
+
+ #
+ # set up subscriber for topic with any msg as long as it has a field "sound_speed"
+ # .. it assumed a float value in m/s.
+ rospy.Subscriber(
+ rospy.get_param('~sound_speed_topic', default='ctd'),
+ AnyMsg, self.on_sound_speed_update)
+
+ # setup manual subscriber which is useful for spoofing a float value on a topic for sound_speed
+ rospy.Subscriber(
+ rospy.get_param('~manual_sound_speed_topic', default='~manual_sound_speed'),
+ Float32, self.on_manual_sound_speed)
+
+ # set up subscriber for TWTT modem pings
+ rospy.Subscriber(
+ rospy.get_param('~ping_reply_topic', default='ping_reply'),
+ PingReply, self.on_ping_reply)
+
+ # set up subscriber for TWTT transponder pings
+ rospy.Subscriber(
+ rospy.get_param('~ping_transponders_reply_topic', default='ping_transponders_reply'),
+ PingTranspondersReply, self.on_ping_transponders_reply)
+
+ rospy.logwarn(f'acoustic_range_node started')
+ self.spin()
+
+ @property
+ def sound_speed_age(self):
+ if self.last_sound_speed_update is None:
+ return self.last_sound_speed_update
+
+ return rospy.Time.now().secs - self.last_sound_speed_update.secs
+
+ def on_manual_sound_speed(self, msg):
+ self.sound_speed = float(msg.data)
+ self.last_sound_speed_update = rospy.Time.now()
+
+ def on_sound_speed_update(self, msg):
+ msg = deserialze_anymsg(msg)
+ if 'sound_speed' in msg.__slots__:
+ self.sound_speed = float(msg.sound_speed)
+ self.last_sound_speed_update = rospy.Time.now()
+
+ def on_ping_reply(self, msg):
+ if msg.owtt <= 0.0:
+ rospy.logwarn(f'Got a ping_reply with a owtt of 0.0... not publishing range: {msg}')
+ return
+
+ acoustic_range_msg = self.generate_acoustic_range(
+ measurement_type=AcousticRange.MEASUREMENT_TYPE_TWTT,
+ src=msg.src,
+ dest=msg.dest,
+ owtt=msg.owtt,
+ )
+ acoustic_range_msg.cst = msg.cst
+
+ rospy.loginfo(f'Publishing Acoustic Range from TWTT modem ping: {acoustic_range_msg}')
+ self.acoustic_range_publisher.publish(acoustic_range_msg)
+
+ def on_ping_transponders_reply(self, msg):
+ for i, dt_id in enumerate(['owtt_a', 'owtt_b', 'owtt_c', 'owtt_d']):
+ # for each DT A-D we publish range if owtt is not 0.0
+ owtt = getattr(msg, dt_id, 0.0)
+
+ if owtt:
+ acoustic_range_msg = self.generate_acoustic_range(
+ measurement_type=AcousticRange.MEASUREMENT_TYPE_TWTT,
+ src=-(i + 1), # -1==A, -2==B, -3==C, -4==D
+ dest=-1,
+ owtt=float(owtt),
+ )
+
+ rospy.loginfo(f'Publishing Acoustic Range from TWTT transponder ping: {acoustic_range_msg}')
+ self.acoustic_range_publisher.publish(acoustic_range_msg)
+
+ def generate_acoustic_range(self, measurement_type, src, dest, owtt):
+ speed_of_sound_source = AcousticRange.SPEED_OF_SOUND_SOURCE_UNKNOWN
+ if self.sound_speed_age is None:
+ speed_of_sound_source = AcousticRange.SPEED_OF_SOUND_SOURCE_STATIC
+ elif self.sound_speed_age < self.sound_speed_max_age_sec:
+ speed_of_sound_source = AcousticRange.SPEED_OF_SOUND_SOURCE_TOPIC
+ else:
+ speed_of_sound_source = AcousticRange.SPEED_OF_SOUND_SOURCE_INVALID
+
+ acoustic_range_msg = AcousticRange(header=Header(stamp=rospy.Time.now()),
+ measurement_type=measurement_type,
+ speed_of_sound_source=speed_of_sound_source,
+ src=src,
+ dest=dest,
+ owtt=owtt,
+ range_m=float(self.sound_speed * owtt),
+ speed_of_sound_ms=self.sound_speed)
+ return acoustic_range_msg
+
+ def spin(self):
+ rate = rospy.Rate(5)
+
+ while not rospy.is_shutdown():
+ rate.sleep()
+
+if __name__ == "__main__":
+ try:
+ acoustic_range_node = AcousticRangeNode()
+ rospy.loginfo("acoustic_range_node shutdown (interrupt)")
+ except rospy.ROSInterruptException:
+ rospy.loginfo("acoustic_range_node shutdown (interrupt)")
+
+
diff --git a/ros_acomms/src/tdma_advanced_node.py b/ros_acomms/src/tdma_advanced_node.py
index c222e3a3adadc0a34f4d921dc9c43b16eb0588b3..5e613a136d5c3d041291003cf2108d7e25049b99 100755
--- a/ros_acomms/src/tdma_advanced_node.py
+++ b/ros_acomms/src/tdma_advanced_node.py
@@ -330,13 +330,16 @@ class TdmaAdvancedMacNode(TdmaMacNode):
nav_ping_duration_sec = rospy.get_time() - start_t
remaining_time = remaining_active_seconds - nav_ping_duration_sec
if remaining_time < self.ping_modem_timeout_sec:
- rospy.logwarn(f'WARNING: Not enough time to send another ping with remaining time: {remaining_time}s. Cancelled ping [{i + 1}/{self.pings_per_slot}]')
+ rospy.logwarn_throttle(self.slot_duration_seconds, f'WARNING: Not enough time to send another ping with remaining time: {remaining_time}s. Cancelled ping [{i + 1}/{self.pings_per_slot}]')
break
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.loginfo(f'[{i + 1}/{self.pings_per_slot}]: Sending TWTT ping to modem_src: {self.ping_modem_src}, request:\n{req}')
+ 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.loginfo(f'INFO: Response from send_modem_ping:\n{resp}')
+ 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
@@ -345,9 +348,11 @@ class TdmaAdvancedMacNode(TdmaMacNode):
start_t = rospy.get_time()
req = PingTranspondersRequest(transponder_dest_mask=[self.ping_transponder_a, self.ping_transponder_b, self.ping_transponder_c, self.ping_transponder_d], timeout_ms=self.transponder_reply_timeout_sec * 1000)
- rospy.loginfo(f"INFO: Sending transponder ping, request:\n{req}")
+ rospy.logdebug(f"DEBUG: Sending transponder ping, request:\n{req}")
+ if self.roslog_level < rospy.DEBUG: rospy.loginfo(f"INFO: Sending transponder ping, req.transponder_dest_mask: {req.transponder_dest_mask}, req.timeout_ms: {req.timeout_ms}")
resp = self.send_transponder_ping(req)
- rospy.loginfo(f"INFO: Sent transponder ping. Response:\n{resp}")
+ rospy.logdebug(f"DEBUG: Sent transponder ping. Response:\n{resp}")
+ if self.roslog_level < rospy.DEBUG: rospy.loginfo(f"INFO: Sent transponder ping. resp.travel_times: {resp.travel_times}")
sent_transponder_ping = True
nav_ping_duration_sec = rospy.get_time() - start_t
diff --git a/ros_acomms/src/tdma_node.py b/ros_acomms/src/tdma_node.py
index e4b4b7144eacae28cc09971799e4c894dd7977db..cf09b9de8107f78b5ed757ba115861829d9a78fe 100755
--- a/ros_acomms/src/tdma_node.py
+++ b/ros_acomms/src/tdma_node.py
@@ -10,6 +10,7 @@ from ros_acomms_msgs.msg import Packet, TdmaStatus
import datetime
import dateutil.parser
import json
+import logging
from mac_utils import PSK_ROS_FDP_Rates, ManualTransmitRequestQueue # pure python class for non-ros functionality
from mac_utils import validate_slot_range
@@ -39,6 +40,10 @@ class TdmaMacNode(object):
self.wait_for_services(timeout=rospy.get_param('~wait_for_services_timeout_sec', None))
+ # Figure out current log level of this rosnode
+ logger = logging.getLogger("rosout")
+ self.roslog_level = logger.getEffectiveLevel()
+
# IF CATXF messages are on, Use CATXF timestamp to signal when we are done transmitting
rospy.Subscriber('txf', Header, self.on_txf)
self.last_txf_time = rospy.get_time()
diff --git a/ros_acomms/src/tdma_slotted_aloha_node.py b/ros_acomms/src/tdma_slotted_aloha_node.py
index 12744bd487025f653c5eb353eb11608b87ebd167..03856140ebcd601d725010182a0ed54dff84893b 100755
--- a/ros_acomms/src/tdma_slotted_aloha_node.py
+++ b/ros_acomms/src/tdma_slotted_aloha_node.py
@@ -261,8 +261,8 @@ class TdmaSlottedAlohaMacNode(TdmaAdvancedMacNode):
if __name__ == "__main__":
try:
tdma_state_node = TdmaSlottedAlohaMacNode()
- rospy.loginfo("tdma_scripted_node shutdown (interrupt)")
+ rospy.loginfo("tdma_slotted_aloha_node shutdown (interrupt)")
except rospy.ROSInterruptException:
- rospy.loginfo("tdma_scripted_node shutdown (interrupt)")
+ rospy.loginfo("tdma_slotted_aloha_node shutdown (interrupt)")
diff --git a/ros_acomms_modeling/src/modem_sim_node.py b/ros_acomms_modeling/src/modem_sim_node.py
index 2be7356a5703aa48aa53ccf206566e7a10e28204..eefcbcaeceaee18abdbf41a9f1fce2ba29c2c1e7 100755
--- a/ros_acomms_modeling/src/modem_sim_node.py
+++ b/ros_acomms_modeling/src/modem_sim_node.py
@@ -3,7 +3,7 @@ import rospy
import roslib.message
from rospy import AnyMsg
from std_msgs.msg import Header, Float32
-from ros_acomms_msgs.msg import PingReply, CST, XST, TransmittedPacket, ReceivedPacket, Packet, SST
+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
from ros_acomms_modeling.msg import SimPacket
@@ -186,7 +186,7 @@ class ModemSimNode(object):
depth = location.depth
self.modem_location = Position(latitude, longitude, depth)
- rospy.Subscriber('location', AnyMsg, self.on_modem_location)
+ rospy.Subscriber(modem_location_topic, AnyMsg, self.on_modem_location)
packet_rx_topic = rospy.names.canonicalize_name(rospy.get_param('~packet_rx_topic', 'packet_rx'))
self.rx_publisher = rospy.Publisher(
@@ -228,6 +228,10 @@ class ModemSimNode(object):
self.ping_transactions = {}
self.ping_open_trans_cond_tbl = {}
+ # AcousticRange publisher (on each rx publish range from src to rcv)
+ self.acoustic_range_publisher = rospy.Publisher(
+ '~acoustic_range', AcousticRange, queue_size=2, latch=True)
+
# Set up a loop in a separate thread that handles received packets so that we can use the sim ticks
self.rx_thread = Thread(target=self.process_rx_queue, daemon=True)
self.tx_thread = Thread(target=self.process_outgoing_queue, daemon=True)
@@ -379,7 +383,7 @@ class ModemSimNode(object):
this_transaction, response, sim_packet_reply = None, None, None
try:
- rospy.logwarn("MODEM {}: Starting transaction...".format(self.src))
+ rospy.loginfo("MODEM {}: Starting transaction...".format(self.src))
this_transaction = self.create_ping_transaction(
sim_packet=sim_packet)
# Equivalent with a real modem would be receiving $CACMR,PNR,...
@@ -387,7 +391,7 @@ class ModemSimNode(object):
timeout=request.reply_timeout)
except (TimeoutError, RuntimeError) as e:
- rospy.logwarn("Error in PingModem ROS Service call: {}".format(e))
+ rospy.logdebug("Timeout in PingModem ROS Service call: {}".format(e))
except ConnectionAbortedError as e:
rospy.loginfo("PingModem ROS Service call cancelled: {}".format(e))
except KeyError as e:
@@ -413,10 +417,10 @@ class ModemSimNode(object):
finally:
if this_transaction:
- rospy.logwarn(
+ rospy.loginfo(
"MODEM {}: Closing transaction...".format(self.src))
if this_transaction.transaction_end_t:
- rospy.logwarn("MODEM {}: Transaction took {} seconds, requested timeout: {}".format(self.src,
+ rospy.loginfo("MODEM {}: Transaction took {} seconds, requested timeout: {}".format(self.src,
(this_transaction.transaction_end_t - this_transaction.transaction_start_t).secs,
this_transaction.timeout))
# this transaction either failed/succeeded, either way clean up
@@ -483,7 +487,7 @@ class ModemSimNode(object):
success = this_transaction.complete_transaction(
sim_packet_reply=sim_packet)
else:
- rospy.logwarn("MODEM {}: Received ping reply with no match. Publishing to ping_reply topic.\r\n{}".format(
+ rospy.logdebug("MODEM {}: Received ping reply with no match. Publishing to ping_reply topic.\r\n{}".format(
self.src, sim_packet))
success = False
self.ping_reply_publisher.publish(sim_packet.ping_reply)
@@ -603,6 +607,23 @@ class ModemSimNode(object):
# TODO: we don't want to spam the user with warnings but, we need to handle the case where location params are,
# .. out of range/invalid. we handle owtt <= 0 but, other cases should potentially be handled here.
pass
+ else:
+ # create acoustic range for this sim_packet
+ measurement_type = AcousticRange.MEASUREMENT_TYPE_OWTT
+ # check if this is a ping packet
+ if sim_packet.ping_type > PingReplyTransaction.PING_TYPE_NONE:
+ # measurement_type = AcousticRange.MEASUREMENT_TYPE_OWTT if sim_packet.is_transponder else AcousticRange.MEASUREMENT_TYPE_TWTT
+ measurement_type = AcousticRange.MEASUREMENT_TYPE_TWTT # pretty sure this is the only type we have for sim
+
+ acoustic_range_msg = AcousticRange(header=Header(stamp=rospy.Time.now()),
+ measurement_type=measurement_type,
+ speed_of_sound_source=AcousticRange.SPEED_OF_SOUND_SOURCE_INVERTED_FROM_DISTANCE,
+ src=sim_packet.packet.src,
+ dest=sim_packet.packet.dest,
+ owtt=owtt,
+ range_m=distance_m,
+ speed_of_sound_ms=sound_speed)
+ self.acoustic_range_publisher.publish(acoustic_range_msg)
return recv_level, arrival_time, finish_time
diff --git a/ros_acomms_msgs/CMakeLists.txt b/ros_acomms_msgs/CMakeLists.txt
index a56d2279a119ef59d7c5977554711151a96bd111..135c62aa55911f87dfb7ec44a09dca5f0a2048c1 100644
--- a/ros_acomms_msgs/CMakeLists.txt
+++ b/ros_acomms_msgs/CMakeLists.txt
@@ -48,6 +48,7 @@ find_package(catkin REQUIRED COMPONENTS
## Generate messages in the 'msg' folder
add_message_files(FILES
+ AcousticRange.msg
AcousticTimesync.msg
CST.msg
NST.msg
diff --git a/ros_acomms_msgs/msg/AcousticRange.msg b/ros_acomms_msgs/msg/AcousticRange.msg
new file mode 100644
index 0000000000000000000000000000000000000000..6f4f46504d911e52a4dc925bfd0f7f2404ff0208
--- /dev/null
+++ b/ros_acomms_msgs/msg/AcousticRange.msg
@@ -0,0 +1,24 @@
+Header header
+
+int8 MEASUREMENT_TYPE_UNKNOWN = 0
+int8 MEASUREMENT_TYPE_OWTT = 1
+int8 MEASUREMENT_TYPE_TWTT = 2
+int8 measurement_type
+
+int8 SPEED_OF_SOUND_SOURCE_INVALID = -1
+int8 SPEED_OF_SOUND_SOURCE_UNKNOWN = 0
+int8 SPEED_OF_SOUND_SOURCE_STATIC = 1
+int8 SPEED_OF_SOUND_SOURCE_TOPIC = 2
+int8 SPEED_OF_SOUND_SOURCE_INVERTED_FROM_DISTANCE = 3
+int8 speed_of_sound_source
+
+int16 src
+int16 dest
+
+float32 owtt
+float32 tat
+
+float32 range_m
+float32 speed_of_sound_ms
+
+CST cst
diff --git a/ros_acomms_tests/launch/test_tdma_auto_ping.launch b/ros_acomms_tests/launch/test_tdma_auto_ping.launch
new file mode 100644
index 0000000000000000000000000000000000000000..be098ea9d553fe9fe880fd13b55be28c01c82a78
--- /dev/null
+++ b/ros_acomms_tests/launch/test_tdma_auto_ping.launch
@@ -0,0 +1,89 @@
+<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="" />
+
+ <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${time:%Y-%m-%dT%H:%M:%S}] [${node}]: ${message}"/>
+
+ <param name="use_sim_time" value="true"/>
+
+ <group ns="modem0">
+ <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>
+
+ <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="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="pings_per_slot" value="2"/>
+ <param name="ping_modem_src" value="1"/>
+ <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>
+
+ <node name="modem_location_sim" pkg="ros_acomms_modeling" type="modem_location_sim_node.py" >
+ <param name="simple_path" value="true" />
+ </node>
+ </group>
+
+ <group ns="modem1">
+ <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="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="2,3"/> -->
+ <param name="software_mute" value="True"/>
+ <param name="packet_length_seconds" value="0"/>
+ <param name="guard_time_seconds" value="0"/>
+ </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" >
+ <param name="simple_path" value="true" />
+ </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>