def cloudCallback(self, data): # print 'Time between cloud calls:', time.time() - self.cloudTime # startTime = time.time() self.pointCloud = data self.transposeGripperToCamera() # Determine location of spoon spoon3D = [0.22, -0.050, 0] spoon = np.dot(self.lGripperTransposeMatrix, np.array([spoon3D[0], spoon3D[1], spoon3D[2], 1.0]))[:3] self.spoonX, self.spoonY = self.pinholeCamera.project3dToPixel(spoon) lowX, highX, lowY, highY = self.boundingBox(0.05, 0.3, 0.05, 20, 100, 50) points2D = [[x, y] for y in xrange(lowY, highY) for x in xrange(lowX, highX)] try: points3D = pc2.read_points(self.pointCloud, field_names=('x', 'y', 'z'), skip_nans=True, uvs=points2D) gripperPoint = pc2.read_points(self.pointCloud, field_names=('x', 'y', 'z'), skip_nans=True, uvs=[[self.lGripX, self.lGripY]]).next() except: # print 'Unable to unpack from PointCloud2.', self.cameraWidth, self.cameraHeight, self.pointCloud.width, self.pointCloud.height return points3D = np.array([point for point in points3D]) self.clusterPoints = points3D # # Perform dbscan clustering # X = StandardScaler().fit_transform(points3D) # labels = self.dbscan.fit_predict(X) # # unique_labels = set(labels) # # # Find the point closest to our gripper and it's corresponding label # index, closePoint = min(enumerate(np.linalg.norm(points3D - gripperPoint, axis=1)), key=operator.itemgetter(1)) # closeLabel = labels[index] # while closeLabel == -1 and points3D.size > 0: # np.delete(points3D, [index]) # np.delete(labels, [index]) # index, closePoint = min(enumerate(np.linalg.norm(points3D - gripperPoint, axis=1)), key=operator.itemgetter(1)) # closeLabel = labels[index] # if points3D.size <= 0: # return # # print 'Label:', closeLabel # # # Find the cluster closest to our gripper # self.clusterPoints = points3D[labels==closeLabel] # # if self.visual: # # Publish depth features for spoon features # self.publishPoints('spoonPoints', self.clusterPoints, g=1.0) # # # Publish depth features for non spoon features # nonClusterPoints = points3D[labels!=closeLabel] # self.publishPoints('nonSpoonPoints', nonClusterPoints, r=1.0) self.updateNumber += 1
def process(self): if self.last_scan is None or self.last_image is None: return last_image = self.last_image last_scan = self.last_scan scan_pc = pc2.read_points(last_scan) image_pc = pc2.read_points(last_image) scan_obstacles = [] tmp = Obstacle() # Enumerate the point cloud from the laser scan to group points that belongs # to the same obstacle. for x, y, _ in scan_pc: dist = x ** 2 + y ** 2 # If it's in range, the point is considered to be part of the current obstacle if dist < self.max_range: tmp.add_point(Point(x, y, dist)) else: if not tmp.is_empty(): scan_obstacles.append(tmp) tmp = Obstacle() if not tmp.is_empty(): scan_obstacles.append(tmp) if len(scan_obstacles) == 0: self.cloud_in.publish(last_image) else: image_points = [] for x, y, z, _ in image_pc: point = Point(x, y) for obs in scan_obstacles: if not obs.is_behind(point): image_points.append((x, y, z)) msg = pc2.create_cloud_xyz32(last_image.header, image_points) self.cloud_in.publish(msg) self.cloud_in.publish(last_scan) self.last_scan = None self.last_image = None
def pc_cb(self, data): # point_cloud2 library's read_points function returns a generator # where each entry is a list of the fields specified gen = pc2.read_points(data, field_names=("x", "y", "z")) # Filepath is just /tmp right now file_to_open = self.filepath + '/etu_points_raw.xyz' f = open(file_to_open, 'w') for xyz in gen: # This creates the message to be added to the group # and eventually published point = XYZ() point.x = xyz[0] point.y = xyz[1] point.z = xyz[2] self.group.append(point) # This is for writing directly to an XYZ file to_write = '%(1)f %(2)f %(3)f\n' % {'1':xyz[0], '2':xyz[1], '3':xyz[2]} f.write(to_write) f.close() # This takes the first published point cloud and sends it to # the xyz_to_mesh.py script if not self.printed: self.printed = True rospack = rospkg.RosPack() package_path = rospack.get_path('uv_decontamination') full_path = package_path + '/scripts/xyz_to_mesh.py' call(["python", full_path])
def _deserialize_numpy(self, str): """ wrapper for factory-generated class that passes numpy module into deserialize """ self.deserialize_numpy(str, numpy) # deserialize (with numpy wherever possible) # for Image msgs if self._type == 'sensor_msgs/Image': self.data = numpy.asarray(bridge.imgmsg_to_cv(self, desired_encoding=self.encoding)) # convert pixel data to numpy array # for PointCloud2 msgs if self._type == 'sensor_msgs/PointCloud2': print 'Cloud is being deserialized...' points = point_cloud2.read_points(self) points_arr = numpy.asarray(list(points)) # Unpack RGB color info _float2rgb_vectorized = numpy.vectorize(_float2rgb) r, g, b = _float2rgb_vectorized(points_arr[:, 3]) r = numpy.expand_dims(r, 1).astype('uint8') # insert blank 3rd dimension (for concatenation) g = numpy.expand_dims(g, 1).astype('uint8') b = numpy.expand_dims(b, 1).astype('uint8') # Concatenate and Reshape pixels_rgb = numpy.concatenate((r, g, b), axis=1) image_rgb = pixels_rgb.reshape(self.height, self.width, 3) points_arr = points_arr[:, :3].reshape(self.height, self.width, 3).astype('float32') # Build record array to separate datatypes -- int16 for XYZ, uint8 for RGB image_xyzrgb = numpy.rec.fromarrays((points_arr, image_rgb), names=('xyz', 'rgb')) self.data = image_xyzrgb return self
def extractLabeledCloud(pointCloud_): npts = 0 points = {} normals = {} xIdx = 0 yIdx = 1 zIdx = 2 nxIdx = 3 nyIdx = 4 nzIdx = 5 labelIdx = 6 for p in pc.read_points(pointCloud_, skip_nans=True, field_names=('x', 'y', 'z', 'normal_x', 'normal_y', 'normal_z', 'label')): label = p[labelIdx] if not label in points: points[label] = [] normals[label] = [] points[label].append([p[xIdx], p[yIdx], p[zIdx]]) normals[label].append([p[nxIdx], p[nyIdx], p[nzIdx]]) npts = npts + 1 return [points, normals, npts]
def cloud_callback(self, cloud): points = point_cloud2.read_points(cloud) points_list = np.asarray(list(points)) points_arr = np.asarray(points_list) # Unpack RGB color info _float2rgb_vectorized = np.vectorize(_float2rgb) r, g, b = _float2rgb_vectorized(points_arr[:, 3]) # Concatenate and Reshape r = np.expand_dims(r, 1) # insert blank 3rd dimension (for concatenation) g = np.expand_dims(g, 1) b = np.expand_dims(b, 1) points_rgb = np.concatenate((points_arr[:, 0:3], r, g, b), axis=1) image_rgb = points_rgb.reshape(cloud.height, cloud.width, 6) z = copy.deepcopy(image_rgb[:, :, 2]) # get depth values (I think) image_np = copy.deepcopy(image_rgb[:, :, 3:].astype('uint8')) #code.interact(local=locals()) # TWO-METER DISTANCE FILTER z[np.isnan(z)] = 0.0 mask = np.logical_or(z > 2, z == 0) for i in range(image_np.shape[2]): image_np[:, :, i][mask] = 0 # Convert to Image msg image_cv = cv.fromarray(image_np) image_msg = self.bridge.cv_to_imgmsg(image_cv, encoding='bgr8') self.pub.publish(image_msg)
def cloudCallback(self, data): print 'Time between cloud calls:', time.time() - self.cloudTime startTime = time.time() self.pointCloud = data self.transposeGripperToCamera() # Determine location of spoon spoon3D = [0.22, -0.050, 0] self.spoon = np.dot(self.lGripperTransposeMatrix, np.array([spoon3D[0], spoon3D[1], spoon3D[2], 1.0]))[:3] self.spoonX, self.spoonY = self.pinholeCamera.project3dToPixel(self.spoon) lowX, highX, lowY, highY = self.boundingBox() self.spoonImageData = self.imageData[lowY:highY, lowX:highX, :] points2D = [[x, y] for y in xrange(lowY, highY) for x in xrange(lowX, highX)] try: points3D = pc2.read_points(self.pointCloud, field_names=('x', 'y', 'z'), skip_nans=True, uvs=points2D) except: print 'Unable to unpack from PointCloud2!', self.cameraWidth, self.cameraHeight, self.pointCloud.width, self.pointCloud.height return self.points3D = np.array([point for point in points3D]) self.updateNumber += 1 print 'Cloud computation time:', time.time() - startTime self.cloudTime = time.time()
def callback(data): #print (data.data) print "-------------------------------------------" height = int(data.height/2) width = int(data.width) #print (data.header) data_out= pc2.read_points(data) #cloud = pc2.create_cloud_xyz32(data.header,data.data) int_data = next(data_out) #print size(data_out) g=[] for i in data_out: k=i if i[0] is None or i[1] is None or i[2] is None: pass #print "skipped" elif i[3]>0.00: #elif i[0] < 5.00 or i[1]<5.00 or i[2]<5.00 or i[3]<5.00: #pass #print "skip g.append(i) #print len(g),len(g[0]) print g # print i print "--------------------------------------------"
def cloudCallback(self, data): # print 'Time between cloud calls:', time.time() - self.cloudTime # startTime = time.time() self.pointCloud = data self.transposeBowlToCamera() self.transposeGripperToCamera() # Determine location of spoon spoon3D = [0.22, -0.050, 0] self.spoon = np.dot(self.lGripperTransposeMatrix, np.array([spoon3D[0], spoon3D[1], spoon3D[2], 1.0]))[:3] lowX, highX, lowY, highY = self.boundingBox() points2D = [[x, y] for y in xrange(lowY, highY) for x in xrange(lowX, highX)] try: points3D = pc2.read_points(self.pointCloud, field_names=('x', 'y', 'z'), skip_nans=True, uvs=points2D) except: print 'Unable to unpack from PointCloud2!', self.cameraWidth, self.cameraHeight, self.pointCloud.width, self.pointCloud.height return self.points3D = np.array([point for point in points3D]) # self.publishImageFeatures() self.updateNumber += 1
def callback(self, ros_msg): """ This method is invoked each time a new ROS message is generated. the message is of type CompressedImage, with data and format """ self.msg = ros_msg # we don't need to be called again self.subscriber.unregister() if self.save_to_file: # create directories if necessary dest_dir = os.path.split(self.path)[0] if not os.path.exists(dest_dir): os.makedirs(dest_dir) # write data to disk if self.msg_type == CompressedImage: f = open(self.path, 'w') f.write(ros_msg.data) f.close() elif self.msg_type == PointCloud2: rawpoints = numpy.array(list(point_cloud2.read_points(ros_msg, skip_nans=False)), dtype=numpy.float32)[:, :3] notnanindices = ~numpy.isnan(rawpoints[:, 0]) f = open(self.path, 'wb') pickle.dump((rawpoints[notnanindices], notnanindices, len(rawpoints)), f) f.close() else: f = open(self.path, 'wb') pickle.dump(ros_msg, f) f.close() self.done = True
def pointcloud_callback(self, msg): pointcloud = pc2.read_points(msg, field_names=("x", "y", "z"), skip_nans=False, uvs=[]) # While there are points in the cloud to read... if self.pc_count<self.pc_to_keep: self.pc_count=self.pc_count+1 #else:self.store_pc.pop(0) while True: try: # new point point = next(pointcloud) #print point #convert point to a_star map coordinates self.xyz_point = point #add point to list of points self.store_points.append(list(self.xyz_point)) #self.store_points.pop(0) # When the last point has been processed except StopIteration: break self.store_pc.append(list(self.store_points))
def callback(self, data): if self.show: print data.fields xs = [] ys = [] zs = [] # For some reason, returns x in [0], y in [1], z in [2], rgb in [3] for point in pc2.read_points(data,skip_nans=False,field_names=("rgb","x","y","z")): xs.append(point[0]) ys.append(point[1]) zs.append(point[2]) print "Length: " + str(len(xs)) #add start cloud = np.array([xs,ys,zs]) nans = np.isnan(cloud) cloud[nans] = 0 cloud = cloud.transpose() print cloud[0] # cloud, _ = perception.calPointCloud(cloud) #add end #cloud = cloud.transpose() cloud = cloud[::10] print cloud xs = cloud[:,0] ys = cloud[:,1] zs = cloud[:,2] #segmentation(cloud) # print xs # print ys print "length after down sample: "+str(len(zs)) if len(xs) > 0: self.show = False print "Saving to file: " + SAVE_LOCATION np.savez(SAVE_LOCATION, xs, ys, zs) print 'done saving, you can exit now with CTRL-C'
def find_centroid(self, request): '''Computes the average point in a point cloud. ''' points = pc2.read_points( request.cluster.pointcloud, field_names=['x', 'y', 'z'], skip_nans=True, ) num_points = 0 avg_x = 0 avg_y = 0 avg_z = 0 for x, y, z in points: num_points += 1 avg_x += x avg_y += y avg_z += z if num_points > 0: avg_x /= num_points avg_y /= num_points avg_z /= num_points rospy.loginfo('Centroid: ({}, {}, {})'.format(avg_x, avg_y, avg_z)) centroid = PointStamped( point=Point(x=avg_x, y=avg_y, z=avg_z), header=Header( frame_id=request.cluster.header.frame_id, stamp=rospy.Time.now(), ) ) return FindCentroidResponse(centroid=centroid)
def pointCloudCallback(msg: PointCloud2): ''' Process point cloud data (generated from the laser) ''' # Store robot position in map grid coordinates robot_map_coord = np.array( [round((MAP_WIDTH/2+robot_pose.x)/MAP_RESOLUTION), # Col round((MAP_HEIGHT/2-robot_pose.y)/MAP_RESOLUTION)], # Row dtype=int) for p in point_cloud2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True): # Get obtained value in the map grid (columm and row) - must be uint8. obstacle_map = np.array( [round((MAP_WIDTH/2+p[0])/MAP_RESOLUTION), # Col / x round((MAP_HEIGHT/2-p[1])/MAP_RESOLUTION)], # Row / y dtype=np.int) # Update map using the line iterator for free space it = createLineIterator(robot_map_coord, # Start point obstacle_map, # End point occ_map) for pt in it[:-1]: occ_map.itemset((pt[1], pt[0]), clipValue(pt[2]+CELL_DELTA_UP, MIN_CELL_VALUE, MAX_CELL_VALUE)) # Update map using the line iterator for occupied space, if # applicable occ_map.itemset((it[-1][1], it[-1][0]), clipValue(it[-1][2]-CELL_DELTA_DOWN, MIN_CELL_VALUE, MAX_CELL_VALUE))
def get_nearest_cloud(self, msg): points = list() # Get all the points in the visible cloud (may be prefiltered by other nodes) for point in point_cloud2.read_points(msg, skip_nans=True): points.append(point[:3]) # Convert to a numpy array points_arr = np.float32([p for p in points]).reshape(-1, 1, 3) # Compute the COG cog = np.mean(points_arr, 0) # Abort if we get an NaN in any component if np.isnan(np.sum(cog)): return # Store the COG in a ROS Point object cog_point = Point() cog_point.x = cog[0][0] cog_point.y = cog[0][1] cog_point.z = cog[0][2] # Give the COG a unit orientation and store as a PoseStamped message target = PoseStamped() target.header.stamp = rospy.Time.now() target.header.frame_id = msg.header.frame_id target.pose.position = cog_point target.pose.orientation.w = 1.0 # Publish the PoseStamped message on the /target_pose topic self.target_pub.publish(target)
def k_means(self, msg): ## Perform K Means on the data # # @param msg A sensor_msgs/PointCloud2 message with num_means clusters starttime = time.clock() points = pc2.read_points(msg, field_names=["x","y","z","rgb"], skip_nans=True) data = [] for point in points: data += [point[0:3]] data = numpy.array(data) kmeans = KM(n_clusters = self.num_means) kmeans.fit(data) print kmeans.cluster_centers_ centers = sorted(kmeans.cluster_centers_, key=lambda p: p[0]) markers = MarkerArray() for id_num, center in enumerate(centers): centerMarker = self.makeMarker(msg.header, id_num, center) markers.markers.append(centerMarker) self.pub.publish(markers) endtime = time.clock() if self.debug: print >> sys.stderr, "KMeans time: " + str(endtime - starttime)
def callback(self, data): """Computes the average point in a point cloud and saves timing info. """ points = pc2.read_points(data, field_names=['x', 'y', 'z'], skip_nans=True) num_points = 0 avg_x = 0 avg_y = 0 avg_z = 0 for x, y, z in points: num_points += 1 avg_x += x avg_y += y avg_z += z if num_points > 0: avg_x /= num_points avg_y /= num_points avg_z /= num_points current_time = rospy.Time.now() self._time_taken += current_time - self._prev_time self._prev_time = current_time self._num_calls += 1 rospy.loginfo('x: {}, y: {}, z: {}, time/point: {}s'.format( avg_x, avg_y, avg_z, self._time_taken.to_sec() / self._num_calls))
def callbackScan(laserScan): global iniX global distX global espacoVazio global estadoCont point_cloud = laser_projector.projectLaser(laserScan) minY = 0 maxY = 0 # calcula os pontos mínimo e máximo de detecção do laser for p in pc2.read_points(point_cloud, field_names = ("x", "y", "z"), skip_nans=True): if p[1] < minY: minY = p[1] if p[1] > maxY: maxY = p[1] # Se o laser não detectar obstáculo, inicia a contagem do espaço vazio if (minY+maxY)/2 < -0.1 and estadoCont == 0: estadoCont = 1 iniX = posX # Se o laser detectar obstáculo, finaliza a contagem do espaço vazio elif (minY+maxY)/2 > 0.1 and estadoCont == 1: estadoCont = 0 espacoVazio = distX # Se o estadoCont == 1, há um espaço vazio sendo mensurado elif estadoCont == 1: distX = posX - iniX rospy.loginfo("oriZ: %.1f dist: %.1f espaco: %.1f" %(oriZ, distX, espacoVazio))
def get_nearest_cloud(self, msg): points = list() points_xy = list() # Get all the points in the visible cloud (may be prefiltered by other nodes) for point in point_cloud2.read_points(msg, skip_nans=True): points.append(point[:3]) points_xy.append(point[:2]) # Convert to a numpy array points_arr = np.float32([p for p in points]).reshape(-1, 1, 3) # Compute the COG cog = np.mean(points_arr, 0) # Convert to a Point cog_point = Point() cog_point.x = cog[0][0] cog_point.y = cog[0][1] cog_point.z = cog[0][2] #cog_point.z = 0.35 # Abort if we get an NaN in any component if np.isnan(np.sum(cog)): return # If we have enough points, find the best fit ellipse around them try: if len(points_xy) > 6: points_xy_arr = np.float32([p for p in points_xy]).reshape(-1, 1, 2) track_box = cv2.fitEllipse(points_xy_arr) else: # Otherwise, find the best fitting rectangle track_box = cv2.boundingRect(points_xy_arr) angle = pi - radians(track_box[2]) except: return #print angle # Convert the rotation angle to a quaternion q_angle = quaternion_from_euler(0, angle, 0, axes='sxyz') q = Quaternion(*q_angle) q.x = 0.707 q.y = 0 q.z = 0.707 q.w = 0 # Publish the COG and orientation target = PoseStamped() target.header.stamp = rospy.Time.now() target.header.frame_id = msg.header.frame_id target.pose.position = cog_point target.pose.orientation = q # Publish the movement command self.target_pub.publish(target)
def callback2(msg): global count2 if count2 == 0: points = list(pc2.read_points(msg, skip_nans=True)) pickle.dump(points, open('kinect2_pc2_read', 'wb')) pickle.dump(msg, open('kinect2_pc2', 'wb')) count2 += 1 print 'Done2'
def callback_points(self, data): """ Function to handle the arrival of pointcloud data """ cloud = list(pc2.read_points(data, skip_nans=True, field_names=("x", "y"))) cloud = np.resize(cloud, [640, 480, 2]) print cloud[100][100]
def do_transform_cloud(cloud, transform): t_kdl = transform_to_kdl(transform) points_out = [] for p_in in read_points(cloud): p_out = t_kdl * PyKDL.Vector(p_in[0], p_in[1], p_in[2]) points_out.append(p_out) res = create_cloud(transform.header, cloud.fields, points_out) return res
def handle_pointcloud(msg): results = [] points = pc2.read_points(msg) for x, y, z in points: results.extend([[x + a, y + b, z + c] for a, b, c in deltas]) pub.publish(pc2.create_cloud_xyz32(msg.header, results))
def isCloudValid(cloud): """ Returns whether a cloud is valid to be processed by this perception module """ for point in pc2.read_points(cloud,skip_nans=False,field_names=("rgb","x","y","z")): return True break return False
def _cat_newcloud(self): data = yield self._plane_subscriber.get_next_message() if self.pointcloud is None: self.pointcloud = data else: gen = list( pc2.read_points( data, skip_nans=True, field_names=('x', 'y', 'z'))) pc_gen = list( pc2.read_points( self.pointcloud, skip_nans=True, field_names=('x', 'y', 'z'))) concat = np.asarray(gen + pc_gen, np.float32) print 'SONAR_POINTCLOUD - current size: {}'.format(concat.shape) self.pointcloud = mil_ros_tools.numpy_to_pointcloud2(concat) yield
def execute(self, userdata): rospy.loginfo( 'Capturing item descriptor in bin {}'.format(userdata.bin_id)) rospy.loginfo(userdata.keys) self._tts.publish('Sensing bin {}'.format(userdata.bin_id)) rospy.sleep(5) self._tuck_arms.wait_for_service() self._tuck_arms(tuck_left=False, tuck_right=False) # Crop shelf. crop_request = CropShelfRequest(cellID=userdata.bin_id) self._crop_shelf.wait_for_service() crop_response = self._crop_shelf(crop_request) # Segment items. segment_request = SegmentItemsRequest(cloud=crop_response.cloud, items=['a', 'b']) self._segment_items.wait_for_service() segment_response = self._segment_items(segment_request) clusters = segment_response.clusters.clusters userdata.clusters = clusters rospy.loginfo('[CaptureItemDescriptor] Found {} clusters.'.format( len(clusters))) for i, cluster in enumerate(clusters): points = pc2.read_points(cluster.pointcloud, skip_nans=True) point_list = [Point(x=x, y=y, z=z) for x, y, z, rgb in points] if len(point_list) == 0: rospy.logwarn('Skipping cluster of size 0') continue viz.publish_cluster(self._markers, point_list, 'bin_{}'.format(userdata.bin_id), 'bin_{}_items'.format(userdata.bin_id), i) self._get_item_descriptor.wait_for_service() descriptor = self._get_item_descriptor(cluster).descriptor rospy.loginfo('Color histogram ({} bins):\n{}'.format( descriptor.histogram.num_bins, descriptor.histogram.histogram)) bounding_box = descriptor.planar_bounding_box bbox_pose = bounding_box.pose bbox_dimensions = bounding_box.dimensions rospy.loginfo('Bounding box centroid: {}'.format(bbox_pose)) rospy.loginfo('Bounding box dimensions: {}'.format(bbox_dimensions)) viz.publish_bounding_box( self._markers, bbox_pose, bbox_dimensions.x, bbox_dimensions.y, bbox_dimensions.z, 0.33, 0.69, 0.31, 0.25, 1234 + i) viz.publish_pose(self._markers, bbox_pose, 1, 0, 0, 1, 1234 + i) action = None while action is None: action = raw_input('Capture [a]nother or [d]one?: ') if action == 'a': return outcomes.CAPTURE_ITEM_NEXT elif action == 'd': return outcomes.CAPTURE_ITEM_DONE else: action = None
def callbackScan(scan): rospy.loginfo("Got scan, projecting") point_cloud = laser_projector.projectLaser(scan) for p in pc2.read_points(point_cloud, field_names = ("x", "y", "z"), skip_nans=True): distY = p[1] if distY < 0: distY = 0 #print " x : %f y: %f z: %f" %(p[0],p[1],p[2]) rospy.loginfo("x: %f y: %f" %(posX, distY))
def callbackScan(laserScan, tipo): global iniX global distX global espacoVazio global estadoCont global obstEsq global obstTraz global obstFrente point_cloud = laser_projector.projectLaser(laserScan) somaX = 0 somaY = 0 pontos = 0 # calcula os pontos mínimo e máximo de detecção do laser for p in pc2.read_points(point_cloud, field_names = ("x", "y", "z"), skip_nans=True): x = p[0] y = p[1] if x < 0: x = 0 if y < 0: y = 0 somaX = somaX + x somaY = somaY + y pontos = pontos + 1 if tipo == 'esq' and pontos > 0: mediaX = (somaX)/pontos mediaY = (somaY)/pontos obstEsq = mediaX-mediaY # Se o laser não detectar obstáculo, inicia a contagem do espaço vazio if obstEsq > 0.15 and estadoCont == 0: estadoCont = 1 iniX = posX # Se o laser detectar obstáculo, finaliza a contagem do espaço vazio elif obstEsq < 0.15 and estadoCont == 1: estadoCont = 0 espacoVazio = distX # Se o estadoCont == 1, há um espaço vazio sendo mensurado elif estadoCont == 1: distX = posX - iniX #rospy.loginfo("obst-%s: %.1f oriZ: %.1f dist: %.1f esp: %.1f" %(tipo, obstEsq, oriZ, distX, espacoVazio)) elif (tipo == 'tr1' or tipo == 'tr2') and pontos > 0: mediaX = (somaX)/pontos mediaY = (somaY)/pontos obstTraz = mediaX-mediaY #rospy.loginfo("obst-%s: %.1f oriZ: %.1f esp: %.1f" %(tipo, obstTraz, oriZ, espacoVazio)) elif tipo == 'fre' and pontos > 0: mediaX = (somaX)/pontos mediaY = (somaY)/pontos obstFrente = mediaX-mediaY
def _3_cloud_callback(message): try: data_out = pc2.read_points(message, field_names=None, skip_nans=True, uvs=[]) i=0 iteration1 = next(data_out) #format x,y,z,rgba while iteration1 != None: iteration1 = next(data_out) i=i+1 except StopIteration: print "(Cloud 2bottom)"
def octomap_update_callback(self, msg): # as pointcloud2. obs_set = set() obs_set_raw = set() for p in pc2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True): #print " x : %f y: %f z: %f" % (p[0], p[1], p[2]) point = self.dg.continuous_to_discrete((p[0], p[1], p[2])) #print ('point:',point) obs_set.add(point) obs_set.add(p) acquired = self.obstacle_set_mutex.acquire(True) # blocking. if acquired: #print('octomap updated!') self.driver.set_obstacle_set(obs_set) self.obs_set_raw = obs_set_raw self.obstacle_set_mutex.release() self.is_obstacle_set_updated = True return else: print('Lock not acquired!')
def callback_kinect(self,data) : data_out = pc2.read_points(data, field_names=("x","y","z","intensity"), skip_nans=False) #print(data_out) i=0 j=0 self.id+=1 for p in data_out: if(p[0]>-10 and p[0]<10 and p[1]>-10 and p[1]<10): # while(i<450): # i+=1 # j=0 # while(j<900): # j=j+1 # self.image[:,:,0][i][j]=int(100*p[0]) # self.image[:,:,1][i][j]=int(100*p[1]) # self.image[:,:,2][i][j]=int(100*(p[2]+2)) self.image_gs[int(abs(30*(p[0]+10)))][int(abs(30*(p[1]+10)))]=255 cv2.imwrite("ima"+str(self.id)+".jpg",self.image_gs) #self.image_gs=cv2.flip(self.image_gs,0) # cv2.imshow("image",self.image_gs) # cv2.waitKey(1) self.image_gs=np.zeros((600,600))
def callback(data): data_out = pc2.read_points(data, skip_nans=True) loop = True while loop: try: int_data = next(data_out) s = struct.pack('>f', int_data[3]) i = struct.unpack('>l', s)[0] pack = ctypes.c_uint32(i).value r = (pack & 0x00FF0000) >> 16 g = (pack & 0x0000FF00) >> 8 b = (pack & 0x000000FF) print("%d %d %d" % r, g, b) except Exception as e: rospy.loginfo(e.message) loop = False data.header.stamp = rospy.Time() data.header.frame_id = data.header.frame_id
def detection_cb(msg, items, point_cloud_data, alvar_marker): global kinect2_img, tf_buffer, estimator if msg.class_id in items: # global pose_pub [u, v] = [int(msg.pose.x_center), int(msg.pose.y_center)] # try: location_xyz = list( pc2.read_points(point_cloud_data.data, ('x', 'y', 'z'), skip_nans=True, uvs=[[u, v]])) # location type --> [(touple)] if location_xyz: converted_location = list_2_stampPose(location_xyz[0]) transform = tf_buffer.lookup_transform( 'base_footprint', converted_location.header.frame_id, #source frame rospy.Time(0), #get the tf at first available time rospy.Duration(1.0)) #wait for 1 second pose_transformed = tf2_geometry_msgs.do_transform_pose( converted_location, transform) alvar_marker.update(pose_transformed, msg.score) estimator.update(pose_transformed) publish_image(converted_location, msg)
def callback_pointcloud(data): assert isinstance(data, PointCloud2) gen = point_cloud2.read_points(data) point_cnt = 0 in_vicinity = 0 for p in gen: point_cnt += 1 if(sqrt(p[0]**2+p[1]**2) < 2): in_vicinity += p[3] #print p # type depends on your data type, first three entries are probably x,y,z_name__ == '__main__': #print point_cnt if(in_vicinity > 300): print "STOP" v.mode = dronekit.VehicleMode("HOLD") else: print "FLY"
def updatemap(self, pcl, range_max, pose): robot_origin=int(pose[0])+int(math.ceil(self.width/self.resolution)*pose[1]) for p in pc2.read_points(pcl, field_names = ('x', 'y', 'z'), skip_nans = True): rad = math.sqrt(p[0]*p[0] + p[1]*p[1]) px = int(rad*cos(pose[2])/self.resolution) py = int(rad*sin(pose[2])/self.resolution) l = bresenham.bresenham([0,0],[px,py]) for j in range(len(l.path)): lpx= int(self.map_origin[0]+pose[0]/self.resolution +l.path[j][0]) lpy=int(self.map_origin[1]+pose[1]/self.resolution +l.path[j][1]) if (0<=lpx<self.width/self. resolution and 0<=lpy<self.height/self.resolution): index= int(lpx + math.floor(self.width/self.resolution)*(lpy)) if(j<len(l.path)-1): self.localmap[index] = 0 else: if rad<self.max_scan_range*range_max: self.localmap[index] = 100
def merge_sensors(self): # print('type', type(data)) # print("\n\n") # tic = time.clock() n_points = self.data_lidar.height*self.data_lidar.width print("Number of data points {}".format(n_points)) dim = 2 self.points_lidar_2d = np.zeros((dim, n_points)) self.points_lidar_z = np.zeros((n_points)) it_count = 0 for pp in pc2.read_points(self.data_lidar, skip_nans=True, field_names=("x", "y", "z")): # Cut off points which are to high self.points_lidar_2d[:, it_count] = [pp[0], pp[1]] self.points_lidar_z[it_count] = pp[2] # print("points are x={}, y={}, z={}".format(pp[0], pp[1], pp[2])) it_count += 1 # print('it count', it_count) # print('n_points', n_points) self.points_lidar_2d += self.points_lidar_2d + np.tile(self.trans_lidar[:2], (n_points, 1)).T self.points_lidar_z += self.points_lidar_z + np.tile(self.trans_lidar[2], (n_points)).T # TODO - only evaluate if no collision ind_good = np.logical_and(self.points_lidar_z > 0, self.points_lidar_z < self.robot_clearance_height) # Describe points in robot center-frame radius_lidar = np.sqrt(self.points_lidar_2d[0, ind_good]*self.points_lidar_2d[0, ind_good] + self.points_lidar_2d[1, ind_good]*self.points_lidar_2d[1, ind_good]) ind_good[ind_good] = (radius_lidar > self.robot_radius) self.points_lidar_2d = self.points_lidar_2d[:, ind_good] angle_lidar = np.arctan2(self.points_lidar_2d[1, :], self.points_lidar_2d[0, :])
def on_new_point_cloud(data): global im pc = pc2.read_points(data, skip_nans=True, field_names=("x", "y", "z", "intensity")) #print pc.type #print data.type cloud_points = [] for p in pc: cloud_points.append(p) npc = np.array(cloud_points) #lidar_to_2d_front_view(npc, v_res=VRES, h_res=HRES, v_fov=VFOV, val="depth", y_fudge=Y_FUDGE) #lidar_to_2d_front_view(npc, v_res=VRES, h_res=HRES, v_fov=VFOV, val="height", y_fudge=Y_FUDGE) #lidar_to_2d_front_view(npc, v_res=VRES, h_res=HRES, v_fov=VFOV, val="reflectance", y_fudge=Y_FUDGE) #im = birds_eye_point_cloud(npc, side_range=(-10, 10), fwd_range=(-10, 10), res=0.1) im = point_cloud_to_panorama(npc, v_res=VRES, h_res=HRES, v_fov=VFOV, y_fudge=5, d_range=(0, 100))
def callback(self, raw_data, republisher): current_time = time.time() if self.__period < current_time - self.__previous_time: self.__previous_time += (1 + int( (current_time - self.__previous_time) / self.__period)) * self.__period points = [] for point in pc2.read_points(raw_data): points.append(list(point[0:3])) points_map_pcl = pcl.PointCloud(points) pc_filter = points_map_pcl.make_voxel_grid_filter() pc_filter.set_leaf_size(*self.__leaf_size) points_map_pcl = pc_filter.filter() header = Header() header.stamp = rospy.Time.now() header.frame_id = self.__frame_id downsampled_data = pc2.create_cloud_xyz32(header, points_map_pcl.to_list()) republisher.publish(downsampled_data)
def scan_cb(msg): # convert the message of type LaserScan to a PointCloud2 pc2_msg = lp.projectLaser(msg) # now we can do something with the PointCloud2 for example: # publish it pc_pub.publish(pc2_msg) # convert it to a generator of the individual points point_generator = pc2.read_points(pc2_msg) # we can access a generator in a loop sum = 0 num = 0 for point in point_generator: distance = ((point[0])**2 + (point[1] - 2)**2)**0.5 #print distance if distance < 2: sum += 1 # we can calculate the average z value for example print(sum)
def octomap_update_callback(self, msg): # as pointcloud2. obs_set = set() for p in pc2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True): # print " x : %f y: %f z: %f" % (p[0], p[1], p[2]) point = self.dg.continuous_to_discrete((p[0], p[1], p[2])) # print("corresponding discrete value: ", point) obs_set.add(point) # save points set if (self.save_pts): save_points3D(obs_set) acquired = self.obstacle_set_mutex.acquire(True) # blocking. if acquired: # print('octomap updated!') self.driver.set_obstacle_set(obs_set) self.obstacle_set_mutex.release() return else: print('Lock not acquired!')
def find_xyz(self, data): # do processing here gen = pc2.read_points(data, field_names=("x", "y", "z"), skip_nans=False, uvs=[[self.rgb_h, self.rgb_v]]) target_xyz_cam = list(gen) # do conversion to global coordinate here listener = tf.TransformListener() listener.waitForTransform("map", "head_rgbd_sensor_rgb_frame", rospy.Time(0), rospy.Duration(4.0)) rgbd_point = PointStamped() rgbd_point.header.frame_id = "head_rgbd_sensor_rgb_frame" rgbd_point.header.stamp = rospy.Time(0) rgbd_point.point.x = target_xyz_cam[0][0] rgbd_point.point.y = target_xyz_cam[0][1] rgbd_point.point.z = target_xyz_cam[0][2] self.map_point = listener.transformPoint("map", rgbd_point) self.found_3d = True sub_once.unregister()
def pointcloudCallBack(PointCloud): # self.lock.acquire() gen = pc2.read_points(PointCloud, skip_nans=True) int_data = list(gen) x_bar = 0 y_bar = 0 z_bar = 0 num_points = 0 for x in int_data: test = x[3] # cast float32 to int so that bitwise operations are possible s = struct.pack('>f', test) i = struct.unpack('>l', s)[0] # you can get back the float value by the inverse operations pack = ctypes.c_uint32(i).value # r,g,b values in the 0-255 range r = (pack & 0x00FF0000) >> 16 g = (pack & 0x0000FF00) >> 8 b = (pack & 0x000000FF) rgbList = [r, g, b] # print rgbList if isColorCorrect(rgbList, 'red'): num_points += 1 # x,y,z can be retrieved from the x[0],x[1],x[2] x_bar += x[1] y_bar += x[2] z_bar += x[3] if (num_points != 0): x_bar /= num_points y_bar /= num_points z_bar /= num_points print('Marker Position') print(num_points) print([x_bar, y_bar, z_bar]) posi = Float64MultiArray(data=[x_bar, y_bar, z_bar]) manipulation_pub = rospy.Publisher('/manipulation_target', Float64MultiArray, queue_size=10) manipulation_pub.publish(posi)
def pcl_callback(pcl_msg): cloud = ros_to_pcl(pcl_msg) cloud = pipeline.passth(cloud) cloud = pipeline.passth(cloud, axis='y', amin=-0.45, amax=0.45) cloud = pipeline.denoise(cloud) filtered = pipeline.voxel(cloud) table_points = pipeline.plane_points(filtered) table_cloud = filtered.extract(table_points, negative=False) obj_cloud = filtered.extract(table_points, negative=True) pcl_objects_pub.publish(pcl_to_ros(obj_cloud)) pcl_table_pub.publish(pcl_to_ros(table_cloud)) cluster_ix = pipeline.cluster_ix(obj_cloud) pcl_cluster_pub.publish( pcl_to_ros(pipeline.color_clusters(obj_cloud, cluster_ix))) detected_objects = [] for i, ixs in enumerate(cluster_ix): pcl_data = obj_cloud.extract(ixs) ros_data = pcl_to_ros(pcl_data) feature = np.concatenate( (features.compute_color_histograms(ros_data), )) prediction = clf.predict(scaler.transform(feature.reshape(1, -1))) label = encoder.inverse_transform(prediction)[0] label_pos = list(next(pc2.read_points(ros_data))[:3]) label_pos[2] += .4 object_markers_pub.publish(make_label(label, label_pos, i)) do = DetectedObject() do.label = label do.cloud = ros_data detected_objects.append(do) detected_objects_pub.publish(detected_objects)
def callback(self, ros_point_cloud): xyz = np.array([[0, 0, 0]]) rgb = np.array([[0, 0, 0]]) #self.lock.acquire() gen = pc2.read_points(ros_point_cloud, skip_nans=True) int_data = list(gen) for x in int_data: test = x[3] # cast float32 to int so that bitwise operations are possible s = struct.pack('>f', test) i = struct.unpack('>l', s)[0] # you can get back the float value by the inverse operations pack = ctypes.c_uint32(i).value r = (pack & 0x00FF0000) >> 16 g = (pack & 0x0000FF00) >> 8 b = (pack & 0x000000FF) # prints r,g,b values in the 0-255 range # x,y,z can be retrieved from the x[0],x[1],x[2] xyz = np.append(xyz, [[x[0], x[1], x[2]]], axis=0) rgb = np.append(rgb, [[r, g, b]], axis=0) self.eval_one_epoch(self.sess, self.ops, xyz, rgb)
def chatterCallback_PCL(self, data): x_vec=[] y_vec=[] z_vec=[] # xaxis, yaxis, zaxis = (1, 0, 0), (0, 1, 0), (0, 0, 1) # I = tf.transformations.identity_matrix() # Rx = tf.transformations.rotation_matrix(-0.0150, xaxis) # Ry = tf.transformations.rotation_matrix(0.0786, yaxis) # Rz = tf.transformations.rotation_matrix(0.0121, zaxis) # R = concatenate_matrices(Rx, Ry, Rz) for data in pc2.read_points(data, skip_nans=True): data2=data x_vec.append(data2[0]) y_vec.append(data2[1]) z_vec.append(data2[2]) with open("calib_pcl.txt", mode='w') as f: # I add the mode='w' print('Saving') for i in range(len(x_vec)): f.write("%f,"%float(x_vec[i])) f.write("%f,"%float(y_vec[i])) f.write("%f,\n"%float(z_vec[i]))
def __callback(self, cameraMsg, rgbMsg, depthMsg, cloudMsg): frame_id = rgbMsg.header.frame_id stamp = rgbMsg.header.stamp rgb = convert.image2matrix(rgbMsg, encoding='bgr8') depth = convert.image2matrix(depthMsg, encoding='passthrough').astype(np.float32) cloud = np.array( list(pc2.read_points(cloudMsg, field_names=["x", "y", "z"]))).reshape( cloudMsg.height, cloudMsg.width, -1) cameraMsg.K = np.array(cameraMsg.K, np.float32).reshape(3, 3) cameraMsg.D = np.array(cameraMsg.D, np.float32) data = {} data["camera"] = cameraMsg data["rgb"] = rgb data["depth"] = depth data["cloud"] = cloud result = self._estimator.process(data) if result is None: markerPoseImage = convert.matrix2image(rgb, frame_id, stamp, encoding='bgr8') self._pubImage.publish(markerPoseImage) return frame, rvec, tvec = result markerPoseImage = convert.matrix2image(frame, frame_id, stamp, encoding='bgr8') self._pubImage.publish(markerPoseImage) markerPose = convert.axis2pose(rvec, tvec, frame_id, stamp) self._pubMarker.publish(markerPose)
def callback_cloud(self, cloud_data): # creating local world objects list we will publish them world_objects = [] # iterating through every object in the class's list and find their actual world position by # using point cloud and camera's world position for obj in self.objects: data_generator = pc2.read_points(cloud_data, ('x', 'y', 'z'), False, [(obj.center_x, obj.center_y)]) for point in data_generator: w_obj = Object(obj.id, self.cam_x + point[0], self.cam_y - point[1], (self.cam_z - point[2]) / 2, obj.center_x, obj.center_y, obj.width, obj.angle, obj.color, obj.shape) world_objects.append(w_obj) #print('x: {}, y: {}, z: {}, center x: {}, center y: {}'.format(self.cam_x + point[0], self.cam_y - point[1], (self.cam_z - point[2]) / 2, obj.center_x, obj.center_y)) # create correct message type to publish to topic /objects objs = ObjectStates(world_objects, len(world_objects)) self.object_pub.publish(objs)
def _deserialize_numpy(self, str): """ wrapper for factory-generated class that passes numpy module into deserialize """ self.deserialize_numpy(str, numpy) # deserialize (with numpy wherever possible) # for Image msgs if self._type == 'sensor_msgs/Image': self.data = numpy.asarray( bridge.imgmsg_to_cv(self, desired_encoding=self.encoding) ) # convert pixel data to numpy array # for PointCloud2 msgs if self._type == 'sensor_msgs/PointCloud2': print 'Cloud is being deserialized...' points = point_cloud2.read_points(self) points_arr = numpy.asarray(list(points)) # Unpack RGB color info _float2rgb_vectorized = numpy.vectorize(_float2rgb) r, g, b = _float2rgb_vectorized(points_arr[:, 3]) r = numpy.expand_dims(r, 1).astype( 'uint8') # insert blank 3rd dimension (for concatenation) g = numpy.expand_dims(g, 1).astype('uint8') b = numpy.expand_dims(b, 1).astype('uint8') # Concatenate and Reshape pixels_rgb = numpy.concatenate((r, g, b), axis=1) image_rgb = pixels_rgb.reshape(self.height, self.width, 3) points_arr = points_arr[:, :3].reshape(self.height, self.width, 3).astype('float32') # Build record array to separate datatypes -- int16 for XYZ, uint8 for RGB image_xyzrgb = numpy.rec.fromarrays((points_arr, image_rgb), names=('xyz', 'rgb')) self.data = image_xyzrgb return self
def callback(data): """ Callback function to take pointcloud data outputted by *.bag test file and perform manipulations on it, then publish the manipulated data. """ cm = CloudManipulations() pointcloud = pc2.read_points(data, skip_nans=True, field_names=["x", "y", "z"]) for p in pointcloud: x = p[0] y = p[1] z = p[2] # Apply some linear translation (offset) p = cm.apply_offset(p, offset) # Rotate by some degrees on an axis if (axis.lower() == "x"): rotated = cm.rotate_x(rotation, False) elif (axis.lower() == "y"): rotated = cm.rotate_y(rotation, False) elif (axis.lower() == "z"): rotated = cm.rotate_z(rotation, False) else: print("Invalid axis ", axis.lower()) return None # Apply the offset for rotation tm = cm.apply_rotational_offset(rotated, x, y, z) # Convert to point cloud pc = cm.mat_to_point_cloud(tm, 'velodyne_edited') # Publish the pointcloud rospy.loginfo(pc) pub.publish(pc)
def transform(self, point_cloud): if not self.key: return print(22222222) self.res = [] if self.target_found: #已检测到目标 # rospy.loginfo('Data acquiring') # pc2.read_points(cloud, skip_nans=True, field_names=("x", "y", "z")) for i in self.points: # pc = pc2.read_points(point_cloud, field_names=("x", "y", "z"), skip_nans=True,uvs=[[self.target_x, self.target_y]]) pc = pc2.read_points(point_cloud, field_names=("x", "y", "z"), skip_nans=True, uvs=[i]) int_data = list(pc) #[(x,y,z)] # while not int_data: # i = [j+1 for j in i] # pc = pc2.read_points(point_cloud, field_names=("x", "y", "z"), skip_nans=True, uvs=[i]) # int_data = list(pc) # [(x,y,z)] if int_data: print("Camera_Position:", int_data) self.camera_position[:3] = list(int_data[0]) self.camera_position[:3] = self.camera_position[:3] * 1000 self.workpiece_position = np.dot( self.martix, self.camera_position) #A*(x,y,z) = (X,Y,Z) (注意单位换算) print(self.workpiece_position) self.workpiece_position[ 0] = self.workpiece_position[0] - 17 self.workpiece_position[2] = self.workpiece_position[2] + 1 self.final_position = self.workpiece_position[0:3] # print("****Final****:",self.final_position) self.res.append(self.final_position) else: print("坐标转换失败!") self.target_found = False print("###########", self.res)
def process_image_and_points(self): if ((self.process_locked) or (not self.image_received) or (not self.points_received)): rp.loginfo("- Process locked") return # lock process self.process_locked = True rp.loginfo("- Process started") # lock data receivers self.receiver_locked = True image = copy.deepcopy(self.image_data) points = copy.deepcopy(self.points_data) # unlock data receivers self.image_received = False self.points_received = False self.receiver_locked = False # process image try: bayer_image = self.bridge.imgmsg_to_cv2(image, "bayer_grbg8") rgb_img = cv2.cvtColor(bayer_image, cv2.COLOR_BAYER_GR2RGB) except CvBridgeError as e: print(e) rp.loginfo(rp.get_caller_id() + "-- Image converted, shape: %s", str(rgb_img.shape)) # cv2.imwrite('/home/parkjaeil0108/challenge/Didi-Release-2/Data/1/test.jpg', rgb_img) # process points x_pos = [] y_pos = [] z_pos = [] for p in pc2.read_points(points, skip_nans=True): x_pos.append(p[0]) y_pos.append(p[1]) z_pos.append(p[2]) rp.loginfo("-- %d Point converted, p0: %.2f %.2f %.2f", len(x_pos), x_pos[0], y_pos[0], z_pos[0]) # unlock process rp.loginfo("- Process finished") self.process_locked = False
def callback_kinect(self, data): data_out = pc2.read_points(data, field_names=("x", "y", "z", "intensity"), skip_nans=False) #print(data_out) marker_array = MarkerArray() id = 0 for p in data_out: if (p[0] > -5 and p[0] < 5 and p[1] > 0.1): marker = Marker() marker.header.frame_id = "base_link" marker.type = visualization_msgs.msg.Marker.CUBE marker.id = id marker.header.stamp = rospy.Time.now() marker.scale.x = 0.1 marker.scale.y = 0.1 marker.scale.z = 0.1 marker.action = visualization_msgs.msg.Marker.ADD marker.color.a = 1.0 red = self.red[int(30 * (p[2] + 2.3))][int(45 * (p[0] + 5))] blue = self.blue[int(30 * (p[2] + 2.3))][int(45 * (p[0] + 5))] green = self.green[int(30 * (p[2] + 2.3))][int(45 * (p[0] + 5))] #print(str(int(40*(p[0]+5)))+" "+str(int(70*(p[2]+2.3)))) #self.reconstruct[450-int(90*(p[2]+1.9))][int(100*(p[0]+5))]=255 marker.color.r = float(red) / 255 marker.color.g = float(green) / 255 marker.color.b = float(blue) / 255 marker.pose.position.x = p[0] marker.pose.position.y = p[1] marker.pose.position.z = p[2] + 1.9 if (id > 10000): id = 0 marker_array = MarkerArray() pass marker_array.markers.append(marker) id += 1 self.pub.publish(marker_array)
def transform_points_robot(points_msg): start_time = time.time() del points_transformed[:] #print "delete level", len(points_transformed) interval = start_time - init_time #print "Time from start: ", interval for p in point_cloud2.read_points(points_msg, skip_nans=True): z_translated = round(p[2], 3) if (z_translated < 0.00): continue x_translated = round(p[0], 3) + robot0_pose[0] y_translated = round(p[1], 3) + robot0_pose[1] rotation_angle = math.radians(robot0_pose[2]) x_transformed = x_translated * math.cos( rotation_angle) + y_translated * math.sin(rotation_angle) y_transformed = y_translated * math.cos( rotation_angle) - x_translated * math.sin(rotation_angle) pt = [x_transformed, y_transformed, z_translated] points_transformed.append(pt) fields = [ PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1) ] header = Header() header.stamp = rospy.Time.now() header.frame_id = "world" pc2 = point_cloud2.create_cloud(header, fields, points_transformed) pub.publish(pc2) duration = time.time() - start_time print len(points_transformed), interval, duration
def pointcloudToPlanningScene(msg): global pointCloud global completed msg = pointCloud if not completed: try: trans = tf_buffer.lookup_transform("map", msg.header.frame_id, rospy.Time(0), rospy.Duration(4)) except tf2.LookupException as ex: rospy.logwarn(ex) return except tf2.ExtrapolationException as ex: rospy.logwarn(ex) return cloud_out = do_transform_cloud(msg, trans) rospy.sleep(2) scene.remove_world_object() rospy.sleep(2) data = pc2.read_points(cloud_out, field_names=("x", "y", "z", "rgb"), skip_nans=True) counter = 0 limitCounter = 0 limit = 5 for value in data: if limitCounter == limit: limitCounter = 0 p = PointStamped() p.header.frame_id = robot.get_planning_frame() p.pose.position.x = value[0] p.pose.position.y = value[1] p.pose.position.z = value[2] scene.add_box("point" + str(counter), p, (0.01, 0.01, 0.01)) counter = counter + 1 completed = True limitCounter = limitCounter + 1 print("completed scene")
def callback_intel_realsense(data): global mag, dir, dist_arr h_comp = 0.0 v_comp = 0.0 for i, point in zip( range(11), pc2.read_points(data, field_names=("x", "y", "z"), skip_nans=False, uvs=[[1, 240], [64, 240], [128, 240], [192, 240], [256, 240], [320, 240], [384, 240], [448, 240], [512, 240], [576, 240], [639, 240]])): pt_x = point[0] pt_y = point[1] pt_z = point[2] if ((isnan(pt_x) or isnan(pt_y)) or (isnan(pt_z))): dist_arr[i] = 5.0 else: dist_arr[i] = sqrt(pt_x**2 + pt_y**2 + pt_z**2) h_comp += dist_arr[i] * cos(18 * i * pi / 180) v_comp += dist_arr[i] * sin(18 * i * pi / 180) mag = sqrt(h_comp**2 + v_comp**2) dir = atan2(v_comp, h_comp) * 180 / pi if dir < 0: dir += 360 dir = int(dir) mag = np.round(mag) mag /= 32.0 # proportionality constant print("MAGNITUDE = ", mag, "DIRECTION = ", dir)
def __init__(self): super(MarkerDetectionNode, self).__init__() # configure self.dynamic_reconfigure = Server(MarkerPoseEstimationConfig, self.dynamic_reconfigure_callback) # # generate marker # image_dir = rospy.get_param("~image_dir") # for i in range(50): # marker_filename = os.path.join(image_dir, "marker_" + str(i) + ".png") # self.generate_marker(marker_filename, i, 600) # connect to the camera self.bridge = CvBridge() camera_name = rospy.get_param("~camera_name") camera_type = rospy.get_param("~camera_type") self.camera = CameraClient(camera_name, camera_type) while not rospy.is_shutdown(): # get frame cloud, texture = self.camera.get_frame(publish=True) cv_image = self.bridge.imgmsg_to_cv2(texture, "bgr8") # convert point cloud2 message to points i = 0 points = np.zeros((cv_image.shape[1] * cv_image.shape[0], 3)) for p in pc2.read_points(cloud, field_names=("x", "y", "z"), skip_nans=False): points[i, 0] = p[0] points[i, 1] = p[1] points[i, 2] = p[2] i = i + 1 self.detect_marker(points, cv_image, target_marker_id=0) rospy.spin() cv2.destroyAllWindows()
def __extractPointCloudData(data): iterData = pc2.read_points(data) points = [] height = sqrt((data.width/float(16))*9) width = 16 * ( height/float(9) ) height = int(height) width = int(width) maxDist = 0 z = 2 print "PointCloud data received." for i in range(width): intermediate = [] for j in range(height): point = next(iterData) if(point[z]>maxDist): maxDist=point[z] intermediate.append(point) points.append(intermediate) points = np.array(points) points = np.swapaxes(points, 0, 1) print "New point cloud available" return (maxDist, points)
def callback(scan, image): rospy.loginfo("image timestamp: %d ns" % image.header.stamp.to_nsec()) rospy.loginfo("scan timestamp: %d ns" % scan.header.stamp.to_nsec()) diff = abs(image.header.stamp.to_nsec() - scan.header.stamp.to_nsec()) rospy.loginfo("diff: %d ns" % diff) img = bridge.imgmsg_to_cv2(image) cloud = lp.projectLaser(scan) points = pc2.read_points(cloud) objPoints = np.array(map(extract, points)) Z = get_z(q, objPoints, K) objPoints = objPoints[Z > 0] if lens == 'pinhole': img_points, _ = cv2.projectPoints(objPoints, rvec, tvec, K, D) elif lens == 'fisheye': objPoints = np.reshape(objPoints, (1,objPoints.shape[0],objPoints.shape[1])) img_points, _ = cv2.fisheye.projectPoints(objPoints, rvec, tvec, K, D) img_points = np.squeeze(img_points) for i in range(len(img_points)): try: cv2.circle(img, (int(round(img_points[i][0])),int(round(img_points[i][1]))), laser_point_radius, (0,255,0), 1) except OverflowError: continue pub.publish(bridge.cv2_to_imgmsg(img))
def getObjectPointCloud(req): global cachedPCmsg, cachedPCmsgFlag print "Returning pointclouds" bin_num = req.bin_num # bin_num is of the format like 'bin_A' #obj_id = req.obj_id #(string) pc = PointCloud() pts = [] with mutex: if cachedPCmsgFlag: source_pc2_kinect = cachedPCmsg # does it do deep copy? else: rospy.logerr("No pointcloud received yet, check your kinect") # convert PointCloud2 to PointCloud manually for point in point_cloud2.read_points(source_pc2_kinect, skip_nans=False): pts.append(list(point)[0:3]) filtered_pc_map = filterPointCloud(bin_num, pts, source_pc2_kinect.header) return GetObjectPointCloudResponse(filtered_pc_map)
def _cloud_cb(self, cloud): points = np.array(list(read_points(cloud))) if points.shape[0] == 0: return pos = points[:,0:3] cor = np.reshape(points[:,-1], (points.shape[0], 1)) # Get 4x4 matrix which transforms point cloud co-ords to odometry frame try: points_to_map = self.tf.asMatrix('/lasths', cloud.header) except tf.ExtrapolationException: return transformed_points = points_to_map.dot(np.vstack((pos.T, np.ones((1, pos.shape[0]))))) transformed_points = transformed_points[:3,:].T self.seq += 1 header = Header() header.seq = self.seq header.stamp = rospy.Time.now() header.frame_id = '/lasths' self.cloud = np.vstack((self.cloud, np.hstack((transformed_points, cor)))) if self.seq % 30 == 0: print "plup!" self.cloud = np.zeros((0, 4)) output_cloud = create_cloud(header, cloud.fields, self.cloud) self.cloud_pub.publish(output_cloud)