✨ add battery_constraint_value param

kas-lab · Mar 21, 2024 · dc1a97d · dc1a97d
1 parent 01dc3b9
commit dc1a97d
Show file tree

Hide file tree

Showing 2 changed files with 21 additions and 5 deletions.
diff --git a/suave_missions/launch/mission.launch.py b/suave_missions/launch/mission.launch.py
@@ -17,6 +17,7 @@ def generate_launch_description():
     mission_type = LaunchConfiguration('mission_type')
     result_filename = LaunchConfiguration('result_filename')
     battery_constraint = LaunchConfiguration('battery_constraint')
+    battery_constraint_value = LaunchConfiguration('battery_constraint_value')
 
     adaptation_manager_arg = DeclareLaunchArgument(
         'adaptation_manager',
@@ -44,6 +45,11 @@ def generate_launch_description():
         description='Desired battery functionality' +
                     '[True or False]'
     )
+    battery_constraint_value_arg = DeclareLaunchArgument(
+        'battery_constraint_value',
+        default_value='0.2',
+        description='battery constraint value'
+    )
 
     pkg_suave_path = get_package_share_directory(
         'suave_missions')
@@ -81,7 +87,12 @@ def generate_launch_description():
         package='suave_missions',
         executable=mission_type,
         name='mission_node',
-        parameters=[mission_config, {'battery_constraint': battery_constraint}],
+        parameters=[
+            mission_config,
+            {
+                'battery_constraint': battery_constraint,
+                'battery_constraint_value': battery_constraint_value,
+            }],
     )
 
     pkg_suave_path = get_package_share_directory('suave')
@@ -123,6 +134,7 @@ def generate_launch_description():
         mission_type_arg,
         result_filename_arg,
         battery_constraint_arg,
+        battery_constraint_value_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
@@ -54,8 +54,9 @@ def __init__(self, node_name='inspection_mission'):
             callback_group=MutuallyExclusiveCallbackGroup()
         )
 
-        self.declare_parameter('battery_constraint', False)    
-
+        self.declare_parameter('battery_constraint', False)
+        self.declare_parameter('battery_constraint_value', 0.2)
+
     def perform_mission(self):
         self.get_logger().info("Pipeline inspection mission starting!!")
         self.timer = self.create_rate(1)
@@ -86,8 +87,11 @@ def battery_level_cb(self, msg):
                 if status.message == 'QA status':
                     for value in status.values:
                         if value.key == 'battery_level':
-                            if float(value.value) < 0.05:
-                                self.get_logger().warn("low battery! Mission abort.")
+                            constraint = self.get_parameter(
+                                'battery_constraint_value').value
+                            if float(value.value) < constraint:
+                                self.get_logger().warn(
+                                    "Low battery! Mission abort.")
                                 self.abort_mission = True
                                 self.call_service(
                                 self.save_mission_results_cli, Empty.Request()