def set_output(self, value, timeout=2.0):
"""
Control the state of the Digital Output.
Use this function for finer control over the wait_for timeout.
@type value: bool
@param value: new state {True, False} of the Output.
@type timeout: float
@param timeout: Seconds to wait for the io to reflect command.
If 0, just command once and return. [0]
"""
if not self._is_output:
raise IOError(
errno.EACCES, "Component is not an output [%s: %s]" %
(self._component_type, self._id))
cmd = DigitalOutputCommand()
cmd.name = self._id
cmd.value = value
self._pub_output.publish(cmd)
if not timeout == 0:
intera_dataflow.wait_for(
test=lambda: self.state == value,
timeout=timeout,
rate=100,
timeout_msg=("Failed to command digital io to: %r" %
(value, )),
body=lambda: self._pub_output.publish(cmd))
def send_configuration(self, config, timeout=5.0):
"""
Send a new JSON EndEffector configuration to the device.
@type config: dict
@param config: new json config to save on device
@type timeout: float
@param timeout: timeout in seconds - currently will wait for a minimum of
3 seconds before returning (to allow for reconfiguration)
"""
rospy.loginfo("Reconfiguring ClickSmart...")
cmd = IOCommand('reconfigure', {
"devices": {
self.name: config
},
"write_config": True
})
msg = cmd.as_msg()
self._node_command_pub.publish(msg)
if timeout:
# allow time for reconfiguration and storage write process
# TODO: use cmd acknowlegment timestamp, not hard-coded delay
delay = 3.0
delay_time = rospy.Time.now() + rospy.Duration(delay)
intera_dataflow.wait_for(
lambda: (rospy.Time.now() > delay_time and
(self.is_ready() or self.needs_init())),
timeout=max(timeout, delay),
body=lambda: self._node_command_pub.publish(msg),
timeout_msg=("Failed to reconfigure gripper."))
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, ee_device_id, initialize=True):
self.name = ee_device_id
self.endpoint_map = None
self.gripper_io = None
self._node_state = None
self._node_device_status = None
self._node_command_pub = rospy.Publisher('io/end_effector/command',
IOComponentCommand,
queue_size=10)
self._node_state_sub = rospy.Subscriber('io/end_effector/state',
IONodeStatus,
self._node_state_cb)
# Wait for the gripper device status to be true
intera_dataflow.wait_for(
lambda: self._node_device_status is not None,
timeout=5.0,
raise_on_error=initialize,
timeout_msg=(
"Failed to get gripper. No gripper attached on the robot."))
self.gripper_io = IODeviceInterface("end_effector", self.name)
self.gripper_io.config_changed.connect(self._load_endpoint_info)
if self.gripper_io.is_config_valid():
self._load_endpoint_info()
if initialize and self.needs_init():
self.initialize()
def __init__(self, versioned=False):
"""
Version checking capable constructor.
@type versioned: bool
@param versioned: True to check robot software version
compatibility on initialization. False (default) to ignore.
The compatibility of robot versions to SDK (intera_interface)
versions is defined in the L{intera_interface.VERSIONS_SDK2ROBOT}.
By default, the class does not check, but all examples do. The
example behavior can be overridden by changing the value of
L{intera_interface.CHECK_VERSION} to False.
"""
self._state = None
state_topic = 'robot/state'
self._state_sub = rospy.Subscriber(state_topic,
AssemblyState,
self._state_callback
)
if versioned and not self.version_check():
sys.exit(1)
intera_dataflow.wait_for(
lambda: not self._state is None,
timeout=5.0,
timeout_msg=("Failed to get robot state on %s" %
(state_topic,)),
)
def __init__(self, path_root, config_msg_type, status_msg_type):
self._path = path_root
self.config_mutex = Lock()
self.state_mutex = Lock()
self.cmd_times = []
self.ports = dict()
self.signals = dict()
self.state = None
self.config = None
self.state_changed = intera_dataflow.Signal()
self.config_changed = intera_dataflow.Signal()
self._config_sub = rospy.Subscriber(self._path + "/config",
config_msg_type,
self.handle_config)
self._state_sub = rospy.Subscriber(self._path + "/state",
status_msg_type, self.handle_state)
self._command_pub = rospy.Publisher(self._path + "/command",
IOComponentCommand,
queue_size=10)
# Wait for the state to be populated
intera_dataflow.wait_for(lambda: not self.state is None,
timeout=5.0,
timeout_msg=("Failed to get state."))
rospy.logdebug("Making new IOInterface on %s" % (self._path, ))
def __init__(self, ee_name="right_gripper", calibrate=True):
"""
Constructor.
@type name: str
@param name: robot gripper name (default: "right_gripper")
@type calibrate: bool
@param calibrate: Attempts to calibrate the gripper when initializing class (defaults True)
"""
self.devices = None
self.name = ee_name
if ee_name == 'right' or ee_name == 'left':
rospy.logwarn((
"Specifying gripper by 'side' is deprecated, please use full name"
" (ex: 'right_gripper')."))
self.name = '_'.join([ee_name, 'gripper'])
self.ee_config_sub = rospy.Subscriber('/io/end_effector/config',
IONodeConfiguration,
self._config_callback)
# Wait for the gripper device status to be true
intera_dataflow.wait_for(
lambda: not self.devices is None,
timeout=5.0,
timeout_msg=(
"Failed to get gripper. No gripper attached on the robot."))
self.gripper_io = IODeviceInterface("end_effector", self.name)
if self.has_error():
self.reboot()
calibrate = True
if calibrate and not self.is_calibrated():
self.calibrate()
def __init__(self, component_id):
"""
Constructor.
@param component_id: unique id of the digital component
"""
self._id = component_id
self._component_type = 'digital_io'
self._is_output = False
self._state = None
self.state_changed = intera_dataflow.Signal()
type_ns = '/robot/' + self._component_type
topic_base = type_ns + '/' + self._id
self._sub_state = rospy.Subscriber(topic_base + '/state',
DigitalIOState, self._on_io_state)
intera_dataflow.wait_for(
lambda: self._state != None,
timeout=2.0,
timeout_msg="Failed to get current digital_io state from %s" \
% (topic_base,),
)
# check if output-capable before creating publisher
if self._is_output:
self._pub_output = rospy.Publisher(type_ns + '/command',
DigitalOutputCommand,
queue_size=10)
def __init__(self, side="right", calibrate=True):
"""
Constructor.
@type side: str
@param side: robot gripper name
@type calibrate: bool
@param calibrate: Attempts to calibrate the gripper when initializing class (defaults True)
"""
self.devices = None
self.name = '_'.join([side, 'gripper'])
self.ee_config_sub = rospy.Subscriber('/io/end_effector/config',
IONodeConfiguration,
self._config_callback)
# Wait for the gripper device status to be true
intera_dataflow.wait_for(
lambda: not self.devices is None,
timeout=5.0,
timeout_msg=(
"Failed to get gripper. No gripper attached on the robot."))
self.gripper_io = IODeviceInterface("end_effector", self.name)
if self.has_error():
self.reboot()
calibrate = True
if calibrate and self.is_calibrated():
self.calibrate()
def __init__(self):
"""
Constructor.
"""
self._node_config = None
self._node_config_sub = rospy.Subscriber('io/internal_camera/config',
IONodeConfiguration,
self._node_config_cb)
self.cameras_io = dict()
camera_param_dict = RobotParams().get_camera_details()
camera_list = camera_param_dict.keys()
# check to make sure cameras is not an empty list
if not camera_list:
rospy.logerr(' '.join(
["camera list is empty: ", ' , '.join(camera_list)]))
return
intera_dataflow.wait_for(
lambda: self._node_config is not None,
timeout=5.0,
timeout_msg=(
"Failed to connect to Camera Node and retrieve configuration."
))
cameras_to_load = self._get_camera_launch_config().keys()
camera_capabilities = {
"mono": ['cognex'],
"color": ['ienso_ethernet'],
"auto_exposure": ['ienso_ethernet'],
"auto_gain": ['ienso_ethernet']
}
for camera in camera_list:
cameraType = camera_param_dict[camera]['cameraType']
try:
interface = IODeviceInterface("internal_camera", camera)
self.cameras_io[camera] = {
'interface':
interface,
'is_color': (cameraType in camera_capabilities['color']),
'has_auto_exposure':
(cameraType in camera_capabilities['auto_exposure']),
'has_auto_gain': (cameraType
in camera_capabilities['auto_gain']),
}
except OSError as e:
if camera not in cameras_to_load:
rospy.logwarn(
"Expected camera ({0}) is not configured to launch in this run"
" configuration. Make sure you are running in SDK Mode."
.format(camera))
else:
rospy.logerr(
"Could not find expected camera ({0}) for this robot.\n"
"Please contact Rethink support: [email protected]"
.format(camera))
def move_to_joint_positions(self,
positions,
timeout=15.0,
threshold=settings.JOINT_ANGLE_TOLERANCE,
test=None):
"""
(Blocking) Commands the limb to the provided positions.
Waits until the reported joint state matches that specified.
This function uses a low-pass filter to smooth the movement.
@type positions: dict({str:float})
@param positions: joint_name:angle command
@type timeout: float
@param timeout: seconds to wait for move to finish [15]
@type threshold: float
@param threshold: position threshold in radians across each joint when
move is considered successful [0.008726646]
@param test: optional function returning True if motion must be aborted
"""
cmd = self.joint_angles()
def genf(joint, angle):
def joint_diff():
return abs(angle - self._joint_angle[joint])
return joint_diff
diffs = [
genf(j, a) for j, a in list(positions.items())
if j in self._joint_angle
]
fail_msg = "{0} limb failed to reach commanded joint positions.".format(
self.name.capitalize())
def test_collision():
if self.has_collided():
rospy.logerr(' '.join(["Collision detected.", fail_msg]))
return True
return False
self.set_joint_positions(positions)
intera_dataflow.wait_for(
test=lambda: test_collision() or \
(callable(test) and test() == True) or \
(all(diff() < threshold for diff in diffs)),
timeout=timeout,
timeout_msg=fail_msg,
rate=100,
raise_on_error=False,
body=lambda: self.set_joint_positions(positions)
)
def stop(self):
"""
Simulate an e-stop button being pressed. Robot must be reset to clear
the stopped state.
"""
pub = rospy.Publisher('robot/set_super_stop', Empty, queue_size=10)
intera_dataflow.wait_for(
test=lambda: self._state.stopped == True,
timeout=5.0,
timeout_msg="Failed to stop the robot",
body=pub.publish,
)
def _toggle_enabled(self, status):
pub = rospy.Publisher('robot/set_super_enable', Bool, queue_size=10)
intera_dataflow.wait_for(
test=lambda: self._state.enabled == status,
timeout=5.0,
timeout_msg=("Failed to %sable robot" %
('en' if status else 'dis', )),
body=lambda: pub.publish(status),
)
rospy.loginfo("Robot %s", ('Enabled' if status else 'Disabled'))
def reset(self):
"""
Reset all joints. Trigger JRCP hardware to reset all faults. Disable
the robot.
"""
error_not_stopped = """\
Robot is not in a Error State. Cannot perform Reset.
"""
error_estop = """\
E-Stop is ASSERTED. Disengage E-Stop and then reset the robot.
"""
error_nonfatal = """Non-fatal Robot Error on reset.
Robot reset cleared stopped state and robot can be enabled, but a non-fatal
error persists. Check diagnostics or rethink.log for more info.
"""
error_env = """Failed to reset robot.
Please verify that the ROS_IP or ROS_HOSTNAME environment variables are set
and resolvable. For more information please visit:
http://sdk.rethinkrobotics.com/intera/SDK_Shell
"""
is_reset = lambda: (self._state.stopped == False and
self._state.error == False and
self._state.estop_button == 0 and
self._state.estop_source == 0)
pub = rospy.Publisher('robot/set_super_reset', Empty, queue_size=10)
if (not self._state.stopped):
rospy.logfatal(error_not_stopped)
raise IOError(errno.EREMOTEIO, "Failed to Reset due to lack of Error State.")
if (self._state.stopped and
self._state.estop_button == AssemblyState.ESTOP_BUTTON_PRESSED):
rospy.logfatal(error_estop)
raise IOError(errno.EREMOTEIO, "Failed to Reset: E-Stop Engaged")
rospy.loginfo("Resetting robot...")
try:
intera_dataflow.wait_for(
test=is_reset,
timeout=5.0,
timeout_msg=error_env,
body=pub.publish
)
except OSError, e:
if e.errno == errno.ETIMEDOUT:
if self._state.error == True and self._state.stopped == False:
rospy.logwarn(error_nonfatal)
return False
raise
def __init__(self):
self.states = []
self.configs = []
self._node_state = None
self._node_config = None
self._node_state_sub = rospy.Subscriber('/io/end_effector/state',
IONodeStatus,
self._node_state_cb)
self._node_config_sub = rospy.Subscriber('/io/end_effector/config',
IONodeConfiguration,
self._node_config_cb)
intera_dataflow.wait_for(
lambda: self._node_state is not None,
timeout=5.0,
timeout_msg=("Failed to connect to end_effector IO Node."))
def initialize(self, timeout=5.0):
"""
Activate ClickSmart - initializes device and signals and attaches
the EE's URDF model to the internal RobotModel. Compensation
of the EE's added mass may cause arm to move.
@type timeout: float
@param timeout: timeout in seconds
"""
rospy.loginfo("Activating ClickSmart...")
cmd = IOCommand('activate', {"devices": [self.name]})
msg = cmd.as_msg()
self._node_command_pub.publish(msg)
if timeout:
intera_dataflow.wait_for(
lambda: self.is_ready(),
timeout=timeout,
timeout_msg=("Failed to initialize gripper."))
def __init__(self, path_root, config_msg_type, status_msg_type):
self._path = path_root
self.config_mutex = Lock()
self.state_mutex = Lock()
self.cmd_times = []
self.ports = dict()
self.signals = dict()
self.config = config_msg_type()
self.state = status_msg_type()
self.config_changed = intera_dataflow.Signal()
self.state_changed = intera_dataflow.Signal()
self._config_sub = rospy.Subscriber(self._path + "/config",
config_msg_type,
self.handle_config)
self._state_sub = rospy.Subscriber(self._path + "/state",
status_msg_type, self.handle_state)
self._command_pub = rospy.Publisher(self._path + "/command",
IOComponentCommand,
queue_size=10)
# Wait for the config to be populated
intera_dataflow.wait_for(
lambda: self.config is not None and self.is_config_valid(),
timeout=5.0,
timeout_msg=("Failed to get config at: {}.".format(self._path +
"/config")))
# Wait for the state to be populated too (optional)
is_init = intera_dataflow.wait_for(
lambda: self.state is not None and self.is_state_valid(),
timeout=5.0,
raise_on_error=False)
if not is_init:
rospy.loginfo(
"Did not receive initial state at: {}."
" Device may not be activated yet.".format(self._path +
"/state"))
rospy.logdebug("Making new IOInterface on %s" % (self._path, ))
def get_current_gripper_interface(self):
"""
Instantiate and return an interface to control the gripper
currently attached to the robot.
Looks for the type of the first detected gripper attached to the robot
and returns an instance of a Gripper or SimpleClickSmartGripper class
configured to control the EE.
@rtype: Gripper|SimpleClickSmartGripper
@return: instance of an interface to control attached gripper
"""
gripper = None
if len(self.states) <= 0 or len(self.configs) <= 0:
# wait a moment for any attached grippers to populate
intera_dataflow.wait_for(
lambda: (len(self.states) > 0 and len(self.configs) > 0),
timeout=5.0,
timeout_msg=(
"Failed to get gripper. No gripper attached on the robot."
))
ee_state = self.states[0]
ee_id = ee_state.name
ee_config = None
for device in self.configs:
if device.name == ee_id:
ee_config = self._parse_config(device.config)
break
gripper_class = self._lookup_gripper_class(ee_config['props']['type'])
needs_init = (ee_state.status.tag == 'down'
or ee_state.status.tag == 'unready')
try:
gripper = gripper_class(ee_id, needs_init)
except:
gripper = gripper_class(ee_id, False)
return gripper
def __init__(self):
"""
Constructor.
"""
self._state = dict()
self._pub_pan = rospy.Publisher('/robot/head/command_head_pan',
HeadPanCommand,
queue_size=10)
state_topic = '/robot/head/head_state'
self._sub_state = rospy.Subscriber(state_topic, HeadState,
self._on_head_state)
self._tf_listener = tf.TransformListener()
intera_dataflow.wait_for(
lambda: len(self._state) != 0,
timeout=5.0,
timeout_msg=("Failed to get current head state from %s" %
(state_topic, )),
)
def __init__(self):
"""
Constructor
"""
self._joint_position_lower = dict()
self._joint_position_upper = dict()
self._joint_velocity_limit = dict()
self._joint_accel_limit = dict()
self._joint_effort_limit = dict()
self._joint_names = list()
joint_limit_topic = '/robot/joint_limits'
_joint_limit_sub = rospy.Subscriber(
joint_limit_topic,
JointLimitsMsg,
self._on_joint_limits,
queue_size=1)
err_msg = ("%init failed to get current joint_limits "
"from %s", joint_limit_topic)
intera_dataflow.wait_for(lambda: len(self._joint_names) > 0,
timeout_msg=err_msg, timeout=5.0)
def calibrate(self):
"""
Calibrate the gripper in order to set maximum and
minimum travel distance.
@rtype: bool
@return: True if calibration was successful, False otherwise
"""
self.gripper_io.set_signal_value("calibrate", True)
success = intera_dataflow.wait_for(lambda: self.is_calibrated(),
timeout=5.0,
raise_on_error=False)
if not success:
rospy.logerr("({0}) calibration failed.".format(self.name))
return success
def set_pan(self,
angle,
speed=1.0,
timeout=10.0,
active_cancellation=False):
"""
Pan at the given speed to the desired angle.
@type angle: float
@param angle: Desired pan angle in radians.
@type speed: int
@param speed: Desired speed to pan at, range is 0-1.0 [1.0]
@type timeout: float
@param timeout: Seconds to wait for the head to pan to the
specified angle. If 0, just command once and
return. [10]
@param active_cancellation: Specifies if the head should aim at
a location in the base frame. If this is set to True,
the "angle" param is measured with respect to
the "/base" frame, rather than the actual head joint
value. Valid range is [-pi, pi) radians.
@type active_cancellation: bool
"""
if speed > HeadPanCommand.MAX_SPEED_RATIO:
rospy.logwarn(("Commanded Speed, ({0}), faster than Max speed of"
" {1}. Clamping to Max.]").format(
speed, HeadPanCommand.MAX_SPEED_RATIO))
speed = HeadPanCommand.MAX_SPEED_RATIO
elif speed < HeadPanCommand.MIN_SPEED_RATIO:
rospy.logwarn(("Commanded Speed, ({0}), slower than Min speed of"
" {1}. Clamping to Min.]").format(
speed, HeadPanCommand.MIN_SPEED_RATIO))
speed = HeadPanCommand.MIN_SPEED_RATIO
if active_cancellation:
def get_current_euler(axis,
source_frame="base",
target_frame="head"):
rate = rospy.Rate(10) # Hz
counter = 1
quat = (0, 0, 0, 1)
while not rospy.is_shutdown():
try:
pos, quat = self._tf_listener.lookupTransform(
source_frame, target_frame,
self._tf_listener.getLatestCommonTime(
source_frame, target_frame))
except tf.Exception:
if not counter % 10: # essentially throttle 1.0 sec
rospy.logwarn(
("Active Cancellation: Trying again "
"to lookup transform from {0} to "
"{1}...").format(source_frame, target_frame))
else:
break
counter += 1
rate.sleep()
euler = tf.transformations.euler_from_quaternion(quat)
return euler[axis]
tf_angle = -pi + (angle + pi) % (2 * pi)
mode = HeadPanCommand.SET_ACTIVE_CANCELLATION_MODE
stop_condition = lambda: (abs(get_current_euler(2) - tf_angle) <=
settings.HEAD_PAN_ANGLE_TOLERANCE)
else:
mode = HeadPanCommand.SET_ACTIVE_MODE
stop_condition = lambda: (abs(self.pan() - angle) <= settings.
HEAD_PAN_ANGLE_TOLERANCE)
msg = HeadPanCommand(angle, speed, mode)
self._pub_pan.publish(msg)
if not timeout == 0:
intera_dataflow.wait_for(
stop_condition,
timeout=timeout,
rate=100,
timeout_msg=("Failed to move head to pan"
" command {0}").format(angle),
body=lambda: self._pub_pan.publish(msg))
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(list(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(list(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)
def _on_trajectory_action(self, goal):
joint_names = goal.trajectory.joint_names
trajectory_points = goal.trajectory.points
# Load parameters for trajectory
self._get_trajectory_parameters(joint_names, goal)
# Create a new discretized joint trajectory
num_points = len(trajectory_points)
if num_points == 0:
rospy.logerr("%s: Empty Trajectory" % (self._action_name, ))
self._server.set_aborted()
return
rospy.loginfo("%s: Executing requested joint trajectory" %
(self._action_name, ))
rospy.logdebug("Trajectory Points: {0}".format(trajectory_points))
for jnt_name, jnt_value in self._get_current_error(
joint_names, trajectory_points[0].positions):
if abs(self._path_thresh[jnt_name]) < abs(jnt_value):
rospy.logerr(("{0}: Initial Trajectory point violates "
"threshold on joint {1} with delta {2} radians. "
"Aborting trajectory execution.").format(
self._action_name, jnt_name, jnt_value))
self._server.set_aborted()
return
control_rate = rospy.Rate(self._control_rate)
dimensions_dict = self._determine_dimensions(trajectory_points)
# Force Velocites/Accelerations to zero at the final timestep
# if they exist in the trajectory
# Remove this behavior if you are stringing together trajectories,
# and want continuous, non-zero velocities/accelerations between
# trajectories
if dimensions_dict['velocities']:
trajectory_points[-1].velocities = [0.0] * len(joint_names)
if dimensions_dict['accelerations']:
trajectory_points[-1].accelerations = [0.0] * len(joint_names)
# Compute Full Bezier Curve Coefficients for all 7 joints
pnt_times = [pnt.time_from_start.to_sec() for pnt in trajectory_points]
try:
if self._interpolation == 'minjerk':
# Compute Full MinJerk Curve Coefficients for all 7 joints
point_duration = [
pnt_times[i + 1] - pnt_times[i]
for i in range(len(pnt_times) - 1)
]
m_matrix = self._compute_minjerk_coeff(joint_names,
trajectory_points,
point_duration,
dimensions_dict)
else:
# Compute Full Bezier Curve Coefficients for all 7 joints
b_matrix = self._compute_bezier_coeff(joint_names,
trajectory_points,
dimensions_dict)
except Exception as ex:
rospy.logerr(("{0}: Failed to compute a Bezier trajectory for {1}"
" arm with error \"{2}: {3}\"").format(
self._action_name, self._name,
type(ex).__name__, ex))
self._server.set_aborted()
return
# Wait for the specified execution time, if not provided use now
start_time = goal.trajectory.header.stamp.to_sec()
if start_time == 0.0:
start_time = rospy.get_time()
intera_dataflow.wait_for(lambda: rospy.get_time() >= start_time,
timeout=float('inf'))
# Loop until end of trajectory time. Provide a single time step
# of the control rate past the end to ensure we get to the end.
# Keep track of current indices for spline segment generation
now_from_start = rospy.get_time() - start_time
end_time = trajectory_points[-1].time_from_start.to_sec()
while (now_from_start < end_time and not rospy.is_shutdown()
and self.robot_is_enabled()):
now = rospy.get_time()
now_from_start = now - start_time
idx = bisect.bisect(pnt_times, now_from_start)
#Calculate percentage of time passed in this interval
if idx >= num_points:
cmd_time = now_from_start - pnt_times[-1]
t = 1.0
elif idx >= 0:
cmd_time = (now_from_start - pnt_times[idx - 1])
t = cmd_time / (pnt_times[idx] - pnt_times[idx - 1])
else:
cmd_time = 0
t = 0
if self._interpolation == 'minjerk':
point = self._get_minjerk_point(m_matrix, idx, t, cmd_time,
dimensions_dict)
else:
point = self._get_bezier_point(b_matrix, idx, t, cmd_time,
dimensions_dict)
# Command Joint Position, Velocity, Acceleration
command_executed = self._command_joints(joint_names, point,
start_time,
dimensions_dict)
self._update_feedback(point, joint_names, now_from_start)
if not command_executed:
return
# Sleep to make sure the publish is at a consistent time
control_rate.sleep()
# Keep trying to meet goal until goal_time constraint expired
last = trajectory_points[-1]
last_time = trajectory_points[-1].time_from_start.to_sec()
end_angles = dict(zip(joint_names, last.positions))
def check_goal_state():
for error in self._get_current_error(joint_names, last.positions):
if (self._goal_error[error[0]] > 0
and self._goal_error[error[0]] < math.fabs(error[1])):
return error[0]
if (self._stopped_velocity > 0.0 and max([
abs(cur_vel)
for cur_vel in self._get_current_velocities(joint_names)
]) > self._stopped_velocity):
return False
else:
return True
while (now_from_start < (last_time + self._goal_time)
and not rospy.is_shutdown() and self.robot_is_enabled()):
if not self._command_joints(joint_names, last, start_time,
dimensions_dict):
return
now_from_start = rospy.get_time() - start_time
self._update_feedback(deepcopy(last), joint_names, now_from_start)
control_rate.sleep()
now_from_start = rospy.get_time() - start_time
self._update_feedback(deepcopy(last), joint_names, now_from_start)
# Verify goal constraint
result = check_goal_state()
if result is True:
rospy.loginfo("%s: Joint Trajectory Action Succeeded for %s arm" %
(self._action_name, self._name))
self._result.error_code = self._result.SUCCESSFUL
self._server.set_succeeded(self._result)
elif result is False:
rospy.logerr(
"%s: Exceeded Max Goal Velocity Threshold for %s arm" %
(self._action_name, self._name))
self._result.error_code = self._result.GOAL_TOLERANCE_VIOLATED
self._server.set_aborted(self._result)
else:
rospy.logerr("%s: Exceeded Goal Threshold Error %s for %s arm" %
(self._action_name, result, self._name))
self._result.error_code = self._result.GOAL_TOLERANCE_VIOLATED
self._server.set_aborted(self._result)
self._command_stop(goal.trajectory.joint_names, end_angles, start_time,
dimensions_dict)