Skip to content

Commit 724e3ad

Browse files
committed
fix: RBR dynamic channels issues
1 parent 32973ac commit 724e3ad

File tree

2 files changed

+29
-12
lines changed

2 files changed

+29
-12
lines changed

Diff for: configs/example.yaml

+18
Original file line numberDiff line numberDiff line change
@@ -83,6 +83,7 @@ arm_ifcb:
8383

8484
ctd_topic: '/arm_ifcb/ctd/depth'
8585

86+
8687
# Note these values are not read by PhytO-ARM, they are sought by
8788
# the included ds_util_nodes package, which publishes CTD data reads.
8889
# https://bitbucket.org/whoidsl/ds_base/src/master/ds_util_nodes/
@@ -168,6 +169,23 @@ arm_chanos: #optional
168169
- 6.5
169170

170171
ctd_topic: '/arm_chanos/ctd/depth'
172+
ctd:
173+
channels:
174+
- conductivity(mS/cm)
175+
- temperature(C)
176+
- pressure(dbar)
177+
- chlorophyll(ug/L)
178+
- phycoerythrin(cells/mL)
179+
- temperature(C)
180+
- O2_concentration(umol/L)
181+
- PAR(umol/m2/s)
182+
- turbidity(NTU)
183+
- pressure(dbar)
184+
- depth(m)
185+
- salinity(PSU)
186+
- speed_of_sound(m/s)
187+
- specific_conductivity(uS/cm)
188+
- O2_air_saturation(%)
171189

172190
motor: #optional
173191
address: "192.168.13.4"

Diff for: src/rbr_maestro3_ctd/src/rbr_maestro3_node.py

+11-12
Original file line numberDiff line numberDiff line change
@@ -103,11 +103,11 @@ async def main():
103103
ctd_publisher = rospy.Publisher('~', Ctd, queue_size=5)
104104
if has_depth:
105105
depth_publisher = rospy.Publisher('~depth', DepthPressure, queue_size=5)
106-
channel_publishers = [
107-
rospy.Publisher(f'~{ros_safe(c)}', RbrMeasurement, queue_size=5)
108-
for c, u in channels
109-
]
110-
106+
channel_publishers = []
107+
for c, u in channels:
108+
if c == 'depth':
109+
continue
110+
channel_publishers.append(rospy.Publisher(f'~{ros_safe(c)}', RbrMeasurement, queue_size=5))
111111

112112
# Subscribe to incoming comms messages
113113
handler = lambda msg: asyncio.run_coroutine_threadsafe(ros_msg_q.put(msg),
@@ -143,9 +143,9 @@ async def main():
143143

144144
# Construct the Ctd message
145145
ctd = Ctd()
146-
ctd.conductivity = c.get('conductivity', math.nan)
147-
ctd.temperature = c.get('temperature', math.nan)
148-
ctd.pressure = c.get('pressure', math.nan)
146+
ctd.conductivity = values.get('conductivity', math.nan)
147+
ctd.temperature = values.get('temperature', math.nan)
148+
ctd.pressure = values.get('pressure', math.nan)
149149

150150
# Clear fields with no measurement
151151
ctd.salinity = math.nan
@@ -158,13 +158,12 @@ async def main():
158158

159159
# Construct the DepthPressure message
160160
dp = DepthPressure()
161-
dp.depth = c.get('depth', math.nan)
161+
dp.depth = values.get('depth', math.nan)
162162
dp.latitude = DepthPressure.DEPTH_PRESSURE_NO_DATA
163163
dp.tare = DepthPressure.DEPTH_PRESSURE_NO_DATA
164-
dp.pressure_raw = c.get('pressure', math.nan)
164+
dp.pressure_raw = values.get('pressure', math.nan)
165165
dp.pressure_raw_unit = DepthPressure.UNIT_PRESSURE_DBAR
166-
dp.pressure = c.get('pressure', math.nan)
167-
166+
dp.pressure = values.get('pressure', math.nan)
168167

169168
# Construct the RbrMeasurement messages
170169
rbr_msgs = []

0 commit comments

Comments
 (0)