Added support to load parameters from backup file (#101)

Author: evan-palmer

blue_bringup/launch/base.launch.py

@@ -237,6 +237,14 @@ def generate_launch_description() -> LaunchDescription:
                         LaunchConfiguration("manager_file"),
                     ]
                 ),
+                "backup_params_file": PathJoinSubstitution(
+                    [
+                        FindPackageShare(description_package),
+                        "config",
+                        configuration_type,
+                        LaunchConfiguration("ardusub_params_file"),
+                    ]
+                ),
                 "use_sim_time": use_sim,
             }.items(),
         ),

blue_description/config/bluerov2/ardusub.parm

@@ -104,3 +104,11 @@ EK3_SRC1_YAW 6        # External nav
 
 # See https://github.com/clydemcqueen/bluerov2_ignition/issues/7
 SIM_BARO_RND 0.01
+
+# Store thruster parameters as backups
+SERVO1_FUNCTION 33
+SERVO2_FUNCTION 34
+SERVO3_FUNCTION 35
+SERVO4_FUNCTION 36
+SERVO5_FUNCTION 37
+SERVO6_FUNCTION 38

blue_description/config/bluerov2_heavy/ardusub.parm

@@ -82,3 +82,13 @@ EK3_SRC1_YAW 6        # External nav
 
 # See https://github.com/clydemcqueen/bluerov2_ignition/issues/7
 SIM_BARO_RND 0.01
+
+# Store thruster parameters as backups
+SERVO1_FUNCTION 33
+SERVO2_FUNCTION 34
+SERVO3_FUNCTION 35
+SERVO4_FUNCTION 36
+SERVO5_FUNCTION 37
+SERVO6_FUNCTION 38
+SERVO7_FUNCTION 39
+SERVO8_FUNCTION 40

blue_description/config/bluerov2_heavy_reach/ardusub.parm

@@ -82,3 +82,13 @@ EK3_SRC1_YAW 6        # External nav
 
 # See https://github.com/clydemcqueen/bluerov2_ignition/issues/7
 SIM_BARO_RND 0.01
+
+# Store thruster parameters as backups
+SERVO1_FUNCTION 33
+SERVO2_FUNCTION 34
+SERVO3_FUNCTION 35
+SERVO4_FUNCTION 36
+SERVO5_FUNCTION 37
+SERVO6_FUNCTION 38
+SERVO7_FUNCTION 39
+SERVO8_FUNCTION 40

blue_manager/blue_manager/manager.py

@@ -24,7 +24,7 @@
 from geographic_msgs.msg import GeoPoint, GeoPointStamped
 from mavros_msgs.msg import OverrideRCIn, ParamEvent
 from mavros_msgs.srv import CommandHome, MessageInterval
-from rcl_interfaces.msg import Parameter
+from rcl_interfaces.msg import Parameter, ParameterValue
 from rcl_interfaces.srv import SetParameters
 from rclpy.callback_groups import ReentrantCallbackGroup
 from rclpy.executors import MultiThreadedExecutor
@@ -43,6 +43,7 @@ def __init__(self) -> None:
         super().__init__("blue_manager")
 
         self.declare_parameter("num_thrusters", 8)
+        self.declare_parameter("backup_params_file", "")
         self.declare_parameters(
             namespace="message_intervals",
             parameters=[  # type: ignore
@@ -95,6 +96,27 @@ def __init__(self) -> None:
             f"SERVO{i}_FUNCTION": None for i in range(1, self.num_thrusters + 1)
         }
 
+        # Try to load the backup parameters from a file
+        backup_filepath = (
+            self.get_parameter("backup_params_file").get_parameter_value().string_value
+        )
+        if not self.backup_thruster_params_from_file(backup_filepath):
+            self.get_logger().info(
+                "Failed to load all thruster parameters from the provided backup file."
+                " Attempting to load parameters from MAVROS."
+            )
+            self.param_event_sub = self.create_subscription(
+                ParamEvent,
+                "/mavros/param/event",
+                self.backup_thruster_params_cb,
+                qos_profile_parameter_events,
+            )
+        else:
+            self.get_logger().info(
+                "Successfully backed up the thruster parameters from the parameters"
+                " file."
+            )
+
         # Publishers
         self.override_rc_in_pub = self.create_publisher(
             OverrideRCIn, "/mavros/rc/override", qos_profile_default
@@ -105,14 +127,6 @@ def __init__(self) -> None:
             qos_profile_default,
         )
 
-        # Subscribers
-        self.param_event_sub = self.create_subscription(
-            ParamEvent,
-            "/mavros/param/event",
-            self.backup_thruster_params_cb,
-            qos_profile_parameter_events,
-        )
-
         # Services
         self.set_pwm_passthrough_srv = self.create_service(
             SetBool,
@@ -246,14 +260,51 @@ def set_home_pos(lat: float, lon: float, alt: float, yaw: float) -> None:
         )
 
     @property
-    def params_successfully_backed_up(self) -> bool:
+    def backup_params_saved(self) -> bool:
         """Whether or not the thruster parameters are backed up.
 
         Returns:
             Whether or not the parameters are backed up.
         """
         return None not in self.thruster_params_backup.values()
 
+    def backup_thruster_params_from_file(self, backup_filepath: str) -> bool:
+        """Backup thruster parameters from a parameters file.
+
+        Args:
+            backup_filepath: The full path to the backup file to load.
+
+        Returns:
+            Whether or not the parameters were successfully backed up from the file.
+        """
+        if not backup_filepath:
+            return False
+
+        with open(backup_filepath, "r") as ardusub_params:
+            params = ardusub_params.readlines()
+
+            for line in params:
+                line = line.rstrip()
+                split = line.split(" ")
+
+                try:
+                    param_id = split[0]
+                except IndexError:
+                    continue
+
+                try:
+                    param_value = int(split[1])
+                except (ValueError, IndexError):
+                    continue
+
+                if param_id in self.thruster_params_backup.keys():
+                    self.thruster_params_backup[param_id] = Parameter(
+                        name=param_id,
+                        value=ParameterValue(type=2, integer_value=param_value),
+                    )
+
+        return self.backup_params_saved
+
     def backup_thruster_params_cb(self, event: ParamEvent) -> None:
         """Backup the default thruster parameter values.
 
@@ -273,7 +324,7 @@ def backup_thruster_params_cb(self, event: ParamEvent) -> None:
                 name=event.param_id, value=event.value
             )
 
-            if self.params_successfully_backed_up:
+            if self.backup_params_saved:
                 self.get_logger().info(
                     "Successfully backed up the thruster parameters."
                 )

blue_manager/launch/manager.launch.py

@@ -39,7 +39,12 @@ def generate_launch_description() -> LaunchDescription:
         DeclareLaunchArgument(
             "use_sim_time",
             default_value="false",
-            description=("Use the simulated Gazebo clock."),
+            description="Use the simulated Gazebo clock.",
+        ),
+        DeclareLaunchArgument(
+            "backup_params_file",
+            default_value="",
+            description="A configuration file with the ArduSub thruster parameters.",
         ),
     ]
 
@@ -51,7 +56,10 @@ def generate_launch_description() -> LaunchDescription:
             output="both",
             parameters=[
                 LaunchConfiguration("config_filepath"),
-                {"use_sim_time": LaunchConfiguration("use_sim_time")},
+                {
+                    "backup_params_file": LaunchConfiguration("backup_params_file"),
+                    "use_sim_time": LaunchConfiguration("use_sim_time"),
+                },
             ],
         ),
     ]