pid_loader_and_saver.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # Copyright 2011 Shadow Robot Company Ltd.
00004 #
00005 # This program is free software: you can redistribute it and/or modify it
00006 # under the terms of the GNU General Public License as published by the Free
00007 # Software Foundation, either version 2 of the License, or (at your option)
00008 # any later version.
00009 #
00010 # This program is distributed in the hope that it will be useful, but WITHOUT
00011 # ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
00012 # FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
00013 # more details.
00014 #
00015 # You should have received a copy of the GNU General Public License along
00016 # with this program.  If not, see <http://www.gnu.org/licenses/>.
00017 #
00018 
00019 import roslib; roslib.load_manifest('sr_gui_controller_tuner')
00020 import rospy
00021 
00022 import yaml
00023 try:
00024     from yaml import CLoader as Loader, CDumper as Dumper
00025 except ImportError:
00026     from yaml import Loader, Dumper
00027 
00028 class PidLoader(object):
00029     def __init__(self):
00030         pass
00031 
00032     def get_settings(self, param_name):
00033         param_dict = {}
00034         if len(param_name) == 2:
00035             try:
00036                 tmp_dict = rospy.get_param(param_name[0])
00037             except KeyError:
00038                 return -1
00039             for item in tmp_dict.items():
00040                 param_dict["pos/"+item[0]] = item[1]
00041 
00042             try:
00043                 tmp_dict = rospy.get_param(param_name[1])
00044             except KeyError:
00045                 return -1
00046             for item in tmp_dict.items():
00047                 param_dict["vel/"+item[0]] = item[1]
00048         else:
00049             try:
00050                 param_dict = rospy.get_param(param_name)
00051             except KeyError:
00052                 return -1
00053         return param_dict
00054 
00055 class PidSaver(object):
00056     def __init__(self, file_path):
00057         self.path = file_path
00058 
00059     def save_settings(self, param_path, parameters_dict):
00060         f = open(self.path,'r')
00061         document = ""
00062         for line in f.readlines():
00063             document += line
00064         f.close()
00065 
00066         yaml_config = yaml.load(document)
00067 
00068         for item in parameters_dict.items():
00069             if "pos/" in item[0]:
00070                 yaml_config[param_path[0]]["position_pid"][item[0].split("pos/")[1]] = item[1]
00071             elif "vel/" in item[0]:
00072                 yaml_config[param_path[0]]["velocity_pid"][item[0].split("vel/")[1]] = item[1]
00073             else:
00074                 yaml_config[param_path[0]][param_path[1]][item[0]] = item[1]
00075 
00076         full_config_to_write = yaml.dump(yaml_config, default_flow_style=False)
00077 
00078         f = open(self.path, 'w')
00079         f.write(full_config_to_write)
00080         f.close()
00081 
00082 if __name__ == '__main__':
00083     path_to_config = "~"
00084     try:
00085         path_to_config = roslib.packages.get_pkg_dir("sr_edc_controller_configuration")
00086 
00087         pid_saver = PidSaver(path_to_config+"/sr_edc_mixed_position_velocity_joint_controllers.yaml")
00088         pid_saver.save_settings(["sh_wrj2_mixed_position_velocity_controller","pid"], {"d":1.0})
00089     except:
00090         rospy.logwarn("couldnt find the sr_edc_controller_configuration package")
00091 


sr_gui_controller_tuner
Author(s): Ugo Cupcic
autogenerated on Fri Jan 3 2014 12:04:50