Add firmware parameter bridge

leo_bringup/config/firmware_diff_drive_params.yaml

@@ -0,0 +1,18 @@
+wheels:
+  encoder_resolution: 878.4
+  torque_constant: 1.17647
+  pid:
+    p: 0.0
+    i: 0.005
+    d: 0.0
+  pwm_duty_limit: 100.0
+
+mecanum_wheels: false
+
+controller:
+  wheel_radius: 0.0625
+  wheel_separation: 0.358
+  angular_velocity_multiplier: 1.76
+  input_timeout: 500
+
+battery_min_voltage: 10.0

leo_bringup/config/firmware_mecanum_params.yaml

@@ -0,0 +1,19 @@
+wheels:
+  encoder_resolution: 878.4
+  torque_constant: 1.17647
+  pid:
+    p: 0.0
+    i: 0.005
+    d: 0.0
+  pwm_duty_limit: 100.0
+
+mecanum_wheels: true
+
+controller:
+  wheel_radius: 0.0635
+  wheel_separation: 0.37
+  wheel_base: 0.3052
+  angular_velocity_multiplier: 1.0
+  input_timeout: 500
+
+battery_min_voltage: 10.0

leo_bringup/launch/leo_bringup.launch.xml

@@ -4,24 +4,26 @@
     default="true"
     description="Whether to start the robot_state_publisher node" />
 
-  <arg
-    name="model"
+  <arg name="model"
     default="$(find-pkg-share leo_description)/urdf/leo.urdf.xacro"
     description="Absolute path to robot urdf.xacro file" />
 
-  <arg
-    name="mecanum_wheels"
+  <arg name="mecanum_wheels"
     default="false"
     description="Flag specifying wheel types of the robot" />
 
   <arg name="tf_frame_prefix"
     default=""
     description="The prefix added to each published TF frame id" />
 
+  <arg name="firmware_override_params_file"
+    default=""
+    description="Absolute path to the yaml file with firmware parameters to be overrided" />
+
   <include if="$(var spawn_state_publisher)"
     file="$(find-pkg-share leo_description)/launch/state_publisher.launch.xml">
     <arg name="model" value="$(var model)" />
-    <arg name="mecanum_wheels" value="$(var mecanum_wheels)"/>
+    <arg name="mecanum_wheels" value="$(var mecanum_wheels)" />
   </include>
 
   <node pkg="web_video_server"
@@ -44,6 +46,16 @@
     <param from="$(find-pkg-share leo_bringup)/config/firmware_message_converter.yaml" />
   </node>
 
+  <let if="$(var mecanum_wheels)" name="firmware_default_params_file"
+    value="$(find-pkg-share leo_bringup)/config/firmware_mecanum_params.yaml" />
+  <let unless="$(var mecanum_wheels)" name="firmware_default_params_file"
+    value="$(find-pkg-share leo_bringup)/config/firmware_diff_drive_params.yaml" />
+
+  <node pkg="leo_fw" exec="firmware_parameter_bridge">
+    <param name="default_params_file_path" value="$(var firmware_default_params_file)" />
+    <param name="override_params_file_path" value="$(var firmware_override_params_file)" />
+  </node>
+
   <node_container namespace=""
     name="image_container"
     pkg="rclcpp_components"

leo_fw/CMakeLists.txt

@@ -46,6 +46,7 @@ install(
     scripts/update
     scripts/test_hw
     scripts/calibrate_imu
+    scripts/firmware_parameter_bridge
   DESTINATION lib/${PROJECT_NAME}
 )
 

leo_fw/data/default_firmware_params.yaml

@@ -0,0 +1,19 @@
+wheels:
+  encoder_resolution: 878.4
+  torque_constant: 1.17647
+  pid:
+    p: 0.0
+    i: 0.005
+    d: 0.0
+  pwm_duty_limit: 100.0
+
+mecanum_wheels: false
+
+controller:
+  wheel_radius: 0.0625
+  wheel_separation: 0.358
+  wheel_base: 0.3052
+  angular_velocity_multiplier: 1.76
+  input_timeout: 500
+
+battery_min_voltage: 10.0

leo_fw/leo_fw/nodes/__init__.py

@@ -0,0 +1,23 @@
+# Copyright 2023 Fictionlab sp. z o.o.
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+
+from .parameter_bridge import ParameterBridge
+
+__all__ = ["ParameterBridge"]

leo_fw/leo_fw/nodes/parameter_bridge.py

@@ -0,0 +1,244 @@
+# Copyright 2023 Fictionlab sp. z o.o.
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+
+from os import path
+
+import yaml  # type: ignore
+
+from ament_index_python import get_package_share_directory
+
+from rcl_interfaces.msg import Parameter as ParameterMsg, SetParametersResult
+from rcl_interfaces.srv import SetParameters
+
+import rclpy
+from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.node import Node
+from rclpy.parameter import Parameter
+from rclpy.qos import (
+    QoSDurabilityPolicy,
+    QoSHistoryPolicy,
+    QoSProfile,
+    QoSReliabilityPolicy,
+)
+
+from std_msgs.msg import Empty
+from std_srvs.srv import Trigger
+
+
+class ParameterBridge(Node):
+    firmware_parameters: list[ParameterMsg] = []
+    default_params: dict = {}
+    override_params: dict = {}
+    type_dict: dict = {
+        str: Parameter.Type.STRING,
+        int: Parameter.Type.INTEGER,
+        bool: Parameter.Type.BOOL,
+        float: Parameter.Type.DOUBLE,
+    }
+
+    def __init__(self, executor: MultiThreadedExecutor) -> None:
+        super().__init__("firmware_parameter_bridge")
+        self.executor = executor
+
+        leo_fw_share = get_package_share_directory("leo_fw")
+
+        self.declare_parameter(
+            "default_params_file_path",
+            path.join(leo_fw_share, "data", "default_firmware_params.yaml"),
+        )
+        self.declare_parameter("override_params_file_path", "")
+
+        self.load_default_params()
+        self.load_override_params()
+
+        cb_group = MutuallyExclusiveCallbackGroup()
+        self.firmware_parameter_service_client = self.create_client(
+            SetParameters,
+            "firmware/set_parameters",
+            callback_group=cb_group,
+        )
+
+        self.frimware_boot_service_client = self.create_client(
+            Trigger,
+            "firmware/boot",
+            callback_group=cb_group,
+        )
+
+        self.param_bridge_srv = self.create_service(
+            Trigger,
+            "upload_params",
+            self.upload_params_callback,
+        )
+
+        self.firmware_subscriber = self.create_subscription(
+            Empty,
+            "firmware/param_trigger",
+            self.param_trigger_callback,
+            QoSProfile(
+                history=QoSHistoryPolicy.KEEP_LAST,
+                depth=1,
+                reliability=QoSReliabilityPolicy.BEST_EFFORT,
+                durability=QoSDurabilityPolicy.VOLATILE,
+            ),
+        )
+
+        self.send_params()
+
+    def load_default_params(self) -> None:
+        default_params_file: str = (
+            self.get_parameter("default_params_file_path")
+            .get_parameter_value()
+            .string_value
+        )
+
+        with open(default_params_file, "r", encoding="utf-8") as file:
+            self.default_params: dict = yaml.safe_load(file)
+
+    def load_override_params(self) -> None:
+        override_params_file: str = (
+            self.get_parameter("override_params_file_path")
+            .get_parameter_value()
+            .string_value
+        )
+
+        self.override_params = {}
+
+        if override_params_file != "":
+            try:
+                with open(override_params_file, "r", encoding="utf-8") as file:
+                    override_yaml = yaml.safe_load(file)
+                    if isinstance(override_yaml, dict):
+                        self.override_params = override_yaml
+            except (FileNotFoundError, PermissionError, yaml.YAMLError) as exc:
+                self.get_logger().error("Failed to load parameter overrides!")
+                self.get_logger().error(str(exc))
+        else:
+            self.get_logger().warning("Path to file with override parameters is empty.")
+
+    def parse_firmware_parameters(self) -> list[ParameterMsg]:
+        def parse_parameters_recursive(
+            parameters: list[ParameterMsg],
+            param_name_prefix: str,
+            default_dict: dict,
+            override_dict: dict,
+        ) -> None:
+            for key, value in default_dict.items():
+                if isinstance(value, dict):
+                    new_name_prefix = param_name_prefix + key + "/"
+                    parse_parameters_recursive(
+                        parameters,
+                        new_name_prefix,
+                        value,
+                        override_dict.get(key, {}),
+                    )
+                    continue
+
+                if key in override_dict:
+                    value = override_dict[key]
+
+                new_param = rclpy.Parameter(
+                    param_name_prefix + key, self.type_dict[type(value)], value
+                )
+                parameters.append(new_param.to_parameter_msg())
+
+        parameters: list[ParameterMsg] = []
+        parse_parameters_recursive(
+            parameters, "", self.default_params, self.override_params
+        )
+        return parameters
+
+    def param_trigger_callback(self, _msg: Empty) -> None:
+        self.get_logger().info("Request for firmware parameters.")
+        success, _ = self.send_params()
+        if success:
+            self.trigger_boot()
+
+    def upload_params_callback(
+        self, _request: Trigger.Request, response: Trigger.Response
+    ) -> Trigger.Response:
+        self.get_logger().info(
+            "Serving user request for setting firmware parameters..."
+        )
+
+        self.load_override_params()
+
+        result, num = self.send_params()
+        if result:
+            response.message = "Successfully set firmware parameters."
+            if num > 0:
+                response.message += (
+                    f" {num} parameter(s) was(were) not set. Check node logs."
+                )
+            response.success = True
+        else:
+            response.message = "Failed to set firmware parameters."
+            response.success = False
+
+        return response
+
+    def send_params(self) -> tuple[bool, int]:
+        self.get_logger().info("Trying to set parameters for firmware node...")
+
+        not_set_params_num = 0
+        if not self.firmware_parameter_service_client.wait_for_service(timeout_sec=1.0):
+            self.get_logger().error("Firmware parameter service not active!")
+            return (False, not_set_params_num)
+
+        param_request = SetParameters.Request()
+        param_request.parameters = self.parse_firmware_parameters()
+        future = self.firmware_parameter_service_client.call_async(param_request)
+
+        self.executor.spin_until_future_complete(future, 5.0)
+
+        if future.result():
+            result: SetParametersResult
+            param: ParameterMsg
+            for result, param in zip(future.result().results, param_request.parameters):
+                if not result.successful:
+                    self.get_logger().warning(
+                        f"Parameter '{param.name}' not set. Reason: '{result.reason}'"
+                    )
+                    not_set_params_num += 1
+
+            self.get_logger().info("Successfully set parameters for firmware node.")
+
+            return (True, not_set_params_num)
+
+        self.get_logger().error("Unable to set parameters for firmware node.")
+        return (False, not_set_params_num)
+
+    def trigger_boot(self) -> bool:
+        self.get_logger().info("Trying to trigger firmware boot.")
+
+        if not self.frimware_boot_service_client.wait_for_service(timeout_sec=1.0):
+            self.get_logger().error("Firmware boot service not active!")
+
+        boot_request = Trigger.Request()
+        boot_future = self.frimware_boot_service_client.call_async(boot_request)
+
+        self.executor.spin_until_future_complete(boot_future, 5.0)
+
+        if boot_future.result():
+            self.get_logger().info("Firmware boot triggered successfully.")
+            return True
+
+        self.get_logger().error("Didn't get response from firmware boot service!")
+        return False