From dd3592bf0374ed8bc6f2bd2a20ee7accee9300b5 Mon Sep 17 00:00:00 2001
From: Eric Gallimore <egallimore@whoi.edu>
Date: Fri, 1 Mar 2024 18:16:23 -0500
Subject: [PATCH 1/5] Optionally query battery info from the modem (via $CCBAT)
in the modem_sensor_data_node.
---
ros_acomms/src/modem_sensor_data_node.py | 73 +++++++++++++++++++++---
1 file changed, 65 insertions(+), 8 deletions(-)
diff --git a/ros_acomms/src/modem_sensor_data_node.py b/ros_acomms/src/modem_sensor_data_node.py
index 6e271a45..88a34a99 100755
--- a/ros_acomms/src/modem_sensor_data_node.py
+++ b/ros_acomms/src/modem_sensor_data_node.py
@@ -1,9 +1,15 @@
#!/usr/bin/env python3
+import sys
import rospy
from std_msgs.msg import String, Header
+from sensor_msgs.msg import BatteryState
from ros_acomms_msgs.msg import ModemSensorData
+def get_float_from_string_with_units(input_string: str) -> float:
+ return float(''.join(c for c in input_string if (c.isdigit() or c == '.' or c == '-')))
+
+
class ModemSensorDataNode(object):
fields = {'pwramp.temperature_degC': 'pwramp_temperature',
'pwramp.vbat': 'pwramp_vbat',
@@ -15,11 +21,22 @@ class ModemSensorDataNode(object):
rospy.init_node('modem_sensor_data')
self.query_interval_seconds = rospy.get_param('~query_interval_seconds', 30)
+ self.enable_battery_query = rospy.get_param('~enable_battery_query', default=False)
+ minimum_interval = 5 if not self.query_battery else 30
+ if self.query_interval_seconds < minimum_interval:
+ rospy.logwarn(f'Query interval must be at least {minimum_interval} seconds. Setting to {minimum_interval}')
+ self.query_interval_seconds = minimum_interval
self.modem_sensor_data_publisher = rospy.Publisher('modem_sensor_data', ModemSensorData, queue_size=10)
+ if self.query_battery:
+ self.battery_state_publisher = rospy.Publisher('battery_state', BatteryState, queue_size=10)
self.nmea_publisher = rospy.Publisher('nmea_to_modem', String, queue_size=10)
self.value_dict = {}
+ self.battery_percent = 0
+ self.battery_voltage_mV = 0
+ self.battery_current_mA = 0
+ self.battery_remaining_capacity_mAh = 0
rospy.Subscriber('nmea_from_modem', String, self.on_nmea_subscriber)
@@ -31,8 +48,13 @@ class ModemSensorDataNode(object):
while not rospy.is_shutdown():
self.query_modem()
rospy.sleep(2)
+ if self.enable_battery_query:
+ self.query_battery()
+ rospy.sleep(2)
self.publish_data_message()
- rospy.sleep(self.query_interval_seconds - 2)
+ if self.enable_battery_query:
+ self.publish_battery_state_message()
+ rospy.sleep(self.query_interval_seconds - 4)
def query_modem(self):
for cfg_param in self.fields.keys():
@@ -40,20 +62,55 @@ class ModemSensorDataNode(object):
msg = String(data=nmea_string)
self.nmea_publisher.publish(msg)
- def on_nmea_subscriber(self, msg):
+ def query_battery(self):
+ nmea_string = "$CCBAT,0"
+ msg = String(data=nmea_string)
+ self.nmea_publisher.publish(msg)
+
+ def on_nmea_subscriber(self, msg: String) -> None:
nmea_string = msg.data
- if "CACFG" in nmea_string:
- msg_parts = nmea_string.split(',')
- cfg_param = msg_parts[1]
- if cfg_param in self.fields:
- value = float(msg_parts[2].split('*')[0])
- self.value_dict[self.fields[cfg_param]] = value
+ try:
+ if "CACFG" in nmea_string:
+ msg_parts = nmea_string.split(',')
+ cfg_param = msg_parts[1]
+ if cfg_param in self.fields:
+ value = float(msg_parts[2].split('*')[0])
+ self.value_dict[self.fields[cfg_param]] = value
+ elif "CABAT" in nmea_string:
+ msg_parts = nmea_string.split(',')
+ self.battery_percent = get_float_from_string_with_units(msg_parts[1])
+ self.battery_voltage_mV = get_float_from_string_with_units(msg_parts[2])
+ self.battery_current_mA = get_float_from_string_with_units(msg_parts[3])
+ self.battery_remaining_capacity_mAh = get_float_from_string_with_units(msg_parts[5])
+ except Exception as e:
+ rospy.logerr(f"Error parsing NMEA. Message was {nmea_string.strip()}, error is: {e}")
def publish_data_message(self):
sensor_data_msg = ModemSensorData(**self.value_dict)
sensor_data_msg.header = Header(stamp=rospy.get_rostime())
self.modem_sensor_data_publisher.publish(sensor_data_msg)
+ def publish_battery_state_message(self):
+ percent_fraction = self.battery_percent / 100
+ battery_voltage_V = self.battery_voltage_mV / 1000
+ battery_current_A = self.battery_current_mA / 1000
+ battery_remaining_capacity_Ah = self.battery_remaining_capacity_mAh / 1000
+ battery_capacity_Ah = battery_remaining_capacity_Ah / (percent_fraction + sys.float_info.epsilon)
+ status = BatteryState.POWER_SUPPLY_STATUS_CHARGING if battery_current_A > 0 else BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
+
+ battery_state_msg = BatteryState(header=Header(stamp=rospy.get_rostime()),
+ voltage=battery_voltage_V,
+ current=battery_current_A,
+ charge=battery_remaining_capacity_Ah,
+ capacity=battery_capacity_Ah,
+ design_capacity=battery_capacity_Ah,
+ percentage=percent_fraction,
+ power_supply_status=status,
+ power_supply_health=BatteryState.POWER_SUPPLY_HEALTH_GOOD,
+ power_supply_technology=BatteryState.POWER_SUPPLY_TECHNOLOGY_LION,
+ present=True)
+
+ self.battery_state_publisher.publish(battery_state_msg)
if __name__ == '__main__':
try:
--
GitLab
From bd37519f9fe2f513158bd9386931ef1aa89cea6e Mon Sep 17 00:00:00 2001
From: Caileigh Fitzgerald <cfitzgerald@whoi.edu>
Date: Mon, 4 Mar 2024 21:35:51 +0000
Subject: [PATCH 2/5] added sensor_msgs run time dep to the package.xml file.
Tested new battery_state query with bench modem box. Added commented out
example launch block to tdma_hw_bench_test.launch
---
ros_acomms/package.xml | 1 +
ros_acomms_tests/launch/tdma_hw_bench_test.launch | 9 +++++++--
2 files changed, 8 insertions(+), 2 deletions(-)
diff --git a/ros_acomms/package.xml b/ros_acomms/package.xml
index bba0d02f..e793dce2 100644
--- a/ros_acomms/package.xml
+++ b/ros_acomms/package.xml
@@ -49,6 +49,7 @@
<exec_depend>dynamic_reconfigure</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>ros_acomms_msgs</exec_depend>
+ <exec_depend>sensor_msgs</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
diff --git a/ros_acomms_tests/launch/tdma_hw_bench_test.launch b/ros_acomms_tests/launch/tdma_hw_bench_test.launch
index a6bef998..7f6a75f8 100644
--- a/ros_acomms_tests/launch/tdma_hw_bench_test.launch
+++ b/ros_acomms_tests/launch/tdma_hw_bench_test.launch
@@ -16,6 +16,13 @@
</rosparam>
</node>
+ <!-- <node name="sensor_data" pkg="ros_acomms" type="modem_sensor_data_node.py" respawn="true" respawn_delay="10">
+ <rosparam>
+ query_interval_seconds: 15
+ enable_battery_query: True
+ </rosparam>
+ </node> -->
+
<node name="tdma_scripted" pkg="ros_acomms" type="tdma_scripted_node.py" output="screen" required="true" respawn="false">
<rosparam param="tdma_scripted_test_plan" command="load" file="$(dirname)/tdma_scripted_test_plan.yaml" />
<rosparam>
@@ -29,7 +36,6 @@
pings_per_slot: 1
ping_cdr: 4
ping_transponders: False
- override_test_plan_packet_duration: False
</rosparam>
</node>
@@ -70,7 +76,6 @@
ping_modem_src: 0
pings_per_slot: 1
ping_cdr: 8
- ping_transponders: False
</rosparam>
</node>
--
GitLab
From ab24534e4da7b6a5d9bc006496d0a9500fb16609 Mon Sep 17 00:00:00 2001
From: Eric Gallimore <egallimore@whoi.edu>
Date: Tue, 5 Mar 2024 13:47:01 -0500
Subject: [PATCH 3/5] Added parameter to set battery state topic name.
---
ros_acomms/src/modem_sensor_data_node.py | 3 ++-
1 file changed, 2 insertions(+), 1 deletion(-)
diff --git a/ros_acomms/src/modem_sensor_data_node.py b/ros_acomms/src/modem_sensor_data_node.py
index 88a34a99..a19a4e10 100755
--- a/ros_acomms/src/modem_sensor_data_node.py
+++ b/ros_acomms/src/modem_sensor_data_node.py
@@ -22,6 +22,7 @@ class ModemSensorDataNode(object):
self.query_interval_seconds = rospy.get_param('~query_interval_seconds', 30)
self.enable_battery_query = rospy.get_param('~enable_battery_query', default=False)
+ self.battery_state_topic = rospy.get_param('~battery_state_topic', 'battery_state')
minimum_interval = 5 if not self.query_battery else 30
if self.query_interval_seconds < minimum_interval:
rospy.logwarn(f'Query interval must be at least {minimum_interval} seconds. Setting to {minimum_interval}')
@@ -29,7 +30,7 @@ class ModemSensorDataNode(object):
self.modem_sensor_data_publisher = rospy.Publisher('modem_sensor_data', ModemSensorData, queue_size=10)
if self.query_battery:
- self.battery_state_publisher = rospy.Publisher('battery_state', BatteryState, queue_size=10)
+ self.battery_state_publisher = rospy.Publisher(self.battery_state_topic, BatteryState, queue_size=10)
self.nmea_publisher = rospy.Publisher('nmea_to_modem', String, queue_size=10)
self.value_dict = {}
--
GitLab
From 8d40ec9954f5ae10876ce298ebeb994063764ab6 Mon Sep 17 00:00:00 2001
From: Eric Gallimore <egallimore@whoi.edu>
Date: Tue, 5 Mar 2024 14:07:49 -0500
Subject: [PATCH 4/5] Correctly indicate that the battery isn't present when it
isn't (when the voltage is at or close to 0).
---
ros_acomms/src/modem_sensor_data_node.py | 6 +++++-
1 file changed, 5 insertions(+), 1 deletion(-)
diff --git a/ros_acomms/src/modem_sensor_data_node.py b/ros_acomms/src/modem_sensor_data_node.py
index a19a4e10..37591703 100755
--- a/ros_acomms/src/modem_sensor_data_node.py
+++ b/ros_acomms/src/modem_sensor_data_node.py
@@ -98,6 +98,10 @@ class ModemSensorDataNode(object):
battery_remaining_capacity_Ah = self.battery_remaining_capacity_mAh / 1000
battery_capacity_Ah = battery_remaining_capacity_Ah / (percent_fraction + sys.float_info.epsilon)
status = BatteryState.POWER_SUPPLY_STATUS_CHARGING if battery_current_A > 0 else BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
+ if battery_voltage_V <= 0.0001:
+ battery_present = False
+ else:
+ battery_present = True
battery_state_msg = BatteryState(header=Header(stamp=rospy.get_rostime()),
voltage=battery_voltage_V,
@@ -109,7 +113,7 @@ class ModemSensorDataNode(object):
power_supply_status=status,
power_supply_health=BatteryState.POWER_SUPPLY_HEALTH_GOOD,
power_supply_technology=BatteryState.POWER_SUPPLY_TECHNOLOGY_LION,
- present=True)
+ present=battery_present)
self.battery_state_publisher.publish(battery_state_msg)
--
GitLab
From d23baa69032fb26a6e01140a09c5baa150eeae05 Mon Sep 17 00:00:00 2001
From: Eric Gallimore <egallimore@whoi.edu>
Date: Tue, 12 Mar 2024 18:41:57 -0400
Subject: [PATCH 5/5] Add a field that includes the full queue parameter
dictionary from the yaml file, so that other programs (like ros_acomms_net)
can add arbitrary parameters.
---
ros_acomms/src/codec_config_parser/config_parser.py | 4 ++++
1 file changed, 4 insertions(+)
diff --git a/ros_acomms/src/codec_config_parser/config_parser.py b/ros_acomms/src/codec_config_parser/config_parser.py
index c7b2882e..260ddf9b 100644
--- a/ros_acomms/src/codec_config_parser/config_parser.py
+++ b/ros_acomms/src/codec_config_parser/config_parser.py
@@ -24,6 +24,7 @@ class QueueParams:
is_active: bool
destination: Optional[int]
allow_fragmentation: bool
+ params_dict: Dict
def load_custom_codecs() -> None:
@@ -99,6 +100,9 @@ def get_queue_params(packet_codec_param: Dict) -> Dict[int, QueueParams]:
# if not specified, MessageQueueNode sets a default destination
message_codec_param.get("default_dest", None),
message_codec_param.get("allow_fragmentation", False),
+ # Capture all values that are set, so that other users of the codec
+ # config parser can add arbitrary entries to the param dictionary
+ params_dict=message_codec_param,
)
else:
# Use new version that separates the queue and codec definitions.
--
GitLab