diff --git a/.gitignore b/.gitignore
index c132e6f6e08e9080928016dda2fbb5d8448d699e..3daf0ad630824334393b5e0440deeea454a8e0e1 100644
--- a/.gitignore
+++ b/.gitignore
@@ -154,7 +154,7 @@ dmypy.json
cython_debug/
-.vscode/*
+.vscode/
!.vscode/settings.json
!.vscode/tasks.json
!.vscode/launch.json
diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 3214b0aa05ef672d26ea134640ebc7aa633f10de..d316de4a72846a87667ba2da295e0b3b2b94026b 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -1,11 +1,25 @@
build_test_image:
+ tags:
+ - privileged
image: docker:stable
stage: build
services:
- - name: docker:dind
+ - name: docker:dind
script:
- docker build --rm -t ros_acomms-tests:latest -f ros_acomms_tests/tests.Dockerfile .
+trigger-ros_acomms_net-pipeline:
+ trigger:
+ project: acomms/ros_acomms_net
+ branch: $NET_DEVEL_BRANCH
+ strategy: depend
+
+trigger-ros_acomms_net_tools-pipeline:
+ trigger:
+ project: acomms/ros_acomms_net_tools
+ branch: $TOOLS_DEVEL_BRANCH
+ strategy: depend
+
gitlab run_tests:
stage: test
image: ros_acomms-tests:latest
@@ -15,7 +29,7 @@ gitlab run_tests:
- cd ../ && mv $CI_PROJECT_NAME ~/catkin_ws/src/
- git clone -b $PYACOMMS_BRANCH https://gitlab-ci-token:$CI_JOB_TOKEN@git.whoi.edu/acomms/pyacomms.git ~/pyacomms
- git clone -b $LTCODEC_BRANCH https://gitlab-ci-token:$CI_JOB_TOKEN@git.whoi.edu/acomms/ltcodec.git ~/ltcodec
- - cd ~/catkin_ws/src/ros_acomms && ./setup.bash
+ - cd ~/catkin_ws/src/ros_acomms && pip install -U -r requirements.txt
- cd ~/ltcodec && pip install .
- cd ~/pyacomms && pip install .
- cd ~/catkin_ws && catkin_make
@@ -24,7 +38,9 @@ gitlab run_tests:
- rosparam set /ros_acomms_tests/fragmentation_test_msg_size $MSG_SIZE
- rosparam set /clock_generator/multiplier $CLOCK_MULTIPLIER
- rosparam set /clock_generator/publish_rate $CLOCK_PUBLISH_RATE
- - source devel/setup.bash && rosrun ros_acomms_tests test_ros_acomms.py
+ - source devel/setup.bash
+ - rosrun ros_acomms_tests test_ros_acomms.py
+ - rosrun ros_acomms_tests test_dynamic_queue.py
pypi run_tests:
stage: test
@@ -33,11 +49,13 @@ pypi run_tests:
- source /ros_entrypoint.sh
- mkdir -p ~/catkin_ws/src
- cd ../ && mv $CI_PROJECT_NAME ~/catkin_ws/src/
- - cd ~/catkin_ws/src/ros_acomms && ./setup.bash
+ - cd ~/catkin_ws/src/ros_acomms && pip install -U -r requirements.txt
- cd ~/catkin_ws && catkin_make
- roscore &
- sleep 5
- rosparam set /ros_acomms_tests/fragmentation_test_msg_size $MSG_SIZE
- rosparam set /clock_generator/multiplier $CLOCK_MULTIPLIER
- rosparam set /clock_generator/publish_rate $CLOCK_PUBLISH_RATE
- - source devel/setup.bash && rosrun ros_acomms_tests test_ros_acomms.py
\ No newline at end of file
+ - source devel/setup.bash
+ - rosrun ros_acomms_tests test_ros_acomms.py
+ - rosrun ros_acomms_tests test_dynamic_queue.py
diff --git a/Dockerfile b/Dockerfile
deleted file mode 100755
index c24016b5259ffc89ba2a29f6fdf7de02cae8b98c..0000000000000000000000000000000000000000
--- a/Dockerfile
+++ /dev/null
@@ -1,44 +0,0 @@
-FROM ros:melodic-ros-base
-
-## install build tools
-######################
-RUN apt-get update &&\
- apt-get install -y python-catkin-tools &&\
- apt-get install -y python3-pip &&\
- apt-get install -y gfortran &&\
- rm -rf /var/lib/apt/lists/*
-
-## install acoustic toolbox bellhop
-###################################
-ENV AT_WS /opt/at
-RUN mkdir -p $AT_WS
-WORKDIR $AT_WS
-RUN git clone https://git.whoi.edu/dgiaya/acoustic_toolbox.git
-WORKDIR $AT_WS/acoustic_toolbox
-RUN make
-RUN cp $AT_WS/acoustic_toolbox/Bellhop/bellhop.exe /usr/bin/bellhop.exe
-RUN rm -rf $AT_WS
-
-## copy over ros-acomms
-#######################
-ENV ROS_WS /opt/ros_ws
-RUN mkdir -p $ROS_WS/src
-WORKDIR $ROS_WS
-
-# Start with requirements.txt
-COPY ./requirements.txt $ROS_WS/src
-# install python package dependencies
-RUN pip3 install -r $ROS_WS/src/requirements.txt
-
-# Copy over the rest
-# Checkout from repository
-#RUN git -C src clone git@acommsgit.whoi.edu:ros/ros_acomms.git
-# Copy from local source
-COPY . $ROS_WS/src
-
-## build ros package source
-RUN catkin config --extend /opt/ros/$ROS_DISTRO && catkin build ros_acomms
-
-RUN sed --in-place --expression '$isource "$ROS_WS/devel/setup.bash"' /ros_entrypoint.sh
-
-CMD ["roslaunch", "ros_acomms", "modem_sim_test.launch"]
diff --git a/README.md b/README.md
index bae0dee75008f78507fcaf64738082f72b4e2057..dc3d6f174bdc75ec87c68985f387643de26e6f99 100755
--- a/README.md
+++ b/README.md
@@ -727,23 +727,11 @@ time, rostime:
You can use the included Dockerfile to build a Docker container, download a pre-built Docker image, or just check out this repository and run one of the example launch files.
-The easiest way to get started with the Modem Simulator is to use the installer bash script:
- 1. Create a ROS workspace, if you don't already have one. See the ROS documentation for more information.
+Install:
+ 1. Create a ROS workspace, if you don't already have one. See the ROS documentation for more information.
2. Clone ros_acomms: `git clone https://git.whoi.edu/acomms/ros_acomms.git`
3. Change directory into ros_acomms `cd ros_acomms`
- 4. Run setup.bash `./setup.bash`
-
-Alternatively, a more comprehensive set of installation instructions can be found below:
-
- 1. Create a ROS workspace, if you don't already have one. See the ROS documentation for more information.
- 2. Check out ros_acomms into your ros_workspace/src/ directory: `git clone https://git.whoi.edu/acomms/ros_acomms.git`
- 3. Install python dependencies: navigate to the root directory of ros_acomms and run `pip install -r requirements.txt` to automatically install required packages. Alternatively you can manually install arlpy geopy and ltcodecs (`pip install --user arlpy geopy ltcodecs`)
- 4. Build Bellhop and make sure it is on your path:
- 1. Check out source into a new directory: `git clone https://git.whoi.edu/dgiaya/acoustic_toolbox.git`
- 2. Run `make`
- 3. Either add acoustic_toolbox/Bellhop to your path or symlink `acoustic_toolbox/Bellhop/bellhop.exe` to `/usr/bin/bellhop.exe`
- 1. For example: `sudo ln -s /<YOUR_PATH_GOES_HERE>/acoustic_toolbox/Bellhop/bellhop.exe /usr/bin/bellhop.exe`
- 2. Note that if you append a directory to $PATH via `export $PATH=$PATH:/path/to/bellhop.exe`, it will only persist for the current terminal session. If you want it to persist, either create a symlink as described above or add the export command to `~/bashrc`.
+ 4. Run setup.bash `pip install -r requirements.txt`
5. Rebuild your workspace with `catkin_make`
6. Run the modem_sim_test launch file from a console. It will print messages to stdout. Assuming you are in your ROS workspace root: `roslaunch src/ros_acomms/src/launch/modem_sim_test.launch`
diff --git a/requirements.txt b/requirements.txt
index fa346ec6d9824c34e8481c0c0533dcc44a2be34b..6b8a6be5d6a9b7c816fd0482e99e8103be21cefc 100755
--- a/requirements.txt
+++ b/requirements.txt
@@ -1,8 +1,6 @@
-arlpy==1.7.0
-geopy==1.21.0
-numpy==1.18.3
-pandas==1.0.3
-scipy==1.4.1
+whoi-uwapm
+geopy
+numpy
python-dateutil
pytz
rospkg
@@ -12,7 +10,7 @@ bitstring
msgpack
ltcodecs>=1.0.0
whoi-gitver
-acomms
+acomms>=2.4.0
importlib-metadata
pyyaml
-pyyaml-include
+rospy-yaml-include
\ No newline at end of file
diff --git a/ros_acomms/setup.py b/ros_acomms/setup.py
index a1c86cbb40d7c7eb918bacd8285f0e49f9f569f6..38e2b59558a805d5d1484c569c97545e61adbec6 100755
--- a/ros_acomms/setup.py
+++ b/ros_acomms/setup.py
@@ -2,8 +2,7 @@ from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup
d = generate_distutils_setup(
- py_modules=['acomms_codecs'],
- packages=['codec_config_parser'],
+ packages=['codec_config_parser', 'acomms_codecs'],
package_dir={'': 'src'},
)
diff --git a/ros_acomms/src/acomms_codecs/__init__.py b/ros_acomms/src/acomms_codecs/__init__.py
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..596ad7d3cf1bff5068372b8e904c265f6048e0b8 100644
--- a/ros_acomms/src/acomms_codecs/__init__.py
+++ b/ros_acomms/src/acomms_codecs/__init__.py
@@ -0,0 +1,57 @@
+import importlib
+import pkgutil
+import os
+import rospy
+import sys
+
+default_codecs = {
+ name.split(".")[-1].split("_")[0]: importlib.import_module(name)
+ for finder, name, ispkg in pkgutil.iter_modules(__path__, __name__ + ".")
+ if name.endswith("_packet_codec") and name != "acomms_codecs.base_packet_codec"
+}
+
+custom_codecs = {}
+custom_packet_codec_paths = rospy.get_param("custom_packet_codec_paths", None)
+
+if custom_packet_codec_paths is not None:
+ try:
+ for codec in (
+ custom_packet_codec_paths
+ if isinstance(custom_packet_codec_paths, list)
+ else [custom_packet_codec_paths]
+ ):
+ path, module_name = os.path.split(codec)
+ sys.path.append(path)
+ codec = importlib.import_module(module_name, path)
+ custom_codecs = {
+ name.split(".")[-1].split("_")[0]: importlib.import_module(name)
+ for finder, name, ispkg in pkgutil.walk_packages(
+ __path__, __name__ + "."
+ )
+ if name.endswith("_packet_codec")
+ }
+ except OSError as exception:
+ rospy.logerr(
+ f"OSError occured when loading custom packet codecs, check provided path: {exception}"
+ )
+ raise OSError(
+ "OSError occured when loading custom packet codecs, check provided path"
+ ) from exception
+ except ImportError as exception:
+ rospy.logerr(
+ f"ImportError occured when loading custom packet codecs: {exception}"
+ )
+ raise ImportError(
+ "ImportError occured when loading custom packet codecs"
+ ) from exception
+ except Exception as exception:
+ rospy.logerr(
+ f"An unhandled error occured while loading custom packet codecs: {exception}"
+ )
+ raise Exception(
+ "An unhandled error occured while loading custom packet codecs"
+ ) from exception
+else:
+ rospy.loginfo("No custom packet codecs specified, using defaults")
+
+packet_codecs = {**default_codecs, **custom_codecs}
diff --git a/ros_acomms/src/acomms_codecs/packet_codec.py b/ros_acomms/src/acomms_codecs/base_packet_codec.py
similarity index 97%
rename from ros_acomms/src/acomms_codecs/packet_codec.py
rename to ros_acomms/src/acomms_codecs/base_packet_codec.py
index fb66758185328576c97f0a2741c4786069732454..fd8bbe7ff5b877bb493e62c2103e6bcfbc577308 100644
--- a/ros_acomms/src/acomms_codecs/packet_codec.py
+++ b/ros_acomms/src/acomms_codecs/base_packet_codec.py
@@ -2,7 +2,7 @@ from abc import ABC, abstractmethod
from ros_acomms_msgs.msg import ReceivedPacket
-class PacketCodec(ABC):
+class BasePacketCodec(ABC):
def __init__(self, message_codecs, message_publishers=None, miniframe_header=[], dataframe_header=[]):
self.message_codecs = message_codecs
self.message_publishers = message_publishers
diff --git a/ros_acomms/src/acomms_codecs/ccl_packet_codec.py b/ros_acomms/src/acomms_codecs/ccl_packet_codec.py
index e085f45f774e7bef7015c3f7cd08b4310c4a2bf9..77d3bf3daa82f8b6e406f011496f664629c3f8d3 100644
--- a/ros_acomms/src/acomms_codecs/ccl_packet_codec.py
+++ b/ros_acomms/src/acomms_codecs/ccl_packet_codec.py
@@ -1,13 +1,13 @@
import rospy
from ros_acomms_msgs.msg import ReceivedPacket
from bitstring import ConstBitStream, Bits, BitArray, ReadError
-from .packet_codec import PacketCodec
+from .base_packet_codec import BasePacketCodec
-class CclPacketCodec(PacketCodec):
+class PacketCodec(BasePacketCodec):
def __init__(self, message_codecs, message_publishers=None,
allow_fragmentation=False, minimum_fragment_size_bits=12*8,
miniframe_header=[], dataframe_header=[]):
- super(CclPacketCodec, self).__init__(message_codecs, message_publishers=message_publishers,
+ super(PacketCodec, self).__init__(message_codecs, message_publishers=message_publishers,
miniframe_header=miniframe_header, dataframe_header=dataframe_header)
self.allow_fragmentation = allow_fragmentation
diff --git a/ros_acomms/src/acomms_codecs/packet_codecs.py b/ros_acomms/src/acomms_codecs/packet_codecs.py
deleted file mode 100644
index 3da223630eb8fdde7cfcf50a2b7eaabbbed77f6c..0000000000000000000000000000000000000000
--- a/ros_acomms/src/acomms_codecs/packet_codecs.py
+++ /dev/null
@@ -1,7 +0,0 @@
-from .ros_packet_codec import RosPacketCodec
-from .ccl_packet_codec import CclPacketCodec
-
-
-packet_codecs = {'ros': RosPacketCodec,
- 'ccl': CclPacketCodec,
- }
\ No newline at end of file
diff --git a/ros_acomms/src/acomms_codecs/ros_packet_codec.py b/ros_acomms/src/acomms_codecs/ros_packet_codec.py
index c01b486e83cfc5b6b01342abc1f6bebf05aa48b6..9598931c4a84e37aa10eba5e3f2d3be6aadf2f67 100644
--- a/ros_acomms/src/acomms_codecs/ros_packet_codec.py
+++ b/ros_acomms/src/acomms_codecs/ros_packet_codec.py
@@ -1,4 +1,5 @@
from __future__ import annotations
+import functools
from typing import Dict, Optional, Any
import typing
if typing.TYPE_CHECKING:
@@ -7,7 +8,7 @@ if typing.TYPE_CHECKING:
import rospy
from ros_acomms_msgs.msg import ReceivedPacket, Packet, EncodedAck
from bitstring import ConstBitStream, Bits, BitArray, ReadError, BitStream
-from .packet_codec import PacketCodec
+from .base_packet_codec import BasePacketCodec
from fragmentation_tracker import FragmentationStartHeader, FragmentationContHeader, FragmentationAckHeader, FragmentationTracker
import hashlib
@@ -15,13 +16,23 @@ import hashlib
# single message ACK
# multiple message ACK
+# Helper functions
+def setattr_nested(obj, attr, value):
+ def rgetattr(obj, attr, *args):
+ def _getattr(obj, attr):
+ return getattr(obj, attr, *args)
+ return functools.reduce(_getattr, [obj] + attr.split('.'))
-class RosPacketCodec(PacketCodec):
+ pre, _, post = attr.rpartition('.')
+ return setattr(rgetattr(obj, pre) if pre else obj, post, value)
+
+
+class PacketCodec(BasePacketCodec):
def __init__(self, message_codecs: Dict, message_publishers=None,
allow_fragmentation=True, minimum_fragment_size_bits=12*8,
sequence_number_bits=8,
miniframe_header=[0xac], dataframe_header=[]):
- super(RosPacketCodec, self).__init__(message_codecs, message_publishers=message_publishers,
+ super(PacketCodec, self).__init__(message_codecs, message_publishers=message_publishers,
miniframe_header=miniframe_header, dataframe_header=dataframe_header)
self.allow_fragmentation = allow_fragmentation
@@ -144,7 +155,12 @@ class RosPacketCodec(PacketCodec):
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)
+ # Only set the field if the message already has it. The exception handling covers this.
+ # ROS messages aren't dictionaries, so we can't just add fields.
+ try:
+ setattr_nested(ros_msg, field, value)
+ except NameError or AttributeError as e:
+ rospy.loginfo(f"Message decode: could not set {field} on {ros_msg}. Error: {e}")
try:
if isinstance(self.message_publishers[id], rospy.Publisher):
self.message_publishers[id].publish(ros_msg)
diff --git a/ros_acomms/src/acomms_codecs/test_message_codec.py b/ros_acomms/src/acomms_codecs/test_message_codec.py
deleted file mode 100644
index ce000b7bd19dbcbdf0272a293ca207aeb9de3b0c..0000000000000000000000000000000000000000
--- a/ros_acomms/src/acomms_codecs/test_message_codec.py
+++ /dev/null
@@ -1,123 +0,0 @@
-from ltcodecs import RosMessageCodec
-from ltcodecs.float_codec import FloatCodec
-from ltcodecs.linspace_float_codec import LinspaceFloatCodec
-from ltcodecs.varint_codec import VarintCodec
-from ltcodecs.fixedint_codec import UInt8Codec, UInt16Codec, UInt32Codec, FixedIntCodec
-from ltcodecs.ccl_latlon_bcd_codec import CclLatLonBcdCodec
-from ros_acomms_msgs.msg import Packet, ReceivedPacket
-from bitstring import ConstBitStream
-from io import BytesIO
-import math
-from binascii import hexlify, unhexlify
-try:
- from ltcodecs.rostime_codec import RosTimeCodec
-except ImportError:
- pass
-
-test_received_packet = ReceivedPacket()
-
-
-f = BytesIO()
-test_received_packet.serialize(f)
-f.seek(0)
-ros_serialized = f.read(-1)
-print("ROS serialized: {} bytes".format(len(ros_serialized)))
-
-#print(test_received_packet)
-
-message_codec = RosMessageCodec(ros_type='ros_acomms/ReceivedPacket', checksum='crc32')
-
-print("fields: ", message_codec.root_field_codec.ltcodecs.values())
-print("Message codec: max {} bits ({:.2f} bytes), min {} bits ({:.2f} bytes)".format(message_codec.max_length_bits,
- message_codec.max_length_bits / 8,
- message_codec.min_length_bits,
- message_codec.min_length_bits / 8))
-
-bits, encoded_dict = message_codec.encode(test_received_packet)
-
-print(bits)
-
-print("Actual size: {} bits {:.2f} bytes".format(len(bits), len(bits)/8))
-
-decoded_message = message_codec.decode(ConstBitStream(bits))
-
-print("float test")
-
-float_codec = FloatCodec(-90, 90, 5)
-print("float ({} bits)".format(float_codec.max_length_bits))
-
-bits, rounded = float_codec.encode(41.123456789)
-
-print("rounded: {}".format(rounded))
-
-decoded_value = float_codec.decode(ConstBitStream(bits))
-print("decoded: {}".format(decoded_value))
-
-print("linspace float test")
-
-linspace_codec = LinspaceFloatCodec(min_value=-90, max_value=90, num_values=2**24)
-print("linspace resolution: {}, num_values: {} ({} bits)".format(linspace_codec.resolution,
- linspace_codec.num_values,
- math.log2(linspace_codec.num_values)))
-bits, rounded = linspace_codec.encode(41.123456789)
-
-print("rounded: {}".format(rounded))
-
-decoded_value = linspace_codec.decode(ConstBitStream(bits))
-print("decoded: {}".format(decoded_value))
-
-print("varint test")
-varint_codec = VarintCodec(min_value=0, max_value=1)
-
-print("varint resolution: {}, {} bits".format(varint_codec.resolution,
- varint_codec.num_bits,
- ))
-bits, rounded = varint_codec.encode(1)
-
-print("rounded: {}".format(rounded))
-
-decoded_value = varint_codec.decode(ConstBitStream(bits))
-print("decoded: {}".format(decoded_value))
-
-
-incoming_bytes = unhexlify("B4054A376502010B0565381300004B05000000000000")
-
-latcodec = CclLatLonBcdCodec(lat_not_lon=True)
-loncodec = CclLatLonBcdCodec(lat_not_lon=False)
-depthcodec = FixedIntCodec(num_bits=16, little_endian=True)
-typecodec = UInt8Codec()
-confidencecodec = UInt8Codec()
-idcodec = FixedIntCodec(num_bits=32, little_endian=True)
-
-incoming_bits = ConstBitStream(bytes=incoming_bytes)
-incoming_bits.bitpos = 8
-lat = latcodec.decode(incoming_bits)
-lon = loncodec.decode(incoming_bits)
-print("position ", incoming_bits.bitpos)
-depth = depthcodec.decode(incoming_bits)
-contact_type = typecodec.decode(incoming_bits)
-confidence = confidencecodec.decode(incoming_bits)
-print("position ", incoming_bits.bitpos)
-contactid = idcodec.decode(incoming_bits)
-
-print(lat, lon, depth, contact_type, confidence, contactid)
-
-try:
- import rospy
- from datetime import datetime
- rospy.init_node('codec_test')
- time_now = rospy.Time.now()
- print("time {}".format(datetime.utcfromtimestamp(time_now.to_sec())))
- time_codec = RosTimeCodec()
- print("codec {} bits".format(time_codec.max_length_bits))
- time_bits, rounded_time = time_codec.encode(time_now)
- print(time_bits)
- print("len ", len(time_bits))
- decoded_time = time_codec.decode(ConstBitStream(time_bits))
- print("time {}".format(datetime.utcfromtimestamp(decoded_time.to_sec())))
-except ImportError:
- pass
-
-
-
-#print(decoded_message)
diff --git a/ros_acomms/src/acomms_driver_node.py b/ros_acomms/src/acomms_driver_node.py
index 823468232546d56a359b010b50460dc6c976e59b..9e855c75d6b3290e88fd92c5d97797c90c80b975 100755
--- a/ros_acomms/src/acomms_driver_node.py
+++ b/ros_acomms/src/acomms_driver_node.py
@@ -507,7 +507,7 @@ if __name__ == '__main__':
rospy.loginfo("Acomms node started")
- rate = rospy.Rate(1)
+ rate = rospy.Rate(5)
while not rospy.is_shutdown():
rate.sleep()
node.process_outgoing_queue()
diff --git a/ros_acomms/src/check_codec_config.py b/ros_acomms/src/check_codec_config.py
index 2c108661809fc4c51574da6e98b14c4923589036..c66f367cda8195a9752506388cbd82f840dbbca6 100755
--- a/ros_acomms/src/check_codec_config.py
+++ b/ros_acomms/src/check_codec_config.py
@@ -2,19 +2,19 @@
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 acomms_codecs import packet_codecs
from ltcodecs import RosMessageCodec
import rospy
import argparse
from packet_dispatch_node import DecoderListEntry
import yaml
-from yamlinclude import YamlIncludeConstructor
+from rospy_yaml_include.yaml_include import RospyYamlInclude
def check_codec_config_yaml(yaml_file_name, codec_dir):
with open(yaml_file_name, 'r') as yaml_file:
- YamlIncludeConstructor.add_to_loader_class(loader_class=yaml.FullLoader, base_dir=codec_dir)
- packet_codec_param = yaml.load(yaml_file)
+ constructor = RospyYamlInclude(base_directory=codec_dir)
+ packet_codec_param = yaml.load(yaml_file, Loader=constructor.add_constructor())
decoder_list: List[DecoderListEntry] = []
for packet_codec in packet_codec_param:
diff --git a/ros_acomms/src/message_queue_node.py b/ros_acomms/src/message_queue_node.py
index 63f298fde7d3a4f32303421e432b129b31fc3b0e..b18f79d0c138c4d607a1e694361d88ccefa55786 100755
--- a/ros_acomms/src/message_queue_node.py
+++ b/ros_acomms/src/message_queue_node.py
@@ -27,10 +27,9 @@ import os
import signal
from ltcodecs import RosMessageCodec, EncodingFailed
-from acomms_codecs.ros_packet_codec import RosPacketCodec
-from acomms_codecs.packet_codecs import packet_codecs
+from acomms_codecs import packet_codecs
import yaml
-from yamlinclude import YamlIncludeConstructor
+from rospy_yaml_include.yaml_include import RospyYamlInclude
from version_node import version_node
@@ -46,7 +45,6 @@ class DynamicQueue:
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
@@ -260,7 +258,7 @@ class MessageQueue(object):
class MessageQueueNode(object):
def __init__(self) -> None:
- rospy.init_node('message_queue', log_level=rospy.INFO)
+ rospy.init_node('message_queue')
self.update_rate = rospy.get_param('~update_rate', 1)
self.unknown_dests_are_reachable: bool = rospy.get_param('~unknown_dests_are_reachable', True)
self.default_dest = rospy.get_param('~default_dest', 121)
@@ -320,12 +318,12 @@ class MessageQueueNode(object):
# 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_filename = rospy.get_param('~packet_codec_file', None)
- codec_dir = rospy.get_param('~codec_directory', 'codecs')
+ codec_dir = rospy.get_param('~codec_directory', None)
if packet_codec_filename:
try:
- 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)
+ with open(packet_codec_filename, 'r') as yaml_file:
+ constructor = RospyYamlInclude(base_directory=codec_dir)
+ packet_codec_param = yaml.load(yaml_file, Loader=constructor.add_constructor())
except Exception as e:
rospy.logerr(f"Fatal error reading message codec config from {packet_codec_filename} (with codec directory {codec_dir}): {e}")
else:
@@ -410,7 +408,7 @@ class MessageQueueNode(object):
# 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?
# The packet codec initializer sets the packet codec on each message codec
- packet_codec = packet_codec_class(message_codecs=message_codecs,
+ packet_codec = packet_codec_class.PacketCodec(message_codecs=message_codecs,
miniframe_header=packet_codec_details.get('miniframe_header', bytes()),
dataframe_header=packet_codec_details.get('dataframe_header', bytes()))
rospy.loginfo("Added packet codec with {} message codecs".format(len(message_codecs)))
@@ -740,7 +738,7 @@ class MessageQueueNode(object):
# been serviced
self.current_packet_dynamic_queue_cache = {}
- rospy.loginfo("Getting next packet data for transmission: " + str(req).replace('\n', ', '))
+ rospy.logdebug("Getting next packet data for transmission: " + str(req).replace('\n', ', '))
if req.match_dest:
requested_dest = req.dest
else:
diff --git a/ros_acomms/src/packet_dispatch_node.py b/ros_acomms/src/packet_dispatch_node.py
index 6cd1189bf8dc8814765649b47cb0450e37c6a3bb..3ed00483406c83ba22af4ad2c4cfc609b1a653f1 100755
--- a/ros_acomms/src/packet_dispatch_node.py
+++ b/ros_acomms/src/packet_dispatch_node.py
@@ -9,10 +9,9 @@ from ros_acomms_msgs.msg import ReceivedPacket, Packet, CST
from bitstring import ConstBitStream, ReadError
from ltcodecs import RosMessageCodec
-from yamlinclude import YamlIncludeConstructor
+from rospy_yaml_include.yaml_include import RospyYamlInclude
-from acomms_codecs.ros_packet_codec import RosPacketCodec
-from acomms_codecs.packet_codecs import packet_codecs
+from acomms_codecs import packet_codecs
from version_node import version_node
from codec_config_parser import QueueParams, QueueId, MessageCodecId, get_queue_params, get_message_codecs
@@ -97,11 +96,12 @@ class PacketDispatchNode(object):
def setup_codecs(self) -> None:
packet_codec_filename = rospy.get_param('~packet_codec_file', None)
- codec_dir = rospy.get_param('~codec_directory', 'codecs')
+ codec_dir = rospy.get_param('~codec_directory', None)
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)
+ with open(packet_codec_filename, 'r') as yaml_file:
+ constructor = RospyYamlInclude(base_directory=codec_dir)
+ packet_codec_param = yaml.load(yaml_file, Loader=constructor.add_constructor())
+
else:
packet_codec_param = rospy.get_param('~packet_codecs')
@@ -147,7 +147,7 @@ class PacketDispatchNode(object):
# replace name with instance of packet codec
# ToDO make this less ugly and stupid
- entry.packet_codec = packet_codecs[entry.packet_codec](self.message_codecs,
+ entry.packet_codec = packet_codecs[entry.packet_codec].PacketCodec(self.message_codecs,
message_publishers=self.message_publishers,
miniframe_header=entry.miniframe_header,
dataframe_header=entry.dataframe_header)
diff --git a/ros_acomms/src/tdma_node.py b/ros_acomms/src/tdma_node.py
index 02a0e552395fa7d34f4f281fdb5ef35278807455..8795085a693e81c98244cc776ff7c2115eea97f9 100755
--- a/ros_acomms/src/tdma_node.py
+++ b/ros_acomms/src/tdma_node.py
@@ -223,12 +223,15 @@ class TdmaMacNode(object):
packet_data_response = self.get_next_packet_data(num_miniframe_bytes=self.maximum_miniframe_bytes,
num_dataframe_bytes=self.maximum_dataframe_bytes)
- rospy.loginfo("packet_data_response {}".format(packet_data_response))
+ rospy.logdebug("packet_data_response {}".format(packet_data_response))
if packet_data_response.num_messages == 0:
return
- rospy.loginfo("Queuing Packet for TX")
+ rospy.loginfo(f"Queuing Packet for TX (dest {packet_data_response.dest}, " +
+ f"{len(packet_data_response.miniframe_bytes)} miniframe bytes @ rate {self.miniframe_rate}, " +
+ f"{len(packet_data_response.dataframe_bytes)} dataframe bytes @ rate {self.dataframe_rate})")
+
queue_packet = Packet()
queue_packet.dataframe_rate = self.dataframe_rate
queue_packet.miniframe_rate = self.miniframe_rate
diff --git a/ros_acomms_modeling/CMakeLists.txt b/ros_acomms_modeling/CMakeLists.txt
index 5d909ff750edc0e31d8ca6b6cd1d3b599eb0755c..9608f962f8e90a05c389ae48ddb53443f0577af0 100644
--- a/ros_acomms_modeling/CMakeLists.txt
+++ b/ros_acomms_modeling/CMakeLists.txt
@@ -65,10 +65,9 @@ add_message_files(FILES
add_service_files(
FILES
GetOptimalTxDepth.srv
- GetPlatformLocation.srv
SimTransmissionLoss.srv
SimPacketPerformance.srv
-
+ SimTravelTime.srv
)
## Generate actions in the 'action' folder
diff --git a/ros_acomms_modeling/src/modem_location_sim_node.py b/ros_acomms_modeling/src/modem_location_sim_node.py
new file mode 100755
index 0000000000000000000000000000000000000000..33ad1f78c272d1b1abae62af90bc5a7ee0ec5d56
--- /dev/null
+++ b/ros_acomms_modeling/src/modem_location_sim_node.py
@@ -0,0 +1,32 @@
+#!/usr/bin/env python3
+
+import rospy
+from std_msgs.msg import Header
+from ros_acomms_modeling.msg import Location
+import random
+
+
+class ModemLocationSimNode:
+ def __init__(self):
+ rospy.init_node('modem_location_sim')
+ rospy.loginfo("Starting modem_location_sim_node")
+ update_period = rospy.get_param('~period', 5)
+
+ self.location_publisher = rospy.Publisher('location', Location, latch=True, queue_size=10)
+ while not rospy.is_shutdown():
+ self.pub_location()
+ rospy.sleep(update_period)
+
+ def pub_location(self):
+ lat = random.randrange(41421580, 41553703) * (10 ** -6)
+ lon = random.randrange(70726027, 70843443) * -(10 ** -6)
+ depth = random.randrange(3, 18)
+ msg = Location(header=Header(stamp=rospy.get_rostime()), latitude=lat, longitude=lon, depth=depth)
+ self.location_publisher.publish(msg)
+
+
+if __name__ == '__main__':
+ try:
+ node = ModemLocationSimNode()
+ except rospy.ROSInterruptException:
+ rospy.loginfo("ModemLocationSimNode shutdown (interrupt)")
diff --git a/ros_acomms_modeling/src/modem_sim_node.py b/ros_acomms_modeling/src/modem_sim_node.py
index 364112c7ec245562924c74a4d54cda5fd112e078..08fed9b9cee8f3ffae9822d37fc8344f19df067d 100755
--- a/ros_acomms_modeling/src/modem_sim_node.py
+++ b/ros_acomms_modeling/src/modem_sim_node.py
@@ -1,11 +1,13 @@
#!/usr/bin/env python3
import rospy
-import sys
+import roslib.message
+from rospy import AnyMsg
from std_msgs.msg import Header, Float32
from ros_acomms_msgs.msg import PingReply, CST, TransmittedPacket, ReceivedPacket, Packet
from ros_acomms_msgs.srv import PingModem, PingModemResponse, QueueTxPacket, QueueTxPacketResponse
from ros_acomms_modeling.msg import SimPacket
-from ros_acomms_modeling.srv import SimTransmissionLoss, SimTransmissionLossRequest, SimPacketPerformance, SimPacketPerformanceRequest, GetPlatformLocationResponse
+from ros_acomms_modeling.srv import SimTransmissionLoss, SimTransmissionLossRequest, SimPacketPerformance, \
+ SimPacketPerformanceRequest, SimTravelTime, SimTravelTimeRequest, SimTravelTimeResponse
from queue import Queue, Empty, Full
from threading import Thread, Event
from collections import namedtuple
@@ -13,14 +15,9 @@ import numpy as np
from typing import Optional, List
# Use ros_groundtruth messages for sim if available, otherwise use internal versions.
try:
- from groundtruth.msg import Tick, Tock, Location
- from groundtruth.srv import ReadLocation, ReadLocationRequest, ReadLocationResponse
+ from groundtruth.msg import Tick, Tock
except ImportError:
- from ros_acomms_modeling.msg import Tick, Tock, Location
- from ros_acomms_modeling.srv import GetPlatformLocation as ReadLocation
- from ros_acomms_modeling.srv import GetPlatformLocationRequest as ReadLocationRequest
- from ros_acomms_modeling.srv import GetPlatformLocationResponse as ReadLocationResponse
-
+ from ros_acomms_modeling.msg import Tick, Tock
ActivePacket = namedtuple('ActivePacket', [
'sim_packet', 'arrival_time', 'finish_time', 'receive_level_db'])
@@ -32,6 +29,14 @@ def sum_incoherent_db(levels_in_db):
return 10.0 * np.log10(np.sum(10**(levels_in_db/10)))
+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 PingReplyTransaction(object):
PING_TYPE_NONE = 0
PING_TYPE_REQUEST = 1
@@ -145,26 +150,28 @@ class ModemSimNode(object):
else:
self.tock_topic_name = 'tock'
- self.modem_location_source = rospy.get_param('~modem_location_source', default='service')
- static_latitude = rospy.get_param('~latitude', default=None)
- static_longitude = rospy.get_param('~longitude', default=None)
- static_depth = rospy.get_param('~depth', default=None)
- # Check for errors
- if self.modem_location_source == 'service' and (static_latitude or static_longitude or static_depth):
- rospy.logwarn(
- 'Static modem sim position specified, but modem_location_source is not set to "static" or "message"')
- if self.modem_location_source in ('static', 'message'):
- if not (static_latitude and static_longitude and static_depth):
- rospy.logfatal(
- 'Static or message modem sim position selected, but initial position is not set (rosparms for latitude, longitude, depth)')
- sys.exit(1)
- self.static_location = Position(
- static_latitude, static_longitude, static_depth)
- if self.modem_location_source == 'message':
- rospy.Subscriber('location', Location, self.on_modem_location)
- else:
- rospy.loginfo("Modem sim node waiting for read_location service")
- rospy.wait_for_service('/read_location')
+ location_source_param = rospy.get_param('~modem_location_source', default=None)
+ if location_source_param:
+ rospy.logwarn("modem_location_source and the GetPlatformLocation service is no longer supported. " +
+ " Use modem_location_topic to specify a topic" +
+ " that publishes location (defaults to 'location') or use the latitude/longitude/depth" +
+ " params to specify a static location. This warning will be removed in a future release.")
+
+ modem_location_topic = rospy.get_param('~modem_location_topic', default='location')
+ latitude = rospy.get_param('~latitude', default=None)
+ longitude = rospy.get_param('~longitude', default=None)
+ depth = rospy.get_param('~depth', default=None)
+ if not longitude or not latitude or not depth:
+ # if we don't have a static position, we need to wait for a position to arrive.
+ rospy.loginfo(f"No static position was specified, waiting for modem location on {modem_location_topic}")
+ msg = rospy.wait_for_message(modem_location_topic, AnyMsg)
+ location = deserialze_anymsg(msg)
+ latitude = location.latitude
+ longitude = location.longitude
+ depth = location.depth
+
+ self.modem_location = Position(latitude, longitude, depth)
+ rospy.Subscriber('location', AnyMsg, self.on_modem_location)
self.rx_publisher = rospy.Publisher(
'packet_rx', ReceivedPacket, queue_size=self.maxsize)
@@ -188,6 +195,8 @@ class ModemSimNode(object):
rospy.loginfo("Modem sim node waiting for simulation services")
rospy.wait_for_service('/sim_transmission_loss')
rospy.wait_for_service('/sim_packet_performance')
+ rospy.wait_for_service('/sim_travel_time')
+ self.call_sim_travel_time = rospy.ServiceProxy('/sim_travel_time', SimTravelTime)
# Handling for sim to initiate and reply to Pings
self.ping_reply_publisher = rospy.Publisher(
@@ -213,14 +222,15 @@ class ModemSimNode(object):
rospy.loginfo("Modem sim node running{}. SRC={}, FC={} Hz, BW={} Hz".format(
" with sim ticks active" if self.use_sim_tick else "", self.src, self.fc, self.bw))
+ rospy.spin()
+
def on_ambient_noise(self, msg: Float32):
self.ambient_noise = msg.data
- def on_modem_location(self, msg):
- rospy.loginfo("New modem location: latitude={}, longitude={}, depth={}".format(msg.latitude,
- msg.longitude,
- msg.depth))
- self.static_location = Position(msg.latitude, msg.longitude, msg.depth)
+ def on_modem_location(self, msg: AnyMsg):
+ msg = deserialze_anymsg(msg)
+ rospy.logdebug(f"New modem location: latitude={msg.latitude}, longitude={msg.longitude}, depth={msg.depth}")
+ self.modem_location = Position(msg.latitude, msg.longitude, msg.depth)
def on_sim_tick(self, msg):
try:
@@ -238,13 +248,6 @@ class ModemSimNode(object):
# TODO: publish a TOCK with status=0x4
# TODO: other errors, respond with generic reset command (0x01)
- def get_ownship_location(self) -> Position:
- if self.modem_location_source in ('static', 'message'):
- return self.static_location
- else:
- location = self.call_get_platform_location()
- return Position(location.latitude, location.longitude, location.depth)
-
def format_ping_for_tx_queue(self, dest, queue=3, packet_type=3):
tx_packet = QueueTxPacket()
tx_packet.queue = queue
@@ -520,13 +523,26 @@ class ModemSimNode(object):
def check_if_packet_is_active(self, current_time: rospy.Time,
sim_packet: SimPacket,
rcv_platform_location: Position) -> Optional[ActivePacket]:
- recv_level, arrival_time, finish_time = self.get_transmission_loss_sync(
- sim_packet, rcv_platform_location)
+
+ # We first check the arrival time, which doesn't require calculating receive level (and therefore doesn't
+ # necessarily require running bellhop
+ response = self.call_sim_travel_time(SimTravelTimeRequest(src_latitude=sim_packet.src_latitude,
+ src_longitude=sim_packet.src_longitude,
+ src_depth=sim_packet.src_depth,
+ rcv_latitude=rcv_platform_location.latitude,
+ rcv_longitude=rcv_platform_location.longitude,
+ rcv_depth=rcv_platform_location.depth,
+ center_frequency_hz=sim_packet.center_frequency_hz))
+ arrival_time = sim_packet.transmit_time + response.travel_time
# If the packet hasn't arrived yet, we're done with this packet for now.
if arrival_time > current_time:
return None
+ # Once the packet arrives, we need receive level.
+ recv_level, arrival_time, finish_time = self.get_transmission_loss_sync(
+ sim_packet, rcv_platform_location)
+
# If this packet is in progress, we need to add it to the list of active packets.
# This list short be sorted, by virtue of the order in which things are added.
active_packet = ActivePacket(
@@ -544,7 +560,7 @@ class ModemSimNode(object):
return received_packet
def process_inwater_packets(self, current_time) -> None:
- rcv_platform_location = self.get_ownship_location()
+ rcv_platform_location = self.modem_location
# If we are using messages to get location, we may not have received a message yet.
if rcv_platform_location.latitude is None:
@@ -719,29 +735,7 @@ class ModemSimNode(object):
if ping:
return queue_tx_packet_resp, sim_packet
return queue_tx_packet_resp
-
- """ Calls GetPlatformLocation Service """
-
- def call_get_platform_location(self):
- try:
- rospy.logdebug("Trying to get platform location")
- get_platform_location = rospy.ServiceProxy(
- '/read_location', ReadLocation)
- platform_location_req = ReadLocationRequest()
- platform_location_req.name = rospy.get_namespace()
- platform_location = get_platform_location(platform_location_req)
- if not platform_location.valid:
- rospy.logerr("Platform not found")
- log_str = "MODEM %d: ReadLocationReply: Lat: %f, Lon: %f, Depth: %f" % (self.src,
- platform_location.latitude,
- platform_location.longitude,
- platform_location.depth)
- rospy.logdebug(log_str)
- return platform_location
- except rospy.ServiceException:
- rospy.logwarn_throttle(
- 1, "MODEM %d: Get Platform Location Service call failed", self.src)
-
+
""" Calls SimPacketPerformance Service """
def call_sim_packet_performance(self, sim_packet_performance_req):
@@ -791,7 +785,7 @@ class ModemSimNode(object):
try:
while not rospy.is_shutdown():
# get platform location
- platform_location = self.get_ownship_location()
+ platform_location = self.modem_location
if platform_location.latitude is None:
# This can happen if the source is 'message' and no message has been published yet
# Wait for actual wall-clock time, since this isn't really part of the simulation, and we can end
@@ -871,12 +865,6 @@ if __name__ == '__main__':
try:
node = ModemSimNode()
rospy.loginfo("MODEM %d: Sim Node started", node.src)
- rate = rospy.Rate(1)
-
- while not rospy.is_shutdown():
- rate.sleep()
-
- rospy.loginfo("MODEM %d: Sim Node shutdown", node.src)
except rospy.ROSInterruptException:
rospy.loginfo("MODEM: Sim Node shutdown (interrupt)")
diff --git a/ros_acomms_modeling/src/platform_location_node.py b/ros_acomms_modeling/src/platform_location_node.py
deleted file mode 100755
index d73ffd071a70938a22c248160774c8881ae5fe2c..0000000000000000000000000000000000000000
--- a/ros_acomms_modeling/src/platform_location_node.py
+++ /dev/null
@@ -1,30 +0,0 @@
-#!/usr/bin/env python3
-
-import rospy
-from ros_acomms_modeling.srv import GetPlatformLocation, GetPlatformLocationResponse
-import random
-
-
-class PlatformLocationNode(object):
- def __init__(self):
- rospy.init_node('platform_location_node')
- rospy.loginfo("Starting get_platform_location_node")
- platform_location = rospy.Service(
- 'read_location', GetPlatformLocation, self.handle_get_platform_location)
- rospy.spin()
-
- def handle_get_platform_location(self, req):
- rospy.logdebug("Getting Platform Location for SRC %s", req.name)
- # Using same setup as test_sim_transmission_loss - don't care about the source
- lat = random.randrange(41421580, 41553703) * (10 ** -6)
- lon = random.randrange(70726027, 70843443) * -(10 ** -6)
- depth = random.randrange(3, 18)
- return GetPlatformLocationResponse(latitude=lat, longitude=lon, depth=depth, valid=True)
-
-
-if __name__ == '__main__':
- try:
- node = PlatformLocationNode()
-
- except rospy.ROSInterruptException:
- rospy.loginfo("get_platform_location_node shutdown (interrupt)")
diff --git a/ros_acomms_modeling/src/sim_packet_performance_node.py b/ros_acomms_modeling/src/sim_packet_performance_node.py
index 22fc3c8593a5310f4cd2feee26c292903b1d1b1b..3ed8a04b6dea6a971cf85022b709d946a9d5b91a 100755
--- a/ros_acomms_modeling/src/sim_packet_performance_node.py
+++ b/ros_acomms_modeling/src/sim_packet_performance_node.py
@@ -45,7 +45,7 @@ class SimPacketPerformanceNode(object):
sim_service = rospy.Service(
'sim_packet_performance', SimPacketPerformance, self.handle_sim)
- sim_service.spin()
+ rospy.spin()
def handle_sim(self, req):
rospy.logdebug("Simulating Packet Performance:\r\n" + str(req))
diff --git a/ros_acomms_modeling/src/sim_transmission_loss_node.py b/ros_acomms_modeling/src/sim_transmission_loss_node.py
index 3b5100cd173ece25e0f1c9df8ba8035556a97822..cd96a8d32b4f9734f6892a96f87c624551ba98ce 100755
--- a/ros_acomms_modeling/src/sim_transmission_loss_node.py
+++ b/ros_acomms_modeling/src/sim_transmission_loss_node.py
@@ -1,16 +1,18 @@
#!/usr/bin/env python3
import rospy
-from ros_acomms_modeling.srv import SimTransmissionLoss, SimTransmissionLossResponse, GetOptimalTxDepth, GetOptimalTxDepthResponse
+from ros_acomms_modeling.srv import SimTransmissionLoss, SimTransmissionLossResponse, GetOptimalTxDepth, \
+ GetOptimalTxDepthResponse, SimTravelTime, SimTravelTimeRequest, SimTravelTimeResponse
from ros_acomms_msgs.msg import SoundSpeedProfile
import numpy as np
from sys import float_info as fi
-import arlpy.uwapm as pm
+import whoi_uwapm.uwapm as pm
from geopy.distance import distance
from shutil import which # Used to test if bellhop is available
+models = ['bellhop', 'bellhopcxx']
class SimTransmissionLossNode(object):
def __init__(self):
@@ -19,10 +21,10 @@ class SimTransmissionLossNode(object):
rospy.loginfo("Starting sim_transmission_loss_node...")
# Get node parameters
+ self.model = rospy.get_param('~model', default='bellhopcxx')
self.sound_speed = rospy.get_param('~sound_speed', default=1500)
self.water_depth = rospy.get_param('~water_depth', default=1000)
self.plot = rospy.get_param('~plot', default=False)
- self.model = rospy.get_param('~model', default='bellhop')
self.bellhop_env_nbeams = rospy.get_param(
'~bellhop_env_nbeams', default=3000)
self.bellhop_transmission_loss_mode = rospy.get_param(
@@ -30,14 +32,7 @@ class SimTransmissionLossNode(object):
self.bellhop_arrivals = rospy.get_param(
'~bellhop_arrivals', default=False)
- if self.model == 'bellhop':
- # Check that bellhop.exe is available. If it isn't, crash our now rather than waiting until we try to
- # simulate.
- if which('bellhop.exe') is None:
- rospy.logfatal("bellhop.exe was not found on your path (check the ros_acomms README for instructions). "
- "sim_transmission_loss_node terminating.")
- exit()
-
+ if self.model in models:
# Check node parameters
# TODO: check validity of the parameters here and output contents to log
@@ -108,6 +103,7 @@ class SimTransmissionLossNode(object):
sim_service = rospy.Service(
'sim_transmission_loss', SimTransmissionLoss, self.handle_sim_transmission_loss)
+ rospy.Service('sim_travel_time', SimTravelTime, self.handle_sim_travel_time)
optimal_tx_depth_service = rospy.Service(
'get_optimal_tx_depth', GetOptimalTxDepth, self.handle_optimal_tx_depth)
@@ -149,6 +145,26 @@ class SimTransmissionLossNode(object):
return SimTransmissionLossResponse(rcv_rx_level_db=receive_level_db,
transmission_delay=rospy.Duration.from_sec(latency))
+ def handle_sim_travel_time(self, req: SimTravelTimeRequest) -> SimTravelTimeResponse:
+ rospy.logdebug(f"Simulating Travel Time: {req}")
+ # Convert lat,lon to compute range
+ src = (req.src_latitude, req.src_longitude)
+ rcv = (req.rcv_latitude, req.rcv_longitude)
+
+ horizontal_range = distance(src, rcv, ellipsoid='WGS-84').meters
+
+ # Compute arrival time and convert to latency
+ if self.bellhop_arrivals:
+ travel_time = self.get_bellhop_latency(
+ horizontal_range, req.src_depth, req.rcv_depth, req.center_frequency_hz)
+ model = 'bellhop'
+ else:
+ travel_time = self.get_geometric_latency(
+ horizontal_range, req.src_depth, req.rcv_depth)
+ model = 'geometric'
+
+ return SimTravelTimeResponse(travel_time=rospy.Duration.from_sec(travel_time), model=model)
+
def handle_optimal_tx_depth(self, req):
rospy.logdebug("Calculating optimal TX depth:\r\n" + str(req))
@@ -172,7 +188,7 @@ class SimTransmissionLossNode(object):
return GetOptimalTxDepthResponse(rcv_rx_level_db=best_rl, optimal_tx_depth=best_depth)
def get_receive_level(self, horizontal_range, src_depth, dest_depth, center_frequency=10000, tx_level_db=185):
- if self.model == 'bellhop':
+ if self.model in models:
return self.get_receive_level_bellhop(horizontal_range, src_depth, dest_depth,
center_frequency, tx_level_db)
else:
@@ -188,7 +204,7 @@ class SimTransmissionLossNode(object):
# Run bellhop transmission loss
time1 = rospy.get_time()
tloss = pm.compute_transmission_loss(
- self.env, mode=self.bellhop_transmission_loss_mode, model='bellhop')
+ self.env, mode=self.bellhop_transmission_loss_mode, model=self.model)
time2 = rospy.get_time()
rospy.logdebug("Bellhop transmission loss takes [" + str(self.bellhop_env_nbeams) + " beams]: " + str(
time2 - time1) + " seconds")
@@ -207,7 +223,7 @@ class SimTransmissionLossNode(object):
self.env['frequency'] = center_frequency
time1 = rospy.get_time()
- arrivals = pm.compute_arrivals(self.env, model='bellhop')
+ arrivals = pm.compute_arrivals(self.env, model=self.model)
latency = arrivals['time_of_arrival'].iloc[0]
time2 = rospy.get_time()
rospy.logdebug("Bellhop arrival times takes: " +
diff --git a/ros_acomms_modeling/srv/GetPlatformLocation.srv b/ros_acomms_modeling/srv/GetPlatformLocation.srv
deleted file mode 100644
index afd93ebdd4627c006273659b51c997817a928ddb..0000000000000000000000000000000000000000
--- a/ros_acomms_modeling/srv/GetPlatformLocation.srv
+++ /dev/null
@@ -1,9 +0,0 @@
-# Request
-string name
-
----
-# Response
-bool valid
-float32 latitude
-float32 longitude
-float32 depth
\ No newline at end of file
diff --git a/ros_acomms_modeling/srv/SimTravelTime.srv b/ros_acomms_modeling/srv/SimTravelTime.srv
new file mode 100644
index 0000000000000000000000000000000000000000..b7e47280e2aef8c0a2afe44a12ca4278623c80e0
--- /dev/null
+++ b/ros_acomms_modeling/srv/SimTravelTime.srv
@@ -0,0 +1,38 @@
+### Request
+
+# Source Info
+# Source Latitude [decimal degrees]. Positive is north of equator; negative is south
+# (-90 <= latitude <= +90).
+float32 src_latitude
+
+# Source Longitude [decimal degrees]. Positive is east of prime meridian; negative is west
+# (-180 <= longitude <= +180)
+float32 src_longitude
+
+# Source Depth [m]. Positive is below the WGS 84 ellipsoid.
+float32 src_depth
+
+# Receiver Info
+# Receiver Latitude [decimal degrees]. Positive is north of equator; negative is south
+# (-90 <= latitude <= +90).
+float32 rcv_latitude
+
+# Receiver Longitude [decimal degrees]. Positive is east of prime meridian; negative is west
+# (-180 <= longitude <= +180)
+float32 rcv_longitude
+
+# Receiver Depth [m]. Positive is below the WGS 84 ellipsoid.
+float32 rcv_depth
+
+# Center frequency [hz]
+float32 center_frequency_hz
+
+---
+
+### Response
+
+# Acoustic travel time between source and receiver
+duration travel_time
+
+# Name of model used to calculate travel time
+string model
diff --git a/ros_acomms_tests/launch/dynamic_codec_config.yaml b/ros_acomms_tests/launch/dynamic_codec_config.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..09e2b870980fde1db1e98e2a112b0fb9cb471424
--- /dev/null
+++ b/ros_acomms_tests/launch/dynamic_codec_config.yaml
@@ -0,0 +1,24 @@
+- codec_name: default
+ match_src: []
+ match_dest: []
+ except_src: []
+ except_dest: []
+ packet_codec: ros
+ miniframe_header: [0xac]
+ dataframe_header: []
+ remove_headers: true
+ message_codecs:
+ - id: 103
+ message_codec: default
+ dynamic_queue_service: "get_next"
+ dynamic_update_topic: "update_queue"
+ publish_topic: "from_acomms/test_dynamic_msg"
+ ros_type: "std_msgs/UInt8"
+ default_dest: 121
+ queue_order: fifo
+ queue_maxsize: 10
+ allow_fragmentation: false
+ priority: 10
+ fields:
+ data:
+ codec: uint8
diff --git a/ros_acomms_tests/launch/dynamic_queue_test.launch b/ros_acomms_tests/launch/dynamic_queue_test.launch
new file mode 100644
index 0000000000000000000000000000000000000000..44ee15fa78f4fe6d286de5b9453367f2eb9eb9aa
--- /dev/null
+++ b/ros_acomms_tests/launch/dynamic_queue_test.launch
@@ -0,0 +1,68 @@
+<launch>
+ <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${time}] [${node}]: ${message}"/>
+
+ <param name="use_sim_time" value="true"/>
+
+ <group ns="modem0">
+ <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" />
+ <param name="modem_location_source" value="local_service" />
+ </node>
+
+ <node name="tdma" pkg="ros_acomms" type="tdma_node.py" output="screen" required="true" respawn="false">
+ <param name="num_slots" value="2"/>
+ <param name="active_slots" value="0"/>
+ <param name="slot_duration_seconds" value="30" />
+ </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/dynamic_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/dynamic_codec_config.yaml"/>
+ </node>
+
+ <node name="modem_location_sim" pkg="ros_acomms_modeling" type="modem_location_sim_node.py" />
+ </group>
+
+ <group ns="modem1">
+ <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" />
+ <param name="modem_location_source" value="local_service" />
+ </node>
+
+ <node name="tdma" pkg="ros_acomms" type="tdma_node.py" output="screen" required="true" respawn="false">
+ <param name="num_slots" value="2"/>
+ <param name="active_slots" value="1"/>
+ <param name="slot_duration_seconds" value="30" />
+ </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/dynamic_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/dynamic_codec_config.yaml"/>
+ </node>
+
+ <node name="modem_location_sim" pkg="ros_acomms_modeling" type="modem_location_sim_node.py" />
+ </group>
+
+ <node name="sim_packet_performance_node" pkg="ros_acomms_modeling" type="sim_packet_performance_node.py" />
+
+ <node name="sim_transmission_loss_node" pkg="ros_acomms_modeling" type="sim_transmission_loss_node.py" >
+ <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" />-->
+
+ <node name="clock_generator" pkg="ros_acomms_modeling" type="clock_generator.py" />
+
+</launch>
diff --git a/ros_acomms_tests/launch/message_codec_config.yaml b/ros_acomms_tests/launch/message_codec_config.yaml
index aad516c8c4e1085f9b118745a6093db683cdfea9..7ebb705067ec5834bcf879d6686a429963e54655 100644
--- a/ros_acomms_tests/launch/message_codec_config.yaml
+++ b/ros_acomms_tests/launch/message_codec_config.yaml
@@ -38,4 +38,4 @@
priority: 50
fields:
data:
- codec: uint8
\ No newline at end of file
+ codec: uint8
diff --git a/ros_acomms_tests/launch/test.launch b/ros_acomms_tests/launch/test.launch
index a8effb12eb94eaf4f1e2751c8034d78de6baa66d..dfba6692c982e79c768077e7b3389391a16c1917 100644
--- a/ros_acomms_tests/launch/test.launch
+++ b/ros_acomms_tests/launch/test.launch
@@ -8,7 +8,6 @@
<param name="center_frequency_hz" value="10000" 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="tdma" pkg="ros_acomms" type="tdma_node.py" output="screen" required="true" respawn="false">
@@ -25,6 +24,8 @@
<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" />
</group>
<group ns="modem1">
@@ -32,7 +33,6 @@
<param name="center_frequency_hz" value="10000" 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="tdma" pkg="ros_acomms" type="tdma_node.py" output="screen" required="true" respawn="false">
@@ -49,13 +49,11 @@
<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" />
</group>
<node name="sim_packet_performance_node" pkg="ros_acomms_modeling" type="sim_packet_performance_node.py" />
- <node name="platform_location_node" pkg="ros_acomms_modeling" type="platform_location_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" />
diff --git a/ros_acomms_tests/src/test_dynamic_queue.py b/ros_acomms_tests/src/test_dynamic_queue.py
new file mode 100755
index 0000000000000000000000000000000000000000..718f8d91ddbd8455a6c63dc2bd8cf29700a20e68
--- /dev/null
+++ b/ros_acomms_tests/src/test_dynamic_queue.py
@@ -0,0 +1,100 @@
+#! /usr/bin/env python3
+
+
+'''
+test_dynamic_queue
+------------------
+
+tests for dynamic queue in ros accoms
+'''
+
+
+import os
+import sys
+import pytest
+import rospy
+import rospkg
+import roslaunch
+from std_msgs.msg import UInt8
+
+from ros_acomms_msgs.srv import GetNextQueuedMessage, GetNextQueuedMessageRequest, GetNextQueuedMessageResponse
+from ros_acomms_msgs.msg import UpdateQueuedMessage
+
+class Helper():
+ def __init__(self) -> None:
+ rospy.Service('modem0/get_next', GetNextQueuedMessage, self.handle_get_next)
+ rospy.Subscriber('modem0/update_queue', UpdateQueuedMessage, self.on_update_queue)
+
+ rospy.Service('modem1/get_next', GetNextQueuedMessage, self.handle_get_next)
+ rospy.Subscriber('modem1/update_queue', UpdateQueuedMessage, self.on_update_queue)
+ self.hasData = None
+
+ def handle_get_next(self, req: GetNextQueuedMessageRequest) -> None :
+ '''
+ :param req: service request data
+ '''
+ if self.hasData is not None:
+ payload = bytes([6])
+ self.hasData = None if self.hasData == 0 else self.hasData-1
+ return GetNextQueuedMessageResponse(has_message=True,
+ message_id=0,
+ dest_address=121,
+ priority=10,
+ data=payload,
+ data_size_in_bits=len(payload)*8,
+ )
+
+ else:
+ return GetNextQueuedMessageResponse(has_message=False)
+
+ def on_update_queue(self, update_msg: UpdateQueuedMessage):
+ pass
+
+
+class TestDynamicQueue:
+ '''ros_acomms dynamic queue test class'''
+
+ @classmethod
+ def setup_class(self):
+ """
+ setup function for tests
+ """
+
+ rospy.init_node("test_dynamic_queue", log_level=rospy.INFO)
+
+ self.rospack = rospkg.RosPack()
+
+ uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
+ roslaunch.configure_logging(uuid)
+
+ launch_file = os.path.join(
+ self.rospack.get_path("ros_acomms_tests"),
+ "launch/dynamic_queue_test.launch",
+ )
+ self.helper = Helper()
+ self.launch = roslaunch.parent.ROSLaunchParent(uuid, [launch_file])
+ self.launch.start()
+
+
+ def test_message_recieved(self):
+ '''
+ Test for simple dynamic queue message, test for message transmission and marked as transmitted
+ '''
+ self.helper.hasData = 2
+ updateMsg = rospy.wait_for_message(
+ "/modem0/update_queue", UInt8, timeout=100)
+ ret = rospy.wait_for_message(
+ "/modem1/from_acomms/test_dynamic_msg", UInt8, timeout=100)
+ assert ret.data == 6, f"return: {ret.data}"
+ assert updateMsg.event == 2, f"update msg event not marked as transmitted"
+
+ @classmethod
+ def teardown_class(self):
+ """
+ teardown function for tests
+ """
+
+ self.launch.shutdown()
+
+if __name__ == "__main__":
+ sys.exit(pytest.main(['--capture=no', __file__]))
diff --git a/ros_acomms_tests/src/test_ros_acomms.py b/ros_acomms_tests/src/test_ros_acomms.py
index 1f43fabdeacffa10dc3feefaa689a1635769122b..b5d4b394baac00e65c7cdb8d2b9363838d35b87d 100755
--- a/ros_acomms_tests/src/test_ros_acomms.py
+++ b/ros_acomms_tests/src/test_ros_acomms.py
@@ -74,7 +74,6 @@ class TestRosAcomms:
rospy.wait_for_message(
"/modem1/from_acomms/test_fragmentation_msg", FragmentationTestMsg, timeout=100)
-
assert msg.hashstr == get_hash(msg.data)
except rospy.exceptions.ROSException:
diff --git a/setup.bash b/setup.bash
deleted file mode 100755
index 1388bc9df812dc90874d86b7ffbabfb3e27c5357..0000000000000000000000000000000000000000
--- a/setup.bash
+++ /dev/null
@@ -1,9 +0,0 @@
- #!/bin/bash
-
-pip install -U -r requirements.txt
-cd ~
-git clone https://git.whoi.edu/dgiaya/acoustic_toolbox.git
-cd acoustic_toolbox
-sudo apt-get install gfortran -y
-make
-sudo ln -s ~/acoustic_toolbox/Bellhop/bellhop.exe /usr/bin/bellhop.exe