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,
