diff --git a/clearpath_generator_common/clearpath_generator_common/param/manipulators.py b/clearpath_generator_common/clearpath_generator_common/param/manipulators.py index 2700bfb..8c568b6 100644 --- a/clearpath_generator_common/clearpath_generator_common/param/manipulators.py +++ b/clearpath_generator_common/clearpath_generator_common/param/manipulators.py @@ -107,11 +107,19 @@ def generate_parameters(self, use_sim_time: bool = False) -> None: arm_param_file.parameters, {r'${name}': f'{arm.name}_{arm.arm_id}'} ) + extra_parameters = replace_dict_items( + arm.ros_parameters, + {r'${name}': f'{arm.name}_{arm.arm_id}'} + ) else: updated_parameters = replace_dict_items( arm_param_file.parameters, {r'${name}': arm.name} ) + extra_parameters = replace_dict_items( + arm.ros_parameters, + {r'${name}': arm.name} + ) # UR Arm Exception. Update Rate if arm.MANIPULATOR_MODEL == UniversalRobots.MANIPULATOR_MODEL: try: @@ -131,8 +139,15 @@ def generate_parameters(self, use_sim_time: bool = False) -> None: updated_parameters, {r'${controller_name}': arm.name} ) + extra_parameters = replace_dict_items( + extra_parameters, + {r'${controller_name}': arm.name} + ) self.param_file.parameters = merge_dict( updated_parameters, self.param_file.parameters) + # Overwrite ros parameters with extra + self.param_file.parameters = merge_dict( + extra_parameters, self.param_file.parameters) # Grippers for arm in self.clearpath_config.manipulators.get_all_arms(): if not arm.gripper: @@ -154,18 +169,35 @@ def generate_parameters(self, use_sim_time: bool = False) -> None: gripper_param_file.parameters, {r'${name}': f'{gripper.name}_{gripper.arm_id}'} ) + extra_parameters = replace_dict_items( + gripper.ros_parameters, + {r'${name}': f'{gripper.name}_{gripper.arm_id}'} + ) else: updated_parameters = replace_dict_items( gripper_param_file.parameters, {r'${name}': gripper.name} ) + extra_parameters = replace_dict_items( + gripper.ros_parameters, + {r'${name}': gripper.name} + ) updated_parameters = replace_dict_items( updated_parameters, {r'${controller_name}': gripper.name} ) + extra_parameters = replace_dict_items( + extra_parameters, + {r'${controller_name}': gripper.name} + ) + self.param_file.parameters = merge_dict( self.param_file.parameters, updated_parameters) + # Overwrite ros parameters with extra + self.param_file.parameters = merge_dict( + extra_parameters, self.param_file.parameters) + # Lifts for lift in self.clearpath_config.manipulators.get_all_lifts(): # Lift Control Parameter File @@ -182,9 +214,27 @@ def generate_parameters(self, use_sim_time: bool = False) -> None: lift_param_file.parameters, {r'${name}': lift.name} ) + updated_parameters = replace_dict_items( + updated_parameters, + {r'${controller_name}': lift.name} + ) + + extra_parameters = replace_dict_items( + lift.ros_parameters, + {r'${name}': lift.name} + ) + extra_parameters = replace_dict_items( + extra_parameters, + {r'${controller_name}': lift.name} + ) + self.param_file.parameters = merge_dict( self.param_file.parameters, updated_parameters) + # Overwrite ros parameters with extra + self.param_file.parameters = merge_dict( + extra_parameters, self.param_file.parameters) + def generate_parameter_file(self): param_writer = ParamWriter(self.param_file) param_writer.write_file() diff --git a/clearpath_generator_common/clearpath_generator_common/param/platform.py b/clearpath_generator_common/clearpath_generator_common/param/platform.py index e03e713..2defade 100644 --- a/clearpath_generator_common/clearpath_generator_common/param/platform.py +++ b/clearpath_generator_common/clearpath_generator_common/param/platform.py @@ -123,17 +123,32 @@ def generate_parameters(self, use_sim_time: bool = False) -> None: arm_param_file.parameters, {r'${name}': f'{arm.name}_{arm.arm_id}'} ) + extra_parameters = replace_dict_items( + arm.ros_parameters, + {r'${name}': f'{arm.name}_{arm.arm_id}'} + ) else: updated_parameters = replace_dict_items( arm_param_file.parameters, {r'${name}': arm.name} ) + extra_parameters = replace_dict_items( + arm.ros_parameters, + {r'${name}': arm.name} + ) updated_parameters = replace_dict_items( updated_parameters, {r'${controller_name}': arm.name} ) + extra_parameters = replace_dict_items( + extra_parameters, + {r'${controller_name}': arm.name} + ) self.param_file.parameters = merge_dict( self.param_file.parameters, updated_parameters) + # Overwrite ros parameters with extra + self.param_file.parameters = merge_dict( + extra_parameters, self.param_file.parameters) # Gripper Control if self.parameter == PlatformParam.CONTROL and use_sim_time: @@ -157,15 +172,27 @@ def generate_parameters(self, use_sim_time: bool = False) -> None: gripper_param_file.parameters, {r'${name}': f'{gripper.name}_{gripper.arm_id}'} ) + extra_parameters = replace_dict_items( + gripper.ros_parameters, + {r'${name}': f'{gripper.name}_{gripper.arm_id}'} + ) else: updated_parameters = replace_dict_items( gripper_param_file.parameters, {r'${name}': gripper.name} ) + extra_parameters = replace_dict_items( + gripper.ros_parameters, + {r'${name}': gripper.name} + ) updated_parameters = replace_dict_items( updated_parameters, {r'${controller_name}': gripper.name} ) + extra_parameters = replace_dict_items( + extra_parameters, + {r'${controller_name}': gripper.name} + ) self.param_file.parameters = merge_dict( self.param_file.parameters, updated_parameters) @@ -186,9 +213,27 @@ def generate_parameters(self, use_sim_time: bool = False) -> None: lift_param_file.parameters, {r'${name}': lift.name} ) + updated_parameters = replace_dict_items( + updated_parameters, + {r'${controller_name}': lift.name} + ) + + extra_parameters = replace_dict_items( + lift.ros_parameters, + {r'${name}': lift.name} + ) + extra_parameters = replace_dict_items( + extra_parameters, + {r'${controller_name}': lift.name} + ) + self.param_file.parameters = merge_dict( self.param_file.parameters, updated_parameters) + # Overwrite ros parameters with extra + self.param_file.parameters = merge_dict( + extra_parameters, self.param_file.parameters) + # Get extra ros parameters from config extras = self.clearpath_config.platform.extras.ros_parameters for node in extras: