openrave_actionlib: setup_controllers.py Source File

Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 import os, roslib, time
00003 roslib.load_manifest('openrave_actionlib')
00004 roslib.load_manifest('pr2_controller_manager')
00005 
00006 import rospy, sys
00007 import rosparam
00008 import yaml
00009 from pr2_controller_manager import pr2_controller_manager_interface
00010 from pr2_mechanism_msgs.msg import MechanismStatistics
00011 from pr2_mechanism_msgs.srv import *
00012 
00013 pr2_controller_manager_interface.stop_controller('l_arm_controller')
00014 pr2_controller_manager_interface.stop_controller('r_arm_controller')
00015 pr2_controller_manager_interface.stop_controller('torso_controller')
00016 
00017 name_yaml = yaml.load(open(os.path.join(roslib.packages.get_pkg_dir('openrave_actionlib'),'pr2_midbody_controller.yaml')))
00018 rosparam._set_param("",name_yaml)
00019 time.sleep(0.5) # wait for parameter server
00020 
00021 rospy.wait_for_service('pr2_controller_manager/load_controller')
00022 rospy.ServiceProxy('pr2_controller_manager/load_controller', LoadController)('midbody_controller')
00023 pr2_controller_manager_interface.start_controller('midbody_controller')