From 438bea872c29ee711aa256483c2ce4f1ca3efef4 Mon Sep 17 00:00:00 2001 From: Gustavo Date: Wed, 29 May 2024 16:46:52 +0100 Subject: [PATCH 1/4] =?UTF-8?q?=E2=9A=A1=20publish=20battery=5Flevel=20as?= =?UTF-8?q?=20soon=20as=20it=20charges?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- suave_monitor/suave_monitor/battery_monitor.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/suave_monitor/suave_monitor/battery_monitor.py b/suave_monitor/suave_monitor/battery_monitor.py index a8d1634..21af6d2 100644 --- a/suave_monitor/suave_monitor/battery_monitor.py +++ b/suave_monitor/suave_monitor/battery_monitor.py @@ -43,7 +43,7 @@ def __init__(self): self.battery_level = 1.0 - self.get_interpolated_path_srv = self.create_service( + self.recharge_battery_srv = self.create_service( Trigger, 'battery_monitor/recharge', self.recharge_battery_cb) @@ -59,6 +59,9 @@ def status_cb(self, msg): self.destroy_subscription(self.mavros_state_sub) def qa_publisher_cb(self): + self.publish_battery_level() + + def publish_battery_level(self): discharge_time = self.get_parameter('discharge_time').value current_time = self.get_clock().now().to_msg().sec @@ -90,6 +93,7 @@ def qa_publisher_cb(self): def recharge_battery_cb(self, req, res): self.battery_level = 1.0 + self.publish_battery_level() res.success = True self.battery_recharged_pub.publish(Bool(data=True)) return res From ad294ff3806c438855b704b4efea0af20d2f3954 Mon Sep 17 00:00:00 2001 From: Gustavo Date: Wed, 29 May 2024 16:49:59 +0100 Subject: [PATCH 2/4] =?UTF-8?q?=E2=9C=A8=20measure=20battery=20low=20react?= =?UTF-8?q?ion=20time?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../suave_metrics/mission_metrics.py | 63 ++++++++++++++++--- suave_missions/config/mission_config.yaml | 1 + 2 files changed, 57 insertions(+), 7 deletions(-) diff --git a/suave_metrics/suave_metrics/mission_metrics.py b/suave_metrics/suave_metrics/mission_metrics.py index 69563f2..8582c61 100644 --- a/suave_metrics/suave_metrics/mission_metrics.py +++ b/suave_metrics/suave_metrics/mission_metrics.py @@ -56,9 +56,12 @@ def __init__(self, node_name: str = 'suave_metrics'): self.declare_parameter('mission_name', 'inspection') # self.declare_parameter('metrics_header', ['']) self.declare_parameter( - 'water_visibiity_threshold', rclpy.Parameter.Type.DOUBLE_ARRAY) + 'water_visibiity_threshold', [3.25, 2.25, 1.25]) self.declare_parameter( - 'expected_altitude', rclpy.Parameter.Type.DOUBLE_ARRAY) + 'expected_altitude', [3.0, 2.0, 1.0]) + + self.declare_parameter( + 'battery_limit', 0.25) #: path where results must be saved self.result_path = self.get_parameter('result_path').value @@ -83,10 +86,13 @@ def __init__(self, node_name: str = 'suave_metrics'): self.wrong_altitude = False self.thrusters_failed = False + self.battery_low = False self.wrong_altitude_time = None self.thrusters_failed_time = None + self.battery_low_time = None self.component_recovery_time = [] self.wv_reaction_time = [] + self.battery_reaction_time = [] self.state_sub = self.create_subscription( State, @@ -148,6 +154,14 @@ def __init__(self, node_name: str = 'suave_metrics'): callback_group=ReentrantCallbackGroup() ) + self.generate_recharge_path_node_state_sub = self.create_subscription( + TransitionEvent, + '/generate_recharge_path_node/transition_event', + self.generate_recharge_path_transition_cb, + self.qos, + callback_group=ReentrantCallbackGroup() + ) + self.param_change_sub = self.create_subscription( ParameterEvent, "/parameter_events", @@ -208,6 +222,7 @@ def diagnostics_cb(self, msg): for diagnostic_status in msg.status: if diagnostic_status.message.lower() in measurement_messages: self.check_altitude(diagnostic_status, time) + self.check_battery(diagnostic_status, time) if diagnostic_status.message.lower() in component_messages: thrusters_ok = self.check_thrusther(diagnostic_status, time) @@ -217,7 +232,8 @@ def check_altitude(self, diagnostic_status, time): altitude = self.get_spiral_altitude() expected = self.get_expected_spiral_altitude(value.value) correct_altitude = altitude == expected - if self.wrong_altitude is False and correct_altitude is False: + if self.battery_low is False and self.wrong_altitude is False \ + and correct_altitude is False: self.wrong_altitude = True self.wrong_altitude_time = time return @@ -229,8 +245,8 @@ def get_spiral_altitude(self): def get_expected_spiral_altitude(self, measured_wv): wv_threshold = self.get_parameter('water_visibiity_threshold').value - wv_expected = self.get_parameter('expected_altitude').value - for threshold, expected in zip(wv_threshold, wv_expected): + expected_altitude = self.get_parameter('expected_altitude').value + for threshold, expected in zip(wv_threshold, expected_altitude): if float(measured_wv) >= threshold: return expected return None @@ -243,6 +259,18 @@ def check_thrusther(self, diagnostic_status, time): self.thrusters_failed_time = time return + def check_battery(self, diagnostic_status, time): + for value in diagnostic_status.values: + if value.key == 'battery_level': + battery_limit = self.get_parameter('battery_limit').value + if self.battery_low is False and float(value.value) < battery_limit: + self.battery_low = True + self.battery_low_time = time + self.wrong_altitude = False + if self.battery_low is True and float(value.value) > battery_limit: + self.battery_low = False + return + def maintain_motion_transition_cb(self, msg): if msg.goal_state.label == "active" and self.thrusters_failed is True: reaction_time = self.get_clock().now() - self.thrusters_failed_time @@ -252,12 +280,22 @@ def maintain_motion_transition_cb(self, msg): self.get_logger().info( 'Component recovery time: {} seconds'.format(reaction_time)) + def generate_recharge_path_transition_cb(self, msg): + if msg.goal_state.label == "active" and self.battery_low is True: + reaction_time = self.get_clock().now() - self.battery_low_time + reaction_time = reaction_time.nanoseconds * 1e-9 + self.battery_reaction_time.append(reaction_time) + # self.battery_low = False + self.get_logger().info( + 'Battery reaction time: {} seconds'.format(reaction_time)) + def param_change_cb(self, msg): + time = self.get_clock().now() if msg.node == "/f_generate_search_path_node" \ and self.wrong_altitude is True: for param in msg.changed_parameters: if param.name == "spiral_altitude": - reaction_time = self.get_clock().now() - self.wrong_altitude_time + reaction_time = time - self.wrong_altitude_time reaction_time = reaction_time.nanoseconds * 1e-9 self.wv_reaction_time.append( reaction_time) @@ -321,7 +359,9 @@ def save_mission_results(self) -> None: detection_time_delta, self.distance_inspected, statistics.fmean( - self.component_recovery_time + self.wv_reaction_time) + self.component_recovery_time + + self.wv_reaction_time + + self.battery_reaction_time) ] self.save_metrics( @@ -348,6 +388,15 @@ def save_mission_results(self) -> None: [self.mission_name, date, t] ) + battery_file = self.result_filename +'_battery_reaction_time' + for t in self.battery_reaction_time: + self.save_metrics( + self.result_path, + battery_file, + ['mission_name', 'datetime', 'reaction time (s)'], + [self.mission_name, date, t] + ) + # TODO: make this a parameter os.system("touch ~/suave_ws/mission.done") diff --git a/suave_missions/config/mission_config.yaml b/suave_missions/config/mission_config.yaml index b89ee3c..0255565 100644 --- a/suave_missions/config/mission_config.yaml +++ b/suave_missions/config/mission_config.yaml @@ -3,6 +3,7 @@ result_path: "~/suave/results" #Path to save results water_visibiity_threshold: [3.25, 2.25, 1.25] expected_altitude: [3.0, 2.0, 1.0] + battery_limit: 0.25 /mission_node: ros__parameters: From e0f6522053d2fa7aa757d4864ccbf58d59aced82 Mon Sep 17 00:00:00 2001 From: Gustavo Date: Wed, 29 May 2024 16:50:49 +0100 Subject: [PATCH 3/4] =?UTF-8?q?=F0=9F=90=9B=20fix=20bug=20in=20BT?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../suave_bt/include/suave_bt/action_recharge_battery.hpp | 2 +- .../suave_bt/src/suave_bt/action_recharge_battery.cpp | 5 +++++ suave_managing/suave_bt/src/suave_bt/suave_mission.cpp | 1 + 3 files changed, 7 insertions(+), 1 deletion(-) diff --git a/suave_managing/suave_bt/include/suave_bt/action_recharge_battery.hpp b/suave_managing/suave_bt/include/suave_bt/action_recharge_battery.hpp index 99d7fdf..892b20b 100644 --- a/suave_managing/suave_bt/include/suave_bt/action_recharge_battery.hpp +++ b/suave_managing/suave_bt/include/suave_bt/action_recharge_battery.hpp @@ -33,7 +33,7 @@ class RechargeBattery : public BT::StatefulActionNode{ public: RechargeBattery(const std::string& name, const BT::NodeConfig & conf); - BT::NodeStatus onStart() override {return BT::NodeStatus::RUNNING;}; + BT::NodeStatus onStart() override; BT::NodeStatus onRunning() override; diff --git a/suave_managing/suave_bt/src/suave_bt/action_recharge_battery.cpp b/suave_managing/suave_bt/src/suave_bt/action_recharge_battery.cpp index f660c0c..11d41c9 100644 --- a/suave_managing/suave_bt/src/suave_bt/action_recharge_battery.cpp +++ b/suave_managing/suave_bt/src/suave_bt/action_recharge_battery.cpp @@ -31,6 +31,11 @@ namespace suave_bt recharged_ = msg.data; } + BT::NodeStatus RechargeBattery::onStart() { + recharged_ = false; + return BT::NodeStatus::RUNNING; + } + BT::NodeStatus RechargeBattery::onRunning() { std::this_thread::sleep_for(std::chrono::milliseconds(50)); diff --git a/suave_managing/suave_bt/src/suave_bt/suave_mission.cpp b/suave_managing/suave_bt/src/suave_bt/suave_mission.cpp index 3e28356..361a6b3 100644 --- a/suave_managing/suave_bt/src/suave_bt/suave_mission.cpp +++ b/suave_managing/suave_bt/src/suave_bt/suave_mission.cpp @@ -76,6 +76,7 @@ namespace suave_bt } void SuaveMission::set_search_started(){ + if(search_started_ == true) return; start_time_ = this->get_clock()->now(); search_started_ = true; } From 24f31d906482e75d4fa4cedad96564fae1aad6e7 Mon Sep 17 00:00:00 2001 From: Gustavo Date: Wed, 29 May 2024 18:35:01 +0100 Subject: [PATCH 4/4] =?UTF-8?q?=F0=9F=90=9B=20check=20if=20the=20correct?= =?UTF-8?q?=20altitude=20was=20set=20before=20measuring=20time?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- suave_metrics/suave_metrics/mission_metrics.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/suave_metrics/suave_metrics/mission_metrics.py b/suave_metrics/suave_metrics/mission_metrics.py index 8582c61..8333a13 100644 --- a/suave_metrics/suave_metrics/mission_metrics.py +++ b/suave_metrics/suave_metrics/mission_metrics.py @@ -94,6 +94,8 @@ def __init__(self, node_name: str = 'suave_metrics'): self.wv_reaction_time = [] self.battery_reaction_time = [] + self.measured_wv = None + self.state_sub = self.create_subscription( State, 'mavros/state', @@ -229,6 +231,7 @@ def diagnostics_cb(self, msg): def check_altitude(self, diagnostic_status, time): for value in diagnostic_status.values: if value.key == 'water_visibility': + self.measured_wv = float(value.value) altitude = self.get_spiral_altitude() expected = self.get_expected_spiral_altitude(value.value) correct_altitude = altitude == expected @@ -293,8 +296,10 @@ def param_change_cb(self, msg): time = self.get_clock().now() if msg.node == "/f_generate_search_path_node" \ and self.wrong_altitude is True: + expected_altitude = self.get_expected_spiral_altitude( + self.measured_wv) for param in msg.changed_parameters: - if param.name == "spiral_altitude": + if param.name == "spiral_altitude" and param.value.double_value == expected_altitude: reaction_time = time - self.wrong_altitude_time reaction_time = reaction_time.nanoseconds * 1e-9 self.wv_reaction_time.append(