From d58bae0464f79c2568beccf4f52690dde8331dbd Mon Sep 17 00:00:00 2001
From: Caileigh Fitzgerald <cfitzgerald@whoi.edu>
Date: Tue, 28 May 2024 18:46:55 +0000
Subject: [PATCH 1/4] fixed bug in logic for depth. Depths should be positive
---
ros_acomms_modeling/src/sim_transmission_loss_node.py | 8 ++++----
1 file changed, 4 insertions(+), 4 deletions(-)
diff --git a/ros_acomms_modeling/src/sim_transmission_loss_node.py b/ros_acomms_modeling/src/sim_transmission_loss_node.py
index 81ae11f9..b74b39ce 100755
--- a/ros_acomms_modeling/src/sim_transmission_loss_node.py
+++ b/ros_acomms_modeling/src/sim_transmission_loss_node.py
@@ -215,12 +215,12 @@ class SimTransmissionLossNode(object):
rospy.logerr(f"Receiver depth ({dest_depth}) is greater than water depth ({self.water_depth}). Can't calculate receive level; returning 0dB")
return 0
- if src_depth >= 0:
- rospy.logwarn(f"Source depth is 0; setting it to 0.1 to calculate receive level")
+ if src_depth <= 0:
+ rospy.logwarn(f"Source depth <= 0; setting it to 0.1 to calculate receive level")
src_depth = 0.1
- if dest_depth >= 0:
- rospy.logwarn(f"Receiver depth is 0; setting it to 0.1 to calculate receive level")
+ if dest_depth <= 0:
+ rospy.logwarn(f"Receiver depth <= 0; setting it to 0.1 to calculate receive level")
dest_depth = 0.1
return self.get_receive_level_bellhop(horizontal_range, src_depth, dest_depth,
--
GitLab
From d85d0fc937be5cb5f8129703775b96ebefc9f36f Mon Sep 17 00:00:00 2001
From: Caileigh Fitzgerald <cfitzgerald@whoi.edu>
Date: Tue, 28 May 2024 18:48:57 +0000
Subject: [PATCH 2/4] removed distance_haversine calculation in favor of geopy
method used in sim_tramission_loss. Added class attr for checking if we are
using sim_time
---
ros_acomms_modeling/src/modem_sim_node.py | 55 +++++++++++------------
1 file changed, 25 insertions(+), 30 deletions(-)
diff --git a/ros_acomms_modeling/src/modem_sim_node.py b/ros_acomms_modeling/src/modem_sim_node.py
index ed262fb7..0c0c6145 100755
--- a/ros_acomms_modeling/src/modem_sim_node.py
+++ b/ros_acomms_modeling/src/modem_sim_node.py
@@ -12,7 +12,7 @@ from queue import Queue, Empty, Full
from threading import Thread, Event
from collections import namedtuple, deque
import datetime
-import math
+from geopy.distance import distance as geopy_distance
import numpy as np
from typing import Optional, List
# Use ros_groundtruth messages for sim if available, otherwise use internal versions.
@@ -36,27 +36,6 @@ def deserialze_anymsg(msg_data: AnyMsg):
msg.deserialize(msg_data._buff)
return msg
-def distance_haversine(lat1, lon1, lat2, lon2, meters=False):
- # approximate radius of earth in km
- R = 6373.0 * 1000
-
- lat1 = math.radians(lat1)
- lon1 = math.radians(lon1)
- lat2 = math.radians(lat2)
- lon2 = math.radians(lon2)
-
- dlon = lon2 - lon1
- dlat = lat2 - lat1
-
- a = math.sin(dlat / 2) ** 2 + math.cos(lat1) * math.cos(lat2) * math.sin(dlon / 2) ** 2
- c = 2 * math.atan2( math.sqrt(a), math.sqrt(1 - a))
-
- distance = R * c
-
- if meters:
- return distance
- return distance / 1000
-
class PingReplyTransaction(object):
PING_TYPE_NONE = 0
PING_TYPE_REQUEST = 1
@@ -168,6 +147,14 @@ class ModemSimNode(object):
else:
self.tock_topic_name = 'tock'
+ use_sim_time_name = rospy.search_param('use_sim_time')
+ if use_sim_time_name:
+ self.use_sim_time = rospy.get_param(
+ use_sim_time_name, default=False)
+ else:
+ self.use_sim_time = False
+ rospy.loginfo("use_sim_time {} ".format(self.use_sim_time))
+
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. " +
@@ -568,22 +555,30 @@ class ModemSimNode(object):
recv_level = transmission_loss.rcv_rx_level_db # receive source level
arrival_time = transmission_loss.transmission_delay + sim_packet.transmit_time
finish_time = arrival_time + sim_packet.transmit_duration
- arrival_time_str = str(datetime.datetime.fromtimestamp(arrival_time.to_sec()))
+ arrival_time_str = str(datetime.datetime.utcfromtimestamp(arrival_time.to_sec()))
+ src = (sim_packet.src_latitude, sim_packet.src_longitude)
+ rcv = (rcv_platform_location.latitude, rcv_platform_location.longitude)
rospy.loginfo(
- "MODEM %d: T2: Receive level: %f Arrival time: %s",
- self.src, recv_level, arrival_time_str
+ "MODEM %d: T2: Receive level: %s Arrival time: %s SRC: (%s) RCV: (%s)",
+ self.src, f'{recv_level:.2f}', arrival_time_str,
+ f'{src[0]:.2f}, {src[1]:.2f}, {sim_packet.src_depth:.2f}',
+ f'{rcv[0]:.2f}, {rcv[1]:.2f}, {rcv_platform_location.depth:.2f}'
)
try:
owtt = (arrival_time - sim_packet.transmit_time).to_sec()
- distance = distance_haversine(sim_packet.src_latitude, sim_packet.src_longitude, rcv_platform_location.latitude, rcv_platform_location.longitude)
- sound_speed = (distance * 1000) / owtt
+ if owtt <= 0:
+ rospy.logwarn(f'WARNING: reception with OWTT <= 0! owtt: {owtt}. Setting to 0.00066 for range calc.. (1m @ 1500m/s)')
+ owtt = 0.00066
+ distance_m = geopy_distance(src, rcv, ellipsoid='WGS-84').meters
+ sound_speed = distance_m / owtt
rospy.loginfo(
- "MODEM %d: T2: SRC lat/lon: %f, %f RCV lat/lon: %f, %f Distance (km): %s OWTT: %s Sound Speed (m/s): %s",
- self.src, sim_packet.src_latitude, sim_packet.src_longitude, rcv_platform_location.latitude, rcv_platform_location.longitude,
- f'{distance:.2f}', f'{owtt:.2f}', f'{sound_speed:.2f}',
+ "MODEM %d: T2: Distance (km): %s, OWTT: %s Sound Speed (m/s): %s",
+ self.src, f'{distance_m / 1000:.2f}', f'{owtt:.2f}', f'{sound_speed:.2f}',
)
except:
+ # TODO: we don't want to spam the user with warnings but, we need to handle the case where location params are,
+ # .. out of range/invalid. we handle owtt <= 0 but, other cases should potentially be handled here.
pass
return recv_level, arrival_time, finish_time
--
GitLab
From 2978cf95938cd526d41eaed7a6efb1d91f90dc17 Mon Sep 17 00:00:00 2001
From: Caileigh Fitzgerald <cfitzgerald@whoi.edu>
Date: Tue, 28 May 2024 18:49:54 +0000
Subject: [PATCH 3/4] updates to modem_location_sim simple_path. Default mode
unchanged. code cleaned up
---
.../src/modem_location_sim_node.py | 37 ++++++++++++++-----
1 file changed, 27 insertions(+), 10 deletions(-)
diff --git a/ros_acomms_modeling/src/modem_location_sim_node.py b/ros_acomms_modeling/src/modem_location_sim_node.py
index 77a107b1..d9aacb41 100755
--- a/ros_acomms_modeling/src/modem_location_sim_node.py
+++ b/ros_acomms_modeling/src/modem_location_sim_node.py
@@ -15,36 +15,53 @@ class ModemLocationSimNode:
self.simple_path = rospy.get_param('simple_path', False)
self.random_seed = rospy.get_param('random_seed', None)
- self._start_lat = 41553703
- self._start_lon = 70843443
- self.offset = 1000
+ self.min_depth = rospy.get_param('min_depth', 3)
+ self.max_depth = rospy.get_param('max_depth', 18)
+ self.offset = rospy.get_param('simple_path_offset', 10)
+
+ self._start_lat = 41421580
+ self._start_lon = 70726027
+ self._end_lat = 41553703
+ self._end_lon = 70843443
+
self.rng = default_rng(self.random_seed)
self.location_publisher = rospy.Publisher('location', Location, latch=True, queue_size=10)
+
+ published_origin = False
while not rospy.is_shutdown():
if self.simple_path:
- start_lat, start_lon = self.next_start_lat_lon()
+ if published_origin:
+ start_lat, start_lon = self.next_start_lat_lon()
+ else:
+ start_lat, start_lon = self._start_lat, self.start_lon
+ published_origin = True
self.pub_path_location(start_lat, start_lon)
else:
self.pub_location()
rospy.sleep(update_period)
+ def next_depth(self):
+ if self.min_depth > self.max_depth:
+ return self.rng.uniform(self.max_depth, self.min_depth)
+ return self.rng.uniform(self.min_depth, self.max_depth)
+
def next_start_lat_lon(self):
- self._start_lat += self.rng.uniform(-10, 10)
- self._start_lon += self.rng.uniform(-10, 10)
+ self._start_lat += self.rng.uniform(-5, 5)
+ self._start_lon += self.rng.uniform(-5, 5)
return self._start_lat, self._start_lon
def pub_path_location(self, start_lat, start_lon):
lat = self.rng.uniform(start_lat, start_lat + self.offset) * (10 ** -6)
lon = self.rng.uniform(start_lon, start_lon + self.offset) * -(10 ** -6)
- depth = self.rng.uniform(3, 18)
+ depth = self.next_depth()
msg = Location(header=Header(stamp=rospy.get_rostime()), latitude=lat, longitude=lon, depth=depth)
self.location_publisher.publish(msg)
def pub_location(self):
- lat = self.rng.uniform(41421580, 41553703) * (10 ** -6)
- lon = self.rng.uniform(70726027, 70843443) * -(10 ** -6)
- depth = self.rng.uniform(3, 18)
+ lat = self.rng.uniform(self._start_lat, self._end_lat) * (10 ** -6)
+ lon = self.rng.uniform(self._start_lon, self._end_lon) * -(10 ** -6)
+ depth = self.next_depth()
msg = Location(header=Header(stamp=rospy.get_rostime()), latitude=lat, longitude=lon, depth=depth)
self.location_publisher.publish(msg)
--
GitLab
From 6eb2838ac92af96e0f5ca4500e1566370364fb24 Mon Sep 17 00:00:00 2001
From: Caileigh Fitzgerald <cfitzgerald@whoi.edu>
Date: Tue, 28 May 2024 18:52:41 +0000
Subject: [PATCH 4/4] added non-default rosparam ~start_sim_at_current_time.
clock generator now has an option to start the sim clock relative to the
starting time. Otherwise, default behavior starts sim clock at 0.0
---
ros_acomms_modeling/src/clock_generator.py | 12 +++++++++++-
1 file changed, 11 insertions(+), 1 deletion(-)
diff --git a/ros_acomms_modeling/src/clock_generator.py b/ros_acomms_modeling/src/clock_generator.py
index 8cf554ab..3e26e746 100755
--- a/ros_acomms_modeling/src/clock_generator.py
+++ b/ros_acomms_modeling/src/clock_generator.py
@@ -9,6 +9,7 @@ This script contains the clock_generator class which publishes a clock message a
import rospy
from rosgraph_msgs.msg import Clock
import time
+import datetime
class ClockGenerator:
"""
@@ -22,8 +23,17 @@ class ClockGenerator:
clock_publisher = rospy.Publisher('clock', Clock, queue_size=10)
rate = rospy.get_param("~publish_rate", 50)
sim_speed_multiplier = rospy.get_param("~multiplier", 5)
+ start_sim_at_current_time = rospy.get_param("~start_sim_at_current_time", False)
sim_clock_msg = Clock()
- current_time = rospy.get_time()
+
+ if not start_sim_at_current_time:
+ # starts the sim from 0.0 (rospy.get_time() returns 0.0 before msg is published on /clock)
+ current_time = rospy.get_time()
+ else:
+ # rather than start the sim from 0, we can start it relative to launch
+ current_time = datetime.datetime.now().timestamp()
+ sim_clock_msg.clock = rospy.Time.from_sec(current_time)
+ clock_publisher.publish(sim_clock_msg)
while not rospy.is_shutdown():
current_time += (1 / rate) * sim_speed_multiplier
--
GitLab