From cadfb47e3c346e74b1d2c270b0a350dfe28456e6 Mon Sep 17 00:00:00 2001
From: Eric Gallimore <egallimore@whoi.edu>
Date: Mon, 1 Mar 2021 16:28:21 -0500
Subject: [PATCH 1/3] Set default ping timeout in ping_modem service.
---
src/acomms_driver_node.py | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/src/acomms_driver_node.py b/src/acomms_driver_node.py
index ee53041c..2d324e7d 100755
--- a/src/acomms_driver_node.py
+++ b/src/acomms_driver_node.py
@@ -83,7 +83,7 @@ class AcommsDriverNode(object):
def handle_ping_modem(self, request):
self.um.send_fdp_ping(request.dest)
if request.reply_timeout <= 0:
- request.reply_timeout = None
+ request.reply_timeout = 5
travel_time, snr_in, snr_out = self.um.wait_for_fdp_ping_reply(request.dest, include_cst=True, timeout=request.reply_timeout)
if not travel_time:
travel_time = -1
--
GitLab
From 6dfd0adf6c517c621ea7bf26456c30983189e975 Mon Sep 17 00:00:00 2001
From: Eric Gallimore <egallimore@whoi.edu>
Date: Sat, 13 Mar 2021 19:07:52 -0500
Subject: [PATCH 2/3] Updated Packet message to match FDP definition. May
break other packages that use Packet message. Updated TDMA node to use new
definition. Publishes ReceivedPackets on FDP RX now (not legacy).
---
msg/Packet.msg | 8 ++++----
src/acomms_driver_node.py | 36 ++++++++++++++++++++++++++++++++----
src/tdma_node.py | 6 +++---
3 files changed, 39 insertions(+), 11 deletions(-)
diff --git a/msg/Packet.msg b/msg/Packet.msg
index 035b590f..1914ffa3 100644
--- a/msg/Packet.msg
+++ b/msg/Packet.msg
@@ -1,6 +1,6 @@
int16 src
int16 dest
-int8 minipacket_rate
-int8 packet_rate
-KeyInt64Value[] header_data
-uint8[] payload_data
\ No newline at end of file
+int8 miniframe_rate
+int8 dataframe_rate
+uint8[] miniframe_bytes
+uint8[] dataframe_bytes
\ No newline at end of file
diff --git a/src/acomms_driver_node.py b/src/acomms_driver_node.py
index 2d324e7d..bc74db9b 100755
--- a/src/acomms_driver_node.py
+++ b/src/acomms_driver_node.py
@@ -1,14 +1,18 @@
#!/usr/bin/env python3
from __future__ import unicode_literals
+from queue import Queue
+from threading import Thread
+
from acomms.unifiedlog import UnifiedLog
from acomms.micromodem import Micromodem
from acomms.modem_connections import UdpConnection
from acomms.flexibledataprotocol import FDPacket
import logging
import rospy
+from std_msgs.msg import Header
from dynamic_reconfigure.server import Server as DynamicReconfigureServer
-from ros_acomms.msg import CST
+from ros_acomms.msg import CST, ReceivedPacket, Packet
from ros_acomms.srv import PingModem, PingModemResponse
from ros_acomms.srv import QueueTxPacket, QueueTxPacketResponse
from ros_acomms.srv import TxInhibit, TxInhibitResponse
@@ -29,6 +33,7 @@ def convert_datetime_to_rospy(time_stamp):
class AcommsDriverNode(object):
def __init__(self):
self.cst_publisher = rospy.Publisher('cst', CST, queue_size=10)
+ self.packet_rx_publisher = rospy.Publisher('packet_rx', ReceivedPacket, queue_size=10)
rospy.init_node('acomms_driver')
modem_connection_type = rospy.get_param('~modem_connection_type', 'serial')
@@ -61,7 +66,6 @@ class AcommsDriverNode(object):
# Wait here until we can talk to the modem
-
if set_modem_time:
self.um.set_time()
@@ -72,6 +76,8 @@ class AcommsDriverNode(object):
self.um.get_config_param("TOP")
# self.um.rxframe_listeners.append(self.on_modem_rxframe)
+ self.modem_rx_thread = Thread(target=self.modem_rx_handler, daemon=True)
+ self.modem_rx_thread.start()
self.ping_modem_service = rospy.Service('ping_modem', PingModem, self.handle_ping_modem)
self.queue_tx_packet_service = rospy.Service('queue_tx_packet', QueueTxPacket, self.handle_queue_tx_packet)
@@ -80,6 +86,28 @@ class AcommsDriverNode(object):
self.reconfigure_server = DynamicReconfigureServer(acomms_driverConfig, self.reconfigure)
+ def modem_rx_handler(self):
+ incoming_fd_packet_queue = Queue()
+ self.um.incoming_fdp_queues.append(incoming_fd_packet_queue)
+ while True:
+ try:
+ incoming_packet: FDPacket = incoming_fd_packet_queue.get(block=True)
+ # publish it
+ packet_msg = Packet(src=incoming_packet.src, dest=incoming_packet.dest,
+ miniframe_rate=incoming_packet.miniframe_rate,
+ dataframe_rate=incoming_packet.dataframe_rate,
+ miniframe_bytes=incoming_packet.minibytes,
+ dataframe_bytes=incoming_packet.databytes)
+ cst_values = incoming_packet.cyclestats
+ cst_msg = CST(**cst_values)
+
+ hdr = Header(stamp=rospy.get_rostime())
+ rx_packet_msg = ReceivedPacket(header=hdr, packet=packet_msg, cst=cst_msg)
+ self.packet_rx_publisher.publish(rx_packet_msg)
+
+ except Exception as e:
+ rospy.logerr_throttle(1, "Error in packet RX handler: {}".format(e))
+
def handle_ping_modem(self, request):
self.um.send_fdp_ping(request.dest)
if request.reply_timeout <= 0:
@@ -150,14 +178,14 @@ class AcommsDriverNode(object):
rospy.loginfo('Sending message via acomms')
if self.use_legacy_packets:
- self.um.send_packet_data(packet.dest, packet.payload_data, rate_num=packet.packet_rate, ack=False)
+ self.um.send_packet_data(packet.dest, packet.frame_data, rate_num=packet.packet_rate, ack=False)
else:
ack = 0
header_data = 0
self.um.send_fdpacket(FDPacket(packet.src, packet.dest, miniframerate=1,
dataframerate=packet.packet_rate, ack=ack,
- minibytes=None, databytes=packet.payload_data))
+ minibytes=packet.miniframe_data, databytes=packet.frame_data))
def process_outgoing_queue(self):
try:
diff --git a/src/tdma_node.py b/src/tdma_node.py
index 074ffbef..06abcd45 100755
--- a/src/tdma_node.py
+++ b/src/tdma_node.py
@@ -134,10 +134,10 @@ class TdmaMacNode(object):
rospy.loginfo("Queuing Packet for TX")
queue_packet = Packet()
- queue_packet.packet_rate = 1
- queue_packet.minipacket_rate = 1
+ queue_packet.dataframe_rate = 1
+ queue_packet.miniframe_rate = 1
queue_packet.dest = bytes_response.dest
- queue_packet.payload_data = bytes_response.payload_bytes
+ queue_packet.dataframe_bytes = bytes_response.payload_bytes
req = QueueTxPacketRequest(insert_at_head=False,
packet=queue_packet)
--
GitLab
From bf4141a1ab9a4e723e77c985860e6b981838b82c Mon Sep 17 00:00:00 2001
From: Eric Gallimore <egallimore@whoi.edu>
Date: Sat, 13 Mar 2021 19:08:57 -0500
Subject: [PATCH 3/3] Pause in the acomms driver node init until we can talk to
the modem (get a response to a $CCCFQ,SRC).
---
src/acomms_driver_node.py | 4 ++++
1 file changed, 4 insertions(+)
diff --git a/src/acomms_driver_node.py b/src/acomms_driver_node.py
index bc74db9b..9e89adad 100755
--- a/src/acomms_driver_node.py
+++ b/src/acomms_driver_node.py
@@ -65,6 +65,10 @@ class AcommsDriverNode(object):
remote_port=modem_remote_port, local_port=modem_local_port)
# Wait here until we can talk to the modem
+ while True:
+ # This returns None if we time out. Repeat until we get a reply.
+ if self.um.get_config("SRC", response_timeout=1):
+ break
if set_modem_time:
self.um.set_time()
--
GitLab