Fix/Feature: UR Arm Controller Update Rate (#225)

* Change controller update rate for universal robots to 500

* Use UniversalRobots update_rate parameters if available

Author: luis-camero

clearpath_generator_common/clearpath_generator_common/param/manipulators.py

@@ -33,6 +33,8 @@
 
 from clearpath_config.clearpath_config import ClearpathConfig
 from clearpath_config.common.utils.dictionary import merge_dict, replace_dict_items
+from clearpath_config.manipulators.types.arms import Franka, UniversalRobots
+from clearpath_config.manipulators.types.grippers import FrankaGripper
 from clearpath_generator_common.common import MoveItParamFile, Package, ParamFile
 from clearpath_generator_common.param.writer import ParamWriter
 
@@ -103,12 +105,39 @@ def generate_parameters(self, use_sim_time: bool = False) -> None:
                     parameters={}
                 )
                 arm_param_file.read()
+
+                # Franka Exception. Add Arm ID.
+                if arm.MANIPULATOR_MODEL == Franka.MANIPULATOR_MODEL:
+                    updated_parameters = replace_dict_items(
+                        arm_param_file.parameters,
+                        {r'${name}': f'{arm.name}_{arm.arm_id}'}
+                    )
+                else:
+                    updated_parameters = replace_dict_items(
+                        arm_param_file.parameters,
+                        {r'${name}': arm.name}
+                    )
+                # UR Arm Exception. Update Rate
+                if arm.MANIPULATOR_MODEL == UniversalRobots.MANIPULATOR_MODEL:
+                    try:
+                        # Update Rate from Parameter File
+                        update_rate_param_file = ParamFile(
+                            name=f'{arm.ur_type}_update_rate',
+                            package=Package('ur_robot_driver'),
+                            path='config',
+                            parameters={}
+                        )
+                        update_rate_param_file.read()
+                        updated_parameters.update(update_rate_param_file.parameters)
+                    except Exception as e:
+                        print(f'Unable to get UniversalRobots {arm.ur_type}_'
+                              f'update_rate.yaml parameter file: {e.args[0]}')
                 updated_parameters = replace_dict_items(
-                    arm_param_file.parameters,
-                    {r'${name}': arm.name}
+                    updated_parameters,
+                    {r'${controller_name}': arm.name}
                 )
                 self.param_file.parameters = merge_dict(
-                    self.param_file.parameters, updated_parameters)
+                    updated_parameters, self.param_file.parameters)
             # Grippers
             for arm in self.clearpath_config.manipulators.get_all_arms():
                 if not arm.gripper:

clearpath_manipulators_description/config/arm/universal_robots/control.yaml

@@ -1,6 +1,6 @@
 controller_manager:
   ros__parameters:
-    update_rate: 1000  # Hz
+    update_rate: 500  # Hz
 
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster