From b4e5039540aea0a4eb32ac41f5cd7f13dd2b1385 Mon Sep 17 00:00:00 2001 From: Nathan Figueroa Date: Tue, 23 Jul 2024 17:11:31 +0000 Subject: [PATCH] feat: configurable window for profiler peak focus --- configs/example.yaml | 4 ++++ src/phyto_arm/src/profiler_node.py | 11 +++++++++++ 2 files changed, 15 insertions(+) diff --git a/configs/example.yaml b/configs/example.yaml index f95e363..4b0b865 100644 --- a/configs/example.yaml +++ b/configs/example.yaml @@ -99,6 +99,10 @@ arm_ifcb: resolution: 0.02 # m data_topic: "ctd/aml/port4/phycoerythrin" data_field: "value" + # Profile will only consider peaks that are at least this deep + peak_min_depth: 0.0 # optional, m + # Profile will only consider peaks that are at most this deep + peak_max_depth: 10.0 # optional, m motor: #optional address: "192.168.13.3" diff --git a/src/phyto_arm/src/profiler_node.py b/src/phyto_arm/src/profiler_node.py index a5dde56..d6c0965 100755 --- a/src/phyto_arm/src/profiler_node.py +++ b/src/phyto_arm/src/profiler_node.py @@ -135,6 +135,17 @@ def on_action_stop(pub, action_msg): S.release() return + # Get min and max depth params + min_depth = rospy.get_param('~peak_min_depth') + max_depth = rospy.get_param('~peak_max_depth') + + # Filter out data points outside the specified depth range + if min_depth is not None and max_depth is not None: + len_before = data.shape[0] + data = data[(data[:,0] >= min_depth) & (data[:,0] <= max_depth)] + rospy.logwarn(f'Profile depths filtered: {min_depth} to {max_depth} m. \ + {data.shape[0]}/{len_before} points remain') + # Create a function that linearly interpolates between points f = scipy.interpolate.interp1d(data[:,0], data[:,1])