Implement battery extension.

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,

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

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: