Update platform nodes from extra ros parameters

Flattened default parameter files

clearpath_control/config/a200/control.yaml

@@ -3,11 +3,9 @@ controller_manager:
     update_rate: 50  # Hz
     use_sim_time: False
 
-    joint_state_broadcaster:
-      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster.type: joint_state_broadcaster/JointStateBroadcaster
 
-    platform_velocity_controller:
-      type: diff_drive_controller/DiffDriveController
+    platform_velocity_controller.type: diff_drive_controller/DiffDriveController
 
 platform_velocity_controller:
   ros__parameters:

clearpath_control/config/a200/teleop_logitech.yaml

@@ -43,22 +43,18 @@
 
 joy_teleop/teleop_twist_joy_node:
   ros__parameters:
-    axis_linear:
-      x: 1
-    scale_linear:
-      x: 0.4
-    scale_linear_turbo:
-      x: 1.0
-    axis_angular:
-      yaw: 0
-    scale_angular:
-      yaw: 0.6
-    scale_angular_turbo:
-      yaw: 1.2
+    use_sim_time: False
+    axis_linear.x: 1
+    scale_linear.x: 0.4
+    scale_linear_turbo.x: 1.0
+    axis_angular.yaw: 0
+    scale_angular.yaw: 0.6
+    scale_angular_turbo.yaw: 1.2
     enable_button: 4
     enable_turbo_button: 5
 joy_teleop/joy_node:
   ros__parameters:
+    use_sim_time: False
     deadzone: 0.1
     autorepeat_rate: 20.0
     dev: /dev/input/f710

clearpath_control/config/a200/teleop_ps4.yaml

@@ -56,22 +56,18 @@
 
 joy_teleop/teleop_twist_joy_node:
   ros__parameters:
-    axis_linear:
-      x: 1
-    scale_linear:
-      x: 0.4
-    scale_linear_turbo:
-      x: 1.0
-    axis_angular:
-      yaw: 0
-    scale_angular:
-      yaw: 0.6
-    scale_angular_turbo:
-      yaw: 1.2
+    use_sim_time: False
+    axis_linear.x: 1
+    scale_linear.x: 0.4
+    scale_linear_turbo.x: 1.0
+    axis_angular.yaw: 0
+    scale_angular.yaw: 0.6
+    scale_angular_turbo.yaw: 1.2
     enable_button: 4
     enable_turbo_button: 5
 joy_teleop/joy_node:
   ros__parameters:
+    use_sim_time: False
     deadzone: 0.1
     autorepeat_rate: 20.0
     dev: /dev/input/ps4

clearpath_control/config/j100/control.yaml

@@ -3,11 +3,9 @@ controller_manager:
     update_rate: 50  # Hz
     use_sim_time: False
 
-    joint_state_broadcaster:
-      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster.type: joint_state_broadcaster/JointStateBroadcaster
 
-    platform_velocity_controller:
-      type: diff_drive_controller/DiffDriveController
+    platform_velocity_controller.type: diff_drive_controller/DiffDriveController
 
 platform_velocity_controller:
   ros__parameters:

clearpath_control/config/j100/teleop_logitech.yaml

@@ -44,18 +44,12 @@
 joy_teleop/teleop_twist_joy_node:
   ros__parameters:
     use_sim_time: False
-    axis_linear:
-      x: 1
-    scale_linear:
-      x: 0.4
-    scale_linear_turbo:
-      x: 2.0
-    axis_angular:
-      yaw: 0
-    scale_angular:
-      yaw: 0.6
-    scale_angular_turbo:
-      yaw: 1.4
+    axis_linear.x: 1
+    scale_linear.x: 0.4
+    scale_linear_turbo.x: 2.0
+    axis_angular.yaw: 0
+    scale_angular.yaw: 0.6
+    scale_angular_turbo.yaw: 1.4
     enable_button: 4
     enable_turbo_button: 5
 joy_teleop/joy_node:

clearpath_control/config/j100/teleop_ps4.yaml

@@ -57,18 +57,12 @@
 joy_teleop/teleop_twist_joy_node:
   ros__parameters:
     use_sim_time: False
-    axis_linear:
-      x: 1
-    scale_linear:
-      x: 0.4
-    scale_linear_turbo:
-      x: 2.0
-    axis_angular:
-      yaw: 0
-    scale_angular:
-      yaw: 0.6
-    scale_angular_turbo:
-      yaw: 1.4
+    axis_linear.x: 1
+    scale_linear.x: 0.4
+    scale_linear_turbo.x: 2.0
+    axis_angular.yaw: 0
+    scale_angular.yaw: 0.6
+    scale_angular_turbo.yaw: 1.4
     enable_button: 4
     enable_turbo_button: 5
 joy_teleop/joy_node:

clearpath_control/config/twist_mux.yaml

@@ -1,21 +1,15 @@
 twist_mux:
   ros__parameters:
     use_sim_time: False
-    topics:
-      joy:
-        topic   : joy_teleop/cmd_vel
-        timeout : 0.5
-        priority: 10
-      interactive_marker:
-        topic   : twist_marker_server/cmd_vel
-        timeout : 0.5
-        priority: 8
-      external:
-        topic   : cmd_vel
-        timeout : 0.5
-        priority: 1
-    locks:
-      e_stop:
-        topic   : platform/emergency_stop
-        timeout : 0.0
-        priority: 255
+    topics.joy.topic: joy_teleop/cmd_vel
+    topics.joy.timeout: 0.5
+    topics.joy.priority: 10
+    topics.interactive_marker.topic: twist_marker_server/cmd_vel
+    topics.interactive_marker.timeout: 0.5
+    topics.interactive_marker.priority: 8
+    topics.external.topic: cmd_vel
+    topics.external.timeout: 0.5
+    topics.external.priority: 1
+    locks.e_stop.topic: platform/emergency_stop
+    locks.e_stop.timeout: 0.0
+    locks.e_stop.priority: 255

clearpath_generator_common/clearpath_generator_common/common.py

@@ -34,6 +34,7 @@
 
 from ament_index_python.packages import get_package_share_directory
 
+from clearpath_config.common.utils.dictionary import flatten_dict
 from clearpath_config.common.utils.yaml import read_yaml
 from clearpath_config.clearpath_config import ClearpathConfig
 
@@ -154,7 +155,8 @@ def __init__(self,
         self.namespace = namespace
         self.name = f'param_file_{name}'
         self.file = f'{name}.yaml'
-        self.parameters = parameters
+        self.parameters = {}
+        self.parameters.update(parameters)
 
     @property
     def full_path(self) -> str:
@@ -185,8 +187,9 @@ def read(self) -> None:
     def update(self, parameters: dict) -> None:
         for node in parameters:
             if node in self.parameters:
-                for param in parameters[node]:
-                    self.parameters[node][param] = parameters[node][param]
+                flat_parameters = flatten_dict(parameters[node])
+                for param in flat_parameters:
+                    self.parameters[node][param] = flat_parameters[param]
 
 
 class BashFile():
@@ -242,8 +245,8 @@ def __init__(self,
         # Parse YAML into config
         self.clearpath_config = ClearpathConfig(self.config)
 
-        self.serial_number = self.clearpath_config.get_serial_number()
-        self.platform_model = self.clearpath_config.get_model()
+        self.serial_number = self.clearpath_config.serial_number
+        self.platform_model = self.clearpath_config.platform.get_platform_model()
         self.namespace = self.clearpath_config.system.namespace
 
     # This method should be overwritten by the child class

clearpath_generator_common/clearpath_generator_common/param/platform.py

@@ -55,7 +55,7 @@ def __init__(self,
                      param_path: str) -> None:
             self.parameter = parameter
             self.clearpath_config = clearpath_config
-            self.platform = self.clearpath_config.platform.get_model()
+            self.platform = self.clearpath_config.platform.get_platform_model()
             self.namespace = self.clearpath_config.system.namespace
             self.param_path = param_path
 
@@ -84,8 +84,14 @@ def generate_parameters(self, use_sim_time: bool = False) -> None:
             )
             self.default_param_file.read()
 
-            # TODO: Get user params
             self.param_file.parameters = self.default_param_file.parameters
+
+            # Get extra ros parameters from config
+            extras = self.clearpath_config.platform.extras.ros_parameters
+            for node in extras:
+                if node in self.param_file.parameters:
+                    self.param_file.update({node: extras.get(node)})
+
             if use_sim_time:
                 for node in self.param_file.parameters:
                     self.param_file.update({node: {'use_sim_time': True}})
@@ -105,6 +111,7 @@ def __init__(self,
             self.default_parameter_file_path = 'config'
 
     class LocalizationParam(BaseParam):
+        EKF_NODE = 'ekf_node'
         imu_config = [False, False, False,
                       False, False, False,
                       False, False, False,
@@ -114,9 +121,9 @@ class LocalizationParam(BaseParam):
         def generate_parameters(self, use_sim_time: bool = False) -> None:
             super().generate_parameters(use_sim_time)
 
-            # TODO: Get user params
-            if False:
-                self.param_file.update(self.clearpath_config.platform.extras)
+            extras = self.clearpath_config.platform.extras.ros_parameters.get(self.EKF_NODE)
+            if extras:
+                self.param_file.update({self.EKF_NODE: extras})
             else:
                 if self.platform == Platform.J100:
                     imu0_parameters = {
@@ -127,7 +134,7 @@ def generate_parameters(self, use_sim_time: bool = False) -> None:
                         # Gravitational acceleration is removed in IMU driver
                         'imu0_remove_gravitational_acceleration': True
                     }
-                    self.param_file.update({'ekf_node': imu0_parameters})
+                    self.param_file.update({self.EKF_NODE: imu0_parameters})
 
                 # Add all additional IMU's
                 imus = self.clearpath_config.sensors.get_all_imu()
@@ -141,7 +148,7 @@ def generate_parameters(self, use_sim_time: bool = False) -> None:
                             f'{imu_name}_queue_size': 10,
                             f'{imu_name}_remove_gravitational_acceleration': True
                         }
-                        self.param_file.update({'ekf_node': imu_parameters})
+                        self.param_file.update({self.EKF_NODE: imu_parameters})
 
     class TeleopJoyParam(BaseParam):
         def __init__(self,