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