From ee8accc124f7457b58b068d6e514a36147b2c483 Mon Sep 17 00:00:00 2001 From: QMissinne Date: Tue, 19 Mar 2024 17:02:04 +0100 Subject: [PATCH 1/2] Implement battery extension. --- suave_missions/launch/mission.launch.py | 11 ++++++- .../suave_missions/inspection_mission.py | 31 +++++++++++++++++++ .../suave_missions/mission_planner.py | 2 +- 3 files changed, 42 insertions(+), 2 deletions(-) diff --git a/suave_missions/launch/mission.launch.py b/suave_missions/launch/mission.launch.py index b5f7c8e..f0696b6 100644 --- a/suave_missions/launch/mission.launch.py +++ b/suave_missions/launch/mission.launch.py @@ -16,6 +16,7 @@ def generate_launch_description(): adaptation_manager = LaunchConfiguration('adaptation_manager') mission_type = LaunchConfiguration('mission_type') result_filename = LaunchConfiguration('result_filename') + battery_constraint = LaunchConfiguration('battery_constraint') adaptation_manager_arg = DeclareLaunchArgument( 'adaptation_manager', @@ -37,6 +38,13 @@ def generate_launch_description(): description='Name of the results file' ) + battery_constraint_arg = DeclareLaunchArgument( + 'battery_constraint', + default_value='False', + description='Desired battery functionality' + + '[none or battery_constraint]' + ) + pkg_suave_path = get_package_share_directory( 'suave_missions') @@ -73,7 +81,7 @@ def generate_launch_description(): package='suave_missions', executable=mission_type, name='mission_node', - parameters=[mission_config], + parameters=[mission_config, {'battery_constraint': battery_constraint}], ) pkg_suave_path = get_package_share_directory('suave') @@ -114,6 +122,7 @@ def generate_launch_description(): adaptation_manager_arg, mission_type_arg, result_filename_arg, + battery_constraint_arg, mission_metrics_node, mission_metrics_node_override, mission_node, diff --git a/suave_missions/suave_missions/inspection_mission.py b/suave_missions/suave_missions/inspection_mission.py index 3c8e2b6..6ff38a3 100644 --- a/suave_missions/suave_missions/inspection_mission.py +++ b/suave_missions/suave_missions/inspection_mission.py @@ -3,8 +3,14 @@ from mavros_msgs.srv import SetMode from rclpy.callback_groups import MutuallyExclusiveCallbackGroup from std_msgs.msg import Bool +from std_srvs.srv import Empty + + +import rclpy + from suave_missions.mission_planner import MissionPlanner +from diagnostic_msgs.msg import DiagnosticArray class InspectionMission(MissionPlanner): def __init__(self, node_name='inspection_mission'): @@ -39,7 +45,17 @@ def __init__(self, node_name='inspection_mission'): CommandBool, 'mavros/cmd/arming') self.set_mode_service = self.create_client( SetMode, 'mavros/set_mode') + + self.battery_sub = self.create_subscription( + DiagnosticArray, + 'diagnostics', + self.battery_level_cb, + 10, + callback_group=MutuallyExclusiveCallbackGroup() + ) + self.declare_parameter('battery_constraint', False) + def perform_mission(self): self.get_logger().info("Pipeline inspection mission starting!!") self.timer = self.create_rate(1) @@ -62,6 +78,21 @@ def perform_mission(self): self.perform_task('search_pipeline', lambda: self.pipeline_detected) self.perform_task('inspect_pipeline', lambda: self.pipeline_inspected) + + def battery_level_cb(self, msg): + battery_constraint_arg = self.get_parameter('battery_constraint').value + for status in msg.status: + if status.message == 'QA status': + for value in status.values: + if value.key == 'battery_level': + if float(value.value) < 0.95: + self.get_logger().warn("low battery! Mission abort.") + self.abort_mission = True + self.call_service( + self.save_mission_results_cli, Empty.Request() + ) + self.destroy_subscription(self.battery_sub) + break def status_cb(self, msg): self.status = msg diff --git a/suave_missions/suave_missions/mission_planner.py b/suave_missions/suave_missions/mission_planner.py index 3d71818..80f6842 100644 --- a/suave_missions/suave_missions/mission_planner.py +++ b/suave_missions/suave_missions/mission_planner.py @@ -51,10 +51,10 @@ def perform_mission(self): self.get_logger().warning("No mission defined!!!") def perform_task(self, task_name, condition): - self.get_logger().info('Starting {} task'.format(task_name)) self.task_timer = self.create_rate(1) task_status = "completed" if self.abort_mission is False: + self.get_logger().info('Starting {} task'.format(task_name)) self.request_task(task_name) while condition() is not True: if self.abort_mission is True: From 05a21bcf04bff3e581975af49c3b4121d8ca4697 Mon Sep 17 00:00:00 2001 From: QMissinne Date: Wed, 20 Mar 2024 18:17:53 +0100 Subject: [PATCH 2/2] Updated battery extension, corrected according to comments on pull request. --- suave_missions/launch/mission.launch.py | 2 +- .../suave_missions/inspection_mission.py | 21 ++++++++++--------- 2 files changed, 12 insertions(+), 11 deletions(-) diff --git a/suave_missions/launch/mission.launch.py b/suave_missions/launch/mission.launch.py index f0696b6..fca58b2 100644 --- a/suave_missions/launch/mission.launch.py +++ b/suave_missions/launch/mission.launch.py @@ -42,7 +42,7 @@ def generate_launch_description(): 'battery_constraint', default_value='False', description='Desired battery functionality' + - '[none or battery_constraint]' + '[True or False]' ) pkg_suave_path = get_package_share_directory( diff --git a/suave_missions/suave_missions/inspection_mission.py b/suave_missions/suave_missions/inspection_mission.py index 6ff38a3..54417bb 100644 --- a/suave_missions/suave_missions/inspection_mission.py +++ b/suave_missions/suave_missions/inspection_mission.py @@ -83,16 +83,17 @@ def battery_level_cb(self, msg): battery_constraint_arg = self.get_parameter('battery_constraint').value for status in msg.status: if status.message == 'QA status': - for value in status.values: - if value.key == 'battery_level': - if float(value.value) < 0.95: - self.get_logger().warn("low battery! Mission abort.") - self.abort_mission = True - self.call_service( - self.save_mission_results_cli, Empty.Request() - ) - self.destroy_subscription(self.battery_sub) - break + if battery_constraint_arg is True: + for value in status.values: + if value.key == 'battery_level': + if float(value.value) < 0.05: + self.get_logger().warn("low battery! Mission abort.") + self.abort_mission = True + self.call_service( + self.save_mission_results_cli, Empty.Request() + ) + self.destroy_subscription(self.battery_sub) + break def status_cb(self, msg): self.status = msg