Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 import rospy, os
00020
00021 import yaml
00022 try:
00023 from yaml import CLoader as Loader, CDumper as Dumper
00024 except ImportError:
00025 from yaml import Loader, Dumper
00026
00027 class PidLoader(object):
00028 """
00029 Loads pid parameters of each controller in parameters_dict from a yaml file
00030 """
00031 def __init__(self):
00032 pass
00033
00034 def get_settings(self, param_name):
00035 param_dict = {}
00036 if len(param_name) == 2:
00037 try:
00038 tmp_dict = rospy.get_param(param_name[0])
00039 except KeyError:
00040 return -1
00041 for item in tmp_dict.items():
00042 param_dict["pos/"+item[0]] = item[1]
00043
00044 try:
00045 tmp_dict = rospy.get_param(param_name[1])
00046 except KeyError:
00047 return -1
00048 for item in tmp_dict.items():
00049 param_dict["vel/"+item[0]] = item[1]
00050 else:
00051 try:
00052 param_dict = rospy.get_param(param_name)
00053 except KeyError:
00054 return -1
00055 return param_dict
00056
00057 class PidSaver(object):
00058 """
00059 Saves pid parameters of each controller in parameters_dict in a yaml file
00060 """
00061 def __init__(self, file_path):
00062 self.path = file_path
00063
00064 def save_settings(self, param_path, parameters_dict):
00065 f = open(self.path,'r')
00066 document = ""
00067 for line in f.readlines():
00068 document += line
00069 f.close()
00070
00071 yaml_config = yaml.load(document)
00072
00073 for item in parameters_dict.items():
00074 if "pos/" in item[0]:
00075 yaml_config[param_path[0]]["position_pid"][item[0].split("pos/")[1]] = item[1]
00076 elif "vel/" in item[0]:
00077 yaml_config[param_path[0]]["velocity_pid"][item[0].split("vel/")[1]] = item[1]
00078 else:
00079 yaml_config[param_path[0]][param_path[1]][item[0]] = item[1]
00080
00081 full_config_to_write = yaml.dump(yaml_config, default_flow_style=False)
00082
00083 f = open(self.path, 'w')
00084 f.write(full_config_to_write)
00085 f.close()
00086
00087 if __name__ == '__main__':
00088 path_to_config = "~"
00089 try:
00090 path_to_config = os.path.join(rospkg.RosPack().get_path("sr_edc_controller_configuration"))
00091
00092 pid_saver = PidSaver(path_to_config+"/sr_edc_mixed_position_velocity_joint_controllers.yaml")
00093 pid_saver.save_settings(["sh_wrj2_mixed_position_velocity_controller","pid"], {"d":1.0})
00094 except:
00095 rospy.logwarn("couldnt find the sr_edc_controller_configuration package")
00096