def load_cartesian(self, isRightArm):
'''
loads the cartesian controller on the arm specified.
stops the impedance controller first.
@param isRightArm: if true then the controller will be loaded on the right arm, else the left arm
'''
rospy.logwarn('starting cartesian controller on the '+ ('right' if isRightArm else 'left') + ' arm')
if isRightArm:
status = pr2cm_interface.stop_controller('r_arm_cart_imped_controller')
#print 'stopping controller status:', status
status = pr2cm_interface.start_controller('r_arm_controller')
#print 'starting controller status:', status
status = pr2cm_interface.start_controller('r_arm_cartesian_trajectory_controller')
#print 'starting trajectory controller status:', status
status = pr2cm_interface.start_controller('r_arm_cartesian_pose_controller')
#print 'starting pose controller status:', status
else:
status = pr2cm_interface.stop_controller('l_arm_cart_imped_controller')
#print 'stopping controller status:', status
status = pr2cm_interface.start_controller('l_arm_controller')
#print 'starting controller status:', status
status = pr2cm_interface.start_controller('l_arm_cartesian_trajectory_controller')
#print 'starting trajectory controller status:', status
status = pr2cm_interface.start_controller('l_arm_cartesian_pose_controller')
#print 'starting pose controller status:', status
rospy.loginfo('done')
return status
def set_controllers_state(self, controllers, start_controller):
print "Dealing with " + str(controllers)
# Need to activate physics or the calls to pr2_controller_manager will wait till end of time
self.set_paused_physics(False)
for controller in controllers:
if start_controller:
pr2_controller_manager_interface.start_controller(controller)
else:
pr2_controller_manager_interface.stop_controller(controller)
# Deactivate physics to make more changes (eventually)
self.set_paused_physics(True)
print "Done with controllers states"
def shutdown(sig, stackframe):
pr2_controller_manager_interface.stop_controller(CONTROLLER_NAME)
pr2_controller_manager_interface.unload_controller(CONTROLLER_NAME)
if prev_handler is not None:
prev_handler(signal.SIGINT,None)
def shutdown(sig, stackframe):
pr2_controller_manager_interface.stop_controller(CONTROLLER_NAME)
pr2_controller_manager_interface.unload_controller(CONTROLLER_NAME)
if prev_handler is not None:
prev_handler(signal.SIGINT, None)
#! /usr/bin/env python
import os, roslib, time
roslib.load_manifest('openrave_actionlib')
roslib.load_manifest('pr2_controller_manager')
import rospy, sys
import rosparam
import yaml
from pr2_controller_manager import pr2_controller_manager_interface
from pr2_mechanism_msgs.msg import MechanismStatistics
from pr2_mechanism_msgs.srv import *
pr2_controller_manager_interface.stop_controller('l_arm_controller')
pr2_controller_manager_interface.stop_controller('r_arm_controller')
pr2_controller_manager_interface.stop_controller('torso_controller')
name_yaml = yaml.load(
open(
os.path.join(roslib.packages.get_pkg_dir('openrave_actionlib'),
'pr2_midbody_controller.yaml')))
rosparam._set_param("", name_yaml)
time.sleep(0.5) # wait for parameter server
rospy.wait_for_service('pr2_controller_manager/load_controller')
rospy.ServiceProxy('pr2_controller_manager/load_controller',
LoadController)('midbody_controller')
pr2_controller_manager_interface.start_controller('midbody_controller')
#! /usr/bin/env python
import os, roslib, time
roslib.load_manifest('openrave_actionlib')
roslib.load_manifest('pr2_controller_manager')
import rospy, sys
import rosparam
import yaml
from pr2_controller_manager import pr2_controller_manager_interface
from pr2_mechanism_msgs.msg import MechanismStatistics
from pr2_mechanism_msgs.srv import *
pr2_controller_manager_interface.stop_controller('l_arm_controller')
pr2_controller_manager_interface.stop_controller('r_arm_controller')
pr2_controller_manager_interface.stop_controller('torso_controller')
name_yaml = yaml.load(open(os.path.join(roslib.packages.get_pkg_dir('openrave_actionlib'),'pr2_midbody_controller.yaml')))
rosparam._set_param("",name_yaml)
time.sleep(0.5) # wait for parameter server
rospy.wait_for_service('pr2_controller_manager/load_controller')
rospy.ServiceProxy('pr2_controller_manager/load_controller', LoadController)('midbody_controller')
pr2_controller_manager_interface.start_controller('midbody_controller')
