def point_cb(self, msg):
#self.point = msg
cloud = read_points_np(msg)
point = PointStamped()
point.header = msg.header
if cloud.shape[1] == 0: return
point.point.x, point.point.y, point.point.z = cloud[0][0]
self.point = point
def point_cb(self, msg):
#self.point = msg
cloud = read_points_np(msg)
point = PointStamped()
point.header = msg.header
if cloud.shape[1] == 0: return
point.point.x, point.point.y, point.point.z = cloud[0][0]
self.point = point
def object_cb(self, msg):
with self.object_lock:
objects = read_points_np(msg, masked=False)
if objects.shape[1] == 0: return
self.object_header = msg.header
self.objects = objects
transformed_objects = self.projectPoints(self.objects, msg.header)
self.projected_objects = cv2.perspectiveTransform(
transformed_objects, self.H)
def cursor_cb(self, msg):
with self.cursor_lock:
objects = read_points_np(msg, masked=False)
if objects.shape[1] == 0: return
self.cursor_header = msg.header
self.cursor_pts = objects
transformed_pts = self.projectPoints(self.cursor_pts, msg.header)
#self.cursor_pts_xyz.extend(self.cursor_pts.tolist()[0])
self.projected_cursor.extend(
cv2.perspectiveTransform(transformed_pts, self.H)[0])
def intersected_cb(self, msg):
with self.intersected_lock:
objects = read_points_np(msg, masked=False)
if objects.shape[1] == 0: return
self.int_object_header = msg.header
self.int_objects = objects
transformed_objects = self.projectPoints(objects, msg.header)
self.int_projected_objects = cv2.perspectiveTransform(
transformed_objects, self.H)
for pt in transformed_objects[0]:
self.int_ages.append(rospy.Time.now())
def handle_look_at_blob(msg):
target_color = np.array([msg.blob_color.r, msg.blob_color.g, msg.blob_color.b])
try:
cloud = _point_cloud.read_points_np(cloud_msg)
colors = _point_cloud.float2rgb(cloud[:,:,3])
color_dists = [np.linalg.norm(c-target_color) for c in colors[0,:]]
target_pt = cloud[0,np.argmin(color_dists),:3]
pt = transform_listener.transform_point('base_footprint', cloud_msg.header.frame_id, Point(*target_pt))
head.look_at_relative_point(pt.x+0.2,pt.y-0.09,pt.z)
except Exception, e:
print e
return LookAtBlobResponse(Bool(False))
def handle_look_at_blob(msg):
target_color = np.array(
[msg.blob_color.r, msg.blob_color.g, msg.blob_color.b])
try:
cloud = _point_cloud.read_points_np(cloud_msg)
colors = _point_cloud.float2rgb(cloud[:, :, 3])
color_dists = [np.linalg.norm(c - target_color) for c in colors[0, :]]
target_pt = cloud[0, np.argmin(color_dists), :3]
pt = transform_listener.transform_point('base_footprint',
cloud_msg.header.frame_id,
Point(*target_pt))
head.look_at_relative_point(pt.x + 0.2, pt.y - 0.09, pt.z)
except Exception, e:
print e
return LookAtBlobResponse(Bool(False))
def cloud_cb(msg): global cloud, cloud_header, fields cloud = read_points_np(msg) cloud_header = msg.header fields = msg.fields
def cloud_cb(msg):
global cloud, cloud_header, fields
cloud = read_points_np(msg)
cloud_header = msg.header
fields = msg.fields