def __init__(self, limb="right"):
"""
Constructor.
@type limb: str
@param limb: limb side to interface
"""
params = RobotParams()
limb_names = params.get_limb_names()
if limb not in limb_names:
rospy.logerr("Cannot detect Cuff's limb {0} on this robot."
" Valid limbs are {1}. Exiting Cuff.init().".format(
limb, limb_names))
return
self.limb = limb
self.device = None
self.name = "cuff"
self.cuff_config_sub = rospy.Subscriber('/io/robot/cuff/config',
IODeviceConfiguration,
self._config_callback)
# Wait for the cuff status to be true
intera_dataflow.wait_for(
lambda: not self.device is None,
timeout=5.0,
timeout_msg=("Failed find cuff on limb '{0}'.".format(limb)))
self._cuff_io = IODeviceInterface("robot", self.name)
def __init__(self, limb="right", synchronous_pub=False):
"""
Constructor.
@type limb: str
@param limb: limb to interface
@type synchronous_pub: bool
@param synchronous_pub: designates the JointCommand Publisher
as Synchronous if True and Asynchronous if False.
Synchronous Publishing means that all joint_commands publishing to
the robot's joints will block until the message has been serialized
into a buffer and that buffer has been written to the transport
of every current Subscriber. This yields predicable and consistent
timing of messages being delivered from this Publisher. However,
when using this mode, it is possible for a blocking Subscriber to
prevent the joint_command functions from exiting. Unless you need exact
JointCommand timing, default to Asynchronous Publishing (False).
For more information about Synchronous Publishing see:
http://wiki.ros.org/rospy/Overview/Publishers%20and%20Subscribers#queue_size:_publish.28.29_behavior_and_queuing
"""
params = RobotParams()
limb_names = params.get_limb_names()
if limb not in limb_names:
rospy.logerr("Cannot detect limb {0} on this robot."
" Valid limbs are {1}. Exiting Limb.init().".format(
limb, limb_names))
return
joint_names = params.get_joint_names(limb)
if not joint_names:
rospy.logerr("Cannot detect joint names for limb {0} on this "
"robot. Exiting Limb.init().".format(limb))
return
self.name = limb
self._joint_angle = dict()
self._joint_velocity = dict()
self._joint_effort = dict()
self._cartesian_pose = dict()
self._cartesian_velocity = dict()
self._cartesian_effort = dict()
self._joint_names = joint_names
self._collision_state = False
self._tip_states = None
ns = '/robot/limb/' + limb + '/'
self._command_msg = JointCommand()
self._pub_speed_ratio = rospy.Publisher(ns + 'set_speed_ratio',
Float64,
latch=True,
queue_size=10)
queue_size = None if synchronous_pub else 1
with warnings.catch_warnings():
warnings.simplefilter("ignore")
self._pub_joint_cmd = rospy.Publisher(ns + 'joint_command',
JointCommand,
tcp_nodelay=True,
queue_size=queue_size)
self._pub_joint_cmd_timeout = rospy.Publisher(ns +
'joint_command_timeout',
Float64,
latch=True,
queue_size=10)
_cartesian_state_sub = rospy.Subscriber(ns + 'endpoint_state',
EndpointState,
self._on_endpoint_states,
queue_size=1,
tcp_nodelay=True)
_tip_states_sub = rospy.Subscriber(ns + 'tip_states',
EndpointStates,
self._on_tip_states,
queue_size=1,
tcp_nodelay=True)
_collision_state_sub = rospy.Subscriber(ns +
'collision_detection_state',
CollisionDetectionState,
self._on_collision_state,
queue_size=1,
tcp_nodelay=True)
joint_state_topic = 'robot/joint_states'
_joint_state_sub = rospy.Subscriber(joint_state_topic,
JointState,
self._on_joint_states,
queue_size=1,
tcp_nodelay=True)
ns_pkn = "ExternalTools/" + limb + "/PositionKinematicsNode/"
self._iksvc = rospy.ServiceProxy(ns_pkn + 'IKService', SolvePositionIK)
self._fksvc = rospy.ServiceProxy(ns_pkn + 'FKService', SolvePositionFK)
rospy.wait_for_service(ns_pkn + 'IKService', 5.0)
rospy.wait_for_service(ns_pkn + 'FKService', 5.0)
err_msg = ("%s limb init failed to get current joint_states "
"from %s") % (self.name.capitalize(), joint_state_topic)
intera_dataflow.wait_for(lambda: len(self._joint_angle.keys()) > 0,
timeout_msg=err_msg,
timeout=5.0)
err_msg = ("%s limb init failed to get current endpoint_state "
"from %s") % (self.name.capitalize(), ns + 'endpoint_state')
intera_dataflow.wait_for(lambda: len(self._cartesian_pose.keys()) > 0,
timeout_msg=err_msg,
timeout=5.0)
err_msg = ("%s limb init failed to get current tip_states "
"from %s") % (self.name.capitalize(), ns + 'tip_states')
intera_dataflow.wait_for(lambda: self._tip_states is not None,
timeout_msg=err_msg,
timeout=5.0)
