4 import roslib; roslib.load_manifest(
'pr2_controller_manager')
12 rospy.wait_for_service(
'pr2_controller_manager/list_controller_types')
13 s = rospy.ServiceProxy(
'pr2_controller_manager/list_controller_types', ListControllerTypes)
14 resp = s.call(ListControllerTypesRequest())
19 rospy.wait_for_service(
'pr2_controller_manager/reload_controller_libraries')
20 s = rospy.ServiceProxy(
'pr2_controller_manager/reload_controller_libraries', ReloadControllerLibraries)
22 list_srv = rospy.ServiceProxy(
'pr2_controller_manager/list_controllers', ListControllers)
23 load_srv = rospy.ServiceProxy(
'pr2_controller_manager/load_controller', LoadController)
24 switch_srv = rospy.ServiceProxy(
'pr2_controller_manager/switch_controller', SwitchController)
26 print "Restore:", restore
28 originally = list_srv.call(ListControllersRequest())
30 resp = s.call(ReloadControllerLibrariesRequest(force_kill))
32 print "Successfully reloaded libraries" 35 print "Failed to reload libraries. Do you still have controllers loaded?" 39 for c
in originally.controllers:
42 for c, s
in zip(originally.controllers, originally.state):
45 switch_srv(start_controllers = to_start,
46 stop_controllers = [],
47 strictness = SwitchControllerRequest.BEST_EFFORT)
48 print "Controllers restored to original state" 53 rospy.wait_for_service(
'pr2_controller_manager/list_controllers')
54 s = rospy.ServiceProxy(
'pr2_controller_manager/list_controllers', ListControllers)
55 resp = s.call(ListControllersRequest())
56 if len(resp.controllers) == 0:
57 print "No controllers are loaded in mechanism control" 59 for c, s
in zip(resp.controllers, resp.state):
63 rospy.wait_for_service(
'pr2_controller_manager/load_controller')
64 s = rospy.ServiceProxy(
'pr2_controller_manager/load_controller', LoadController)
65 resp = s.call(LoadControllerRequest(name))
70 print "Error when loading", name
74 rospy.wait_for_service(
'pr2_controller_manager/unload_controller')
75 s = rospy.ServiceProxy(
'pr2_controller_manager/unload_controller', UnloadController)
76 resp = s.call(UnloadControllerRequest(name))
78 print "Unloaded %s successfully" % name
81 print "Error when unloading", name
97 rospy.wait_for_service(
'pr2_controller_manager/switch_controller')
98 s = rospy.ServiceProxy(
'pr2_controller_manager/switch_controller', SwitchController)
101 strictness = SwitchControllerRequest.STRICT
106 resp = s.call(SwitchControllerRequest(start, stop, strictness))
109 print "Started %s successfully" % names
111 print "Stopped %s successfully" % names
115 print "Error when starting ", names
117 print "Error when stopping ", names
def start_controller(name)
def load_controller(name)
def stop_controllers(names)
def reload_libraries(force_kill, restore=False)
def stop_controller(name)
def start_stop_controllers(names, st)
def list_controller_types()
def start_controllers(names)
def unload_controller(name)