pt = PointStamped() pt.header.frame_id = '/table' pt.header.stamp = now() pt.point.x, pt.point.y, pt.point.z = point t1 = now() services.hilight_object(pt, ColorRGBA(255,0,0,0)) click_pts_msg = services.get_cursor_stats() click_pt = [ click_pts_msg.click_pos.point.x, click_pts_msg.click_pos.point.y, click_pts_msg.click_pos.point.z ] r = Rate(10) while not sameObject(click_pt, point): click_pts_msg = services.get_cursor_stats() click_pt = [ click_pts_msg.click_pos.point.x, click_pts_msg.click_pos.point.y, click_pts_msg.click_pos.point.z ] r.sleep() click_pts = pointcloud2_to_xyz_array(click_pts_msg.points) t2 = now() times.append((t2-t1).to_sec()) history_size = click_pts.shape[0] cursor_history.extend(click_pts) stds.append(click_pts.std(0)) print 'std = ', stds[-1] sleep(3)
pt = PointStamped() pt.header.frame_id = '/table' pt.header.stamp = now() pt.point.x, pt.point.y, pt.point.z = point t1 = now() services.hilight_object(pt, ColorRGBA(255,0,0,0)) click_pts_msg = services.get_cursor_stats() click_pt = [ click_pts_msg.click_pos.point.x, click_pts_msg.click_pos.point.y, click_pts_msg.click_pos.point.z ] r = Rate(10) while not sameObject(closestPoint(points, click_pt), point): click_pts_msg = services.get_cursor_stats() click_pt = [ click_pts_msg.click_pos.point.x, click_pts_msg.click_pos.point.y, click_pts_msg.click_pos.point.z ] r.sleep() click_pts = pointcloud2_to_xyz_array(click_pts_msg.points) t2 = now() times.append((t2-t1).to_sec()) history_size = click_pts.shape[0] cursor_history.extend(click_pts) stds.append(click_pts.std(0)) print 'std = ', stds[-1] sleep(3)
services.clear_hilights() pt = PointStamped() pt.header.frame_id = '/table' pt.header.stamp = now() pt.point.x, pt.point.y, pt.point.z = point t1 = now() services.hilight_object(pt, ColorRGBA(255, 0, 0, 0)) click_pts_msg = services.get_cursor_stats() click_pt = [ click_pts_msg.click_pos.point.x, click_pts_msg.click_pos.point.y, click_pts_msg.click_pos.point.z ] r = Rate(10) while not sameObject(closestPoint(points, click_pt), point): click_pts_msg = services.get_cursor_stats() click_pt = [ click_pts_msg.click_pos.point.x, click_pts_msg.click_pos.point.y, click_pts_msg.click_pos.point.z ] r.sleep() click_pts = pointcloud2_to_xyz_array(click_pts_msg.points) t2 = now() times.append((t2 - t1).to_sec()) history_size = click_pts.shape[0] cursor_history.extend(click_pts) stds.append(click_pts.std(0)) print 'std = ', stds[-1] sleep(3)