diff --git a/clearpath_generator_common/clearpath_generator_common/param/manipulators.py b/clearpath_generator_common/clearpath_generator_common/param/manipulators.py index a58aa19d..c1440605 100644 --- a/clearpath_generator_common/clearpath_generator_common/param/manipulators.py +++ b/clearpath_generator_common/clearpath_generator_common/param/manipulators.py @@ -107,17 +107,33 @@ 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) + # Grippers for arm in self.clearpath_config.manipulators.get_all_arms(): if not arm.gripper: @@ -139,18 +155,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 @@ -167,9 +200,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 e03e7138..2defade3 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: