def _sync_callback(self, color, depth):
self._cam_img_lock.acquire()
try:
bgr_img = self._cv_bridge.imgmsg_to_cv2(color, "bgr8")
self._rgb_img = bgr_img[:, :, ::-1]
self._depth_img = self._cv_bridge.imgmsg_to_cv2(depth,
"passthrough")
if self._rgb_img_shape is None:
self._rgb_img_shape = self._rgb_img.shape
if self._depth_img_shape is None:
self._depth_img_shape = self._depth_img.shape
except CvBridgeError as e:
ar.log_error(e)
self._cam_img_lock.release()
def wait_to_reach_jnt_goal(goal,
get_func,
joint_name=None,
get_func_derv=None,
timeout=10.0,
max_error=0.01):
"""
Block the code to wait for the joint moving to the specified goal.
The goal can be a desired velocity(s) or a desired position(s).
if get_func returns the poisitions (velocities),
then the get_func_derv should
return the velocities (accelerations) (if provided).
If the robot cannot reach the goal,
providing get_func_derv can prevent the
program from waiting until timeout.
It uses the derivative information to make a faster judgement.
Args:
goal (float or list): goal positions or velocities.
get_func (function): name of the function with which we can get
the current joint values.
joint_name (str): if it's none, all the actuated
joints are compared.
Otherwise, only the specified joint is compared.W
get_func_derv (function): the name of the function with which we
can get the derivative of the joint values.
timeout (float): maximum waiting time.
max_error (float): tolerance of error.
Returns:
bool: if the goal is reached or not.
"""
success = False
start_time = time.time()
vel_stop_time = None
goal = np.array(goal)
while True:
try:
if time.time() - start_time > timeout:
pt_str = 'Unable to move to joint goals (%s)' \
' within %f s' % (str(goal),
timeout)
ar.log_error(pt_str)
return success
if reach_jnt_goal(goal, get_func, joint_name, max_error):
success = True
break
if get_func_derv is not None:
vel_threshold = 0.006
nargs = get_func_derv.__code__.co_argcount
if nargs == 1:
jnt_vel = get_func_derv()
else:
jnt_vel = get_func_derv(joint_name)
if np.max(np.abs(jnt_vel)) <= vel_threshold \
and vel_stop_time is None:
vel_stop_time = time.time()
elif np.max(np.abs(jnt_vel)) > vel_threshold:
vel_stop_time = None
if vel_stop_time is not None and time.time(
) - vel_stop_time > 0.1:
pt_str = 'Unable to move to joint goals (%s)' % str(goal)
ar.log_error(pt_str)
return success
time.sleep(0.001)
except KeyboardInterrupt:
success = False
return success