def publish_pc2(pc, obj): global rospy_node import rospy import sensor_msgs.point_cloud2 as pc2 from sensor_msgs.msg import PointCloud2 import std_msgs if rospy_node is None: rospy_node = rospy.init_node("pc2_publisher") pub = rospy.Publisher("/points_raw", PointCloud2, queue_size=1000000) header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = "velodyne" points = pc2.create_cloud_xyz32(header, pc[:, :3]) pub2 = rospy.Publisher("/points_raw1", PointCloud2, queue_size=1000000) header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = "velodyne" points2 = pc2.create_cloud_xyz32(header, obj) # r = rospy.Rate(0.1) # while not rospy.is_shutdown(): pub.publish(points) pub2.publish(points2)
def computeICPBetweenScans(no1,no2, init_x = 0 , init_y = 0, init_yaw = 0): header = Header() header.stamp = rospy.Time.now() header.frame_id = 'ibeo' example = cython_catkin_example.PyCCExample() # test_cloud = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_'+str(no1)+'.txt') # test_cloud = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_filtered_'+str(no1)+'.txt') # test_cloud = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_direct_'+str(no1)+'.txt') # test_cloud = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_lm_filtered_'+str(no1)+'.txt') # test_cloud = read2DPointsFromTextFile('/home/avavav/avdata/alphard/onenorth/20150821-114839_sss/scan_filtered_'+str(no1)+'.txt') if opts.use_accumulated: test_cloud = read2DPointsFromTextFile(os.path.join(dir_prefix,'scan_lm_filtered_'+str(no1)+'.txt')) if opts.icp_use_filtered_data: test_cloud = read2DPointsFromTextFile(os.path.join(dir_prefix,'scan_filtered_'+str(no1)+'.txt')) else: test_cloud = read2DPointsFromTextFile(os.path.join(dir_prefix,'scan_'+str(no1)+'.txt')) H_points = [[p[0], p[1],1] for p in test_cloud] # H_points = [[np.float32(p[0]), np.float32(p[1]),1] for p in test_cloud] pc_point_t1 = pc2.create_cloud_xyz32(header, H_points) # print 'text1: ',len(test_cloud) # print test_cloud # example.load_2d_array('ref_map',np.array(test_cloud, np.float32)) # test_cloud2 = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_'+str(no2)+'.txt') # test_cloud2 = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_filtered_'+str(no2)+'.txt') # test_cloud = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_direct_'+str(no2)+'.txt') # test_cloud2 = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_lm_filtered_'+str(no2)+'.txt') # test_cloud2 = read2DPointsFromTextFile('/home/avavav/avdata/alphard/onenorth/20150821-114839_sss/scan_filtered_'+str(no2)+'.txt') if opts.use_accumulated: test_cloud = read2DPointsFromTextFile(os.path.join(dir_prefix,'scan_lm_filtered_'+str(no2)+'.txt')) if opts.icp_use_filtered_data: test_cloud2 = read2DPointsFromTextFile(os.path.join(dir_prefix,'scan_filtered_'+str(no2)+'.txt')) else: test_cloud2 = read2DPointsFromTextFile(os.path.join(dir_prefix,'scan_'+str(no2)+'.txt')) H_points = [[p[0], p[1],1] for p in test_cloud2] # H_points = [[np.float32(p[0]), np.float32(p[1]),1] for p in test_cloud] pc_point_t2 = pc2.create_cloud_xyz32(header, H_points) # print 'text2: ',len(test_cloud) # example.load_2d_array('que_map',np.array(test_cloud, np.float32)) # names = example.get_point_xyz_clouds_names() # print("Current point clouds: " + ",".join(names)) # icp_ros_result = g_processICP_srv(pc_point_t1, pc_point_t2) # print icp_ros_result # cython_icp_result = example.processICP(np.array(test_cloud, np.float32),np.array(test_cloud2, np.float32)) cython_icp_result = example.processICP(np.array([x[0] for x in test_cloud], np.float32),np.array([x[1] for x in test_cloud], np.float32),np.array([x[0] for x in test_cloud2], np.float32),np.array([x[1] for x in test_cloud2], np.float32), init_x, init_y, init_yaw) # print cython_icp_result return cython_icp_result
def est_sensory_function_cb(msg_var): global scaled_polygon_pcl_est_lvl, est_sensory_level, header, plot_image_publisher, est_sensory_function_current, list_current_position est_sensory_function_current = np.asarray(msg_var.data).reshape( (len(par.Xaxis), len(par.Xaxis))) est_points_lvl1 = [] est_points_lvl2 = [] est_points_lvl3 = [] est_points_lvl4 = [] est_points_lvl5 = [] maxSF = np.max(np.max(est_sensory_function_current)) minSF = np.min(np.min(est_sensory_function_current)) gapSF = (maxSF - minSF) / est_sensory_level est_threshold_lvl1_2 = minSF + gapSF est_threshold_lvl2_3 = minSF + 2 * gapSF est_threshold_lvl3_4 = minSF + 3 * gapSF est_threshold_lvl4_5 = minSF + 4 * gapSF for h in range(len(par.Xaxis)): for k in range(len(par.Yaxis)): if (est_sensory_function_current[k, h] < est_threshold_lvl1_2): est_points_lvl1.append([ par.Xaxis[h] * par.RealScale, par.Yaxis[k] * par.RealScale, par.StaticAltitude ]) elif (est_sensory_function_current[k, h] > est_threshold_lvl1_2) and (est_sensory_function_current[k, h] < est_threshold_lvl2_3): est_points_lvl2.append([ par.Xaxis[h] * par.RealScale, par.Yaxis[k] * par.RealScale, par.StaticAltitude ]) elif (est_sensory_function_current[k, h] > est_threshold_lvl2_3) and (est_sensory_function_current[k, h] < est_threshold_lvl3_4): est_points_lvl3.append([ par.Xaxis[h] * par.RealScale, par.Yaxis[k] * par.RealScale, par.StaticAltitude ]) elif (est_sensory_function_current[k, h] > est_threshold_lvl3_4) and (est_sensory_function_current[k, h] < est_threshold_lvl4_5): est_points_lvl4.append([ par.Xaxis[h] * par.RealScale, par.Yaxis[k] * par.RealScale, par.StaticAltitude ]) else: est_points_lvl5.append([ par.Xaxis[h] * par.RealScale, par.Yaxis[k] * par.RealScale, par.StaticAltitude ]) scaled_polygon_pcl_est_lvl[0] = pcl2.create_cloud_xyz32( header, est_points_lvl1) scaled_polygon_pcl_est_lvl[1] = pcl2.create_cloud_xyz32( header, est_points_lvl2) scaled_polygon_pcl_est_lvl[2] = pcl2.create_cloud_xyz32( header, est_points_lvl3) scaled_polygon_pcl_est_lvl[3] = pcl2.create_cloud_xyz32( header, est_points_lvl4) scaled_polygon_pcl_est_lvl[4] = pcl2.create_cloud_xyz32( header, est_points_lvl5)
def publish_three_pc(object1, object2): #, pc3): """publish two processed data at a time to the ROS node""" pub = rospy.Publisher("/pointcloud1", PointCloud2, queue_size=1000000) rospy.init_node("pc2_publisher") header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = "velodyne" points = pc2.create_cloud_xyz32(header, object1[:, :3]) pub_2 = rospy.Publisher("/pointcloud2", PointCloud2, queue_size=1000000) header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = "velodyne" points2 = pc2.create_cloud_xyz32(header, object2[:, :3]) # pub_3=rospy.Publisher("/pc3", PointCloud2, queue_size=1000000) # header = std_msgs.msg.Header() # header.stamp = rospy.Time.now() # header.frame_id = "velodyne" # points3 = pc2.create_cloud_xyz32(header, pc3[:, :3]) r = rospy.Rate(0.1) while not rospy.is_shutdown(): pub.publish(points) pub_2.publish(points2) #pub_3.publish(points3) r.sleep()
def talker(): rate = rospy.Rate(RATE_ROS_PUBLISHING) counter_index = NO_START_FRAME # 1#21#30#x1 pcloud = PointCloud2() header = Header() header.stamp = rospy.Time.now() header.frame_id = "velodyne" cloud = [] while not rospy.is_shutdown(): file_string = "scan" + format(counter_index, "03d") cloud_new, cloud_raw = publish_xyz(file_string, counter_index, force_2d=FORCE_2D) # cloud = cloud + cloud_new cloud = cloud_new print file_string counter_index = counter_index + 1 # rate.sleep() if not NO_ROS_PUBLISHING: pcloud = PointCloud2() header = Header() header.stamp = rospy.Time.now() header.frame_id = "velodyne" #'odom' #'pose' #'frame' #'velodyne' # header.frame_id = 'frame' #'odom' #'pose' #'frame' #'velodyne' # header.frame_id = 'odom' #'odom' #'pose' #'frame' #'velodyne' pcloud = pc2.create_cloud_xyz32(header, cloud) pub_cloud.publish(pcloud) header.frame_id = "odom" #'odom' #'pose' #'frame' #'velodyne' pcloud = pc2.create_cloud_xyz32(header, cloud_raw) pub_raw.publish(pcloud) if counter_index > NO_LAST_FRAME: # 47:#91:#65:#19 :#65: #65: # 20 :#65 :# 468: break counter_index = 1 rate.sleep() saveCloud("points.pts", cloud) # pcloud = pc2.create_cloud_xyz32(header, cloud) # print pcloud.header, pcloud.height, pcloud.width, pcloud.point_step, pcloud.row_step, pcloud.fields # pub_cloud.publish(pcloud) # rospy.spin() # pos = np.random.random(size=(100,3)) # print cloud pos = np.matrix(cloud) # pos *= [10,-10,10] # pos[0] = (0,0,0) # color = np.ones((pos.shape[0], 4)) # d2 = (pos**2).sum(axis=1)**0.5 # size = np.random.random(size=pos.shape[0])*10 size = np.ones((1, pos.shape[0])) sp2 = gl.GLScatterPlotItem(pos=pos, color=(1, 1, 1, 1), size=size) w.addItem(sp2) if (sys.flags.interactive != 1) or not hasattr(QtCore, "PYQT_VERSION"): QtGui.QApplication.instance().exec_()
def FilterC(self): pontos = pc2.read_points_list(self.old_cloud, field_names=("x", "y", "z"), skip_nans=True) new_pontos = list() d_max = 10 d_trig = 1.5 for k in range(270): encontrou = False for p in pontos: if ((180 * atan2(p[1], p[0]) / pi) - (k - 135))**2 < 0.01: Point = namedtuple("Point", ("x", "y", "z")) l = (p[0], p[1], 0) new_p = Point._make(l) new_pontos.append(new_p) encontrou = True break if not encontrou: Point = namedtuple("Point", ("x", "y", "z")) l = (d_max * cos(pi * (k - 135) / 180), d_max * sin(pi * (k - 135) / 180), 0) new_p = Point._make(l) new_pontos.append(new_p) disc = list() # p = new_pontos[0] # if sqrt(p[0]**2 + p[1]**2) < d_max: # disc.append(p) d_max = 9 for k1 in range(1, 269): k2 = k1 + 1 p1 = new_pontos[k1] p2 = new_pontos[k2] d_p1 = sqrt(p1[0]**2 + p1[1]**2) d_p2 = sqrt(p2[0]**2 + p2[1]**2) if d_p1 == d_max and d_p2 < d_max: disc.append(p2) elif abs(d_p1 - d_p2) >= d_trig: if d_p1 < d_p2: disc.append(p1) else: disc.append(p2) # p = new_pontos[269] # if sqrt(p[0]**2 + p[1]**2) < d_max: # disc.append(p) print(len(disc)) return (pc2.create_cloud_xyz32(self.old_cloud.header, new_pontos), pc2.create_cloud_xyz32(self.old_cloud.header, disc))
def processPose2d(msg): global g_point_cloud_built global g_pcloud global g_pose global g_dist_accum global g_last_dist_accum global g_last_x global g_last_y global g_last_captured_x global g_last_captured_y global g_last_captured_yaw g_pose = msg quat = [msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w] euler = tf.transformations.euler_from_quaternion(quat) yaw = -euler[2] g_dist_accum = g_dist_accum + math.hypot(msg.pose.pose.position.x - g_last_x, msg.pose.pose.position.y - g_last_y) g_last_x = msg.pose.pose.position.x g_last_y = msg.pose.pose.position.y if g_dist_accum > g_last_dist_accum + g_dist_thres: g_last_dist_accum = g_last_dist_accum + g_dist_thres tranformation_matrix = numpy.matrix([[math.cos(yaw),math.sin(yaw),msg.pose.pose.position.x],[-math.sin(yaw),math.cos(yaw),msg.pose.pose.position.y],[0,0,1]]) # tranformation_matrix = numpy.matrix([[math.cos(yaw),math.sin(yaw),0],[-math.sin(yaw),math.cos(yaw),0],[0,0,1]]) # tranformation_matrix = numpy.matrix([[1,0,-msg.pose.pose.position.x],[0,1,-msg.pose.pose.position.y],[0,0,1]]) header = Header() header.stamp = rospy.Time.now() header.frame_id = 'ibeo' H_points = [[p[0], p[1],1] for p in g_points_new_filtered] M_points = numpy.matrix(H_points) # print M_points point_t = tranformation_matrix *M_points.getT() point_t = point_t.getT().tolist() g_point_cloud_built = g_point_cloud_built + point_t # g_point_cloud_built = point_t g_pcloud = pc2.create_cloud_xyz32(header, g_point_cloud_built) pc_point_t = pc2.create_cloud_xyz32(header, point_t) observed_x = -g_last_captured_x + msg.pose.pose.position.x observed_y = -g_last_captured_y + msg.pose.pose.position.y observed_x_local = math.cos(-yaw)*observed_x + math.sin(-yaw)*observed_y observed_y_local = -math.sin(-yaw)*observed_x + math.cos(-yaw)*observed_y print msg.pose.pose.position.x, msg.pose.pose.position.y, yaw, observed_x_local,observed_y_local,-g_last_captured_yaw +yaw, msg.pose.covariance[0], msg.pose.covariance[1],msg.pose.covariance[2],msg.pose.covariance[3],msg.pose.covariance[4], msg.pose.covariance[5] g_pub_cloud.publish(g_pcloud) g_pub_cloud_only_current_filtered.publish(pc_point_t) H_points = [[p[0], p[1],1] for p in g_points_new] M_points = numpy.matrix(H_points) point_t = tranformation_matrix *M_points.getT() point_t = point_t.getT().tolist() pc_point_t = pc2.create_cloud_xyz32(header, point_t) g_pub_cloud_only_current.publish(pc_point_t) g_last_captured_x = msg.pose.pose.position.x g_last_captured_y = msg.pose.pose.position.y g_last_captured_yaw = yaw pass
def publish_transformed_cloud(cloud_array, flag=True): header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'map' if not flag: header.frame_id = 'sensor/side_camera_depth_optical_frame' cloud = pcl2.create_cloud_xyz32(header, cloud_array) pub2.publish(cloud) else: cloud = pcl2.create_cloud_xyz32(header, cloud_array) pub1.publish(cloud) return
def callback(self, msg): cloud = pointcloud2_2_npxyz(msg) if np.isnan(cloud).sum() > 0: raise ValueError('Cloud should have no Nans') ground, obst = self.compute(cloud.squeeze()) ground_msg = pc2.create_cloud_xyz32(msg.header, ground) obst_msg = pc2.create_cloud_xyz32(msg.header, obst) self.pub_ground.publish(ground_msg) self.pub_obst.publish(obst_msg)
def cloud_callback(self, data): """process the cloud""" # get points from the subscribed pointcloud # for some reason storing pc into Pcam is working with point cloud that is voxel filtered. Investigate!! pc = ros_numpy.numpify(data) Pcam = np.zeros((pc.shape[0], 3)) Pcam[:, 0] = pc['x'] Pcam[:, 1] = pc['y'] Pcam[:, 2] = pc['z'] # make a cloud of points on the max z face of the camera xx, yy = np.meshgrid(self.x, self.y) xvalues = xx.ravel() yvalues = yy.ravel() zvalues = np.ones(len(xvalues)) * self.SensorRange Pgrid = np.zeros((len(xvalues), 3)) Pgrid[:, 0] = xvalues Pgrid[:, 1] = yvalues Pgrid[:, 2] = zvalues # project existing points to the plane containing new points header = std_msgs.msg.Header() header.stamp = data.header.stamp #rospy.Time.now() header.frame_id = 'world' #header.frame_id = 'camera_color_optical_frame' #total_points = np.concatenate((Pcam, Pgrid), 0) #p = pc2.create_cloud_xyz32(header, total_points) if len(Pcam) != 0: Pprojected = self.camera_location + np.array( [(Pcam[i] - self.camera_location) * self.SensorRange / Pcam[i, 2] for i in range(len(Pcam))]) #projected_points = self.camera_location + map(lambda i, j : (i-self.camera_location)*self.SensorRange/j, points, points[:,2]) PCprojected = pcl_cloud(np.array(Pprojected, dtype=np.float32)) PCgrid = pcl_cloud(np.array(Pgrid, dtype=np.float32)) kdt = pcl.KdTreeFLANN(PCgrid) indices, sqr_distances = kdt.nearest_k_search_for_cloud( PCprojected, 10) Pgrid_modified = np.delete(Pgrid, indices, axis=0) total_points = np.concatenate((Pcam, Pgrid_modified), 0) p = pc2.create_cloud_xyz32(header, total_points) else: #total_points = np.concatenate((Pcam, Pgrid), 0) p = pc2.create_cloud_xyz32(header, Pgrid) self.pcl_pub.publish(p)
def createImageEdges(self, image, drone3dPosInMap, stamp): ##Show image edges as 3D Points imgHeight, imgWidth, channels = image.shape # upperleft ptul2D = [1, 1] # upperleft ptur2D = [1, imgHeight] # upperleft ptll2D = [imgWidth, 1] # upperleft ptlr2D = [imgWidth, imgHeight] # Create Points which mark the edges of the camerapic in the world (RVIZ) ptul3D = imagePointTransformations.getPixel3DPosOnWater(ptul2D, drone3dPosInMap, self.camModel, stamp) # upperright ptur3D = imagePointTransformations.getPixel3DPosOnWater(ptur2D, drone3dPosInMap, self.camModel, stamp) # lowerleft ptll3D = imagePointTransformations.getPixel3DPosOnWater(ptll2D, drone3dPosInMap, self.camModel, stamp) # lowerright ptlr3D = imagePointTransformations.getPixel3DPosOnWater(ptlr2D, drone3dPosInMap, self.camModel, stamp) pixelProjection3DPoints = [ptul3D, ptur3D, ptll3D, ptlr3D] header = Header() header.frame_id = "map" header.stamp = stamp image3dProjection = PointCloud2() image3dProjection = pc2.create_cloud_xyz32(header, pixelProjection3DPoints) self.picProjPub.publish(image3dProjection)
def convert_to_pcl2(msg): global pcl_points, listener, tot, bag if msg.range < 0.01: # to filter out infinity return pts = PointStamped() pts.header.frame_id = 'laser_frame' pts.header.stamp = msg.header.stamp # print msg.header.stamp.secs, pts.point.x = msg.range # print pts.header.frame_id, pts.point.x, pts.point.y, pts.point.z, # print 'time:', pts.header.stamp.secs # start = time.time() listener.waitForTransform( "laser_frame", "world", rospy.Time().now(), rospy.Duration(0.5)) pts2 = listener.transformPoint('world', pts) # diff = time.time() - start # tot += diff # print diff, tot # print pts2.header.frame_id, pts2.point.x, pts2.point.y, pts2.point.z pcl_points.append([pts2.point.x, pts2.point.y, pts2.point.z]) # if len(pcl_points) > 200: # pcl_points.pop(0) pcl = pc2.create_cloud_xyz32(pts2.header, pcl_points[-201:-1]) pub.publish(pcl)
def goal_callback(self, status, result): rospy.loginfo("Goal callback %s %s", status, result) x, y, w = self.goal if status == GoalStatus.SUCCEEDED: self.measurements.append({ "depth": self.depth, "timestamp": datetime.now().replace(microsecond=0).isoformat(), "x": x, "y": y }) self.points.append([x, y, self.depth]) header = Header(frame_id="map", stamp=rospy.Time.now()) msg = point_cloud2.create_cloud_xyz32(header, self.points) self.point_pub.publish(msg) self.goal = self.next_goal(self.goal) rospy.loginfo("Next goal %s", self.goal) self.send_goal()
def publish_walls(): #build the point cloud imagining we are at 0,0,0. cloud_points = [] if(wall_left>0): cloud_points.append([0, 13.0/wall_left, 0.5]) if(wall_frontleft>0): cloud_points.append([13.0/wall_frontleft, 0.5, 0.5]) if(wall_frontright>0): cloud_points.append([13.0/wall_frontright, -0.5, 0.5]) if(wall_right>0): cloud_points.append([0, -13.0/wall_right, 0.5]) header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'world' mypointcloud = pcl2.create_cloud_xyz32(header, cloud_points) #We need to build a transformation of our location. t = geometry_msgs.msg.TransformStamped() t.header.stamp = rospy.Time.now() t.header.frame_id = "world" t.child_frame_id = "zumo" t.transform.translation.x = transx t.transform.translation.y = transy t.transform.translation.z = transz t.transform.rotation.x = rotx t.transform.rotation.y = roty t.transform.rotation.z = rotz t.transform.rotation.w = rotw #convert the pointcloud to take into account our location. cloud_out = do_transform_cloud(mypointcloud, t) pcl_pub.publish(cloud_out)
def processing_xyz(self): for i in range(self.len_files): print("[+] {} th file name : {} ".format( i, self.npy_files_list_xyz[i])) bin_points = np.fromfile(os.path.join(self.npy_path, self.npy_files_list_xyz[i]), dtype=np.float32).reshape(-1, 4) pc2_msg = PointCloud2() header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'velodyne_link' cloud_points = [] for j in range(len(bin_points)): if (bin_points[j][0] >= 0.1 and bin_points[j][1] != 0 and bin_points[j][2] <= 5 and bin_points[j][3] < 10): cloud_points.append(list(bin_points[j, :3])) pc2_msg = pcl2.create_cloud_xyz32(header, cloud_points) # ed: /velodyne_points_npy publish self.velo_pub.publish(pc2_msg) self.loop_rate.sleep() if rospy.is_shutdown(): return
def generatePointCloud2(xyzreader, fileId): cloud_stamp = 0 points = [] i = 0 for row in xyzreader: if i == 0: i += 1 continue if i == 1: cloud_stamp = row[0] x = float(row[1]) y = float(row[2]) z = float(row[3]) p = [x, y, z] points.append(p) header = Header() header.frame_id = "map" if cloud_interval == 0: times = cloud_stamp.split(".") header.stamp.secs = int(times[0]) header.stamp.nsecs = int(times[1]) else: header.stamp.secs = 1 + cloud_interval * fileId header.stamp.nsecs = 0 pc = pc2.create_cloud_xyz32(header, points) return pc
def pub_point_cloud(self, stamp, profile3d): self.sequence = self.sequence + 1 self.pcloud = pc2.create_cloud_xyz32(self.pcloud.header, profile3d) self.pcloud.header = std_msgs.msg.Header(frame_id="/camera0", stamp=stamp, seq=self.sequence) self.cloud_pub.publish(self.pcloud)
def Visualize(filename): data_path = param.DATA_DIR + filename + ".pcd" calib_path = param.CALIB_DIR + filename + ".txt" label_path = param.LABEL_DIR + filename + ".txt" print "data path : " + data_path print "calib path : " + calib_path print "label path : " + label_path pc_pub = rospy.Publisher("cnn_3d_points_raw", PointCloud2, queue_size=100000) rospy.init_node("cnn_3d") header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = "cnn_3d" marker_array_pub = rospy.Publisher("cnn_3d_anchor_obj", MarkerArray, queue_size=1000) sleep_rate = rospy.Rate(1) point_cloud, gt_objectness = Loader.get_visualize_input( data_path, calib_path, label_path) points = pc2.create_cloud_xyz32(header, point_cloud[:, :3]) marker_array = Utils.get_marker_array(gt_objectness, type="anchor") while not rospy.is_shutdown(): pc_pub.publish(points) marker_array_pub.publish(marker_array) sleep_rate.sleep()
def downsample_and_publish(self): while self.received_frames < self.max_frames: # wait until cloud_callback processes 8 messages pass downsampled_cloud = pcl.PointCloud() downsampled_cloud.from_array( np.asarray(self.input_cloud, dtype=np.float32)) self.input_cloud = [] # clean input cloud self.received_frames = 0 # allow to copy next 8 msgs chunk pass_fill = downsampled_cloud.make_passthrough_filter() pass_fill.set_filter_field_name("z") pass_fill.set_filter_limits(0, 1) downsampled_cloud = pass_fill.filter() sor = downsampled_cloud.make_voxel_grid_filter() sor.set_leaf_size(0.005, 0.005, 0.005) downsampled_cloud = sor.filter() msg_header = Header() msg_header.frame_id = "xtion_rgb_optical_frame" self.pub.publish( point_cloud2.create_cloud_xyz32(msg_header, downsampled_cloud.to_list()))
def findMovement(self, data, publisher): m = Munkres() print "processing new scan" convert_data = self.xyConvert(data) if not (len(self.filter_scans) < self.filter_size): print "starting filter" self.update_filter(convert_data) print "adding odom" odom_scan = self.addOdom() matches = [[] for ii in range(len(self.filter_scans[1]))] for ii in range(1, self.filter_size - 1): print "calculating cost mat" cost_mat = self.makeCostMatrix(self.filter_scans[0], self.filter_scans[ii]) print len(cost_mat) print "doing matching" #indexes = m.compute(cost_mat) row_ind, col_ind = linear_sum_assignment(cost_mat) print "done matching for scan: ", ii #for item in indexes: # matches[item[0]].append(self.filter_scans[ii][item[1]]) for jj, item in enumerate(row_ind): matches[item].append(self.filter_scans[ii][jj]) matched_scans = [] print "processing" for item in matches: res = apply(zip, item) matched_scans.append(res) filtered_points = [] print "computing avg and std" for item in matched_scans: x_avg = np.mean(item[0]) x_std = np.std(item[0]) y_avg = np.mean(item[1]) y_std = np.std(item[1]) if x_std > self.max_var and y_std > self.max_var: filtered_points.append([x_avg, y_avg, 0]) print filtered_points pc_cloud = PointCloud2() pc_cloud = pc2.create_cloud_xyz32(pc_cloud.header, filtered_points) publisher.publish(pc_cloud) print "ending filter" else: print "building filter" self.filter_scans.append(convert_data)
def callback(msg): angle_rad = msg.angle_min #assign first angle angle_deg = angle_rad * 57.2957795 #angle in degrees rangingsCount = len(msg.ranges) #number of rangings[usually 360] print("[" + str(rangingsCount) + "] rangings\n") pc_array = [] for r in range(rangingsCount): #print("[" + str(r) + "] rad: " + str(angle_rad)) #print("deg: " + str(angle_deg)) #print("dist: " + str(msg.ranges[r])) angle_rad += msg.angle_increment #new angle calculation angle_deg = angle_rad * 57.2957795 #angle in deg point_x = math.cos(angle_rad) * msg.ranges[r] point_y = math.sin(angle_rad) * msg.ranges[r] #print("[X;Y]: [" + str(point_x) + ";" + str(point_y) +"]\n") pc_array.append((point_x, point_y, 0)) sem = pc2.create_cloud_xyz32(msg.header, pc_array) #now we need to delete points from map where we aim #rospy.wait_for_service('clear_bbx', 1) #clear_bbx = rospy.ServiceProxy(SRV_NAME, SRV_INTERFACE) #try: # #resp1 = clear_bbx(bounding_box) # resp1 = clear_bbx(min, max) #except rospy.ServiceException as exc: # print("Service did not process request: " + str(exc)) #send new points pub.publish(sem)
def publish_point_cloud(pcl_pub, point_cloud): rospy.loginfo("point cloud published") header = Header() header.stamp = rospy.Time.now() header.frame_id = 'map' pcl_pub.publish(pcl2.create_cloud_xyz32(header, point_cloud[:, :3]))
def PC_reduc_sep(self, bbox, CloudImage): cv_image = np.array( self.bridge.imgmsg_to_cv2(CloudImage), dtype='float32') # Image with distance in channels x,y,z pc_list = np.reshape( np.reshape(cv_image, (CloudImage.height, CloudImage.width * 3)).T, CloudImage.height * CloudImage.width * 3) if bbox != []: cx = bbox.bbox.center.x sx = bbox.bbox.size_y cy = bbox.bbox.center.y sy = bbox.bbox.size_y Start_x = int(cx - sx / 2) End_x = int(cx + sx / 2) Start_y = int(cy - sy / 2) End_y = int(cy + sy / 2) bbox_i = [] time1 = rospy.Time.now().to_sec() for y in range(Start_y, End_y): bbox_i += list( range((y * 672 + Start_x) * 3, (y * 672 + End_x + 1) * 3)) pc_list = np.delete(pc_list, bbox_i) time2 = rospy.Time.now().to_sec() time3 = rospy.Time.now().to_sec() pc_list = pc_list.reshape(int(len(pc_list) / 3), 3) pc_list = pc_list[~np.isnan(pc_list).any(axis=1)] pc_list = pc_list[~np.isinf(pc_list).any(axis=1)] time4 = rospy.Time.now().to_sec() print(pc_list.shape) Reduced_PC2 = pc2.create_cloud_xyz32(CloudImage.header, pc_list) return Reduced_PC2
def align_gt_object(gt_object_filepath, downsample_factor=10, rate=5): ply = curvox.mesh_conversions.read_mesh_msg_from_ply_filepath(gt_object_filepath) points = [] for i in range(len(ply.vertices)): if i % downsample_factor == 0: points.append((ply.vertices[i].x, ply.vertices[i].y, ply.vertices[i].z)) header = std_msgs.msg.Header() header.frame_id = '/gt_object' pcl_arr = pcl2.create_cloud_xyz32(header, np.array(points)) pc_pub = rospy.Publisher('/gt_overlay', sensor_msgs.msg.PointCloud2, queue_size=10) r = rospy.Rate(rate) tfl = tf.TransformListener() for i in range(1000): tfl.waitForTransform("/base_link", "/gt_object", rospy.Time(0), rospy.Duration(4)) tran, rot = tfl.lookupTransform("/base_link", "/gt_object", rospy.Time(0)) print '<node pkg="lcsr_tf_tools" type="interactive_transform_publisher" name="static_tf_world_to_gt" args="{} {} {} {} {} {} {} /base_link /gt_object 100"/>'.format( tran[0], tran[1], tran[2], rot[0], rot[1], rot[2], rot[3]) print 'object_ps.pose = Pose(Point({}, {}, {}), Quaternion({}, {}, {}, {}))'.format(tran[0], tran[1], tran[2], rot[0], rot[1], rot[2], rot[3]) pc_pub.publish(pcl_arr) r.sleep()
def camera_callback(self, pcd_msg): # transform frame trans = self.tf_buffer.lookup_transform('world', pcd_msg.header.frame_id, rospy.Time(0), rospy.Duration(1)) # ask for last known info points = do_transform_cloud(pcd_msg, trans) if self.filter_state == self.NO_FILTER: print('##########NOT filtering point cloud########') points.header = std_msgs.msg.Header() points.header.stamp = rospy.Time.now() points.header.frame_id = 'world' self.pcd_pub.publish(points) else: print('##########filtering point cloud########') pcd = list(pcl2.read_points(points, field_names = ("x", "y", "z"), skip_nans=True)) pcd = np.array(pcd) pcd_add_axis = np.ones((len(pcd),4)) pcd_add_axis[:,:3] = pcd pcd_transformed = self.filter_transform.dot(pcd_add_axis.T).T pcd_transformed = pcd_transformed[:,:3] box_mask_min = np.prod((pcd_transformed - self.filter_min_xyz) >= 0, axis=1) # AND across x-y-z axis box_mask_max = np.prod((self.filter_max_xyz - pcd_transformed) >= 0, axis=1) # AND across x-y-z axis box_mask = box_mask_min * box_mask_max # should satisfy both min and max constraints to be inside the box # anything else stays # filter by the mask print('before filter: pcd length: %d' % (len(pcd))) pcd = pcd[box_mask == 0] # anything not inside the box print('after filter: pcd length: %d' % (len(pcd))) header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'world' #create pcl from points filtered_pcd = pcl2.create_cloud_xyz32(header, pcd) #publish self.pcd_pub.publish(filtered_pcd)
def convertToPointCloud2(self, cloud, normals=None): ''' Transform the Infinitam volume to a point cloud in the /base frame. @type cloud: @param cloud: ''' header = HeaderMsg() header.frame_id = "/base" header.stamp = rospy.Time.now() if normals is None: return point_cloud2.create_cloud_xyz32(header, cloud) # concatenate xyz and normals vertically pts = numpy.zeros((cloud.shape[0],6)) pts[:,0:3] = cloud[:,0:3] pts[:,3:6] = normals[:,0:3] # create message fields = [PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1), PointField('normal_x', 12, PointField.FLOAT32, 1), PointField('normal_y', 16, PointField.FLOAT32, 1), PointField('normal_z', 20, PointField.FLOAT32, 1)] return point_cloud2.create_cloud(header, fields, pts)
def publishFrontiers(frontier): delta = 0.25 size = 100 cloud = [] for frontierCell in frontier: point = [] i = frontierCell[0] j = frontierCell[1] x = (-100 + j * delta) + delta / 2 y = (-100 + i * delta) + delta / 2 point.append(x) point.append(y) point.append(0.0) cloud.append(point) cloud_points = cloud pcl_pub = rospy.Publisher("/frontiers", PointCloud2, queue_size=10) rospy.sleep(1.) header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'map' #create pcl from points scaled_polygon_pcl = pcl2.create_cloud_xyz32(header, cloud_points) #publish rospy.loginfo("Publishing Frontiers.. !") pcl_pub.publish(scaled_polygon_pcl)
def callback(self, data): if len(self.plane) != 0: #define cloud in readable format and save to obejct field self.cloud = [] for p in pc2.read_points(data, field_names=("x", "y", "z"), skip_nans=True): #print "x : %f y : %f z : %f" %(p[0],p[1],p[2]) self.cloud.append(p) #print len(self.cloud) #filter using given intial rectangular region #print "old array:" l = len(data.data) for i in range(len(self.cloud) - 1, -1, -1): if not self.is_inside_region(self.cloud[i]): del self.cloud[i] print "new array:" self.header.stamp = rospy.Time.now() self.header.seq += 1 newCloud = pc2.create_cloud_xyz32(self.header, self.cloud) print newCloud pub.publish(newCloud) #print newCloud.header #print newCloud.fields print "old legnth: " + str(l) print "new length: " + str(len(self.cloud))
def process_radar_tracks(msg): assert msg._md5sum == '6a2de2f790cb8bb0e149d45d297462f8' tracks = RadarObservation.from_msg(msg) num_tracks = len(tracks) acc = [] cloud = np.zeros([num_tracks, 3], dtype=np.float32) for i, track in enumerate(tracks): cloud[i] = [track.x, track.y, track.z] - np.array(RADAR_TO_LIDAR) if np.abs(track.y) < 2: #acc.append(track.x) #acc.append(track.power) print track.vx * 3.7, track.vy * 3.7 #print x, y, z #print acc #msg.header.stamp header = Header() header.stamp = msg.header.stamp header.frame_id = 'velodyne' cloud_msg = pc2.create_cloud_xyz32(header, cloud) cloud_msg.width = 1 cloud_msg.height = num_tracks rospy.Publisher('radar_points', PointCloud2, queue_size=1).publish(cloud_msg)
def publish_static_robots(self, force_clear): with self.neighbors_lock: time = rospy.Time.now() cloud_points = [] if self.me is None or (time - self.me.header.stamp ).to_sec() > last_seen_threshold: return my_pose = self.me.pose.pose _, _, my_theta = tf.transformations.euler_from_quaternion( quat_array_from_msg(my_pose.orientation)) change = False for name in self.neighbors: if (time - self.neighbors[name]['last_seen']).to_sec() < last_seen_threshold \ and ((abs(self.neighbors[name]['twist'].twist.linear.x) < 0.05 and abs( self.neighbors[name]['twist'].twist.linear.y) < 0.05)): if not self.neighbors[name]['stationary']: change = True self.neighbors[name]['stationary'] = True cur_pose = self.neighbors[name]['position'].pose _, _, cur_theta = tf.transformations.euler_from_quaternion( quat_array_from_msg(cur_pose.orientation)) relative_pose_x = cur_pose.position.x - my_pose.position.x relative_pose_y = cur_pose.position.y - my_pose.position.y xform_foot = make_rotation_transformation(cur_theta - my_theta) xform_to_baselink = make_rotation_transformation(-my_theta) pos_rel = xform_to_baselink( (relative_pose_x, relative_pose_y)) for p in self.neighbors[name]['footprint'].polygon.points: p_x = p.x p_y = p.y p_rotated = xform_foot((p_x, p_y)) p_x = STATIC_SCALE * p_rotated[0] + pos_rel[0] p_y = STATIC_SCALE * p_rotated[1] + pos_rel[1] cloud_points.append((p_x, p_y, Z_HEIGHT)) for _ in range(5): scale = random.uniform(0.98, 1.02) cloud_points.append( (scale * p_x, scale * p_y, Z_HEIGHT)) else: if self.neighbors[name]['stationary']: self.neighbors[name]['stationary'] = False change = True if change or force_clear: self.clearing_laser_scan.header.stamp = self.me.header.stamp self.clearing_laser_pub.publish(self.clearing_laser_scan) static_robots_cloud = pcl2.create_cloud_xyz32( self.cloud_header, cloud_points) static_robots_cloud.header.stamp = self.me.header.stamp self.point_cloud_pub.publish(static_robots_cloud)
def PC_reduc_seg(bbox, Segmented_labels, pc_list, cloud): if bbox == []: reduced_PC2 = cloud else: """ Start_x = Target[TargetOrder("Start_x")] Start_y = Target[TargetOrder("Start_y")] End_x = Target[TargetOrder("End_x")] End_y = Target[TargetOrder("End_y")] """ Start_x = bbox[0] End_x = bbox[2] Start_y = bbox[1] End_y = bbox[3] bbox_i = [] Seg_index = 0 for y in range(Start_y, End_y): for x in range(Start_x, End_x): pc_index = (y * 672 + x) * 3 if Segmented_labels[Seg_index] == True: for i in [0, 1, 2]: bbox_i.append(pc_index + i) Seg_index += 1 #print(pc_list.shape) pc_list = np.delete(pc_list, bbox_i) #print(pc_list.shape) pc_list = pc_list.reshape(int(len(pc_list) / 3), 3) pc_list = pc_list[~np.isnan(pc_list).any(axis=1)] pc_list = pc_list[~np.isinf(pc_list).any(axis=1)] header = cloud.header Reduced_PC2 = pc2.create_cloud_xyz32(header, pc_list) return Reduced_PC2
def publish_walls(): cloud_points=[] if(wall_left>0):cloud_points.append([0, 13.0/wall_left, 0.5]) if(wall_frontleft>0):cloud_points.append([13.0/wall_frontleft, 0.5, 0.5]) if(wall_frontright>0):cloud_points.append([13.0/wall_frontright, -0.5, 0.5]) if(wall_right>0):cloud_points.append([0,-13.0/wall_right, 0.5]) header= std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'world' mypointcloud = pcl2.create_cloud_xyz32(header, cloud_points) t = geometry_msgs.msg.TransformStamped() t.header.stamp = rospy.Time.now() t.header.frame_id = 'world' t.child_frame_id = 'zumo' t.transform.translation.x = transx t.transform.translation.y = transy t.transform.translation.z = transz t.transform.rotation.x = rotx t.transform.rotation.y = roty t.transform.rotation.z = rotz t.transform.rotation.w = rotw cloud_out = do_transform_cloud(mypointcloud, t) pcl_pub.publish(cloud_out)
def generatePointCloud2(points): header = Header() header.frame_id = "map" header.stamp.secs = 1 header.stamp.nsecs = 0 pc = pc2.create_cloud_xyz32(header, points) return pc
def talker(): header = Header() header.stamp = rospy.Time.now() header.frame_id = 'map' g_pcloud = pc2.create_cloud_xyz32(header, wmap) g_pub_map.publish(g_pcloud) rospy.spin()
def publish_clusters(publisher, header, points, corners): clusters = [] num_boxes = len(corners) / 8 init = False for i in range(num_boxes): corner = corners[i * 8:(i + 1) * 8] # print corner clusters_idx = np.logical_and((points[:, 0] > min(corner[:, 0])), (points[:, 0] < max(corner[:, 0]))) clusters_tmp = points[clusters_idx] clusters_idx = np.logical_and( (clusters_tmp[:, 1] > min(corner[:, 1])), (clusters_tmp[:, 1] < max(corner[:, 1]))) clusters_tmp = clusters_tmp[clusters_idx] clusters_idx = np.logical_and( (clusters_tmp[:, 2] > min(corner[:, 2])), (clusters_tmp[:, 2] < max(corner[:, 2]))) clusters_tmp = clusters_tmp[clusters_idx] if not init: clusters = clusters_tmp init = True else: # np.append ==> 1D clusters = np.append(clusters, clusters_tmp).reshape(-1, 4) # print clusters msg_clusters = pc2.create_cloud_xyz32(header, clusters[:, :3]) publisher.publish(msg_clusters)
def get_pointcloud2(self, grid, position, target_frame, transform, range=12.0): header = std_msg.Header(0, rospy.Time.now(), target_frame) grid_np = np.array(grid.data, dtype='u1').reshape((grid.info.height,grid.info.width)) origin = np.array((grid.info.origin.position.x, grid.info.origin.position.y, grid.info.origin.position.z)) world2map = lambda x:np.clip(trunc((x-origin)/grid.info.resolution), zeros((3,)), np.r_[grid.info.width-1, grid.info.height-1,0]) ll = world2map(np.r_[position.x-range, position.y-range, 0]) ur = world2map(np.r_[position.x+range, position.y+range, 0]) submap = grid_np[ll[1]:ur[1],ll[0]:ur[0]] mappts = np.c_[np.where(submap==100)][:,::-1] + ll[:2] map2world = lambda x:(x+0.5)*grid.info.resolution+origin[:2] wpts = np.array([map2world(x) for x in mappts]) pcpts=[] if len(wpts>0): for z in np.linspace(0,1,11): pcpts.append(np.c_[wpts,z*ones_like(wpts[:,0])]) pcpts = np.vstack(pcpts) pcpts = np.einsum("ki,...ji", transform, np.c_[pcpts,np.ones((len(pcpts),1))])[:,:3] else: pcpts = [] pc = pc2.create_cloud_xyz32(header, pcpts) return pc
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 publishScan(no): header = Header() header.stamp = rospy.Time.now() header.frame_id = 'ibeo' test_cloud = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_'+str(no)+'.txt') H_points = [[p[0], p[1],1] for p in test_cloud] pc_point_t = pc2.create_cloud_xyz32(header, H_points) g_pub_ros.publish(pc_point_t)
def talker(): header = Header() header.stamp = rospy.Time.now() # header.frame_id = 'velodyne' #'map' header.frame_id = "world" #'map' wmap = getCloudFromFile(opts.filename) g_pcloud = pc2.create_cloud_xyz32(header, wmap) g_pub_map.publish(g_pcloud) rospy.spin()
def pub_point_cloud(self, stamp, profile3d): # ERROR: Calibration done in meters #cloud = profile3d * 0.001 # Conversion from milimeters to meters self.sequence = self.sequence + 1 self.pcloud = pc2.create_cloud_xyz32(self.pcloud.header, profile3d) self.pcloud.header = std_msgs.msg.Header(frame_id="/camera0", stamp=stamp, seq=self.sequence) self.cloud_pub.publish(self.pcloud)
def publish_obstacles(self, data): for obs in data['obstacles']: pose = self.create_pose(obs[0], obs[1], obs[2]) self.publishers['obstacle'].publish(pose) header = Header() header.stamp = rospy.Time.now() header.frame_id = '/world' cloud = pcl2.create_cloud_xyz32(header, data['obstacles']) self.publishers['obstacle_points'].publish(cloud)
def mat2pointcloud(filename): m = scipy.io.loadmat(filename) scan = numpy.transpose(m['SCAN']['XYZ'][0][0]).astype(numpy.float32) stamp = m['SCAN']['timestamp_laser'][0][0][0][0] * 1e-6 cloud = PointCloud2() cloud.header.stamp = rospy.Time.from_sec(stamp) cloud.header.frame_id = 'velodyne' cloud = pc2.create_cloud_xyz32(cloud.header, scan) return cloud
def visualize_pc(pc): pcl_pub = rospy.Publisher("/custom_pc", PointCloud2, queue_size=10) points = pc.tolist() rate = rospy.Rate(1) while not rospy.is_shutdown(): header = msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'base' point_cloud = pc2.create_cloud_xyz32(header, points) pcl_pub.publish(point_cloud) rate.sleep()
def mouseMoveEvent(self, e): pos = np.array(e.pos().toTuple()) pos[1] = self.size().height() - pos[1] # make +y up pos_world = pos * self.scale + self.offset cloud = create_cloud_xyz32( rospy.Header(frame_id=self.frame_id, stamp=rospy.Time.now()), np.array([[ pos_world[0], pos_world[1], 0 ]])) self.point_pub.publish(cloud)
def _compute_sensor_msg(self, sensor_data, cur_time): header = Header() header.frame_id = self.name header.stamp = cur_time # we take the oposite of y axis (as lidar point are express in left handed coordinate system, and ros need right handed) # we need a copy here, because the data are read only in carla numpy array new_sensor_data = sensor_data.data.copy() new_sensor_data = -new_sensor_data # we also need to permute x and y new_sensor_data = new_sensor_data[..., [1, 0, 2]] point_cloud_msg = create_cloud_xyz32(header, new_sensor_data) topic = self.name self.process_msg_fun(topic, point_cloud_msg)
def callback(msg): pcloud = PointCloud2() pcloud.header.frame_id = '/radar' cloud = [] for ii in range(0,64): x = msg.range[ii] * cos(deg2rad(msg.angle[ii])) y = msg.range[ii] * sin(deg2rad(msg.angle[ii])) z = 0.0 cloud.append([x,y,z]) pcloud = pc2.create_cloud_xyz32(pcloud.header, cloud) pc_pub.publish(pcloud)
def pub_point_cloud(self): stamp = rospy.Time.now() points3d = np.random.random((100, 3)) points3d[:,0] = 0.01 * points3d[:,1] points3d[:,1] = np.linspace(-0.1, 0.1, 100) points3d[:,2] = 0.05 * points3d[:,2] + 0.25 #rospy.loginfo(points3d) # ERROR: Calibration done in meters #cloud = profile3d * 0.001 # Conversion from milimeters to meters self.sequence = self.sequence + 1 self.pcloud = pc2.create_cloud_xyz32(self.pcloud.header, points3d) self.pcloud.header = std_msgs.msg.Header(frame_id="/camera0", stamp=stamp, seq=self.sequence) self.cloud_pub.publish(self.pcloud)
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 __init__(self): rospy.init_node('pointcloud_generator') pub_cloud = rospy.Publisher("~/cloud", PointCloud2, queue_size=10) r = rospy.Rate(10) pcloud = PointCloud2() cloud = [[1.0,-0.5,0],[2.0,-0.5,0],[3.0,-0.5,0], [1.0,0.5, 0],[2.0,0.5, 0],[3.0, 0.5,0]] while not rospy.is_shutdown(): pcloud = pc2.create_cloud_xyz32(pcloud.header, cloud) pcloud.header.frame_id = "base_footprint" pub_cloud.publish(pcloud) r.sleep()
def __init__(self): rospy.init_node('pointcloud_generator') pub_cloud = rospy.Publisher('/cloud_in', PointCloud2, queue_size=10) s = rospy.get_param('~size', 26) r = rospy.Rate(10) pcloud = PointCloud2() cloud = [[s, s, 0], [-s, -s, 0]] print cloud while not rospy.is_shutdown(): pcloud = pc2.create_cloud_xyz32(pcloud.header, cloud) pcloud.header.frame_id = 'odom' pub_cloud.publish(pcloud) r.sleep()
def geometry_to_cloud2(self, fore, left, right, chin): fore_cloud = [(fore.translation.x)/1000, (fore.translation.y)/1000, (fore.translation.z)/1000] left_cloud = [(left.translation.x)/1000, (left.translation.y)/1000, (left.translation.z)/1000] right_cloud = [(right.translation.x)/1000, (right.translation.y)/1000, (right.translation.z)/1000] chin_cloud = [(chin.translation.x)/1000, (chin.translation.y)/1000, (chin.translation.z)/1000] #cloud_header header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'cloud_map' cloud = [fore_cloud, left_cloud, right_cloud, chin_cloud] #create pcl2 clouds from points scaled_pcl = pcl2.create_cloud_xyz32(header, cloud) transformed_cloud = do_transform_cloud(scaled_pcl, self.transformer) self.handle_head_pose(self.supername) self.pcl_pub.publish(transformed_cloud)
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 send_cloud(self, cloud, duration): ref_frame = '/base_footprint' self.obs_index += 1 obstacle_frame = "/obs{}".format(self.obs_index) (trans, rotQ) = self.tf_listener.lookupTransform("/odom", ref_frame, rospy.Time(0)) pcloud = PointCloud2() rospy.loginfo("Publishing obstacle on frame {} for {}s and exiting".format(obstacle_frame, duration)) r = rospy.Rate(10) start_time = rospy.get_time() while not rospy.is_shutdown() and (duration == -1 or (rospy.get_time() - start_time) <= duration): self.br.sendTransform(trans,rotQ,rospy.Time.now(), obstacle_frame, "/odom") pcloud = pc2.create_cloud_xyz32(pcloud.header, cloud) pcloud.header.frame_id = obstacle_frame self.pub_cloud.publish(pcloud) r.sleep()
def publish_points(self, data, mask): print "I should be publishing points!" mybridge = CvBridge() img = mybridge.imgmsg_to_cv(mask) # TODO: I dislike having this dependency here on the cv encodings ... the binary transform should happen in grabcut3d_segmentation # TODO: depend on the read_points that should be in common_msgs? # TODO: why didn't the uvs=idxs code work? x pt_gen = pts.read_points(data.points)#, uvs=idxs, skip_nans=True) # this didn't work!! out_pts = [] for jj in range(data.points.height): for ii in range(data.points.width): pt = pt_gen.next() if not math.isnan(pt[0]): if (img[jj,ii] == 1 or img[jj,ii]==3): out_pts.append(pt[0:3]) print "done creating output point cloud" out_cloud = pts.create_cloud_xyz32(data.points.header, out_pts) self.point_publisher.publish(out_cloud)
def occupancy2pointcloud(grid, position, range=10.0): grid_np = np.array(grid.data, dtype='u1').reshape((grid.info.height,grid.info.width)) origin = np.array((grid.info.origin.position.x, grid.info.origin.position.y, grid.info.origin.position.z)) world2map = lambda x:trunc((x-origin)/grid.info.resolution) ll = world2map(np.r_[position[0]-range, position[1]-range, 0]) ll = np.clip(ll, zeros((3,)), np.r_[grid.info.height-1, grid.info.width-1,0]) ur = world2map(np.r_[position[0]+range, position[1]+range, 0]) ur = np.clip(ur, zeros((3,)), np.r_[grid.info.height-1, grid.info.width-1,0]) submap = grid_np[ll[1]:ur[1],ll[0]:ur[0]] mappts = np.c_[np.where(submap==100)] map2world = lambda x:(x+0.5)*og.info.resolution+origin[:2] wpts = np.array([map2world(x) for x in mappts]) pcpts=[] for z in linspace(0,1,11): pcpts.append(np.c_[wpts,z*ones_like(wpts[:,0])]) pcpts = np.vstack(pcpts) hdr = grid.header pc = pc2.create_cloud_xyz32(hdr, pcpts) return pc
def create_point_cloud_message(self, pts): header = Header() header.stamp = rospy.Time.now() header.frame_id = '/world' cloud_message = pcl2.create_cloud_xyz32(header, pts) return cloud_message
def create_point_cloud(points, frame_id='/'): _points_check(points) header = rospy.Header(frame_id=frame_id) return create_cloud_xyz32(header, points)
print(i) pl = np.where(pc[:,i] >= bounds[0][0])[0] ph = np.where(pc[:,i] <= bounds[0][1])[0] p = np.intersect1d(pl, ph) potentials.append(p) p = np.intersect1d(potentials[0], potentials[1]) p = np.intersect1d(p, potentials[2]) return pc[p] def merge_dimg_to_pcs(dimg1, dimg2, trans1, rot1, trans2, rot2): h1, w1 = dimg1.shape h2, w2 = dimg2.shape K1 = np.array([[525,0,w1/2],[0,525,h1/2],[0,0,1]], dtype=np.uint16) K2 = np.array([[525,0,w2/2],[0,525,h2/2],[0,0,1]], dtype=np.uint16) # K1 should equal K2 when the depth images are of the same size pc1 = dimg_to_pc(dimg1, K1, trans1, rot1) pc2 = dimg_to_pc(dimg2, K2, trans2, rot2) return np.array(pc1 + pc2) def rviz_pc_visualizer(pc): rospy.init_node('pc_publisher', anonymous=True) pcl_pub = rospy.Publisher("/custom_pc", PointCloud2, queue_size=10) points = pc.tolist() rate = rospy.Rate(1) while not rospy.is_shutdown(): header = msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'base' point_cloud = pc2.create_cloud_xyz32(header, points) pcl_pub.publish(point_cloud) rate.sleep()
if __name__ == "__main__": rospy.init_node("sample_simulate_tabletop_cloud") pub = rospy.Publisher("~output", PointCloud2) pub_polygon = rospy.Publisher("~output/polygon", PolygonArray) pub_coef = rospy.Publisher("~output/coef", ModelCoefficientsArray) pub_boxes = rospy.Publisher("~output/candidate_boxes", BoundingBoxArray) r = rospy.Rate(10) counter = 0 model_index = 4 reset = False while not rospy.is_shutdown(): points = generatePoints(model_index) header = Header() header.frame_id = "odom" header.stamp = rospy.Time.now() msg = create_cloud_xyz32(header, points) pub.publish(msg) (polygon, coef) = generatePolygons(header, model_index) pub_polygon.publish(polygon) pub_coef.publish(coef) counter = counter + 1 pub_boxes.publish(candidateBoxes(header, model_index)) if reset and counter > 1.0 / r.sleep_dur.to_sec() * 10: reset = rospy.ServiceProxy("/plane_supported_cuboid_estimator/reset", Empty) counter = 0 model_index = model_index + 1 if model_index >= 5: model_index = 0 reset() r.sleep()