def point_cloud_parser(self, msg): """Each publication, we do a lookup for where the drone currently is, in x,y,z, r,p,y. """ self.point_cloud_last_timestamp = msg.header self.point_cloud = pc2.read_points_list(msg, skip_nans=True) try: trans = self.tfBuffer.lookup_transform('world', prefix, rospy.Time(0)) q = (trans.transform.rotation.x, trans.transform.rotation.y, trans.transform.rotation.z, trans.transform.rotation.w) euler = euler_from_quaternion(q, axes='sxyz') x = trans.transform.translation.x y = trans.transform.translation.y z = trans.transform.translation.z roll = euler[0] pitch = euler[1] yaw = euler[2] self.pos = [x, y, z, roll, pitch, yaw] #rospy.loginfo("pos: {}\n\n\n".format(self.pos)) except: rospy.logdebug("tf lookup -- {} not found".format(prefix))
def callback(data): global laserscan, pplheader, ppl, max_x, lidartime, stoppublishing, rawlidardata, tf_listener base_frame = 'world' camera_frame = 'SimpleFlight/odom_local_ned' print('tf frames', tf_listener.frameExists(base_frame), tf_listener.frameExists(camera_frame)) return #rospy.loginfo(rospy.get_caller_id() + " frameid %s width %d data in bytes %d", data.header.frame_id, data.width, len(data.data)) #important! check if data point is 1024. if not, the data order is not normal and mess up the conversion start angle, which result in a rotation offset in scan. #this is resolved now. incomplete data size are segmented according to their starting angles if not data.width == 1024: #print(' return on data len ', data.width) #return pass curr_time = rospy.get_rostime() if curr_time.secs == lidartime.secs and curr_time.nsecs - lidartime.nsecs < 20000000: return pass lidartime = curr_time #print('lidar msg time offset ', data.header.stamp.secs-curr_time.secs, data.header.stamp.nsecs-curr_time.nsecs) # proceed to process pc2, save the current pc2 in global variable for async process. otherwise the conversion time too much and cause strange buffer delay. points_list = [] #for point in pc2.read_points(data, skip_nans=True): # points_list.append([point[0], point[1], point[2]]) #print(len(points_list), points_list[0]) rawlidardata = data ppl = pc2.read_points_list(data, skip_nans=True) pplheader = data.header max_x = ppl_max_x(ppl[0:64])
def _on_point_cloud(self, msg): # type: (PointCloud2) -> None point_list = point_cloud2.read_points_list(msg, field_names=('x', 'y', 'z'), skip_nans=True) n_point = len(point_list) # The point cloud in camera frame np_cloud_camera = np.zeros(shape=(n_point, 3)) for i in range(n_point): point_i = point_list[i] np_cloud_camera[i, 0] = point_i.x np_cloud_camera[i, 1] = point_i.y np_cloud_camera[i, 2] = point_i.z # Transform into world frame camera2world = self.get_camera2world() np_cloud_world = camera2world[0:3, 0:3].dot(np_cloud_camera.T) np_cloud_world = np_cloud_world.T for i in range(3): np_cloud_world[:, i] += camera2world[i, 3] # Set the color color = self._meshcat_vis._make_meshcat_color(n_point, 254, 254, 254) # Update the frame self._vis_lock.acquire() self._meshcat_vis.add_pointcloud(np_cloud_world, color) self._vis_lock.release()
def PublishPC2callback(self, data): #data is the pc pc = point_cloud2.read_points_list(data, skip_nans=True) #fetch xyz xyz = np.array(pc)[:, 0:3] #check if it is inside hull isInHull = in_hull(xyz, self.roipoly) #pick indexes xyz = xyz[np.where(isInHull == True)] #setup pointfield fields = [ PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1), ] #print points header = data.header pc2 = point_cloud2.create_cloud(header, fields, xyz) #pctemp = data #while not rospy.is_shutdown(): pc2.header.stamp = rospy.Time.now() self.pub.publish(pc2) print("Published")
def convert_bag_to_ply(bag, topic, out_dir): field_names = ["x", "y", "z", "intensity"] max_intensity = 1000 # HACK: expose parameter bag_msgs = bag.read_messages(topics=[topic]) msg_count = bag.get_message_count(topic) for idx, (_, msg, _) in tqdm(enumerate(bag_msgs), total=msg_count): points_xyz = [] points_i = [] points = pc2.read_points_list(cloud=msg, field_names=field_names, skip_nans=True) for point_xyzi in points: x, y, z, i = point_xyzi points_xyz.append([x, y, z]) points_i.append([i, i, i]) points_xyz = np.asarray(points_xyz) points_i = (np.asarray(points_i) / max_intensity).clip(0.0, 1.0) assert points_xyz.shape == points_i.shape, "Dimension Missmatch" filename = out_dir + str(idx).zfill(6) + ".ply" o3d_cloud = o3d.geometry.PointCloud() o3d_cloud.points = o3d.utility.Vector3dVector(points_xyz) o3d_cloud.colors = o3d.utility.Vector3dVector(points_i) o3d.io.write_point_cloud(filename, o3d_cloud)
def scan_cb(msg): # convert the message of type LaserScan to a PointCloud2 pc2_msg = lp.projectLaser(msg) pc2_msg.header.frame_id = "/camera" # 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.0 num = 0 for point in point_generator: if not math.isnan(point[2]): sum += point[2] num += 1 # we can calculate the average z value for example print(str(sum / num)) # or a list of the individual points which is less efficient point_list = pc2.read_points_list(pc2_msg) # we can access the point list with an index, each element is a namedtuple # we can access the elements by name, the generator does not yield namedtuples! # if we convert it to a list and back this possibility is lost print(point_list[len(point_list) / 2].x)
def callback(self, msg): assert isinstance(msg, PointCloud2) print(msg.header.seq) # gen=point_cloud2.read_points(msg,field_names=("x","y","z")) points = point_cloud2.read_points_list(msg, field_names=("x", "y", "z")) # print(points) self.rate.sleep()
def main(argv): #start node rospy.init_node('echoes_act_III', anonymous=True) #read pointcloud boundaries pchull = rospy.wait_for_message("/boxcast", PointCloud2) pc = point_cloud2.read_points_list(pchull, skip_nans=False) x = [] y = [] z = [] camname = argv[0] for point in pc: x.append(point[0]) y.append(point[1]) z.append(point[2]) roi = np.vstack([x, y, z]) tfer = tf.TransformListener() print(tfer.frameExists(camname)) print(tfer.frameExists(pchull.header.frame_id)) tfer.waitForTransform(camname, pchull.header.frame_id, rospy.Time(0), rospy.Duration(40.0)) #gets transformation trans, rot = tfer.lookupTransform(camname, pchull.header.frame_id, rospy.Time(0)) print(trans, rot) H = tf.transformations.quaternion_matrix(rot) #H = mmnip.Rt2Homo(H[0:3,0:3],trans) R = H[0:3, 0:3] t = np.array(trans) #transform roi = mmnip.Transform(roi, R, t) #initialize callback receiver pc2cropper = Pc2Crop(roi.T, camname) #pctopic="/camera/depth_registered/points" pctopic = "/depth/points" rospy.Subscriber(camname + pctopic, PointCloud2, pc2cropper.PublishPC2callback) try: rospy.spin() except KeyboardInterrupt: print("shut")
def parseData(self, data): if not self.field_indices: self.field_indices = self.findFieldIndices(data.fields) points = np.array(point_cloud2.read_points_list(data, skip_nans=True)) points = points[:, self.field_indices] if self.relative_timestamp: points[:, 3] += data.header.stamp.to_sec() return points
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 callback(data): global laserscan, pplheader, ppl, max_x, lidarnexttime, stoppublishing rospy.loginfo( rospy.get_caller_id() + " frameid %s width %d data in bytes %d", data.header.frame_id, data.width, len(data.data)) points_list = [] ppl = pc2.read_points_list(data, skip_nans=True) pplheader = data.header max_x = ppl_max_x(ppl[0:64]) print('pc2 ', len(ppl), ppl[0]) return
def updateObstacles(self, cloud): # Transform to odom frame: try: trans = self.tfb.lookup_transform(self.odom_frame, cloud.header.frame_id, rospy.Time(0)) except: rospy.logwarn("Error looking up transform!") return cloud = tf2sm.do_transform_cloud(cloud, trans) # Extract obstacle locations: self.obstacles = pc2.read_points_list(cloud, field_names=["x", "y"], skip_nans=True)
def scanConverter(msg): pc2_msg = lp.projectLaser(msg) pc_pub.publish(pc2_msg) point_generator = pc2.read_points(pc2_msg) sum = 1.0 num = 0 for Point in point_generator: if not math.isnan(Point[2]): sum += Point[2] num += 1 print(str(sum/num)) piont_list = pc2.read_points_list(pc2_msg) print(piont_list[len(piont_list)/2].x)
def pointcloud2_callback(msg): global processing_service rospy.loginfo("pointcloud2_callback called") try: rospy.wait_for_service('/mesh_from_pointclouds', timeout=3) mesh_from_pointcloud = rospy.ServiceProxy('/mesh_from_pointclouds', MeshFromPointCloud2) points = pc2.read_points_list(msg, field_names=("x", "y", "z"), skip_nans=True) cloud = pcl.PointCloud(np.array(points, dtype=np.float32)) clip_distance = 20 passthrough = cloud.make_passthrough_filter() passthrough.set_filter_field_name('x') passthrough.set_filter_limits(-clip_distance, clip_distance) cloud_filtered = passthrough.filter() passthrough = cloud_filtered.make_passthrough_filter() passthrough.set_filter_field_name('y') passthrough.set_filter_limits(-clip_distance, clip_distance) cloud_filtered = passthrough.filter() passthrough = cloud_filtered.make_passthrough_filter() passthrough.set_filter_field_name('z') passthrough.set_filter_limits(-clip_distance, clip_distance) cloud_filtered = passthrough.filter() vg = cloud_filtered.make_voxel_grid_filter() vg.set_leaf_size(0.01, 0.01, 0.01) cloud_filtered = vg.filter() filtered_msg = pcl_to_ros(cloud_filtered, frame_id=msg.header.frame_id) time1 = time.time() mesh_src_point = Point(0.0, 0.0, 0.0) resp1 = mesh_from_pointcloud(filtered_msg, mesh_src_point) time2 = time.time() rospy.loginfo("pointcloud processed result: %s", resp1) rospy.loginfo("service executed in %f seconds", (time2 - time1)) except rospy.ServiceException as e: rospy.logerr("Service call failed: %s", e) processing_service = False except Exception as e: rospy.logerr("Exception: %s", e) processing_service = False traceback.print_exc()
def FilterC(self): pontos = pc2.read_points_list(self.old_cloud, field_names=("x", "y", "z"), skip_nans=True) new_pontos = list() for p in pontos: # print('p = (', p[0], p[1], p[2],')') if p[2] >= -0.1 and p[2] <= 0.1 and sqrt(p[0]**2 + p[1]**2 + p[2]**2) > 0.5: rp = (p[0], p[1]) wp = self.world_frame(rp) if not self.inTC(wp): new_pontos.append(p) return pc2.create_cloud(self.old_cloud.header, self.old_cloud.fields, new_pontos)
def detect_cart_legs(self): # convert the message of type LaserScan to a PointCloud2 pc2_msg = self.laser_projection.projectLaser( self.cart_isolated_laser_msg) point_list = pc2.read_points_list(pc2_msg) dataset = [] legs = [[np.nan, np.nan], [np.nan, np.nan], [np.nan, np.nan], [np.nan, np.nan]] for i in point_list: dataset.append([i.x, i.y]) # K-mean clustering the dataset to get center of each leg: if len(dataset) >= 4: kmeans = KMeans(n_clusters=4, random_state=0).fit(dataset) legs = kmeans.cluster_centers_ print(legs) def check_leg_position(ref, source): check = 0 polygon = shape_polygon(ref) for i in source: point = shape_point(i[0], i[1]) if polygon.contains(point): check += 1 if check >= len(ref): return True else: return False # Manually check position of the cart: if check_leg_position(self.ref_cart_legs, legs) and check_leg_position( self.ref_cart_legs, dataset): self.the_cart = True self.cart_footprint_estimator.header.stamp = rospy.Time.now() leg1 = Point(legs[0][0], legs[0][1], 0.0) leg2 = Point(legs[3][0], legs[3][1], 0.0) leg3 = Point(legs[1][0], legs[1][1], 0.0) leg4 = Point(legs[2][0], legs[2][1], 0.0) self.cart_footprint_estimator.polygon.points = [ leg1, leg2, leg3, leg4 ] self.visual_cart_footprint_estimator.publish( self.cart_footprint_estimator) # self.close_grippers() else: self.the_cart = False rospy.loginfo("Cart detected: " + str(self.the_cart))
def FilterC(self): pontos = pc2.read_points_list(self.old_cloud, field_names=("x", "y", "z"), skip_nans=True) new_pontos = list() for p in pontos: rp = (p[0], p[1]) wp = self.world_frame(rp) Point = namedtuple("Point", ("x", "y", "z")) l = (wp[0], wp[1], p[2]) new_p = Point._make(l) new_pontos.append(new_p) return pc2.create_cloud_xyz32(self.old_cloud.header, new_pontos)
def clip_pointcloud_msg(msg, src, clip_distance=8): """ Clip pointcloud from source position given an offset This reduces the size of the pointcloud to a reazonable size to perform the reconstruction :param msg: :param x: :param y: :param z: :param offset: :return: """ points = pc2.read_points_list(msg, field_names=("x", "y", "z"), skip_nans=True) cloud = pcl.PointCloud(np.array(points, dtype=np.float32)) passthrough = cloud.make_passthrough_filter() passthrough.set_filter_field_name('x') passthrough.set_filter_limits( -clip_distance + src.pose.pose.position.x, clip_distance + src.pose.pose.position.x) cloud_filtered = passthrough.filter() passthrough = cloud_filtered.make_passthrough_filter() passthrough.set_filter_field_name('y') passthrough.set_filter_limits( -clip_distance + src.pose.pose.position.y, clip_distance + src.pose.pose.position.y) cloud_filtered = passthrough.filter() passthrough = cloud_filtered.make_passthrough_filter() passthrough.set_filter_field_name('z') passthrough.set_filter_limits( -clip_distance + src.pose.pose.position.z, clip_distance + src.pose.pose.position.z) cloud_filtered = passthrough.filter() vg = cloud_filtered.make_voxel_grid_filter() vg.set_leaf_size(0.1, 0.1, 0.1) cloud_filtered = vg.filter() filtered_msg = RepublishPointCloud.pcl_to_ros( cloud_filtered, frame_id=msg.header.frame_id) return filtered_msg
def callback_disc(self, data): pontos = pc2.read_points_list(data, field_names=("x", "y", "z"), skip_nans=True) self.disc_pontos = list() self.rp_disc_pontos = list() for p in pontos: rp = (p[0], p[1]) wp = self.world_frame(rp) if True: #not self.inTC(wp): if self.pos_y >= 0: if self.pos_x > wp[0] - 0.1: self.disc_pontos.append(wp) self.rp_disc_pontos.append(rp) else: if self.pos_x < wp[0] + 0.1: self.disc_pontos.append(wp) self.rp_disc_pontos.append(rp)
def scan(self, cloud): xyz_generator = pc2.read_points_list(cloud, skip_nans=True, field_names=("x", "y", "z", "intensity")) x = [] y = [] z = [] intensity = [] for xyz in xyz_generator: x.append(xyz.x) y.append(xyz.y) z.append(xyz.z) intensity.append(xyz.intensity) self.x = x self.y = y self.z = z self.intensity = intensity
def callback(data): global laserscan, pplheader, ppl, p_angles, p_segs, p_segindex, max_x, lidarnexttime, stoppublishing, pc2pub, pc2data # rospy.loginfo(rospy.get_caller_id() + " frameid %s width %d data in bytes %d", data.header.frame_id, data.width, len(data.data)) points_list = [] ppl = pc2.read_points_list(data, skip_nans=True) p_angles = ppl_angles(ppl) p_segs, p_segindex = ppl_16seg(p_angles, data.data) pplheader = data.header #max_x =ppl_max_x(ppl[0:64]) #print('pc2 ', len(ppl), ppl[0]) segid = rospy.get_param("/segid") data.width = len(p_segs[segid]) / 12 data.row_step = data.width * 12 data.data = p_segs[segid] pc2data = data #data.data=data.data[0:data.row_step] return
def scan_cb(msg): global point_list # избавляемся от inf without_inf = list() for k in range(0, len(msg.ranges)): if np.isinf(msg.ranges[k]) or msg.ranges[k] > max_dist_lidar: without_inf.append(max_dist_lidar) else: without_inf.append(msg.ranges[k]) # инвертируем сонар без inf msg.ranges = without_inf # получаем курс в градусах course = int(np.rad2deg(current_course)) # поворачиваем массив расстояний с лидара(360 знач) на курс в градусах msg.ranges = msg.ranges[-course:] + msg.ranges[:-course] # convert the message of type LaserScan to a PointCloud2 pc2_msg = lp.projectLaser(msg) # convert it to a pointlist point_list = pc2.read_points_list(pc2_msg)
def scan_cb(msg): # convert the message of type LaserScan to a PointCloud2 points_list = [] for point in msg.points: points_list.append([point.x, point.y, point.z]) array = np.asarray(points_list) ev = [ np.where([ np.logical_and((x_min <= point[0] <= x_max), (y_min <= point[1] <= y_max)) for point in array ]) ] print(ev) # or a list of the individual points which is less efficient point_list = pc2.read_points_list(pc2_msg) # we can access the point list with an index, each element is a namedtuple # we can access the elements by name, the generator does not yield namedtuples! # if we convert it to a list and back this possibility is lost print(point_list[len(point_list) / 2].x)
range_list = [] x_list = [] y_list = [] z_list = [] remission_list = [] range_std_list = [] x_std_list = [] y_std_list = [] z_std_list = [] remission_std_list = [] for topic, msg, t in bag.read_messages(topics=topics): if (topic == str('/lslidar_point_cloud')): point_list = pcl2.read_points_list(msg, skip_nans=True) point_array = np.array(point_list, dtype=np.float32) range_temp = [] x_temp = [] y_temp = [] z_temp = [] remission_temp = [] for line in point_array: range_temp.append( math.sqrt(line[0] * line[0] + line[1] * line[1] + line[2] * line[2])) x_temp.append(line[0]) y_temp.append(line[1]) z_temp.append(line[2])
def process_data(self): if not self.current_map_pointcloud_msg: rospy.logerr("No current_map_pointcloud_msg received") return False if not self.cmd_vel_msg: rospy.logerr("No cmd_vel_msg received") return False if not self.last_odom_msg: self.last_odom_msg = self.odom_msg return False # else: # last_point = (self.last_odom_msg.pose.pose.position.x, self.last_odom_msg.pose.pose.position.y) # curr_point = (self.odom_msg.pose.pose.position.x, self.odom_msg.pose.pose.position.y) # odom_diff_xy = abs(last_point[0] - curr_point[0]) + abs(last_point[1] - curr_point[1]) # # _, _, last_y = tf.transformations.euler_from_quaternion([self.last_odom_msg.pose.pose.orientation.x, # self.last_odom_msg.pose.pose.orientation.y, # self.last_odom_msg.pose.pose.orientation.z, # self.last_odom_msg.pose.pose.orientation.w]) # # _, _, curr_y = tf.transformations.euler_from_quaternion([self.odom_msg.pose.pose.orientation.x, # self.odom_msg.pose.pose.orientation.y, # self.odom_msg.pose.pose.orientation.z, # self.odom_msg.pose.pose.orientation.w]) # # odom_diff_yaw = abs(last_y - curr_y) # if odom_diff_xy < 0.1 and odom_diff_yaw < 0.6: # rospy.logwarn("No movement detected on XY (%s) and yaw (%s)", odom_diff_xy, odom_diff_yaw) # rospy.logwarn("last_y (%s) curr_y (%s)", last_y, curr_y) # return # self.last_odom_msg = self.odom_msg if abs(self.cmd_vel_msg.linear.x) < 0.05 and abs(self.cmd_vel_msg.angular.z) < 0.05: rospy.logwarn("No movement (time:%.2f) x:%.2f z:%.2f odom_dist:%.2f rmse:%.2f", self.current_timestep, self.cmd_vel_msg.linear.x, self.cmd_vel_msg.angular.z, self.odom_dist, self.rmse) return False last_point = (self.last_odom_msg.pose.pose.position.x, self.last_odom_msg.pose.pose.position.y, self.last_odom_msg.pose.pose.position.z) curr_point = (self.odom_msg.pose.pose.position.x, self.odom_msg.pose.pose.position.y, self.odom_msg.pose.pose.position.z) local_odom_dist = math.sqrt((last_point[0] - curr_point[0]) ** 2 + (last_point[1] - curr_point[1]) ** 2 + (last_point[2] - curr_point[2]) ** 2) self.odom_dist += local_odom_dist self.last_odom_msg = self.odom_msg points = pc2.read_points_list(self.current_map_pointcloud_msg, field_names=("x", "y", "z"), skip_nans=True) cloud = pcl.PointCloud(np.array(points, dtype=np.float32)) cloud = ExplorationMetrics.voxelize_cloud(cloud, self.leaf_size) self.rmse = ExplorationMetrics.similarity(self.ground_truth_pointcloud, cloud) self.pointcloud_plot_data.append({'timestep': self.current_timestep, 'rmse': self.rmse, 'odom_dist': local_odom_dist, 'robot_action_number': self.robot_action_number}) rospy.loginfo("timestep:%s action_num:%s odom_dist:%.2f rmse:%.2f", self.current_timestep, self.robot_action_number, self.odom_dist, self.rmse) self.current_timestep += 1 self.last_time_updated = time.time() return True
def process_lidar_msg(): global lidar_msg if not lidar_msg: return mlab.figure(1, bgcolor=(0, 0, 0)) mlab.clf() points = pc2.read_points_list(lidar_msg, field_names=("x", "y", "z"), skip_nans=True) print "size points:", len(points) points = [ p for p in points if p[2] < -0.39 or p[2] > -0.35 and math.sqrt(p[0]**2 + p[1]**2 + p[2]**2) < 3.0 ] print "size points after Z basic cleanout:", len(points) X = np.array(points) pts2 = mlab.points3d(X[:, 0], X[:, 1], X[:, 2], color=(1.0, 1.0, 1.0), scale_factor=0.01, scale_mode='none', resolution=20) border_kdtree = spatial.KDTree(points) border_tresh = 0.4 db = DBSCAN(eps=0.20, min_samples=2).fit(points) core_samples_mask = np.zeros_like(db.labels_, dtype=bool) core_samples_mask[db.core_sample_indices_] = True labels = db.labels_ # Number of clusters in labels, ignoring noise if present. n_clusters_ = len(set(labels)) - (1 if -1 in labels else 0) n_noise_ = list(labels).count(-1) rospy.loginfo('Estimated number of clusters: %d, noise_points: %s', n_clusters_, n_noise_) unique_labels = set(labels) colors = plt.cm.get_cmap('gist_rainbow', len(unique_labels)) X = np.array(points) for idx, label in enumerate(unique_labels): if label == -1: # Black used for noise. # col = [0, 0, 0, 1] continue else: col = colors(idx) # print "col", col class_member_mask = (labels == label) xyz = X[class_member_mask & core_samples_mask] # print "xyz:", xyz z_std = np.std(xyz[:, 2]) print "np.std(xyz[:, 2]):", z_std #normaldefinition_3D_real(xyz, 16) # mesh_filepath = None # try: # fields = [ # sensor_msgs.msg.PointField('x', 0, sensor_msgs.msg.PointField.FLOAT32, 1), # sensor_msgs.msg.PointField('y', 4, sensor_msgs.msg.PointField.FLOAT32, 1), # sensor_msgs.msg.PointField('z', 8, sensor_msgs.msg.PointField.FLOAT32, 1) # ] # # pcloud = pc2.create_cloud(lidar_msg.header, fields, xyz.tolist()) # rospy.wait_for_service('/mesh_from_pointclouds', timeout=3) # mesh_from_pointcloud = rospy.ServiceProxy('/mesh_from_pointclouds', MeshFromPointCloud2) # resp1 = mesh_from_pointcloud(pcloud) # mesh_filepath = resp1.path # rospy.loginfo("pointcloud processed result: %s", resp1) # except rospy.ServiceException as e: # rospy.logerr("Service call failed: %s", e) # except Exception as e: # rospy.logerr("Exception: %s", e) # # if mesh_filepath is None: # rospy.logerr("mesh_filepath is None, cannot continue with the planning") # rate_slow.sleep() # continue # fxyz = np.array([centroids[v] for v in intersecting_frontiers]) pts2 = mlab.points3d(xyz[:, 0], xyz[:, 1], xyz[:, 2], color=(col[0], col[1], col[2]), scale_factor=0.05, scale_mode='none', resolution=20) # if this cluster is not having multiple Z levels discard it if z_std < 0.1: continue centroid = get_centroid_of_pts(xyz) # print "centroid:", centroid pts3 = mlab.points3d(centroid[:, 0], centroid[:, 1], centroid[:, 2], color=(col[0], col[1], col[2]), scale_factor=0.5, scale_mode='none', resolution=20) mlab.show()
def labelData(self): # Detected and idxs values to False and [], to make sure we are not using information from a previous labelling self.labels['detected'] = False self.labels['idxs'] = [] # Labelling process dependent of the sensor type if self.msg_type_str == 'LaserScan': # 2D LIDARS ------------------------------------- # For 2D LIDARS the process is the following: First cluster all the range data into clusters. Then, # associate one of the clusters with the calibration pattern by selecting the cluster which is closest to # the rviz interactive marker. clusters = [] # initialize cluster list to empty cluster_counter = 0 # init counter points = [] # init points # Compute cartesian coordinates xs, ys = interactive_calibration.utilities.laser_scan_msg_to_xy(self.msg) # Clustering: first_iteration = True for idx, r in enumerate(self.msg.ranges): # Skip if either this point or the previous have range smaller than minimum_range_value if r < self.minimum_range_value or self.msg.ranges[idx - 1] < self.minimum_range_value: continue if first_iteration: # if first iteration, create a new cluster clusters.append(LaserScanCluster(cluster_counter, idx)) first_iteration = False else: # check if new point belongs to current cluster, create new cluster if not x = xs[clusters[-1].idxs[-1]] # x coordinate of last point of last cluster y = ys[clusters[-1].idxs[-1]] # y coordinate of last point of last cluster distance = math.sqrt((xs[idx] - x) ** 2 + (ys[idx] - y) ** 2) if distance > self.threshold: # if distance larger than threshold, create new cluster cluster_counter += 1 clusters.append(LaserScanCluster(cluster_counter, idx)) else: # same cluster, push this point into the same cluster clusters[-1].pushIdx(idx) # Association stage: find out which cluster is closer to the marker x_marker, y_marker = self.marker.pose.position.x, self.marker.pose.position.y # interactive marker pose idx_closest_cluster = 0 min_dist = sys.maxint for cluster_idx, cluster in enumerate(clusters): # cycle all clusters for idx in cluster.idxs: # cycle each point in the cluster x, y = xs[idx], ys[idx] dist = math.sqrt((x_marker - x) ** 2 + (y_marker - y) ** 2) if dist < min_dist: idx_closest_cluster = cluster_idx min_dist = dist closest_cluster = clusters[idx_closest_cluster] # Find the coordinate of the middle point in the closest cluster and bring the marker to that point x_sum, y_sum = 0, 0 for idx in closest_cluster.idxs: x_sum += xs[idx] y_sum += ys[idx] self.marker.pose.position.x = x_sum / float(len(closest_cluster.idxs)) self.marker.pose.position.y = y_sum / float(len(closest_cluster.idxs)) self.marker.pose.position.z = 0 self.menu_handler.reApply(self.server) self.server.applyChanges() # Update the dictionary with the labels self.labels['detected'] = True percentage_points_to_remove = 0.0 # remove x% of data from each side number_of_idxs = len(clusters[idx_closest_cluster].idxs) idxs_to_remove = int(percentage_points_to_remove * float(number_of_idxs)) clusters[idx_closest_cluster].idxs_filtered = clusters[idx_closest_cluster].idxs[ idxs_to_remove:number_of_idxs - idxs_to_remove] self.labels['idxs'] = clusters[idx_closest_cluster].idxs_filtered # Create and publish point cloud message with the colored clusters (just for debugging) cmap = cm.prism(np.linspace(0, 1, len(clusters))) points = [] z, a = 0, 255 for cluster in clusters: for idx in cluster.idxs: x, y = xs[idx], ys[idx] r, g, b = int(cmap[cluster.cluster_count, 0] * 255.0), \ int(cmap[cluster.cluster_count, 1] * 255.0), \ int(cmap[cluster.cluster_count, 2] * 255.0) rgb = struct.unpack('I', struct.pack('BBBB', b, g, r, a))[0] pt = [x, y, z, rgb] points.append(pt) fields = [PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1), PointField('rgba', 12, PointField.UINT32, 1)] header = Header() header.frame_id = self.parent header.stamp = self.msg.header.stamp pc_msg = point_cloud2.create_cloud(header, fields, points) self.publisher_clusters.publish(pc_msg) # Create and publish point cloud message containing only the selected calibration pattern points points = [] for idx in clusters[idx_closest_cluster].idxs_filtered: x_marker, y_marker, z_marker = xs[idx], ys[idx], 0 r = int(0 * 255.0) g = int(0 * 255.0) b = int(1 * 255.0) a = 255 rgb = struct.unpack('I', struct.pack('BBBB', b, g, r, a))[0] pt = [x_marker, y_marker, z_marker, rgb] points.append(pt) pc_msg = point_cloud2.create_cloud(header, fields, points) self.publisher_selected_points.publish(pc_msg) elif self.msg_type_str == 'Image': # Cameras ------------------------------------------- # Convert to opencv image and save image to disk image = self.bridge.imgmsg_to_cv2(self.msg, "bgr8") result = self.pattern.detect(image) if result['detected']: c = [] for corner in result['keypoints']: c.append({'x': float(corner[0][0]), 'y': float(corner[0][1])}) x = int(round(c[0]['x'])) y = int(round(c[0]['y'])) cv2.line(image, (x, y), (x, y), (0, 255, 255), 20) # Update the dictionary with the labels self.labels['detected'] = True self.labels['idxs'] = c # For visual debugging self.pattern.drawKeypoints(image, result) msg_out = self.bridge.cv2_to_imgmsg(image, encoding="passthrough") msg_out.header.stamp = self.msg.header.stamp msg_out.header.frame_id = self.msg.header.frame_id self.publisher_labelled_image.publish(msg_out) elif self.msg_type_str == 'PointCloud2': # RGB-D pointcloud ------------------------------------------- # print("Found point cloud!") # Get 3D coords points = pc2.read_points_list(self.msg, skip_nans=False, field_names=("x", "y", "z")) # Get the marker position x_marker, y_marker, z_marker = self.marker.pose.position.x, self.marker.pose.position.y, self.marker.pose.position.z # interactive marker pose cam_model = PinholeCameraModel() # Wait for camera info message camera_info = rospy.wait_for_message('/top_center_rgbd_camera/depth/camera_info', CameraInfo) cam_model.fromCameraInfo(camera_info) # Project points seed_point = cam_model.project3dToPixel((x_marker, y_marker, z_marker)) seed_point = (int(math.floor(seed_point[0])), int(math.floor(seed_point[1]))) # Wait for depth image message imgmsg = rospy.wait_for_message('/top_center_rgbd_camera/depth/image_rect', Image) # img = self.bridge.imgmsg_to_cv2(imgmsg, desired_encoding="8UC1") img = self.bridge.imgmsg_to_cv2(imgmsg, desired_encoding="passthrough") img_float = img.astype(np.float32) img_float = img_float / 1000 # print('img type = ' + str(img.dtype)) # print('img_float type = ' + str(img_float.dtype)) # print('img_float shape = ' + str(img_float.shape)) h, w = img.shape mask = np.zeros((h + 2, w + 2, 1), np.uint8) # mask[seed_point[1] - 2:seed_point[1] + 2, seed_point[0] - 2:seed_point[0] + 2] = 255 img_float2 = deepcopy(img_float) cv2.floodFill(img_float2, mask, seed_point, 128, 0.1, 0.1, 8 | (128 << 8) | cv2.FLOODFILL_MASK_ONLY) # | cv2.FLOODFILL_FIXED_RANGE) # Switch coords of seed point # mask[seed_point[1]-2:seed_point[1]+2, seed_point[0]-2:seed_point[0]+2] = 255 tmpmask = mask[1:h + 1, 1:w + 1] # calculate moments of binary image M = cv2.moments(tmpmask) if M["m00"] != 0: # calculate x,y coordinate of center cX = int(M["m10"] / M["m00"]) cY = int(M["m01"] / M["m00"]) mask[cY-2:cY+2, cX-2:cX+2] = 255 cv2.imshow("mask", mask) cv2.waitKey(20) # msg_out = self.bridge.cv2_to_imgmsg(showcenter, encoding="passthrough") # msg_out.header.stamp = self.msg.header.stamp # msg_out.header.frame_id = self.msg.header.frame_id # self.publisher_labelled_depth_image.publish(msg_out) coords = points[cY * 640 + cX] if not math.isnan(coords[0]): self.marker.pose.position.x = coords[0] self.marker.pose.position.y = coords[1] self.marker.pose.position.z = coords[2] self.menu_handler.reApply(self.server) self.server.applyChanges()
def octomap_cb(msg): global point_list # конвертируем PointCloud2 в List # del point_list[:] point_list = pc2.read_points_list(msg)
def scandata(msg): global point_list pointcloud_msg = lasergeometry.LaserProjection().projectLaser(msg) point_list = point.read_points_list(pointcloud_msg)
def process_lidar_msg(n_bins=72, z_std_thresh=0.1): global lidar_msg if not lidar_msg: return rospy.loginfo("process_lidar_msg") points = pc2.read_points_list(lidar_msg, field_names=("x", "y", "z"), skip_nans=True) print "size points:", len(points) #points = [p for p in points if p[2] < -0.39 or p[2] > -0.35 and math.sqrt(p[0] ** 2 + p[1] ** 2 + p[2] ** 2) < 3.0] #print "size points after Z basic cleanout:", len(points) #points = [p for p in points if math.sqrt(p[0] ** 2 + p[1] ** 2 + p[2] ** 2) < 3.0] #print "size points after distance cleanout:", len(points) cloud = pcl.PointCloud(np.array(points, dtype=np.float32)) clip_distance = 2.5 passthrough = cloud.make_passthrough_filter() passthrough.set_filter_field_name('x') passthrough.set_filter_limits(-clip_distance, clip_distance) cloud_filtered = passthrough.filter() passthrough = cloud_filtered.make_passthrough_filter() passthrough.set_filter_field_name('y') passthrough.set_filter_limits(-clip_distance, clip_distance) cloud_filtered = passthrough.filter() passthrough = cloud_filtered.make_passthrough_filter() passthrough.set_filter_field_name('z') passthrough.set_filter_limits(-clip_distance, clip_distance) cloud_filtered = passthrough.filter() vg = cloud_filtered.make_voxel_grid_filter() vg.set_leaf_size(0.01, 0.01, 0.01) cloud_filtered = vg.filter() # divide the pointcloud in bins bin_size = 360 / float(n_bins) colors = get_color_list(n_bins) np_p = cloud_filtered.to_array() bin_idx = -1 #viewer.InitCameraParameters() marker_array = MarkerArray() closest_p_dist = float("inf") closest_p = None for i in xrange((n_bins / 2)): for sign in [1, -1]: bin_idx += 1 bin_start = (i * bin_size) * sign bin_end = ((i + 1) * bin_size) * sign if sign > 0: fov = [bin_start, bin_end] else: fov = [bin_end, bin_start] cond = hv_in_range(x=np_p[:, 0], y=np_p[:, 1], z=np_p[:, 2], fov=fov, fov_type='h') np_p_ranged = np_p[cond] z_std = np.std(np_p_ranged[:, 2]) if z_std < z_std_thresh: cloud_cluster = pcl.PointCloud() cloud_cluster.from_array(np_p_ranged) pccolor = pcl.pcl_visualization.PointCloudColorHandleringCustom( cloud_cluster, 255, 0, 0) viewer.AddPointCloud_ColorHandler( cloud_cluster, pccolor, b'cluster_{}'.format(bin_idx), 0) print "\tz_std:", z_std continue color = colors[bin_idx] cloud_bin = pcl.PointCloud(np_p_ranged) tree = cloud_bin.make_kdtree() ec = cloud_bin.make_EuclideanClusterExtraction() ec.set_ClusterTolerance(0.15) ec.set_MinClusterSize(20) ec.set_MaxClusterSize(25000) ec.set_SearchMethod(tree) cluster_indices = ec.Extract() if len(cluster_indices) <= 0: cloud_cluster = pcl.PointCloud() cloud_cluster.from_array(np_p_ranged) pccolor = pcl.pcl_visualization.PointCloudColorHandleringCustom( cloud_cluster, 255, 0, 0) viewer.AddPointCloud_ColorHandler( cloud_cluster, pccolor, b'cluster_{}'.format(bin_idx), 0) print "\tlen(cluster_indices)", len(cluster_indices) continue max_cluster_idx = find_max_list_idx(cluster_indices) len_max_cluster = len(cluster_indices[max_cluster_idx]) #print 'cluster_indices :', len(cluster_indices), " count." #print 'max_cluster_idx :', max_cluster_idx, " len:", len_max_cluster if len(cluster_indices[max_cluster_idx]) < 100: continue cluster_points = np.zeros((len_max_cluster, 3), dtype=np.float32) for j, indice in enumerate(cluster_indices[max_cluster_idx]): cluster_points[j][0] = cloud_bin[indice][0] cluster_points[j][1] = cloud_bin[indice][1] cluster_points[j][2] = cloud_bin[indice][2] cloud_cluster = pcl.PointCloud() cloud_cluster.from_array(cluster_points) # # pccolor = pcl.pcl_visualization.PointCloudColorHandleringCustom( cloud_cluster, color[0], color[1], color[2]) viewer.AddPointCloud_ColorHandler(cloud_cluster, pccolor, b'cluster_{}'.format(bin_idx), 0) print "z_std:", z_std, "bin_start:", bin_start, "bin_end:", bin_end, "bin_idx:", bin_idx, "color:", color centroid = get_centroid_of_pts(cluster_points)[0] #print "centroid:", centroid x, y, z = centroid f_marker = create_marker((x, y, z), color=(0.6, 0.1, 0.0), duration=2, m_scale=0.25, marker_id=bin_idx) marker_array.markers.append(f_marker) d = math.sqrt(x**2 + y**2 + z**2) if d < closest_p_dist: closest_p_dist = d closest_p = centroid # pccolor = pcl.pcl_visualization.PointCloudColorHandleringCustom(cloud_bin, color[0], color[1], color[2]) # viewer.AddPointCloud_ColorHandler(cloud_bin, pccolor, b'cluster_{}'.format(bin_idx), 0) viewer.AddCube(-0.25, 0.25, -0.15, 0.15, -0.4, -0.2, 255, 255, 255, "robot") # color = colors[0] # pccolor = pcl.pcl_visualization.PointCloudColorHandleringCustom(cloud_filtered, color[0], color[1], color[2]) # viewer.AddPointCloud_ColorHandler(cloud_filtered, pccolor, b'cluster_{}'.format(0), 0) # seg = cloud_filtered.make_segmenter() # seg.set_optimize_coefficients(True) # seg.set_model_type(pcl.SACMODEL_PLANE) # seg.set_method_type(pcl.SAC_RANSAC) # seg.set_MaxIterations(100) # seg.set_distance_threshold(0.25) # # indices, model = seg.segment() # tmp = cloud_filtered.to_array() # tmp = np.delete(tmp, indices, 0) # cloud_filtered.from_array(tmp) # tree = cloud_filtered.make_kdtree() # ec = cloud_filtered.make_EuclideanClusterExtraction() # ec.set_ClusterTolerance(0.25) # ec.set_MinClusterSize(2) # ec.set_MaxClusterSize(25000) # ec.set_SearchMethod(tree) # cluster_indices = ec.Extract() # # print 'cluster_indices :', len(cluster_indices), " count." # # colors = get_color_list(len(cluster_indices)) # # cloud_cluster = pcl.PointCloud() # # for j, indices in enumerate(cluster_indices): # # cloudsize = indices # print 'j:', j, 'indices:', str(len(indices)) # # points = np.zeros((len(indices), 3), dtype=np.float32) # # for i, indice in enumerate(indices): # points[i][0] = cloud_filtered[indice][0] # points[i][1] = cloud_filtered[indice][1] # points[i][2] = cloud_filtered[indice][2] # # z_std = np.std(points[:, 2]) # print "z std:", z_std # # cloud_cluster.from_array(points) # ss = "/tmp/cluster/cloud_cluster_" + str(j) + ".pcd" # pcl.save(cloud_cluster, ss) # # color = colors[j] # pccolor = pcl.pcl_visualization.PointCloudColorHandleringCustom(cloud, color[0], color[1], color[2]) # viewer.AddPointCloud_ColorHandler(cloud_cluster, pccolor, b'cluster_{}'.format(j), 0) if closest_p is not None: closest_p_marker = create_marker( (closest_p[0], closest_p[1], closest_p[2]), color=(0.9, 0.1, 0.0), duration=2, m_scale=0.5, marker_id=0) pub_closest_obstacle_pt.publish(closest_p_marker) pub_obstacles_pts.publish(marker_array) v = True while v: v = not (viewer.WasStopped()) viewer.SpinOnce() #time.sleep(0.5) #break # for j, indices in enumerate(cluster_indices): # viewer.RemovePointCloud(b'cluster_{}'.format(j), 0) # for i in xrange(n_bins): # viewer.RemovePointCloud(b'cluster_{}'.format(i), 0) viewer.remove_all_pointclouds() viewer.remove_all_shapes()