diff --git a/ros_acomms_modeling/CMakeLists.txt b/ros_acomms_modeling/CMakeLists.txt
index 5d909ff750edc0e31d8ca6b6cd1d3b599eb0755c..5fa26749710429b541860a722b58f12aa9d6224c 100644
--- a/ros_acomms_modeling/CMakeLists.txt
+++ b/ros_acomms_modeling/CMakeLists.txt
@@ -65,7 +65,6 @@ add_message_files(FILES
add_service_files(
FILES
GetOptimalTxDepth.srv
- GetPlatformLocation.srv
SimTransmissionLoss.srv
SimPacketPerformance.srv
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..7e48c0d69f60c7538644fcdbb1ad436e1754ae7e 100755
--- a/ros_acomms_modeling/src/modem_sim_node.py
+++ b/ros_acomms_modeling/src/modem_sim_node.py
@@ -1,11 +1,12 @@
#!/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
from queue import Queue, Empty, Full
from threading import Thread, Event
from collections import namedtuple
@@ -13,14 +14,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 +28,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 +149,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)
@@ -213,14 +219,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 +245,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
@@ -544,7 +544,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 +719,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 +769,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 +849,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 91723191b09066c2b1dac691d05779b8940334d9..0b3e69ce0b00e6cb4a45bb1e01ee51029495051b 100755
--- a/ros_acomms_modeling/src/sim_transmission_loss_node.py
+++ b/ros_acomms_modeling/src/sim_transmission_loss_node.py
@@ -19,6 +19,7 @@ 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)
@@ -181,7 +182,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")
@@ -200,7 +201,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_tests/launch/test.launch b/ros_acomms_tests/launch/test.launch
index 2a54438e269ee1c15a86b815017821e4574a6eef..f7c848669e66d57074a4f51804eb705d5e708959 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,20 +49,16 @@
<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" />
</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_tests" type="clock_generator.py" />
</launch>