import roslib; roslib.load_manifest('pr2_python') import numpy as np from sensor_msgs.msg import PointCloud2 from pr2_python import pointclouds points = np.random.random((10, 3)) cloud_msg = pointclouds.xyz_array_to_pointcloud2(points) print len(cloud_msg.data) points_new = pointclouds.pointcloud2_to_xyz_array(cloud_msg) print points - points_new print points print points_new
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) services.clear_hilights() path = '/home/robotics/lazewatskyd/ros-pkgs/wu-ros-pkg/3d_interaction/projector_interface/study/data/4_object_designation_%s.mat' timestr = datetime.datetime.today().strftime('%d-%m-%y-%H.%M.%f') scipy.io.savemat(path % timestr, dict(targets=targets, stds=stds, times=times, cursor_history=cursor_history,
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) services.clear_hilights() path = '/home/robotics/lazewatskyd/ros-pkgs/wu-ros-pkg/3d_interaction/projector_interface/study/data/1_object_designation_%s.mat' timestr = datetime.datetime.today().strftime('%d-%m-%y-%H.%M.%f') scipy.io.savemat(path % timestr, dict(targets=targets, stds=stds, times=times, cursor_history=cursor_history,