def get_current_controller(self):
req = ListControllersRequest()
resp = self.controllers_srv.call(req)
for c in resp.controller:
if self.use_wrist_in_position is True:
if c.name == 'wrist_controller':
if c.state == 'running':
self.controller_mode = 'gravity'
self.gui_disabled_status['arm_1_slider'] = True
self.gui_disabled_status['arm_2_slider'] = True
self.gui_disabled_status['arm_3_slider'] = True
self.gui_disabled_status['arm_4_slider'] = True
elif c.name == 'arm_controller':
if c.state == 'running':
self.controller_mode = 'position'
self.gui_disabled_status['arm_1_slider'] = False
self.gui_disabled_status['arm_2_slider'] = False
self.gui_disabled_status['arm_3_slider'] = False
self.gui_disabled_status['arm_4_slider'] = False
else:
if c.name == 'arm_controller':
if c.state == 'running':
self.controller_mode = 'position'
self.gui_disabled_status['arm_1_slider'] = False
self.gui_disabled_status['arm_2_slider'] = False
self.gui_disabled_status['arm_3_slider'] = False
self.gui_disabled_status['arm_4_slider'] = False
self.gui_disabled_status['arm_5_slider'] = False
self.gui_disabled_status['arm_6_slider'] = False
self.gui_disabled_status['arm_7_slider'] = False
def init_controller_list(self):
"""
Get all the controllers running and store their name per group
"""
service_name = self._prefix + \
'/controller_manager/list_controllers'
rospy.loginfo("Waiting for %s", service_name)
try:
rospy.wait_for_service(service_name, 5.0)
except rospy.ROSException:
rospy.logerr("%s did not show up. Giving up", service_name)
return False
cm_list_client = rospy.ServiceProxy(service_name, ListControllers)
rospy.loginfo("Found %s", service_name)
# get all the controllers
try:
resp = cm_list_client(ListControllersRequest())
except rospy.ServiceException:
rospy.logerr("Could not call list_controllers")
return
# loop on the controllers
if resp:
for controller in resp.controller:
cname_split = controller.name.split("_")
if len(cname_split) > 1:
if cname_split[0] in ["torso", "zlift", "head"]:
self._controller_list[cname_split[0]] = controller.name
else:
if len(cname_split) > 2:
if cname_split[1] in ["arm", "hand"]:
group_name = cname_split[0] + "_" +\
cname_split[1]
self._controller_list[group_name] =\
controller.name
for group_name in self._controller_list:
if group_name not in self._sub:
try:
self.set_up_subscriber(group_name)
# give some time for the subscriber
# to receive its first data
rospy.sleep(0.5)
except rospy.ROSException:
rospy.logerr(
"Could not set up subscriber \
for group %s.", group_name)
resp.error_code.val =\
PostureRecordErrorCodes.NOCONTROLLER
def is_controller_in_state(controller_name,
desired_state,
list_controller_response=None):
"""
Check if the controller is there and if its in it's desired state.
E.G.: check if gravity_controller is there and is in state stopped
:param controller_name:
:param desired_state:
:return:
"""
if list_controller_response is None:
list_srv = rospy.ServiceProxy(CONTROLLER_MANAGER_LIST_SRV,
ListControllers)
lcr = ListControllersRequest()
list_controller_response = list_srv.call(lcr)
for ctrler in list_controller_response.controller:
if ctrler.name == controller_name and ctrler.state == desired_state:
return True
return False
def __init__(self):
rospy.loginfo("Initializing InteractiveJointTrajCtrl")
ctl_mngr_srv = rospy.ServiceProxy(CONTROLLER_MNGR_SRV, ListControllers)
rospy.loginfo("Connecting to " + CONTROLLER_MNGR_SRV)
ctl_mngr_srv.wait_for_service()
rospy.loginfo("Connected.")
self.rviz_config_str = ""
req = ListControllersRequest()
resp = ctl_mngr_srv.call(req)
#ListControllersResponse()
ctl_mngr_srv.close()
joints_dict = get_joint_limits()
publishers_dict = {}
# dictionary that contains
# key: head_controller
# value:
# joints: ['head_1_joint', 'head_2_joint']
# publisher: pub
# ims: [interactive_marker_server_head_1_joint, interactive_marker_server_head_2_joint]
for cs in resp.controller: # For every controller, create a publisher
#cs = ControllerState()
print "cs.name: " + str(cs.name),
if len(cs.resources
) > 0: # If the controller controls any joint only
print "...controls joints!"
publishers_dict[cs.name] = {}
publishers_dict[cs.name]['joints'] = cs.resources
cmd_topic = "/" + cs.name + "/command"
publishers_dict[cs.name]['pub'] = rospy.Publisher(
cmd_topic, JointTrajectory)
publishers_dict[cs.name]['ims'] = []
for joint_name in publishers_dict[cs.name]['joints']:
publishers_dict[cs.name]['ims'].append(
LinkInteractiveMarker(joint_name, cs.name,
cs.resources))
curr_im_rviz_cfg = self.create_im_config_rviz_block(
joint_name)
self.rviz_config_str += curr_im_rviz_cfg
self.save_rviz_config(self.rviz_config_str)
def switch_to_position_controller():
rospy.wait_for_service("/controller_manager/list_controllers")
rate = rospy.Rate(1)
list_controllers = rospy.ServiceProxy(
"/controller_manager/list_controllers", ListControllers)
position_controller_available = False
while not position_controller_available:
res = list_controllers(ListControllersRequest())
position_controller_available = any([
controller.name == "joint_group_position_controller"
for controller in res.controller
])
rate.sleep()
rospy.logdebug("Switching controllers")
rospy.wait_for_service("/controller_manager/switch_controller")
switch_controller = rospy.ServiceProxy(
'/controller_manager/switch_controller', SwitchController)
req = SwitchControllerRequest()
req.start_controllers = ["joint_group_position_controller"]
req.stop_controllers = ["arm_controller"]
req.strictness = 2
switch_controller(req)
def get_controllers_list():
list_srv = rospy.ServiceProxy(CONTROLLER_MANAGER_LIST_SRV, ListControllers)
lcr = ListControllersRequest()
return list_srv.call(lcr)
def _is_controller_running(self, controller_name):
response = self._list_controller_service(ListControllersRequest())
for controller in response.controller:
if controller.name == controller_name:
return controller.state == 'running'
return False
def _is_controller_loaded(self, controller_name):
response = self._list_controller_service(ListControllersRequest())
for controller in response.controller:
if controller.name == controller_name:
return True
return False