diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 34da92d280681ea15a289fe64b506856cfcc9cfd..e4a2c366501bf564bddf9349402c4c181e86adcf 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -4,20 +4,28 @@ gitlab run_tests:
script:
- source /opt/ros/noetic/setup.bash
- pip install -r requirements.txt
+ - pip install -U pytest
- cd ../
- rm -r ~/catkin_ws/src/ros_acomms
- - mv ros_acomms ~/catkin_ws/src/
- - cd ~/catkin_ws/src/acomms_tests && git pull
+ - cp -r ros_acomms ~/catkin_ws/src/
+ - cd ~/catkin_ws/src/acomms_tests && git pull origin master
- cd ~/catkin_ws/src/ltcodec && git pull && pip install .
- cd ~/catkin_ws/src/pyacomms && git pull && pip install .
- cd ~/catkin_ws/ && catkin_make
- source ~/catkin_ws/devel/setup.bash
- screen -S sim -L -dm bash -c "cd ~/catkin_ws/src/acomms_tests/launch && roslaunch acomms_tests queue_test.launch"
- sleep 5
- - rosrun acomms_tests queueTest.py
- - rosrun acomms_tests fragmentationTest.py "$MSG_SIZE"
+ - cd ~/catkin_ws/src/acomms_tests/src
+ - pytest --junitxml=report.xml
- screen -X -S sim quit
+ after_script:
+ - cd ~/catkin_ws/
- cat screenlog.0
+ - cp ~/catkin_ws/src/acomms_tests/src/report.xml $CI_PROJECT_DIR/report.xml
+ artifacts:
+ when: always
+ reports:
+ junit: report.xml
pypi run_tests:
stage: test
@@ -26,15 +34,24 @@ pypi run_tests:
- source /opt/ros/noetic/setup.bash
- cd ../
- rm -r ~/catkin_ws/src/ros_acomms
- - mv ros_acomms ~/catkin_ws/src/
- - cd ~/catkin_ws/src/acomms_tests && git pull
+ - cp -r ros_acomms ~/catkin_ws/src/
+ - cd ~/catkin_ws/src/acomms_tests && git pull origin master
- cd ~/catkin_ws/ && catkin_make
- source ~/catkin_ws/devel/setup.bash
- roscd ros_acomms
- pip install -I -r requirements.txt
+ - pip install -U pytest
+ - cd ~/catkin_ws/
- screen -S sim -L -dm bash -c "cd ~/catkin_ws/src/acomms_tests/launch && roslaunch acomms_tests queue_test.launch"
- sleep 5
- - rosrun acomms_tests queueTest.py
- - rosrun acomms_tests fragmentationTest.py "$MSG_SIZE"
+ - cd ~/catkin_ws/src/acomms_tests/src
+ - pytest --junitxml=report.xml
- screen -X -S sim quit
- - cat screenlog.0
\ No newline at end of file
+ after_script:
+ - cd ~/catkin_ws/
+ - cat screenlog.0
+ - cp ~/catkin_ws/src/acomms_tests/src/report.xml $CI_PROJECT_DIR/report.xml
+ artifacts:
+ when: always
+ reports:
+ junit: report.xml
diff --git a/.gitlab/merge_request_templates/Default.md b/.gitlab/merge_request_templates/Default.md
new file mode 100644
index 0000000000000000000000000000000000000000..bfce0bc008cb7a463c8177bb948311dc8f2e293d
--- /dev/null
+++ b/.gitlab/merge_request_templates/Default.md
@@ -0,0 +1,5 @@
+
+
+
+
+If I am not a WHOI employee, I agree to the terms in the [WHOI Contributor Agreement](CONTIBUTING.md) (`CONTRIBUTING.md`) in this repository.
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8042cfc2a1657a3c32334621ffa8624433fadeb7..b4df0c0397f98e0136c0266b3af209685a492505 100755
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -52,7 +52,6 @@ find_package(catkin REQUIRED COMPONENTS
## Generate messages in the 'msg' folder
add_message_files(FILES
AcousticTimesync.msg
- CallStatus.msg
CST.msg
SST.msg
EncodedAck.msg
@@ -62,30 +61,31 @@ add_message_files(FILES
NumBytes.msg
Packet.msg
PingReply.msg
+ PingTranspondersReply.msg
QueueEnable.msg
QueuePriority.msg
QueueStatus.msg
QueueSummary.msg
- ReceivedIridiumPacket.msg
ReceivedPacket.msg
SimPacket.msg
SoundSpeedProfile.msg
Source.msg
SummaryMessageCount.msg
TdmaStatus.msg
+ TdmaNavStatus.msg
Tick.msg
Tock.msg
TransmittedPacket.msg
+ UpdateQueuedMessage.msg
XST.msg
)
## Generate services in the 'srv' folder
add_service_files(FILES
- EstablishCall.srv
GetNextPacketData.srv
+ GetNextQueuedMessage.srv
GetOptimalTxDepth.srv
GetPlatformLocation.srv
- Hangup.srv
PingModem.srv
PingTransponders.srv
QueueTxPacket.srv
@@ -124,6 +124,7 @@ generate_messages(DEPENDENCIES
## Generate dynamic reconfigure parameters in the 'cfg' folder
generate_dynamic_reconfigure_options(
cfg/acomms_driver.cfg
+ cfg/tdma_nav_header.cfg
)
###################################
diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md
new file mode 100644
index 0000000000000000000000000000000000000000..6436f64f803c0f30793ac5645dc57d30b964f722
--- /dev/null
+++ b/CONTRIBUTING.md
@@ -0,0 +1,42 @@
+Contributor License Agreement
+=============================
+
+This Contributor License Agreement (the “Agreementâ€) is between Woods Hole Oceanographic Institution and the person or entity designated in this Agreement as the Contributor (and sometimes referred to as “You†or “Your†below). You agree to the terms and conditions of this Agreement as of the date of such acceptance.
+
+This Agreement is meant to clarify the intellectual property license granted with Contributions by You. This Agreement is meant to protect both You as a Contributor and WHOI as the recipient of the Contribution, and does not change your rights to use your own Contributions for any other purpose.
+You accept and agree to the following terms and conditions for Your present and future Contributions submitted to WHOI. Except for the license granted herein to WHOI and recipients of related Work distributed by WHOI, You reserve all right, title and interest in and to Your Contributions.
+
+## 1. Definitions.
+
+"You" (or "Your") shall mean the copyright owner or legal entity authorized by the copyright owner that is making this Agreement with WHOI. For legal entities, the entity making a Contribution and all other entities that control, are controlled by, or are under common control with that entity are considered to be a single Contributor. For the purposes of this definition, "control" means (i) the power, direct or indirect, to cause the direction or management of such entity, whether by contract or otherwise, or (ii) ownership of fifty percent (50%) or more of the outstanding shares or other equity interests, or (iii) beneficial ownership of such entity.
+
+"Contribution" shall mean any original work of authorship, including any modifications or additions to an existing work, that is intentionally submitted by You to WHOI for inclusion in, or documentation of, any of the developments, works or products owned or managed by WHOI (the "Work"). For the purposes of this definition, "submitted" means any form of electronic, verbal, or written communication sent to WHOI or its representatives, including but not limited to communication on electronic mailing lists, source code control systems, and issue tracking systems that are managed by, or on behalf of, WHOI for the purpose of discussing and improving the Work, but excluding communication that is conspicuously marked or otherwise designated in writing by You as "Not a Contribution."
+
+## 2. Grant of Copyright License.
+
+Subject to the terms and conditions of this Agreement, You hereby grant to WHOI and to recipients of any Work distributed by WHOI a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable copyright license to reproduce, prepare derivative works of, publicly display, publicly perform, sublicense, and distribute Your Contributions and such derivative works.
+
+## 3. Grant of Patent License.
+
+Subject to the terms and conditions of this Agreement, You hereby grant to WHOI and to recipients of any Work distributed by WHOI a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable (except as stated in this section) patent license to make, have made, use, offer to sell, sell, import, and otherwise transfer the Work, where such license applies only to those patent claims licensable by You that are necessarily infringed by Your Contribution(s) alone or by combination of Your Contribution(s) with the Work to which such Contribution(s) was submitted. If any entity institutes patent litigation against You or any other entity (including a cross-claim or counterclaim in a lawsuit) alleging that your Contribution, or the Work to which you have contributed, constitutes direct or contributory patent infringement, then any patent licenses granted to that entity under this Agreement for that Contribution or Work shall terminate as of the date such litigation is filed.
+
+## 4. Representations By You.
+
+You represent that you are legally entitled to grant the above license. If your employer(s) has rights to intellectual property that you create that includes your Contributions, you represent that you have received permission to make Contributions on behalf of that employer, that your employer has waived such rights for your Contributions to WHOI, or that your employer has executed a separate contributor license agreement with WHOI. You represent that each of Your Contributions is Your original creation (see section 6 for submissions on behalf of others). You represent that Your Contribution submissions include complete details of any third-party license or other restriction (including, but not limited to, related patents and trademarks) of which you are personally aware and which are associated with any part of Your Contributions.
+
+## 5. Support Matters.
+
+You are not expected to provide support for Your Contributions, except to the extent You desire to provide support. You may provide support for free, for a fee (which must be mutually agreed by WHOI), or not at all.
+
+## 6. Disclaimer of Certain Warranties.
+
+UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING (INCLUDING AS AGREED TO IN THIS AGREEMENT), YOU PROVIDE YOUR CONTRIBUTIONS ON AN "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED, INCLUDING, WITHOUT LIMITATION, ANY WARRANTIES OR CONDITIONS OF TITLE, NON- INFRINGEMENT, MERCHANTABILITY, OR FITNESS FOR A PARTICULAR PURPOSE.
+
+## 7. Submitting Material Other Than Your Original Creations.
+
+Should You wish to submit any material that is not Your original creation, You may submit it to WHOI separately from any Contribution, identifying the complete details of its source and of any license or other restriction (including, but not limited to, related patents, trademarks, and license agreements) of which you are personally aware, and conspicuously marking the material as "Submitted on behalf of a third-party: [named here]".
+
+## 8. Notification of Inaccurate Representations.
+
+You agree to notify WHOI of any facts or circumstances of which you become aware that would make any of the representations in this Agreement inaccurate in any respect.
+
diff --git a/cfg/tdma_nav_header.cfg b/cfg/tdma_nav_header.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..c22d442329bfde1d596808d71c9461b5dc330652
--- /dev/null
+++ b/cfg/tdma_nav_header.cfg
@@ -0,0 +1,26 @@
+#! /usr/bin/env python
+import roslib
+from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, str_t, int_t, bool_t, double_t
+
+PACKAGE = 'ros_acomms'
+
+roslib.load_manifest(PACKAGE)
+
+gen = ParameterGenerator()
+
+# Name, Type, Level, Description, Default, Min, Max
+gen.add("nav_duration_seconds", int_t, 0, "Nav portion of TDMA slot in seconds", 15, 0, 14400) # 4 hours max (arbitrary)
+gen.add("nav_guard_time_seconds", int_t, 0, "Nav portion of TDMA slot guard seconds", 5, 0, 1800) # 30 mins max (arbitrary)
+gen.add("transponder_A", bool_t, 0, "Ping transponder A", True)
+gen.add("transponder_B", bool_t, 0, "Ping transponder B", True)
+gen.add("transponder_C", bool_t, 0, "Ping transponder C", True)
+gen.add("transponder_D", bool_t, 0, "Ping transponder D", True)
+gen.add("transponder_reply_timeout_ms", int_t, 0, "Transponder reply timeout ms", 5000, 0, 300000)
+gen.add("ping_modem_dest_id", int_t, 0, "Ping modem dest id", 121, 0, 255)
+gen.add("ping_modem_on", bool_t, 0, "Ping modem at ping_modem_dest_id every nav portion of the TDMA slot", True)
+gen.add("ping_transponders_on", bool_t, 0, "Ping transponders at transponder_group_dest_id every nav portion of the TDMA slot", True)
+
+# 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, "tdma_nav_header", "tdma_nav_header"))
\ No newline at end of file
diff --git a/launch/modem_sim_tdma_nav_header.launch b/launch/modem_sim_tdma_nav_header.launch
new file mode 100644
index 0000000000000000000000000000000000000000..a093433c676f8bf13c4d7136306f5331b38a3944
--- /dev/null
+++ b/launch/modem_sim_tdma_nav_header.launch
@@ -0,0 +1,102 @@
+<launch>
+ <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${time}] [${node}]: ${message}"/>
+
+ <group ns="modem0">
+ <node name="modem_sim_node" pkg="ros_acomms" 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="modem_location_source" value="local_service" />
+ </node>
+
+ <node name="noise_model" pkg="ros_acomms" type="simple_noise_model_node.py" output="screen">
+ <rosparam>
+ mean_noise_db: 63.0
+ noise_stddev_db: 4.0
+ update_every_seconds: 10
+ </rosparam>
+ </node>
+
+ <node name="message_queue" pkg="ros_acomms" type="message_queue_node.py" respawn="true" respawn_delay="30">
+ <rosparam param="packet_codecs" command="load" file="$(dirname)/message_codec_config.yaml" />
+ </node>
+ <node name="packet_dispatch" pkg="ros_acomms" type="packet_dispatch_node.py" respawn="true" respawn_delay="30" >
+ <rosparam param="packet_codecs" command="load" file="$(dirname)/message_codec_config.yaml" />
+ </node>
+ <node name="tdma_nav_header" pkg="ros_acomms" type="tdma_nav_header_node.py" output="screen" respawn="true" respawn_delay="10">
+ <rosparam>
+ active_slots: 0
+ num_slots: 2
+ slot_duration_seconds: 30
+ guard_time_seconds: 3
+ nav_duration_seconds: 15
+ nav_guard_time_seconds: 5
+ packet_length_seconds: 8
+ maximum_miniframe_bytes: 64
+ maximum_dataframe_bytes: 64
+ transponder_group_dest_id: 4
+ ping_modem_dest_id: 1
+ ping_modem_rate: 1
+ ping_modem_on: True
+ ping_transponders_on: False
+ </rosparam>
+ </node>
+
+ </group>
+
+ <group ns="modem1">
+ <node name="modem_sim_node" pkg="ros_acomms" 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="modem_location_source" value="local_service" />
+ </node>
+
+ <node name="noise_model" pkg="ros_acomms" type="simple_noise_model_node.py" output="screen">
+ <rosparam>
+ mean_noise_db: 63.0
+ noise_stddev_db: 4.0
+ update_every_seconds: 10
+ </rosparam>
+ </node>
+
+ <node name="message_queue" pkg="ros_acomms" type="message_queue_node.py" respawn="true" respawn_delay="30">
+ <rosparam param="packet_codecs" command="load" file="$(dirname)/message_codec_config.yaml" />
+ </node>
+ <node name="packet_dispatch" pkg="ros_acomms" type="packet_dispatch_node.py" respawn="true" respawn_delay="30" >
+ <rosparam param="packet_codecs" command="load" file="$(dirname)/message_codec_config.yaml" />
+ </node>
+ <node name="tdma_nav_header" pkg="ros_acomms" type="tdma_nav_header_node.py" output="screen" respawn="true" respawn_delay="10">
+ <rosparam>
+ active_slots: 1
+ num_slots: 2
+ slot_duration_seconds: 30
+ guard_time_seconds: 3
+ nav_duration_seconds: 15
+ nav_guard_time_seconds: 5
+ packet_length_seconds: 8
+ maximum_miniframe_bytes: 64
+ maximum_dataframe_bytes: 64
+ transponder_group_dest_id: 4
+ ping_modem_dest_id: 0
+ ping_modem_rate: 1
+ ping_modem_on: True
+ ping_transponders_on: False
+ </rosparam>
+ </node>
+
+ </group>
+
+ <node name="sim_packet_performance_node" pkg="ros_acomms" type="sim_packet_performance_node.py" output="screen" />
+
+ <node name="platform_location_node" pkg="ros_acomms" type="platform_location_node.py" output="screen" />
+
+ <node name="sim_transmission_loss_node" pkg="ros_acomms" type="sim_transmission_loss_node.py" output="screen" >
+ <param name="water_depth" value="20" type="int" />
+ <param name="bellhop_arrivals" value="false" type="bool" />
+ </node>
+
+ <!-- <node name="test_modem_sim_node" pkg="ros_acomms" type="test_modem_sim_node.py" output="screen" /> -->
+
+
+</launch>
diff --git a/launch/modem_tdma_nav_header.launch b/launch/modem_tdma_nav_header.launch
new file mode 100644
index 0000000000000000000000000000000000000000..453c79dbd798eff20ed7e7b0a4b0624f0a0c5500
--- /dev/null
+++ b/launch/modem_tdma_nav_header.launch
@@ -0,0 +1,95 @@
+<launch>
+ <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${time}] [${node}]: ${message}"/>
+
+ <group ns="modem0">
+ <node name="acomms_driver_node" pkg="ros_acomms" type="acomms_driver_node.py" output="screen" >
+ <param name="modem_serial_port" value="/dev/ttyUSB0" type="str" />
+ <param name="modem_baud_rate" value="19200" type="int" />
+ <param name="pwramp_txlevel" value="3" type="int" />
+ <rosparam param="modem_config">
+ SRC: 0
+ BND: 0
+ FC0: 25000
+ BW0: 5000
+ pwramp.txlevel: 3
+ psk.packet.mod_hdr_version: 1
+ </rosparam>
+ </node>
+
+ <node name="message_queue" pkg="ros_acomms" type="message_queue_node.py" respawn="true" respawn_delay="30">
+ <rosparam param="packet_codecs" command="load" file="$(dirname)/message_codec_config.yaml" />
+ </node>
+ <node name="packet_dispatch" pkg="ros_acomms" type="packet_dispatch_node.py" respawn="true" respawn_delay="30" >
+ <rosparam param="packet_codecs" command="load" file="$(dirname)/message_codec_config.yaml" />
+ </node>
+ <node name="tdma_nav_header" pkg="ros_acomms" type="tdma_nav_header_node.py" output="screen" respawn="true" respawn_delay="10">
+ <rosparam>
+ active_slots: 0
+ num_slots: 2
+ slot_duration_seconds: 30
+ guard_time_seconds: 3
+ nav_duration_seconds: 15
+ nav_guard_time_seconds: 5
+ packet_length_seconds: 8
+ maximum_miniframe_bytes: 64
+ maximum_dataframe_bytes: 64
+ transponder_A: True
+ transponder_B: True
+ transponder_C: True
+ transponder_D: True
+ transponder_reply_timeout_ms: 5000
+ ping_modem_dest_id: 1
+ ping_modem_rate: 1
+ ping_modem_on: False
+ ping_transponders_on: True
+ </rosparam>
+ </node>
+
+ </group>
+
+ <group ns="modem1">
+ <node 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" />
+ <rosparam param="modem_config">
+ SRC: 1
+ BND: 0
+ FC0: 25000
+ BW0: 5000
+ pwramp.txlevel: 3
+ psk.packet.mod_hdr_version: 1
+ </rosparam>
+ </node>
+
+ <node name="message_queue" pkg="ros_acomms" type="message_queue_node.py" respawn="true" respawn_delay="30">
+ <rosparam param="packet_codecs" command="load" file="$(dirname)/message_codec_config.yaml" />
+ </node>
+ <node name="packet_dispatch" pkg="ros_acomms" type="packet_dispatch_node.py" respawn="true" respawn_delay="30" >
+ <rosparam param="packet_codecs" command="load" file="$(dirname)/message_codec_config.yaml" />
+ </node>
+ <node name="tdma_nav_header" pkg="ros_acomms" type="tdma_nav_header_node.py" output="screen" respawn="true" respawn_delay="10">
+ <rosparam>
+ active_slots: 1
+ num_slots: 2
+ slot_duration_seconds: 30
+ guard_time_seconds: 3
+ nav_duration_seconds: 15
+ nav_guard_time_seconds: 5
+ packet_length_seconds: 8
+ maximum_miniframe_bytes: 64
+ maximum_dataframe_bytes: 64
+ transponder_group_dest_id: 4
+ ping_modem_dest_id: 0
+ ping_modem_rate: 1
+ ping_modem_on: True
+ ping_transponders_on: False
+ </rosparam>
+ </node>
+
+ </group>
+
+ <!-- <node name="test_modem_sim_node" pkg="ros_acomms" type="test_modem_sim_node.py" output="screen" /> -->
+
+
+</launch>
diff --git a/msg/CallStatus.msg b/msg/CallStatus.msg
deleted file mode 100644
index 7e2b55a3c7fa5f730531dd6142edca4a36285617..0000000000000000000000000000000000000000
--- a/msg/CallStatus.msg
+++ /dev/null
@@ -1,8 +0,0 @@
-# Queue type constants:
-#################
-
-int8 CALL_UP=1
-int8 CALL_DOWN=2
-
-# Call status
-int8 call_status
\ No newline at end of file
diff --git a/msg/FragmentationStatus.msg b/msg/FragmentationStatus.msg
index 1d44a7e198d82eb51e55a173b45b004df2e910b9..299d71bf95e255983f047d87ef057be1299b6988 100644
--- a/msg/FragmentationStatus.msg
+++ b/msg/FragmentationStatus.msg
@@ -1,6 +1,6 @@
Header header
-uint8 sequence_num
+uint32 sequence_num
uint8 fragment_src
uint8 fragment_dest
uint32 unix_time
diff --git a/msg/PingTranspondersReply.msg b/msg/PingTranspondersReply.msg
new file mode 100644
index 0000000000000000000000000000000000000000..30658f1b15a6bd0dab284e95081dd7f760230636
--- /dev/null
+++ b/msg/PingTranspondersReply.msg
@@ -0,0 +1,11 @@
+Header header
+
+bool[] transponder_dest_mask
+uint16 timeout_ms
+
+time time_of_ping
+
+float32 owtt_a
+float32 owtt_b
+float32 owtt_c
+float32 owtt_d
\ No newline at end of file
diff --git a/msg/ReceivedIridiumPacket.msg b/msg/ReceivedIridiumPacket.msg
deleted file mode 100644
index 322bc4a66a04f49d68c26f154e560badf3a98d2a..0000000000000000000000000000000000000000
--- a/msg/ReceivedIridiumPacket.msg
+++ /dev/null
@@ -1,2 +0,0 @@
-Header header
-IridiumPacket iridiumpacket
\ No newline at end of file
diff --git a/msg/TdmaNavStatus.msg b/msg/TdmaNavStatus.msg
new file mode 100644
index 0000000000000000000000000000000000000000..a446f98510f96ec7f44ebd2eddd0e90127bfce44
--- /dev/null
+++ b/msg/TdmaNavStatus.msg
@@ -0,0 +1,12 @@
+#we_are_active, current_slot, remaining_slot_seconds, remaining_active_seconds, time_to_next_active
+
+Header header
+
+int8 current_slot
+bool we_are_active
+bool is_nav_portion
+float32 remaining_nav_seconds
+float32 remaining_slot_seconds
+float32 remaining_active_seconds
+float32 time_to_next_active
+string tdma_nav_mode
\ No newline at end of file
diff --git a/msg/UpdateQueuedMessage.msg b/msg/UpdateQueuedMessage.msg
new file mode 100644
index 0000000000000000000000000000000000000000..c5b0257d3b16740f033d60cbaba17d9cfe6cb63b
--- /dev/null
+++ b/msg/UpdateQueuedMessage.msg
@@ -0,0 +1,8 @@
+Header header
+
+int32 message_id
+
+int8 EVENT_LINK_DEQUEUED=0
+int8 EVENT_LINK_QUEUED=1
+int8 EVENT_LINK_TRANSMITTED=2
+int8 event
diff --git a/requirements.txt b/requirements.txt
index 5b2a1a573d648cbc6ae668e3dfd0a698fb8c22fe..34d8d014cb14a3d1c1ef5ae86117371f4a64cc92 100755
--- a/requirements.txt
+++ b/requirements.txt
@@ -13,4 +13,6 @@ msgpack
ltcodecs
whoi-gitver
acomms
-importlib-metadata
\ No newline at end of file
+importlib-metadata
+pyyaml
+pyyaml-include
diff --git a/src/acomms_codecs/ccl_packet_codec.py b/src/acomms_codecs/ccl_packet_codec.py
index 4ce6228fbc66b26b7a15a1e3da22f37d1858d5e6..2f8e6ec108082814124d88a19a2f07ff32b74df8 100644
--- a/src/acomms_codecs/ccl_packet_codec.py
+++ b/src/acomms_codecs/ccl_packet_codec.py
@@ -29,8 +29,8 @@ class CclPacketCodec(PacketCodec):
dataframe_bits = BitArray()
# CCL is easy, especially as we do it here: one message per packet, always a dataframe.
- next_message = message_queue.get_next_message_smaller_than_bits(
- size_in_bits=remaining_dataframe_bits,
+ next_message = message_queue.get_next_message(
+ max_size_in_bits=remaining_dataframe_bits,
packet_codec=self,
)
@@ -39,7 +39,7 @@ class CclPacketCodec(PacketCodec):
message_queue.mark_transmitted(next_message)
- message_header = next_message.queue.header
+ message_header = next_message.queue.message_codec_id
message_bits = next_message.payload_bits
messages_in_packet = [next_message]
@@ -66,7 +66,14 @@ class CclPacketCodec(PacketCodec):
id = payload_bits.read('uint:8')
ros_msg = self.message_codecs[id].decode(payload_bits)
- self.message_publishers[id].publish(ros_msg)
+ if isinstance(self.message_publishers[id], rospy.Publisher):
+ self.message_publishers[id].publish(ros_msg)
+ else:
+ try:
+ self.message_publishers[id][received_packet.packet.src].publish(ros_msg)
+ except KeyError:
+ rospy.loginfo("Got a message from an un-named SRC {}, not publishing it".format(
+ received_packet.packet.src))
except KeyError:
# We don't have a config entry that matches this ID
rospy.loginfo("Unknown message ID ({}) received, unable to continue parsing packet".format(id))
diff --git a/src/acomms_codecs/packet_codec.py b/src/acomms_codecs/packet_codec.py
index a1a0289a5df732418bfd719e3a3df74ecc5462c5..e633e369151679f49b0b1c44b09414c4dcf47a8f 100644
--- a/src/acomms_codecs/packet_codec.py
+++ b/src/acomms_codecs/packet_codec.py
@@ -22,7 +22,7 @@ class PacketCodec(ABC):
return True
@abstractmethod
- def encode_payload(self, num_miniframe_bytes, num_dataframe_bytes, message_queue):
+ def encode_payload(self, num_miniframe_bytes: int, num_dataframe_bytes: int, message_queue) -> (int, bytes, list):
pass
@abstractmethod
diff --git a/src/acomms_codecs/ros_packet_codec.py b/src/acomms_codecs/ros_packet_codec.py
index f8cb9f9663ac362cfe9476cdba21dd815af73dc0..4738d5c3afd59c2ac1c7c72d10eb9bc46bf7da4c 100644
--- a/src/acomms_codecs/ros_packet_codec.py
+++ b/src/acomms_codecs/ros_packet_codec.py
@@ -1,6 +1,8 @@
+from typing import Dict, Optional, Any
+
import rospy
from ros_acomms.msg import ReceivedPacket, Packet, EncodedAck
-from bitstring import ConstBitStream, Bits, BitArray, ReadError
+from bitstring import ConstBitStream, Bits, BitArray, ReadError, BitStream
from .packet_codec import PacketCodec
from fragmentation_tracker import FragmentationStartHeader, FragmentationContHeader, FragmentationAckHeader, FragmentationTracker
import hashlib
@@ -11,9 +13,9 @@ import hashlib
class RosPacketCodec(PacketCodec):
- def __init__(self, message_codecs, message_publishers=None,
+ def __init__(self, message_codecs: Dict, message_publishers=None,
allow_fragmentation=True, minimum_fragment_size_bits=12*8,
- sequence_number_bits=16,
+ sequence_number_bits=8,
miniframe_header=[0xac], dataframe_header=[]):
super(RosPacketCodec, self).__init__(message_codecs, message_publishers=message_publishers,
miniframe_header=miniframe_header, dataframe_header=dataframe_header)
@@ -57,9 +59,10 @@ class RosPacketCodec(PacketCodec):
# todo: fragmentation, acks
# the packet codec is responsible for adding message headers, to allow flexiblity for sequence
# numbers, acks, etc.
- next_message = message_queue.get_next_message_smaller_than_bits(
- size_in_bits=total_remaining_bits-self.header_size_bits,
- packet_codec=self)
+ next_message = message_queue.get_next_message(
+ max_size_in_bits=total_remaining_bits - self.header_size_bits,
+ packet_codec=self,
+ exclude_messages=messages_in_packet)
if not next_message:
break
@@ -67,7 +70,8 @@ class RosPacketCodec(PacketCodec):
message_header, message_bits = next_message.get_next_payload_smaller_than_bits(
size_in_bits=total_remaining_bits - self.header_size_bits,
dest_sequence_nums=message_queue.dest_sequence_num,
- remove_from_queue=True)
+ remove_from_queue=True,
+ )
messages_in_packet.append(next_message)
@@ -84,7 +88,7 @@ class RosPacketCodec(PacketCodec):
return messages_in_packet, miniframe_bits.tobytes(), dataframe_bits.tobytes()
- def decode_packet(self, received_packet: ReceivedPacket):
+ def decode_packet(self, received_packet: ReceivedPacket) -> None:
if not self.message_publishers:
# This codec is encode-only
return
@@ -97,12 +101,27 @@ class RosPacketCodec(PacketCodec):
payload_bytes = miniframe_bytes + dataframe_bytes
payload_bits = ConstBitStream(bytes=payload_bytes)
+ self.decode_payload(payload_bits, received_packet=received_packet)
+
+ def decode_payload(self, payload_bits: BitStream, received_packet: Optional[ReceivedPacket] = None,
+ src: Optional[int] = None,
+ set_fields: Dict[str, Any] = {}) -> None:
+ if not self.message_publishers:
+ # This codec is encode-only
+ return
+
# the format of a ros acomms packet is:
# ros_acomms header (already stripped)
# repeat:
# message header (id), uint8 (eventually extend this as a varint)
# message bits
+ # Get the SRC for this packet, which can be set explictly or in the received_packet
+ if src is None:
+ #rospy.logwarn("missing SRC")
+ if received_packet:
+ src = received_packet.packet.src
+
# loop through until we are out of bits (ReadError will be raised)
try:
while True:
@@ -115,10 +134,22 @@ class RosPacketCodec(PacketCodec):
if id < 128:
message_codec = self.message_codecs[id]
rospy.loginfo("ROS Message decode: ID {} with {}".format(id, message_codec))
- rospy.loginfo(payload_bits)
+ rospy.logdebug(payload_bits)
ros_msg = self.message_codecs[id].decode(payload_bits, received_packet)
+ for field, value in set_fields.items():
+ setattr(ros_msg, field, value)
try:
- self.message_publishers[id].publish(ros_msg)
+ if isinstance(self.message_publishers[id], rospy.Publisher):
+ self.message_publishers[id].publish(ros_msg)
+ else:
+ if src is not None:
+ try:
+ self.message_publishers[id][src].publish(ros_msg)
+ except KeyError:
+ rospy.loginfo("Got a message from an un-named SRC {}, not publishing it".format(
+ src))
+ else:
+ rospy.logwarn("Got a message that needs metadata to publish, but no metadata was supplied")
rospy.logdebug("Published msg id: {0}".format(id))
except Exception as e:
rospy.logerr("Error publishing ROS message: {}".format(e))
@@ -207,8 +238,6 @@ class RosPacketCodec(PacketCodec):
rospy.logwarn(
"Fragmented packets out of sync. Receiving continuation headers but do not have start header.")
-
-
else:
# Ack...
pass
@@ -230,7 +259,13 @@ class RosPacketCodec(PacketCodec):
rospy.loginfo("ROS Message decode: ID {} with {}".format(id % 128, message_codec))
ros_msg = self.message_codecs[id % 128].decode(ConstBitStream(incoming.payload_bits), received_packet)
- self.message_publishers[id % 128].publish(ros_msg)
+ if isinstance(self.message_publishers[id % 128], rospy.Publisher):
+ self.message_publishers[id % 128].publish(ros_msg)
+ else:
+ try:
+ self.message_publishers[id % 128][received_packet.packet.src].publish(ros_msg)
+ except KeyError:
+ rospy.loginfo("Got a message from an un-named SRC {}, not publishing it".format(received_packet.packet.src))
#TODO: Keep a record of published messages for now but eventually we need to clean them up
#self.incoming_fragmented_messages.pop((incoming.fragment_src, incoming.sequence_num))
@@ -238,7 +273,6 @@ class RosPacketCodec(PacketCodec):
else:
rospy.logdebug("Received repeat fragment for already published message.")
-
except KeyError:
# We don't have a config entry that matches this ID
rospy.loginfo("Unknown message ID ({}) received, unable to continue parsing packet".format(id))
diff --git a/src/acomms_driver_node.py b/src/acomms_driver_node.py
index e17c0a6e9c5683e51b9d955934e04f54118d1758..4abfc9fd81565ac943e7958189682cc08f6f5315 100755
--- a/src/acomms_driver_node.py
+++ b/src/acomms_driver_node.py
@@ -1,9 +1,9 @@
#!/usr/bin/env python3
from __future__ import unicode_literals
-from queue import Queue
+from queue import Queue, Empty
from threading import Thread
-from time import sleep
+from time import sleep, time
from acomms.unifiedlog import UnifiedLog
from acomms.micromodem import Micromodem
@@ -14,8 +14,9 @@ import rospy
from std_msgs.msg import Header, String
from dynamic_reconfigure.server import Server as DynamicReconfigureServer
from ros_acomms.msg import XST, SST
-from ros_acomms.msg import CST, ReceivedPacket, Packet, PingReply, Source
+from ros_acomms.msg import CST, ReceivedPacket, Packet, PingReply, PingTranspondersReply, Source
from ros_acomms.srv import PingModem, PingModemResponse
+from ros_acomms.srv import PingTransponders, PingTranspondersResponse
from ros_acomms.srv import QueueTxPacket, QueueTxPacketResponse
from ros_acomms.srv import TxInhibit, TxInhibitResponse
from ros_acomms.srv import WriteNmeaString, WriteNmeaStringResponse
@@ -59,6 +60,7 @@ class AcommsDriverNode(object):
self.packet_rx_publisher = rospy.Publisher('packet_rx', ReceivedPacket, queue_size=10)
self.nmea_publisher = rospy.Publisher('nmea_from_modem', String, queue_size=10)
self.ping_reply_publisher = rospy.Publisher('ping_reply', PingReply, queue_size=10)
+ self.ping_transponders_reply_publisher = rospy.Publisher('ping_transponders_reply', PingTranspondersReply, queue_size=10)
self.txf_publisher = rospy.Publisher('txf', Header, queue_size=10)
if nodestatusavail:
self.pub_nodestatus = rospy.Publisher(rospy.names.canonicalize_name('/node_status/' + rospy.get_name()),
@@ -149,6 +151,7 @@ class AcommsDriverNode(object):
rospy.Subscriber('nmea_to_modem', String, self.on_nmea_subscriber)
self.ping_modem_service = rospy.Service('ping_modem', PingModem, self.handle_ping_modem)
+ self.ping_transponders_service = rospy.Service('ping_transponders', PingTransponders, self.handle_ping_transponders)
self.queue_tx_packet_service = rospy.Service('queue_tx_packet', QueueTxPacket, self.handle_queue_tx_packet)
self.tx_inhibit_service = rospy.Service('tx_inhibit', TxInhibit, self.handle_tx_inhibit)
self.write_nmea_string_service = rospy.Service('write_nmea_string', WriteNmeaString, self.handle_write_nmea)
@@ -275,6 +278,73 @@ class AcommsDriverNode(object):
except Exception as e:
rospy.logerr_throttle(1, "Error in NMEA RX handler: {}".format(e))
+ def handle_ping_transponders(self, request):
+ rospy.loginfo("Requesting modem send transponder ping")
+
+ # request.transponder_dest_mask = request.transponder_dest_mask.decode('utf-8')
+
+ params = [str(1), str(1), str(0), str(0),
+ str(request.timeout_ms),
+ str(1 if request.transponder_dest_mask[0] else 0),
+ str(1 if request.transponder_dest_mask[1] else 0),
+ str(1 if request.transponder_dest_mask[2] else 0),
+ str(1 if request.transponder_dest_mask[3] else 0)
+ ]
+ msg = {'type': 'CCPDT', 'params': params}
+
+ incoming_msg_queue = Queue()
+ self.um.attach_incoming_msg_queue(incoming_msg_queue)
+ rospy.sleep(0.2)
+ self.um.write_nmea(msg)
+
+ matching_msg = None
+ remaining_time = request.timeout_ms / 1000
+ end_time = time() + remaining_time
+
+ while remaining_time > 0:
+ try:
+ new_msg = incoming_msg_queue.get(timeout=remaining_time)
+ if new_msg['type'] in 'SNTTA':
+ matching_msg = new_msg
+ break
+ else:
+ remaining_time = end_time - time()
+ continue
+ except Empty:
+ break
+ sntta = matching_msg
+
+ self.um.detach_incoming_msg_queue(incoming_msg_queue)
+
+ if sntta:
+ rospy.loginfo(f'Response from transponders: {sntta}')
+ time_of_ping = datetime.strptime(datetime.now().strftime('%Y%m%d') + sntta['params'][4], '%Y%m%d%H%M%S.%f')
+ msg = PingTranspondersReply(
+ transponder_dest_mask=request.transponder_dest_mask, #.encode('utf-8'),
+ timeout_ms=request.timeout_ms,
+ owtt_a=float(sntta['params'][0]) if sntta['params'][0] != '' else -1.0,
+ owtt_b=float(sntta['params'][1]) if sntta['params'][1] != '' else -1.0,
+ owtt_c=float(sntta['params'][2]) if sntta['params'][2] != '' else -1.0,
+ owtt_d=float(sntta['params'][3]) if sntta['params'][3] != '' else -1.0,
+ time_of_ping=convert_datetime_to_rospy(time_of_ping),
+ )
+ msg.header.stamp = convert_datetime_to_rospy(time_of_ping)
+ self.ping_transponders_reply_publisher.publish(msg)
+
+ response = PingTranspondersResponse(travel_times=[
+ float(sntta['params'][0]) if sntta['params'][0] != '' else -1.0,
+ float(sntta['params'][1]) if sntta['params'][1] != '' else -1.0,
+ float(sntta['params'][2]) if sntta['params'][2] != '' else -1.0,
+ float(sntta['params'][3]) if sntta['params'][3] != '' else -1.0
+ ])
+ else:
+ rospy.loginfo(f'timed-out before SNTTA....')
+ response = PingTranspondersResponse(travel_times=[-1.0, -1.0, -1.0, -1.0])
+
+ rospy.loginfo(f'Service call response: {response}')
+
+ return response
+
def handle_ping_modem(self, request):
rospy.loginfo("Requesting modem send ping")
self.um.send_fdp_ping(request.dest, request.rate, request.cdr, request.hexdata)
diff --git a/src/check_codec_config.py b/src/check_codec_config.py
new file mode 100644
index 0000000000000000000000000000000000000000..1fdb6c2276cb931b8f025ae54fd77c39e5e02c66
--- /dev/null
+++ b/src/check_codec_config.py
@@ -0,0 +1,73 @@
+#! /usr/bin/python3
+
+import yaml
+from codec_config_parser.config_parser import get_message_codecs, get_queue_params
+from typing import Dict, List
+from acomms_codecs.packet_codecs import packet_codecs
+from ltcodecs import RosMessageCodec
+import rospy
+import argparse
+from packet_dispatch_node import DecoderListEntry
+
+
+def check_codec_config_yaml(yaml_file_name):
+ with open(yaml_file_name, 'r') as yaml_file:
+ packet_codec_param = yaml.load(yaml_file)
+
+ decoder_list: List[DecoderListEntry] = []
+ for packet_codec in packet_codec_param:
+ message_codecs: Dict[int, RosMessageCodec] = get_message_codecs(packet_codec)
+ queue_params = get_queue_params(packet_codec)
+ # Convert SRC IDs to integers, since yaml dict keys have to be strings
+ try:
+ src_names: dict = {int(k):v for k,v in packet_codec.get('src_names', {}).items()}
+ print(f"Names: {src_names}")
+ except Exception as e:
+ print(f"Error parsing src_names parameter: {e}")
+
+ pub_names = []
+
+ for queue_id, params in queue_params.items():
+ pub_name = rospy.names.canonicalize_name(params.publish_topic)
+ codec = message_codecs[queue_id]
+ min_size = codec.min_size_bits
+ max_size = codec.max_size_bits
+
+ print(f"ID: {queue_id}\t{pub_name}\t{params.msg_class}\t{min_size}\t{max_size}")
+
+ if ("{src}" in params.publish_topic) or ("{name}" in params.publish_topic):
+ # In this case, we will have a dict of message publishers that correspond to different SRC IDs
+ for src, name in src_names.items():
+ pub_name = params.publish_topic.format(src=src, name=name)
+ pub_name = rospy.names.canonicalize_name(pub_name)
+ pub_names.append(pub_name)
+ else:
+ pub_name = rospy.names.canonicalize_name(params.publish_topic)
+ pub_names.append(pub_name)
+
+ print(f"Publishers:\n{pub_names}")
+
+ packet_codec.pop('message_codecs')
+ try:
+ packet_codec.pop('src_names')
+ except:
+ pass
+
+ entry = DecoderListEntry(**packet_codec)
+ decoder_list.append(entry)
+ rospy.loginfo("Added decoder: {}".format(entry))
+
+ # replace name with instance of packet codec
+ # ToDO make this less ugly and stupid
+ entry.packet_codec = packet_codecs[entry.packet_codec](message_codecs,
+ message_publishers=[],
+ miniframe_header=entry.miniframe_header,
+ dataframe_header=entry.dataframe_header)
+
+if __name__ == '__main__':
+ ap = argparse.ArgumentParser(description='check message codec config')
+ ap.add_argument("file_name", help="Path to yaml file")
+
+ args = ap.parse_args()
+
+ check_codec_config_yaml(args.file_name)
\ No newline at end of file
diff --git a/src/codec_config_parser/config_parser.py b/src/codec_config_parser/config_parser.py
index d6fae8bee0921b498d16cfef5d41fe09e5e9a38a..525747d07268bb687145ffcf6102718f8d9b7fb6 100644
--- a/src/codec_config_parser/config_parser.py
+++ b/src/codec_config_parser/config_parser.py
@@ -1,7 +1,7 @@
from dataclasses import dataclass
from ltcodecs import RosMessageCodec
import roslib
-from typing import Dict, NewType
+from typing import Dict, NewType, Optional
QueueId = NewType('QueueId', int)
MessageCodecId = NewType('MessageCodecId', int)
@@ -9,8 +9,10 @@ MessageCodecId = NewType('MessageCodecId', int)
@dataclass
class QueueParams:
msg_class: type
- subscribe_topic: str
- publish_topic: str
+ dynamic_queue_service: Optional[str]
+ dynamic_update_topic: Optional[str]
+ subscribe_topic: Optional[str]
+ publish_topic: Optional[str]
codec_id: MessageCodecId
queue_order: str # TODO: I'd like this to be an enum corresponding to 'fifo', 'lifo'
queue_maxsize: int
@@ -20,7 +22,7 @@ class QueueParams:
allow_fragmentation: bool
-def get_queue_params(packet_codec_param):
+def get_queue_params(packet_codec_param: Dict) -> Dict[int, QueueParams]:
# Queues are indexed by their ID because that's what goes in the header
# to identify the appropriate publication parameters for packet dispatch
queue_params: Dict[QueueId, QueueParams] = {}
@@ -35,10 +37,12 @@ def get_queue_params(packet_codec_param):
raise TypeError('Invalid ROS type "{}" for codec ID {}'
.format(ros_type_name, both_ids))
queue_params[both_ids] = QueueParams(msg_class,
- message_codec_param["subscribe_topic"],
- message_codec_param["publish_topic"],
+ message_codec_param.get('dynamic_queue_service', None),
+ message_codec_param.get('dynamic_update_topic', None),
+ message_codec_param.get("subscribe_topic", None),
+ message_codec_param.get("publish_topic", None),
both_ids,
- message_codec_param["queue_order"],
+ message_codec_param.get("queue_order", 'fifo'),
message_codec_param["queue_maxsize"],
message_codec_param["priority"], # NB: current implementation MessageQueueNode has a default priority
message_codec_param.get("is_active", True),
@@ -67,7 +71,9 @@ def get_message_codecs(packet_codec_param):
if not msg_class:
raise TypeError('Invalid ROS type "{}" for codec ID {}'
.format(ros_type_name, both_ids))
- message_codecs[both_ids] = RosMessageCodec(ros_type=msg_class, fields_dict=message_codec_param['fields'])
+ # If there is no fields dictionary, allow the codec to determine the fields through introspection.
+ fields = message_codec_param.get('fields', None)
+ message_codecs[both_ids] = RosMessageCodec(ros_type=msg_class, fields_dict=fields)
else:
# Use new version that separates the queue and codec definitions.
raise(Exception("NYI"))
diff --git a/src/fragmentation_tracker.py b/src/fragmentation_tracker.py
index b7b8b49914d90b943a26701bede917049bf20549..e020c5b4eb256355360d6934b3df3a9684b27afa 100755
--- a/src/fragmentation_tracker.py
+++ b/src/fragmentation_tracker.py
@@ -6,7 +6,7 @@ from std_msgs.msg import Header
from ros_acomms.msg import FragmentationStatus
class FragmentationStartHeader(object):
- length_bits = 120
+ length_bits = 128
def __init__(self,
pid,
@@ -28,7 +28,7 @@ class FragmentationStartHeader(object):
def from_bits(cls, pid, headerbits: BitStream):
parsed_id = headerbits.read('uint:8')
- sequence_num = headerbits.read('uint:8')
+ sequence_num = headerbits.read('uint:16')
fragment_src = headerbits.read('uint:8')
fragment_dest = headerbits.read('uint:8')
unix_time = headerbits.read('uint:32')
@@ -51,7 +51,7 @@ class FragmentationStartHeader(object):
headerbits = BitStream()
headerbits.append(Bits(uint=self.pid, length=8))
- headerbits.append(Bits(uint=self.sequence_num, length=8))
+ headerbits.append(Bits(uint=self.sequence_num, length=16))
headerbits.append(Bits(uint=self.fragment_src, length=8))
headerbits.append(Bits(uint=self.fragment_dest, length=8))
headerbits.append(Bits(uint=self.unix_time, length=32))
@@ -63,7 +63,7 @@ class FragmentationStartHeader(object):
return headerbits
class FragmentationContHeader(object):
- length_bits = 48
+ length_bits = 56
def __init__(self,
pid,
@@ -77,7 +77,7 @@ class FragmentationContHeader(object):
@classmethod
def from_bits(cls, pid, headerbits: BitStream):
parsed_id = headerbits.read('uint:8')
- sequence_num = headerbits.read('uint:8')
+ sequence_num = headerbits.read('uint:16')
start_fragment_index = headerbits.read('uint:16')
end_fragment_index = headerbits.read('uint:16')
@@ -93,7 +93,7 @@ class FragmentationContHeader(object):
headerbits = BitStream()
headerbits.append(Bits(uint=self.pid, length=8))
- headerbits.append(Bits(uint=self.sequence_num, length=8))
+ headerbits.append(Bits(uint=self.sequence_num, length=16))
headerbits.append(Bits(uint=self.start_fragment_index, length=16))
headerbits.append(Bits(uint=self.end_fragment_index, length=16))
@@ -115,7 +115,7 @@ class FragmentationAckHeader(object):
def from_bits(cls, headerbits: BitStream):
ack_flag = headerbits.read('bool:1')
n_blocks = headerbits.read('uint:7')
- sequence_num = headerbits.read('uint:8')
+ sequence_num = headerbits.read('uint:16')
blocks = IndexList()
for n in range(n_blocks):
@@ -134,7 +134,7 @@ class FragmentationAckHeader(object):
headerbits.append(Bits(bool=self.ack_flag))
headerbits.append(Bits(uint=self.n_blocks, length=7))
- headerbits.append(Bits(uint=self.sequence_num, length=8))
+ headerbits.append(Bits(uint=self.sequence_num, length=16))
for n in range(self.n_blocks):
headerbits.append(Bits(uint=self.blocks[n][0], length=16))
diff --git a/src/legacy_translator_node.py b/src/legacy_translator_node.py
index ac8be17748a28b10f9db68e5a4abe46700d45423..5a88693b4ad20d6f955f72c5164847e2de9f55d0 100755
--- a/src/legacy_translator_node.py
+++ b/src/legacy_translator_node.py
@@ -146,6 +146,7 @@ class LegacyTranslatorNode(object):
remote_address = rospy.get_param('~remote_address', '10.11.62.175')
remote_port = rospy.get_param('~remote_port', 4002)
local_port = rospy.get_param('~local_port', 4002)
+ vehicle_namespace = rospy.get_param('~vehicle_namespace', '/from_acomms')
self.rxd_src = rospy.get_param('~rxd_src', 8)
self.nmea_to_modem_publisher = rospy.Publisher('nmea_to_modem', String, queue_size=10)
@@ -156,9 +157,9 @@ class LegacyTranslatorNode(object):
self.last_motherboard = None
rospy.Subscriber('nmea_from_modem', String, self.on_nmea_from_modem)
- rospy.Subscriber('/from_acomms/status', Status, self.on_vehicle_status)
- rospy.Subscriber('/from_acomms/battery_state', BatteryState, self.on_battery_state)
- rospy.Subscriber('/from_acomms/motherboard', Motherboard, self.on_motherboard)
+ rospy.Subscriber(vehicle_namespace + '/status', Status, self.on_vehicle_status)
+ rospy.Subscriber(vehicle_namespace + '/battery_state', BatteryState, self.on_battery_state)
+ rospy.Subscriber(vehicle_namespace + '/motherboard', Motherboard, self.on_motherboard)
rospy.spin()
def on_nmea_from_modem(self, ros_msg):
diff --git a/src/message_queue_node.py b/src/message_queue_node.py
index fd716ec5be6168f4bda47aca4b05e303fa322f17..af9266de2781afd18ec33b48dd176ca8db24fb3c 100755
--- a/src/message_queue_node.py
+++ b/src/message_queue_node.py
@@ -13,15 +13,19 @@ from collections import namedtuple, deque, defaultdict
from dataclasses import dataclass
from datetime import datetime, timedelta
from ros_acomms.srv import GetNextPacketData, GetNextPacketDataRequest, GetNextPacketDataResponse
+from ros_acomms.srv import GetNextQueuedMessage, GetNextQueuedMessageRequest, GetNextQueuedMessageResponse
+from ros_acomms.msg import UpdateQueuedMessage
from ros_acomms.msg import (EncodedAck, NeighborReachability, QueueEnable,
QueuePriority, QueueStatus, QueueSummary, SummaryMessageCount)
-from bitstring import Bits, BitStream
+from bitstring import Bits, BitStream, BitArray
from fragmentation_tracker import (FragmentationTracker, FragmentationStartHeader,
FragmentationContHeader, FragmentationAckHeader)
from ltcodecs import RosMessageCodec
from acomms_codecs.ros_packet_codec import RosPacketCodec
from acomms_codecs.packet_codecs import packet_codecs
+import yaml
+from yamlinclude import YamlIncludeConstructor
from version_node import version_node
@@ -32,6 +36,17 @@ from codec_config_parser import (QueueParams, QueueId, MessageCodecId,
get_queue_params, get_message_codecs)
+@dataclass
+class DynamicQueue:
+ dest_address: int
+ message_codec_id: int # the ID from the message codec params
+ message_codec: RosMessageCodec
+ dynamic_message_service: str
+ packet_codec: RosPacketCodec = None
+ allow_fragmentation: bool = False
+ dynamic_update_topic: Optional[str] = None
+
+
@dataclass
class QueuedMessage:
queue: Any # Should be MessageQueue, but that's not defined yet.
@@ -41,13 +56,15 @@ class QueuedMessage:
payload_bits: Bits
length_bits: int
time_added: datetime
- transmitted: bool
- next_hop_acked: bool
priority: int
fragmentation_tracker: Optional[FragmentationTracker]
+ message_id_in_queue: Optional[int] = None
+ transmitted: bool = False
+ next_hop_acked: bool = False
sequence_number: Optional[int] = None
allow_fragmentation: bool = False
fragment_size_bits: int = 8
+ dynamic_queue_name: Optional[str] = None
def get_min_fragment_size_bits(self) -> int:
if self.fragmentation_tracker is None:
@@ -72,7 +89,8 @@ class QueuedMessage:
if self.length_bits < size_in_bits:
message_bits = self.payload_bits
- message_header = self.queue.header
+ message_header = self.queue.message_codec_id
+
else:
fragmented = True
rospy.logdebug("Fragmenting...")
@@ -104,20 +122,21 @@ class QueuedMessage:
rospy.logdebug("No acked bytes yet")
message_bits = self.fragmentation_tracker.next_fragment_to_transfer(size_in_bits)
- message_header = self.queue.header + 128
+ message_header = self.queue.message_codec_id + 128
rospy.logdebug("Returning {0} of {1} bits".format(message_bits.length, self.payload_bits.length))
- if remove_from_queue and not fragmented:
+ if remove_from_queue and not fragmented and isinstance(self.queue, MessageQueue):
self.queue.mark_transmitted(self)
return message_header, message_bits
+
class MessageQueue(object):
def __init__(self,
master_message_queue: List[QueuedMessage],
dest_address: int,
# QUESTION(lindzey): Can we rename this to queue_id for clarity?
- header: int, # the ID from the message codec params
+ message_codec_id: int, # the ID from the message codec params
topic: str,
message_codec: RosMessageCodec,
ack_next_hop: bool = False,
@@ -125,19 +144,19 @@ class MessageQueue(object):
priority: int = 1,
order: str = 'lifo',
maxsize: int = 100,
- packet_codec: RosPacketCodec = None,
allow_fragmentation: bool = False,
+ dynamic_message_service: Optional[str] = None,
roundrobin_tx=False) -> None:
self.master_message_queue = master_message_queue
+ self.dynamic_message_service = dynamic_message_service
# Acks are TODO
self.ack_next_hop: bool = ack_next_hop
self.ack_end_to_end: bool = ack_end_to_end
self.order = order
self.dest_address: int = dest_address
self.priority: int = priority
- self.header = header
+ self.message_codec_id = message_codec_id
self.subscribe_topic: str = topic
- self.packet_codec = packet_codec
self.message_codec = message_codec
self.allow_fragmentation: bool = allow_fragmentation
@@ -261,6 +280,11 @@ class MessageQueueNode(object):
except:
rospy.logwarn("Unable to query version information")
+ # Init dynamic queues
+ self.dynamic_queues: List[DynamicQueue] = []
+ self.dynamic_get_next_message_services = {}
+ self.dynamic_queue_update_publishers = {}
+
# This needs to be called before any subscribers are created
self.setup_queues()
@@ -284,8 +308,6 @@ class MessageQueueNode(object):
self.spin()
def setup_queues(self) -> None:
- # QUESTION: It looks like self.queues isn't ever used, and topic_queue takes its place?
- self.queues = {} # type: Dict[str, MessageQueue]
self.topic_queue = {}
self.topic_headers = {}
@@ -300,7 +322,14 @@ class MessageQueueNode(object):
# this is a little awkward, since it's conceptually backward here.
# when queuing for TX, we go from topic -> message -> packet
# However, organizing it this way allows us to use a single config YAML for both TX and RX
- packet_codec_param = rospy.get_param('~packet_codecs')
+ packet_codec_filename = rospy.get_param('~packet_codec_file', None)
+ codec_dir = rospy.get_param('~codec_directory', 'codecs')
+ if packet_codec_filename:
+ with open(packet_codec_filename, 'r') as f:
+ YamlIncludeConstructor.add_to_loader_class(loader_class=yaml.FullLoader, base_dir=codec_dir)
+ packet_codec_param = yaml.full_load(f)
+ else:
+ packet_codec_param = rospy.get_param('~packet_codecs')
# QUESTION(lindzey): Do we ever have more than one packet codec? I'm
# not convinced that the code in this block actually handles that case
@@ -311,34 +340,95 @@ class MessageQueueNode(object):
self.queue_params: Dict[int, QueueParams] = {}
for packet_codec_details in packet_codec_param:
rospy.loginfo("Packet codec details: {}".format(packet_codec_details))
- packet_codec_class = packet_codecs[packet_codec_details['packet_codec']] # type: PacketCodec
+ packet_codec_class = packet_codecs[packet_codec_details['packet_codec']] # type: PacketCodec
self.queue_params.update(get_queue_params(packet_codec_details))
message_codecs = get_message_codecs(packet_codec_details)
+ # Convert SRC IDs to integers, since yaml dict keys have to be strings
+ try:
+ src_names: dict = {int(k):v for k,v in packet_codec_details.get('src_names', {}).items()}
+ except Exception as e:
+ rospy.logerr(f"Error parsing src_names parameter: {e}")
+
for queue_id, params in self.queue_params.items():
+ # We support three cases here: Publish only, subscribe to a topic, or call a dynamic queue service
+ # If there is no subscribe_topic or dynamic queue, this is a publish-only entry, so we skip it here
+ if params.subscribe_topic is None and params.dynamic_queue_service is None:
+ rospy.logdebug(f"ID {queue_id} has no subscribe topic or dynamic queue, skipping it.")
+ continue
+ # If both the subscribe_topic and dynamic_queue are specified, that's bad.
+ # Throw an error (but don't crash for now)
+ if params.subscribe_topic and params.dynamic_queue_service:
+ rospy.logerr(f"ID {queue_id} has both a subscribe_topic and dynamic_queue specified. You can't have both.")
+ continue
+
# QUESTION: this made default_dest a non-optional parameter, but later, it is accessed with a default value.
# Which do we want? Have the node's default propagate to the queue? Or not (and get rid of the node's)?
self.destination_reachable[params.destination] = self.unknown_dests_are_reachable
- if params.is_active:
- self.active_queue_names.add(params.subscribe_topic)
-
- new_queue = MessageQueue(master_message_queue=self._queued_messages,
- dest_address=params.destination,
- priority=params.priority,
- header=queue_id,
- topic=params.subscribe_topic,
- order=params.queue_order,
- maxsize=params.queue_maxsize,
- allow_fragmentation=params.allow_fragmentation,
- # QUESTION: Why does the queue own the codec?
- message_codec=message_codecs[params.codec_id],
- )
- self.topic_queue[params.subscribe_topic] = new_queue
- rospy.Subscriber(params.subscribe_topic,
- AnyMsg,
- partial(self.on_incoming_message, topic_name=params.subscribe_topic))
+ # Now, the "normal" case: the subscribe_topic is specified, so we want to set up a new queue for it.
+ if params.subscribe_topic:
+ topic_name = params.subscribe_topic
+ if '{src}' in topic_name or '{name}' in topic_name:
+ # We are going to attach multiple subscribers to this queue, one for each entry in the
+ # src_names dict
+ for src, name in src_names.items():
+ complete_topic_name = topic_name.format(src=src, name=name)
+ if params.is_active:
+ self.active_queue_names.add(complete_topic_name)
+
+ new_queue = MessageQueue(master_message_queue=self._queued_messages,
+ dest_address=params.destination,
+ priority=params.priority,
+ message_codec_id=queue_id,
+ topic=complete_topic_name,
+ order=params.queue_order,
+ maxsize=params.queue_maxsize,
+ allow_fragmentation=params.allow_fragmentation,
+ # QUESTION: Why does the queue own the codec?
+ message_codec=message_codecs[params.codec_id],
+ )
+ self.topic_queue[complete_topic_name] = new_queue
+ rospy.Subscriber(complete_topic_name,
+ AnyMsg,
+ partial(self.on_incoming_message, topic_name=complete_topic_name))
+ else:
+ if params.is_active:
+ self.active_queue_names.add(params.subscribe_topic)
+
+ new_queue = MessageQueue(master_message_queue=self._queued_messages,
+ dest_address=params.destination,
+ priority=params.priority,
+ message_codec_id=queue_id,
+ topic=params.subscribe_topic,
+ order=params.queue_order,
+ maxsize=params.queue_maxsize,
+ allow_fragmentation=params.allow_fragmentation,
+ # QUESTION: Why does the queue own the codec?
+ message_codec=message_codecs[params.codec_id],
+ )
+ self.topic_queue[params.subscribe_topic] = new_queue
+ rospy.Subscriber(params.subscribe_topic,
+ AnyMsg,
+ partial(self.on_incoming_message, topic_name=params.subscribe_topic))
+ elif params.dynamic_queue_service:
+ service_name = rospy.names.canonicalize_name(params.dynamic_queue_service)
+ update_topic_name = rospy.names.canonicalize_name(params.dynamic_update_topic)
+ rospy.logdebug(f"Setting up dynamic queue for ID {queue_id}: {service_name}")
+ try:
+ self.dynamic_queues.append(DynamicQueue(message_codec_id=queue_id,
+ dynamic_message_service=service_name,
+ dynamic_update_topic=update_topic_name,
+ dest_address=params.destination,
+ message_codec=message_codecs[params.codec_id], ))
+ self.dynamic_get_next_message_services[service_name] = rospy.ServiceProxy(service_name,
+ GetNextQueuedMessage)
+ self.dynamic_queue_update_publishers[service_name] = rospy.Publisher(
+ update_topic_name, UpdateQueuedMessage, queue_size=10)
+
+ except Exception as e:
+ rospy.logerr(f"Error setting up dynamic queue for {service_name}: {e}")
# QUESTION(lindzey): It looks like the packet codec is never used?
# => AAAAH. This is called for its side effects. Maybe rename to make more clear?
@@ -369,7 +459,7 @@ class MessageQueueNode(object):
summary_message_counts.append(summary)
queue_summaries = []
for topic, queue in self.topic_queue.items():
- queue_summary = QueueSummary(queue_id=queue.header,
+ queue_summary = QueueSummary(queue_id=queue.message_codec_id,
priority=queue.priority,
enabled=(topic in self.active_queue_names),
message_count=len(queue._queue))
@@ -383,7 +473,6 @@ class MessageQueueNode(object):
queue_summaries=queue_summaries)
self.queue_status_pub.publish(msg)
-
def on_incoming_message(self, msg_data, topic_name):
# type: (AnyMsg, str) -> None
connection_header = msg_data._connection_header['type'].split('/')
@@ -406,7 +495,6 @@ class MessageQueueNode(object):
else:
rospy.logdebug("TOPIC: {} not in active queue list, skipping msg".format(topic_name))
-
def on_neighbor_reachability(self, msg: NeighborReachability) -> None:
self.destination_reachable[msg.dest] = msg.reachable
@@ -438,28 +526,66 @@ class MessageQueueNode(object):
self._queued_messages.remove(message)
def mark_transmitted(self, msg: QueuedMessage) -> None:
- msg.queue.mark_transmitted(msg)
-
- def get_next_message(self, check_reachability=True, dest=None):
- # type: (bool, Optional[int]) -> Optional[QueuedMessage]
- # Use the master message queue for this
+ # For now, we don't do this for locally queued messages (since we don't track it properly)
+ #msg.queue.mark_transmitted(msg)
+
+ # ... but we do track it for dynamic queues
+ if isinstance(msg.queue, DynamicQueue):
+ if msg.queue.dynamic_message_service in self.dynamic_queue_update_publishers:
+ update_msg = UpdateQueuedMessage(header=Header(stamp=rospy.Time.now()),
+ message_id=msg.message_id_in_queue,
+ event=UpdateQueuedMessage.EVENT_LINK_TRANSMITTED)
+ self.dynamic_queue_update_publishers[msg.dynamic_queue_name].publish(update_msg)
+ rospy.logdebug(f"Marked message as transmitted: {msg.message_id_in_queue} on {msg.queue.dynamic_message_service}")
+
+ def get_next_message(self, max_size_in_bits: int = 131072,
+ dest_address: Optional[int] = None,
+ packet_codec: Optional[str] = None,
+ check_reachability: bool = True,
+ exclude_messages: Optional[List[QueuedMessage]] = []) -> Optional[QueuedMessage]:
next_message = None
- for message in sorted(self._queued_messages, key=lambda m: m.priority, reverse=True):
- if message.queue.subscribe_topic not in self.active_queue_names:
+
+ # Query registered dynamic queues
+ highest_priority = -255
+ for dynamic_queue in self.dynamic_queues:
+ dynamic_queue_name = dynamic_queue.dynamic_message_service
+ # Figure out which messages to exclude (because they are already queued)
+ this_queue_exclusion_ids = [m.message_id_in_queue for m in exclude_messages if m.dynamic_queue_name == dynamic_queue_name]
+
+ # Get a reference to the service
+ dynamic_get_next_queued_message = self.dynamic_get_next_message_services[dynamic_queue_name]
+
+ try:
+ queue_response = dynamic_get_next_queued_message(max_size_in_bits=max_size_in_bits,
+ dest_address=dest_address,
+ packet_codec=(str(packet_codec) if packet_codec else ''),
+ exclude_message_ids=this_queue_exclusion_ids)
+ except rospy.service.ServiceException as e:
+ rospy.logerr_throttle(1, f"Error calling get_next_message service on {dynamic_queue_name}: {e}")
continue
- if check_reachability and not self.destination_reachable[message.dest]:
+
+ if not queue_response.has_message:
+ rospy.logdebug(f"Dynamic queue {dynamic_queue_name} has no messages matching criteria")
continue
- if dest and (message.dest != dest):
+ if queue_response.priority <= highest_priority:
+ rospy.logdebug(
+ f"Highest priority message in {dynamic_queue_name} has priority {queue_response.priority} <= {highest_priority}")
continue
- next_message = message
- break
- return next_message
- def get_next_message_smaller_than_bits(self, size_in_bits, dest_address=None,
- packet_codec=None,
- check_reachability=True):
- # type: (int, Optional[int], Optional[int], bool) -> Union[None, QueuedMessage]
- next_message = None
+ message_bits = BitArray(bytes=queue_response.data, length=queue_response.data_size_in_bits)
+
+ next_message = QueuedMessage(queue=dynamic_queue,
+ message=None,
+ message_codec=dynamic_queue.message_codec,
+ dest=120,
+ time_added=datetime.utcnow(),
+ fragmentation_tracker=None,
+ allow_fragmentation=False,
+ dynamic_queue_name=dynamic_queue_name,
+ message_id_in_queue=queue_response.message_id,
+ priority=queue_response.priority,
+ payload_bits=message_bits,
+ length_bits=len(message_bits))
for message in sorted(self._queued_messages, key=lambda m: m.priority, reverse=True):
if message.queue.subscribe_topic not in self.active_queue_names:
@@ -476,17 +602,20 @@ class MessageQueueNode(object):
# Move on to check the next message
rospy.logdebug("Disqualified: packet_codec")
continue
- if not message.allow_fragmentation and message.length_bits > size_in_bits:
+ if not message.allow_fragmentation and message.length_bits > max_size_in_bits:
rospy.logdebug("Disqualified: too large, no fragmentation")
continue
- if message.allow_fragmentation and message.get_min_fragment_size_bits() > size_in_bits:
+ if message.allow_fragmentation and message.get_min_fragment_size_bits() > max_size_in_bits:
rospy.logdebug("Disqualified: fragments too large")
continue
if message.allow_fragmentation and message.get_min_fragment_size_bits() == 0:
rospy.logdebug("Disqualified: no fragments to send at this time")
continue
- next_message = message
+ if not next_message:
+ next_message = message
+ if message.priority >= next_message.priority:
+ next_message = message
break
return next_message
@@ -527,7 +656,7 @@ class MessageQueueNode(object):
# To get the next packet, the first thing we do is get the highest priority message to transmit
# Then, we get a packet using its packet codec.
# This is also where we should eventually evaluate which rate to use
- highest_priority_message = self.get_next_message(dest=requested_dest)
+ highest_priority_message = self.get_next_message(dest_address=requested_dest)
if not highest_priority_message:
return GetNextPacketDataResponse(num_miniframe_bytes=0, num_dataframe_bytes=0, num_messages=0)
@@ -537,8 +666,8 @@ class MessageQueueNode(object):
num_dataframe_bytes,
self)
- #for msg in messages_in_packet:
- # self.mark_transmitted(msg)
+ for msg in messages_in_packet:
+ self.mark_transmitted(msg)
return GetNextPacketDataResponse(dest=highest_priority_message.dest,
miniframe_bytes=miniframe_bytes,
@@ -548,26 +677,6 @@ class MessageQueueNode(object):
num_dataframe_bytes=len(dataframe_bytes),
num_messages=len(messages_in_packet))
- # QUESTION: Both of the following functions use self.queues which doesn't
- # appear to ever be updated ... switch to self.topic_queue, or delete as dead code?
- @property
- def message_count(self) -> int:
- return sum([q.message_count for q in self.queues.values()])
-
- @property
- def age_of_oldest_queue(self):
- # type: () -> timedelta
- return max([q.time_since_last_tx for q in self.queues.values()])
-
- # QUESTION: This seems unused -- can we remove?
- def get_filtered_message_count(self, dest_address: int, min_priority: int) -> int:
- filtered_queues = [q for q in self.queues.values() if q.priority >= min_priority]
- filtered_queues = [q for q in filtered_queues if q.dest_address == dest_address]
- if len(filtered_queues) == 0:
- return 0
- else:
- return sum([q.message_count for q in filtered_queues])
-
def spin(self) -> None:
# Issue status periodically.
rate = rospy.Rate(self.update_rate)
diff --git a/src/packet_dispatch_node.py b/src/packet_dispatch_node.py
index d604936abc0524818a93db812a214770bb612109..c98336353683437c809151efb2b97024f7b24349 100755
--- a/src/packet_dispatch_node.py
+++ b/src/packet_dispatch_node.py
@@ -9,11 +9,14 @@ from ros_acomms.msg import ReceivedPacket, Packet, CST
from bitstring import ConstBitStream, ReadError
from ltcodecs import RosMessageCodec
+from yamlinclude import YamlIncludeConstructor
+
from acomms_codecs.ros_packet_codec import RosPacketCodec
from acomms_codecs.packet_codecs import packet_codecs
from version_node import version_node
from codec_config_parser import QueueParams, QueueId, MessageCodecId, get_queue_params, get_message_codecs
+import yaml
@dataclass
class DecoderListEntry:
@@ -85,7 +88,23 @@ class PacketDispatchNode(object):
except:
rospy.logwarn("Unable to query version information")
- packet_codec_param = rospy.get_param('~packet_codecs')
+ self.setup_codecs()
+
+ rospy.Subscriber('packet_rx', ReceivedPacket, self.on_packet_rx)
+
+ rospy.loginfo("Message Dispatch started")
+ rospy.spin()
+
+ def setup_codecs(self) -> None:
+ packet_codec_filename = rospy.get_param('~packet_codec_file', None)
+ codec_dir = rospy.get_param('~codec_directory', 'codecs')
+ if packet_codec_filename:
+ with open(packet_codec_filename, 'r') as f:
+ YamlIncludeConstructor.add_to_loader_class(loader_class=yaml.FullLoader, base_dir=codec_dir)
+ packet_codec_param = yaml.full_load(f)
+ else:
+ packet_codec_param = rospy.get_param('~packet_codecs')
+
self.decoder_list: List[DecoderListEntry] = []
for packet_codec in packet_codec_param:
# QUESTION(lindzey): Storing the message codec dict as a member
@@ -94,13 +113,33 @@ class PacketDispatchNode(object):
# packet codec in the config file. Is that true?
self.message_codecs = get_message_codecs(packet_codec)
self.queue_params = get_queue_params(packet_codec)
+ # Convert SRC IDs to integers, since yaml dict keys have to be strings
+ try:
+ src_names: dict = {int(k): v for k, v in packet_codec.get('src_names', {}).items()}
+ except Exception as e:
+ rospy.logerr(f"Error parsing src_names parameter: {e}")
+
self.message_publishers: Dict[QueueId, rospy.Publisher] = {}
for queue_id, params in self.queue_params.items():
- pub_name = rospy.names.canonicalize_name(params.publish_topic)
- self.message_publishers[queue_id] = rospy.Publisher(
- pub_name, params.msg_class, queue_size=10)
+ if ("{src}" in params.publish_topic) or ("{name}" in params.publish_topic):
+ # In this case, we will have a dict of message publishers that correspond to different SRC IDs
+ for src, name in src_names.items():
+ pub_name = params.publish_topic.format(src=src, name=name)
+ pub_name = rospy.names.canonicalize_name(pub_name)
+ if queue_id not in self.message_publishers.keys():
+ self.message_publishers[queue_id] = {}
+ self.message_publishers[queue_id][src] = rospy.Publisher(pub_name, params.msg_class,
+ queue_size=10)
+ else:
+ pub_name = rospy.names.canonicalize_name(params.publish_topic)
+ self.message_publishers[queue_id] = rospy.Publisher(
+ pub_name, params.msg_class, queue_size=10)
packet_codec.pop('message_codecs')
+ try:
+ packet_codec.pop('src_names')
+ except:
+ pass
entry = DecoderListEntry(**packet_codec)
self.decoder_list.append(entry)
rospy.loginfo("Added decoder: {}".format(entry))
@@ -114,11 +153,6 @@ class PacketDispatchNode(object):
rospy.loginfo("Receive packet codec initialized with {} message codecs.".format(len(self.message_codecs)))
- rospy.Subscriber('packet_rx', ReceivedPacket, self.on_packet_rx)
-
- rospy.loginfo("Message Dispatch started")
- rospy.spin()
-
def on_packet_rx(self, received_packet: ReceivedPacket) -> None:
rospy.loginfo("Got incoming packet")
# Decide what to do with the incoming packet, based on the match criteria
@@ -133,36 +167,6 @@ class PacketDispatchNode(object):
except KeyError:
rospy.logwarn("Unrecognized packet decoder: {}".format(d))
- def decode_ros_acomms_packet(self,
- received_packet: ReceivedPacket,
- stripped_data: Tuple[bytes, bytes]
- ) -> None:
- payload_bytes = stripped_data[0] + stripped_data[1]
- payload_bits = ConstBitStream(bytes=payload_bytes)
-
- # the format of a ros acomms packet is:
- # ros_acomms header (already stripped)
- # repeat:
- # message header (id), uint8 (eventually extend this as a varint)
- # message bits
-
- # loop through until we are out of bits (ReadError will be raised)
- try:
- while True:
- queue_id = payload_bits.read('uint:8')
- codec_id = self.queue_params[queue_id].codec_id
- ros_msg = self.message_codecs[codec_id].decode(payload_bits)
- self.message_publishers[queue_id].publish(ros_msg)
- except KeyError:
- # We don't have a config entry that matches this ID
- rospy.loginfo("Unknown message ID ({}) received, unable to continue parsing packet".format(id))
- return
-
- except ReadError:
- # We are done reading
- pass
-
-
if __name__ == '__main__':
try:
node = PacketDispatchNode()
diff --git a/src/sim_packet_performance_node.py b/src/sim_packet_performance_node.py
index 9a0fff706d8edccc3ac2c8db712c9e2814c8a9e0..f3236afd2e122a962763712ed9b1ca8fbb74cb73 100755
--- a/src/sim_packet_performance_node.py
+++ b/src/sim_packet_performance_node.py
@@ -37,6 +37,7 @@ class SimPacketPerformanceNode(object):
rospy.loginfo("Starting sim_packet_performance_node...")
# Get node parameters
+ self.fixed_frame_error_rate = rospy.get_param('~fixed_frame_error_rate', None)
# Check node parameters
# TODO: check validity of the parameters here and output contents to log
@@ -56,13 +57,19 @@ class SimPacketPerformanceNode(object):
frame_success = []
if rate == 1:
- frame_error_rate = np.interp(snr, rate1snr, rate1probability)
+ if self.fixed_frame_error_rate:
+ frame_error_rate = self.fixed_frame_error_rate
+ else:
+ frame_error_rate = np.interp(snr, rate1snr, rate1probability)
for i in range(1, 3):
success = random.random() > frame_error_rate
frame_success.append(success)
elif rate == 5:
- frame_error_rate = np.interp(snr, rate5snr, rate5probability)
+ if self.fixed_frame_error_rate:
+ frame_error_rate = self.fixed_frame_error_rate
+ else:
+ frame_error_rate = np.interp(snr, rate5snr, rate5probability)
for i in range(1, 8):
success = random.random() > frame_error_rate
frame_success.append(success)
diff --git a/src/tdma_nav_header_node.py b/src/tdma_nav_header_node.py
new file mode 100755
index 0000000000000000000000000000000000000000..518ce4afac40038eac1ffd717bda7eefb191ba18
--- /dev/null
+++ b/src/tdma_nav_header_node.py
@@ -0,0 +1,202 @@
+#!/usr/bin/env python3
+
+import rospy
+from std_msgs.msg import Header
+from ros_acomms.srv import QueueTxPacket, QueueTxPacketRequest
+from ros_acomms.srv import GetNextPacketData
+from ros_acomms.srv import PingModem, PingTransponders
+from ros_acomms.msg import Packet, TdmaNavStatus
+from dynamic_reconfigure.server import Server as DynamicReconfigureServer
+from ros_acomms.cfg import tdma_nav_headerConfig
+import struct
+import datetime
+import dateutil.parser
+
+from tdma_node import TdmaMacNode
+
+class TdmaNavHeaderMacNode(TdmaMacNode):
+ def __init__(self):
+ rospy.init_node('tdma_nav_header_mac')
+ super().__init__(subclass=True)
+
+ self.tdma_status_publisher = rospy.Publisher('tdma_nav_status', TdmaNavStatus, queue_size=10)
+
+ # get duration of tdma slot to dedicate to nav messages
+ self.nav_duration_seconds = rospy.get_param('~nav_duration_seconds', 15)
+ self.nav_guard_time_seconds = rospy.get_param('~nav_guard_time_seconds', 5)
+ # params for nav header pinging that happens during the nav portion of every tdma cycle
+ self.transponder_A = rospy.get_param('~transponder_A', True)
+ self.transponder_B = rospy.get_param('~transponder_B', True)
+ self.transponder_C = rospy.get_param('~transponder_C', True)
+ self.transponder_D = rospy.get_param('~transponder_D', True)
+ self.transponder_reply_timeout_ms = rospy.get_param('~transponder_reply_timeout_ms', 5000)
+ self.ping_modem_dest_id = rospy.get_param('~ping_modem_dest_id', 121)
+ self.ping_modem_rate = rospy.get_param('~ping_modem_rate', 1)
+ self.ping_modem_on = rospy.get_param('~ping_modem_on', True)
+ self.ping_transponders_on = rospy.get_param('~ping_transponders_on', False)
+
+ # setup dynamic reconf for periodic ping params
+ self.reconfigure_server = DynamicReconfigureServer(tdma_nav_headerConfig, self.reconfigure)
+
+ # wait for ping_modem service to come online
+ rospy.loginfo("tdma_nav_header_mac waiting for ping_modem service")
+ rospy.wait_for_service('ping_modem')
+ self.send_modem_ping = rospy.ServiceProxy('ping_modem', PingModem)
+
+ # wait for ping_transponder service to come online
+ rospy.loginfo("tdma_nav_header_mac waiting for ping_transponders service")
+ rospy.wait_for_service('ping_transponders')
+ self.send_transponder_ping = rospy.ServiceProxy('ping_transponders', PingTransponders)
+
+ self.non_nav_slot_duration = self.slot_duration_seconds - self.nav_duration_seconds
+ rospy.loginfo("tdma_nav_header_mac running")
+ self.spin()
+
+ def transponder_dest_mask(self, as_list=True):
+ if as_list:
+ return [True if self.transponder_A else False,
+ True if self.transponder_B else False,
+ True if self.transponder_C else False,
+ True if self.transponder_D else False,]
+ return '{}{}{}{}'.format(
+ True if self.transponder_A else False,
+ True if self.transponder_B else False,
+ True if self.transponder_C else False,
+ True if self.transponder_D else False,
+ )
+
+ @property
+ def tdma_nav_mode(self):
+ tdma_nav_mode_str = ""
+ if self.ping_modem_on:
+ tdma_nav_mode_str += f"PingModem:dest:{self.ping_modem_dest_id},rate:{self.ping_modem_rate},"
+ if self.ping_transponders_on:
+ tdma_nav_mode_str += f"PingTransponders:transponder_dest_mask:{self.transponder_dest_mask()}"
+ if not self.ping_modem_on and not self.ping_transponders_on:
+ # no nav portion whole slot can be used now until ping modem/transponder turned back on
+ tdma_nav_mode_str += "NAV-PORTION-OFF"
+ return tdma_nav_mode_str
+
+ def reconfigure(self, config, level):
+ rospy.loginfo("tdma_nav_header_mac driver reconfigure request received.")
+ rospy.loginfo(f"config: {config}")
+
+ self.nav_duration_seconds = config['nav_duration_seconds']
+ self.nav_guard_time_seconds = config['nav_guard_time_seconds']
+ self.transponder_A = config['transponder_A']
+ self.transponder_B = config['transponder_B']
+ self.transponder_C = config['transponder_C']
+ self.transponder_D = config['transponder_D']
+ self.transponder_reply_timeout_ms = config['transponder_reply_timeout_ms']
+ self.ping_modem_dest_id = config['ping_modem_dest_id']
+ self.ping_modem_on = config['ping_modem_on']
+ self.ping_transponders_on = config['ping_transponders_on']
+
+ return config
+
+ def get_current_slot_info(self):
+ now_secs = rospy.Time.now().to_sec()
+ offset_secs = now_secs - self.cycle_start_secs
+ in_cycle_secs = offset_secs % self.cycle_duration
+ current_slot = int(in_cycle_secs // self.slot_duration_seconds)
+ we_are_active = self.slots[current_slot]['active']
+ remaining_slot_seconds = self.slot_duration_seconds - (in_cycle_secs % self.slot_duration_seconds)
+ # handle the case where we are active in subsequent slots
+ remaining_active_seconds = remaining_slot_seconds
+ for i in range(current_slot + 1, current_slot+self.num_slots):
+ next_slot_num = i % self.num_slots
+ if self.slots[next_slot_num]['active']:
+ remaining_active_seconds += self.slot_duration_seconds
+ else:
+ break
+
+ # Figure out if we're in nav_portion of slot
+ is_nav_portion = False
+ # Figure out how many seconds remain until we go active
+ if we_are_active:
+ time_to_next_active = 0
+ if remaining_active_seconds - self.nav_guard_time_seconds > self.non_nav_slot_duration:
+ is_nav_portion = True
+ else:
+ time_to_next_active = remaining_slot_seconds
+ for i in range(current_slot + 1, current_slot + self.num_slots):
+ next_slot_num = i % self.num_slots
+ if not self.slots[next_slot_num]['active']:
+ time_to_next_active += self.slot_duration_seconds
+ else:
+ break
+
+ # every time we update this, publish the current status. This may need to move if we ever start
+ # calling this function quickly
+ remaining_nav_seconds = remaining_active_seconds - self.non_nav_slot_duration if is_nav_portion else 0.0
+ msg = TdmaNavStatus(
+ is_nav_portion=is_nav_portion,
+ we_are_active=we_are_active,
+ current_slot=current_slot,
+ remaining_nav_seconds=remaining_nav_seconds,
+ remaining_slot_seconds=remaining_slot_seconds,
+ remaining_active_seconds=remaining_active_seconds,
+ time_to_next_active=time_to_next_active,
+ tdma_nav_mode=self.tdma_nav_mode,
+ )
+ msg.header.stamp = rospy.Time.now()
+ self.tdma_status_publisher.publish(msg)
+
+ return is_nav_portion, we_are_active, current_slot, remaining_slot_seconds, remaining_active_seconds, remaining_nav_seconds, time_to_next_active
+
+ def spin(self):
+ rate = rospy.Rate(5)
+ last_tx_time = 0
+ while not rospy.is_shutdown():
+ is_nav_portion, we_are_active, current_slot, remaining_slot_seconds, remaining_active_seconds, remaining_nav_seconds, time_to_next_active = self.get_current_slot_info()
+ if not we_are_active:
+ rate.sleep()
+ continue
+
+ # Make sure we have enough time left to send a packet
+ if remaining_active_seconds < (self.guard_time_seconds + self.packet_length_seconds):
+ rate.sleep()
+ continue
+
+ # Make sure that the modem is ready to accept a packet
+ # TODO: Check modem state here rather than relying on last_tx_time.
+ # Note that this isn't atomic, so the state could change
+ if rospy.get_time() <= (last_tx_time + self.guard_time_seconds + self.packet_length_seconds) or \
+ rospy.get_time() <= self.last_txf_time + self.guard_time_seconds:
+ rate.sleep()
+ continue
+
+ # if not the nav portion OR it is and we aren't sending nav pings,
+ # send a regular packet, else send nav packet
+ if not is_nav_portion or not self.ping_modem_on and not self.ping_transponders_on:
+ self.send_next_packet()
+ else:
+ self.send_next_nav_packet(timeout=remaining_nav_seconds)
+
+ last_tx_time = rospy.get_time()
+
+ def send_next_nav_packet(self, timeout):
+ start_t = rospy.get_time()
+ if self.ping_modem_on:
+ # make service call and wait for reply
+ rospy.loginfo(f"Sending nav ping to dest id:{self.ping_modem_dest_id} at rate: {self.ping_modem_rate}")
+ resp = self.send_modem_ping(self.ping_modem_dest_id, self.ping_modem_rate, str(0).encode('utf-8'), bytes(), timeout)
+ stop_t = rospy.get_time()
+ timeout -= stop_t - start_t
+ rospy.loginfo(f"Sent ping and got reply with {timeout} seconds remaining in nav portion of tdma slot. Response: {resp}")
+ if self.ping_transponders_on:
+ # make service call and wait for reply
+ rospy.loginfo(f"Sending transponder ping. Mask: {self.transponder_dest_mask()}, timeout_ms: {self.transponder_reply_timeout_ms}")
+ resp = self.send_transponder_ping(transponder_dest_mask=self.transponder_dest_mask(), timeout_ms=self.transponder_reply_timeout_ms)
+ stop_t = rospy.get_time()
+ timeout -= stop_t - start_t
+ rospy.loginfo(f"Sent transponder ping and got reply with {timeout} seconds remaining in nav portion of tdma slot. Response: {resp}")
+
+if __name__ == "__main__":
+ try:
+ tdma_state_node = TdmaNavHeaderMacNode()
+
+ except rospy.ROSInterruptException:
+ rospy.loginfo("tdma_nav_header_node shutdown (interrupt)")
+
+
diff --git a/src/tdma_node.py b/src/tdma_node.py
index 8021254476db3081c1483b9c0d9c68f9a89bb72a..05beab50a76546d4943dcf6e0efa329b45e27bd0 100755
--- a/src/tdma_node.py
+++ b/src/tdma_node.py
@@ -10,10 +10,10 @@ import datetime
import dateutil.parser
class TdmaMacNode(object):
- def __init__(self):
- rospy.init_node('tdma_mac')
-
- self.tdma_status_publisher = rospy.Publisher('tdma_status', TdmaStatus, queue_size=10)
+ def __init__(self, subclass=False):
+ if not subclass:
+ rospy.init_node('tdma_mac')
+ self.tdma_status_publisher = rospy.Publisher('tdma_status', TdmaStatus, queue_size=10)
self.num_slots = rospy.get_param('~num_slots', 4)
self.slot_duration_seconds = rospy.get_param('~slot_duration_seconds', 30)
@@ -42,8 +42,9 @@ class TdmaMacNode(object):
rospy.Subscriber('txf', Header, self.on_txf)
self.last_txf_time = rospy.get_time()
- rospy.loginfo("tdma_mac node running.")
- self.spin()
+ if not subclass:
+ rospy.loginfo("tdma_mac node running.")
+ self.spin()
def on_txf(self, msg):
# Reset our "time since last TX done" timer (used with guard time)
diff --git a/srv/EstablishCall.srv b/srv/EstablishCall.srv
deleted file mode 100644
index b156ef5059b097c50564d396321d5d07dab1c403..0000000000000000000000000000000000000000
--- a/srv/EstablishCall.srv
+++ /dev/null
@@ -1,26 +0,0 @@
-# Request type constants:
-#################
-
-int8 RUDICS=1
-int8 SBD=2
-int8 AUTO=3
-
-# Response type constants
-int8 SUCCESS=1
-int8 CALL_ALREADY_OPEN=2
-int8 FAIL=3
-
-# Request
-#################
-
-# Type of call to establish
-int8 call_type
-
----
-
-# Response
-#################
-
-# Status of call
-int8 call_status
-
diff --git a/srv/GetNextQueuedMessage.srv b/srv/GetNextQueuedMessage.srv
new file mode 100644
index 0000000000000000000000000000000000000000..721d9e4a292f6daeae8a0a455b3609bc67e2c508
--- /dev/null
+++ b/srv/GetNextQueuedMessage.srv
@@ -0,0 +1,16 @@
+# Request
+
+int32 max_size_in_bits
+int16 minimum_priority
+int16 dest_address
+string packet_codec
+int32[] exclude_message_ids
+
+---
+# Response
+bool has_message
+int32 message_id
+int16 dest_address
+int16 priority
+uint8[] data
+int32 data_size_in_bits
diff --git a/srv/Hangup.srv b/srv/Hangup.srv
deleted file mode 100644
index 2239980fe41c1df59a7abc88982803bfe0098e37..0000000000000000000000000000000000000000
--- a/srv/Hangup.srv
+++ /dev/null
@@ -1,11 +0,0 @@
-# Request
-#################
-
-# hangup
-bool hangup
----
-# Response
-###########
-
-# hangup status
-bool hungup
diff --git a/srv/PingTransponders.srv b/srv/PingTransponders.srv
index 8aae9c314dd4d5820faa5f5e55d73e68fc368893..2e970f498f5b18f7eefac9b5965e453f0c5eacd4 100644
--- a/srv/PingTransponders.srv
+++ b/srv/PingTransponders.srv
@@ -2,7 +2,8 @@
# This message is not yet complete
# Request
-uint8 dest_address
+bool[] transponder_dest_mask
+uint16 timeout_ms
---
# Response