def switch_controller(self, controller_name):
"""
:param controller_name: Controller to activate.
:return: Success True/False
"""
rospy.sleep(0.5)
start_controllers = [self.controllers[controller_name]]
stop_controllers = [
self.controllers[n] for n in self.controllers
if n != controller_name
]
controller_switch_msg = cm_srv.SwitchControllerRequest()
controller_switch_msg.strictness = 1
controller_switch_msg.start_controllers = start_controllers
controller_switch_msg.stop_controllers = stop_controllers
res = self.switcher_srv(controller_switch_msg).ok
if res:
rospy.logdebug(
'Successfully switched to controller %s (%s)' %
(controller_name, self.controllers[controller_name]))
return res
else:
return False
def change_controller(move_group, second_try=False):
"""
Changes between motor controllers
move_group -> Name of required move group.
"""
global list_controllers_service
global switch_controllers_service
controller_map = {
'gripper': 'cartesian_motor_controller',
'whole_arm': 'cartesian_motor_controller',
'realsense': 'cartesian_motor_controller_realsense',
'sucker': 'cartesian_motor_controller_sucker',
'wrist_only': 'cartesian_motor_controller_wrist'
}
rospy.loginfo('SWITCHING CONTROLLERS')
if move_group not in controller_map:
rospy.logerr('%s is not a valid move group for switching controllers' % move_group)
return False
wanted_controller = controller_map[move_group]
c_list = list_controllers_service.call()
running_controllers = []
for c in c_list.controller:
if c.name == 'joint_state_controller':
continue
if c.name == wanted_controller and c.state == 'running':
rospy.loginfo('Controller %s is already running' % wanted_controller)
return True
if c.state == 'running':
running_controllers.append(c.name)
controllerSwitch = cmsv.SwitchControllerRequest()
controllerSwitch.strictness = 1
controllerSwitch.start_controllers = [wanted_controller]
controllerSwitch.stop_controllers = running_controllers
# Return True if controller was successfully switched
res = switch_controllers_service(controllerSwitch).ok
if res:
rospy.loginfo('Successfully switched controllers for move group %s' % move_group)
return res
elif second_try == False:
rospy.logerr('Failed to switch controllers for move group %s' % move_group)
rospy.sleep(1.0)
return change_controller(move_group, True)
else:
return False
def switch_controller(self, controller_name):
controllers = self.lister_srv().controller
selected = None
for controller in controllers:
if controller.name == controller_name:
selected = controller
break
required = [
item for claimed in controller.claimed_resources
for item in claimed.resources
]
start_controllers = [controller_name] if controller_name else []
stop_controllers = [self.get_current_name()
] if not controller_name else []
for controller in controllers:
if controller.name == controller_name:
continue
resources = [
item for claimed in controller.claimed_resources
for item in claimed.resources
]
if len(
list(resource for resource in resources
if resource.startswith(self.robot_resource_name))):
stop_controllers.append(controller.name)
controller_switch_msg = cm_srv.SwitchControllerRequest()
controller_switch_msg.strictness = 1
controller_switch_msg.start_controllers = start_controllers
controller_switch_msg.stop_controllers = stop_controllers
res = self.switcher_srv(controller_switch_msg).ok
if res:
rospy.loginfo('Successfully switched to controller %s' %
(controller_name))
self.__current = controller_name
return res
else:
return False
def switch_controller(self, controller_name):
rospy.sleep(0.5)
start_controllers = [self.controllers[controller_name]]
stop_controllers = [
self.controllers[n] for n in self.controllers
if n != controller_name
]
controller_switch_msg = cm_srv.SwitchControllerRequest()
controller_switch_msg.strictness = 1
controller_switch_msg.start_controllers = start_controllers
controller_switch_msg.stop_controllers = stop_controllers
res = self.switcher_srv(controller_switch_msg).ok
if res:
rospy.loginfo('Successfully switched to controller %s (%s)' %
(controller_name, self.controllers[controller_name]))
return res
else:
return False
#! /usr/bin/env python
import rospy as rp
import controller_manager_msgs.srv as cmm
import time
controller_set = set()
controller_set.add('joint_state_controller')
controller_set.add('cartesian_motor_controller')
if __name__ == '__main__':
rp.init_node('controller_starter')
lp = rp.ServiceProxy('/controller_manager/list_controllers',
cmm.ListControllers)
lp.wait_for_service()
while True:
res = lp()
controllers = set([r.name for r in res.controller])
if controller_set <= controllers:
break
time.sleep(0.5)
sp = rp.ServiceProxy('/controller_manager/switch_controller',
cmm.SwitchController)
sp.wait_for_service()
req = cmm.SwitchControllerRequest()
req.start_controllers.append('joint_state_controller')
req.start_controllers.append('cartesian_motor_controller')
req.strictness = req.BEST_EFFORT
res = sp(req)
if res.ok == True:
rp.loginfo('Controllers started')