def build_and_publish_obstacle_point_clouds(self, reachable_workspace_points): """ Builds and publishes obstacles point clouds for both the left and right Baxter arms. Publishes obstacle clouds in the base from, as they are transformed and filtered points. :param reachable_workspace_points: the overall environment points list, filtered and transformed to base frame, in reachable workspace """ obstacle_cloud = PointCloud() header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'base' obstacle_cloud.header = header left_filtered_pts = self.filter_out_arm(reachable_workspace_points, "left") # update collision checker obstacle list self.left_cc.update_obstacles(left_filtered_pts) for point in left_filtered_pts: obstacle_cloud.points.append(Point32(point[0], point[1], point[2])) print "publishing new left obstacle cloud!" self.left_obs_pub.publish(obstacle_cloud) obstacle_cloud = PointCloud() header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'base' obstacle_cloud.header = header right_filtered_pts = self.filter_out_arm(reachable_workspace_points, "right") # update collision checker obstacle list self.right_cc.update_obstacles(right_filtered_pts) for point in right_filtered_pts: obstacle_cloud.points.append(Point32(point[0], point[1], point[2])) print "publishing new right obstacle cloud!" self.right_obs_pub.publish(obstacle_cloud)
def match_detections(): distances = [] global legs global faces global pub pc = PointCloud() pc.header = Header(0, rospy.get_rostime(), '/map') for m in range(len(faces)): pc.points.append(Point32(faces[m].x, faces[m].y, 1.5)) for l in range(len(legs)): pc.points.append(Point32(legs[l].x, legs[l].y, 1.0)) distancecol = [] for m in range(len(faces)): # measure the distance between the detections and store them dist = (sqrt((faces[m].x - legs[l].x)**2 + (faces[m].y - legs[l].y)**2)) distancecol.append(dist) distances.append(distancecol) print "distances" print distances pub.publish(pc)
def lidarCallback(data): global angle global ekf global p_acc p = PointCloud() p.points = [] p.header = data.header p.header.frame_id = "map" p_acc.header.stamp = data.header.stamp p_acc.header.seq = data.header.seq current_angle = data.angle_min pitch = angle.data + ekf.pitch roll = ekf.roll for i in data.ranges: if i < MAX_DISTANCE: point = Point32() yaw = current_angle+ekf.yaw point.x = i * math.cos(yaw) * math.cos(pitch) point.y = i * math.sin(yaw) * math.cos(pitch) current_angle += data.angle_increment point.z = i * math.sin(pitch) p.points += [point] p_acc.points += p.points pub.publish(p) pub_acc.publish(p_acc)
def getFrontiers(self): if self.gmap is not None: self.lookingForFrontiers = True rospy.loginfo("[frontier_search] Looking for frontiers") #First check to see if the cells are free and if we already decided they are not a frontier cells = [x for x in range(0, len(self.gmap.data)) if self.gmap.data[x] == 0] # and x not in self.notFrontier removed from previous line waay too slow! #The check to see if the remaining are frontiers frontiers = [cell for cell in cells if -1 in [self.gmap.data[val] for val in self.getNeighbors(cell)]] #Add all cells that are not frontiers to our list of not frontiers waay too slow! #self.notFrontier.extend([cell for cell in cells if cell not in frontiers]) #Check to see if the area around them is clear of obstacles freeFrontiers = [frontier for frontier in frontiers if self.validFrontier(frontier)] rospy.loginfo("[frontier_search] Found %s frontiers", len(freeFrontiers)) points = self.getPoints(freeFrontiers) cloud = PointCloud() h = std_msgs.msg.Header() h.stamp = rospy.Time.now() h.frame_id = 'map' cloud.header = h cloud.points = points rospy.loginfo("[frontier_search] Publishing frontiers") # Publish a point cloud for your viewing pleasure self.publishFrontiers.publish(cloud) self.lookingForFrontiers = False return cloud else: rospy.loginfo("[frontier_search] No map available") return None
def pt_cloud_setup(self, nx, ny, gx, gy): pt_cloud = PointCloud() pt_cloud.header = std_msgs.msg.Header() pt_cloud.header.stamp = rospy.Time.now() pt_cloud.header.frame_id = self.frameid pt_cloud.points = [None] * (nx * ny) colors = ['r', 'g', 'b'] for color in colors: ch = Channel() ch.name = color ch.values = [] pt_cloud.channels.append(ch) cnt = 0 for i1 in range(0, nx): for i2 in range(0, ny): pt_cloud.points[cnt] = Point(i1 * gx, i2 * gy, 0) pt_cloud.channels[0].values.append(0.0) pt_cloud.channels[1].values.append(0.0) pt_cloud.channels[2].values.append(0.0) cnt = cnt + 1 return pt_cloud
def publish_pc2(event=None): global pc2data, ppl, p_angles, p_segs, p_segindex, laserpub, shiftscan segid = rospy.get_param("/segid") if pc2data == None: pass else: ppl_seg = ppl[p_segindex[segid][0]:p_segindex[segid][1]] pangle_seg = p_angles[p_segindex[segid][0]:p_segindex[segid][1]] #print('segid ', segid, p_segindex[segid], ppl_seg) pc2pub.publish(pc2data) my_pc = PointCloud() header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = pc2data.header.frame_id my_pc.header = header for pt in ppl_seg: my_pc.points.append(pt) print "happily publishing pointcloud" pc_pub.publish(my_pc) #/scan #laserscan = laser_dummy(3) laserscan, slopesample = convert_to_scan(ppl_seg, pangle_seg, my_pc.header, shiftscan) laserpub.publish(laserscan) if slopesample > 20 or slopesample < -20: print(' slope exceed 20 abnormal')
def get_data(self, var): PC = PointCloud( ) # create the new object PointCloud to be published later PC.header = var.header # copy the content of topic to another PointCloud in order to manipulate it PC.channels = var.channels PC.points = var.points """ Function to get the "3D" map """ for i in range((len(PC.points))): PC.points[i].z = PC.channels[0].values[ i] / 100 # write in the z axis of the points the values of the "intensity" divided by 100 to make it readable on rviz self.pub2.publish(PC) # publish it into the new topic """ Function to filtter the map and get only one point by degree """ index_max = PC.channels[0].values.index(max(PC.channels[0].values)) for i in range(len(PC.points)): #396 if i != index_max: PC.points[i].x = PC.points[i].y = PC.points[i].z = 0 else: PC.points[i].z = 0 self.pub1.publish(PC) # publish it into the new topic
def match_detections(): distances = [] global legs global faces global pub pc = PointCloud() pc.header = Header(0, rospy.get_rostime(), "/map") for m in range(len(faces)): pc.points.append(Point32(faces[m].x, faces[m].y, 1.5)) for l in range(len(legs)): pc.points.append(Point32(legs[l].x, legs[l].y, 1.0)) distancecol = [] for m in range(len(faces)): # measure the distance between the detections and store them dist = sqrt((faces[m].x - legs[l].x) ** 2 + (faces[m].y - legs[l].y) ** 2) distancecol.append(dist) distances.append(distancecol) print "distances" print distances pub.publish(pc)
def laser_callback(current_laser_scan): real_angles = np.array( [-90.0, -50.0, -30.0, -10.0, 10.0, 30.0, 50.0, 90.0]) real_angles = real_angles / 360.0 * 2 * np.pi opening_angle = 40.0 / 360.0 * 2 * np.pi sensor_angles = np.arange( current_laser_scan.angle_min, current_laser_scan.angle_max + current_laser_scan.angle_increment, current_laser_scan.angle_increment) sensor_ranges = np.array(current_laser_scan.ranges) sonar_readings = np.zeros(len(real_angles)) for i in range(len(real_angles)): sonar_readings[i] = np.min(sensor_ranges[np.where( np.logical_and( sensor_angles >= real_angles[i] - opening_angle / 2, sensor_angles <= real_angles[i] + opening_angle / 2))]) pub = rospy.Publisher("robotic_games/sonar", PointCloud, queue_size=1) output = PointCloud() header = Header() header.stamp = rospy.Time.now() header.frame_id = "robot" output.header = header for i in range(8): #TODO add position of Sensor on Pioneer! output.points.append((Point32( np.sin(real_angles[i]) * sonar_readings[i], np.cos(real_angles[i]) * sonar_readings[i], 0))) pub.publish(output)
def callback(self, data): #rospy.loginfo(rospy.get_caller_id() + "-Received %s,%s,%s,%s", data.num1, data.num2, data.num3, data.num4) #convert data uints into 3D points points = PointCloud() points.header = Header() points.header.frame_id = 'map' point1 = Point() point2 = Point() point3 = Point() point4 = Point() point1.x = self.sensor1_x_from_origin point1.y = self.sensor1_y_from_origin point1.z = (self.sensor1_z_from_origin - data.num1) point2.x = self.sensor2_x_from_origin point2.y = self.sensor2_y_from_origin point2.z = self.sensor2_z_from_origin - data.num2 point3.x = self.sensor3_x_from_origin point3.y = self.sensor3_y_from_origin point3.z = self.sensor3_z_from_origin - data.num3 point4.x = self.sensor4_x_from_origin point4.y = self.sensor4_y_from_origin point4.z = self.sensor4_z_from_origin - data.num4 point_list = [point1, point2, point3, point4] points.points = point_list self.sonar_pts.publish(points)
def aplicarFiltro(data): global taps global N scan = PointCloud() scan.header = data.header for i in range(0, len(data.points)): scan.points.append( Point(data.points[i].x, data.points[i].y, data.points[i].z)) signal = [0.0] * 2 * len(data.points) #x = [0.0]*(kFin-kIni+1) # j = 0 for i in range(0, len(data.points)): signal[i] = data.points[i].z signal[i + len(data.points)] = data.points[len(data.points) - 1].z # x[j+2*(kFin-kIni+1)] = data.ranges[i]; # j = j + 1 filtered_signal = lfilter(taps, 1.0, signal) delay = int(0.5 * (N - 1)) for i in range(len(data.points)): scan.points[i].z = filtered_signal[delay + i] return scan
def cloud_build(self): # add sonar readings (robot-local coordinate frame) to cloud pl = Point32() pm = Point32() pr = Point32() # Instanziiere leere PointCloud cloud = PointCloud() # filling pointcloud header header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'base_link' cloud.header = header # Linke Seite if (self.dist_left < 0.95 and self.dist_left > 0.05): pl.x = self.dist_left + 0.05 pl.y = 0.03 pl.z = 0.0 cloud.points.append(pl) pm.x = (self.dist_left + self.dist_right) / 2 + 0.05 pm.y = 0.0 pm.z = 0.0 cloud.points.append(pm) # Rechte Seite punkt einfügen (x,y,z) if (self.dist_right < 0.95 and self.dist_right > 0.05): pr.x = self.dist_right + 0.05 pr.y = -0.03 pr.z = 0.0 cloud.points.append(pr) # Senden self.cloud_pub.publish(cloud)
def update_particles_with_laser(self, msg): """ Updates the particle weights in response to the scan contained in the msg Args: msg (LaserScan): incoming message """ # Transform to cartesian coordinates scan_points = PointCloud() scan_points.header = msg.header for i, range in enumerate(msg.ranges): if range == 0: continue # Calculate point in laser coordinate frame angle = msg.angle_min + i * msg.angle_increment x = range * np.cos(angle) y = range * np.sin(angle) scan_points.points.append(Point32(x=x, y=y)) # Transform into base_link coordinates scan_points = self.tf_listener.transformPointCloud('base_link', scan_points) # For each particle... for particle in self.particle_cloud: # Create a 3x3 matrix that transforms points from the origin to the particle rotmatrix = np.matrix([[np.cos(particle.theta), -np.sin(particle.theta), 0], [np.sin(particle.theta), np.cos(particle.theta), 0], [0, 0, 1]]) transmatrix = np.matrix([[1, 0, particle.x], [0, 1, particle.y], [0, 0, 1]]) mat33 = np.dot(transmatrix, rotmatrix) # Iterate through the points in the laser scan probabilities = [] for point in scan_points.points: # Move the point onto the particle xy = np.dot(mat33, np.array([point.x, point.y, 1])) # Figure out the probability of that point distToWall = self.occupancy_field.get_closest_obstacle_distance(xy.item(0), xy.item(1)) if np.isnan(distToWall): continue probabilities.append(self.laser_uncertainty_model(distToWall)) # Combine those into probability of this scan given hypothesized location # This is the bullshit thing Paul showed # TODO: exponent should be a rosparam totalProb = np.sum([p ** 3 for p in probabilities]) / len(probabilities) # Update the particle's probability with new info particle.w *= totalProb # Normalize particles self.normalize_particles()
def calcularPosicaoTanque(data): iMin,iMax, estado = calcularLimitesSolda(data) if estado != 3: return -1 dado = PointCloud() dado.header = data.header parteEsquerda = PointCloud() parteEsquerda.header = data.header for i in range (0,min(iMax,iMin)+1): parteEsquerda.points.append(Point(data.points[i].x,0,data.points[i].z)) A,B = determinarCoeficientes(parteEsquerda) C = calcularParametroTanque(A,B) for i in range(0,len(parteEsquerda.points)): ponto_x = data.points[i].x ponto_z = C[0][0] + C[1][0]*data.points[i].x + C[2][0]*(data.points[i].x**2) dado.points.append(Point(ponto_x,0,ponto_z)) if min(iMin,iMax)>4 and max(iMin,iMax) < len(data.points)-4: a,b = estimarLinha(data.points[min(iMin,iMax)-4],data.points[max(iMin,iMax)+4]) else: a,b = estimarLinha(data.points[min(iMin,iMax)],data.points[max(iMin,iMax)]) for i in range (min(iMin,iMax),max(iMin,iMax)): dado.points.append(Point(data.points[i].x,0,a*data.points[i].x+b)) parteDireita = PointCloud() parteDireita.header = data.header for i in range (max(iMax,iMin),len(data.points)): parteDireita.points.append(Point(data.points[i].x,0,data.points[i].z)) A,B = determinarCoeficientes(parteDireita) C = calcularParametroTanque(A,B) for i in range(max(iMax,iMin),len(data.points)): ponto_x = data.points[i].x ponto_z = C[0][0] + C[1][0]*data.points[i].x + C[2][0]*(data.points[i].x**2) dado.points.append(Point(ponto_x,0,ponto_z)) return dado
def points3d_to_pcl(self, points3d): pcl = PointCloud() #transforms the points3d list into a pointcloud pcl.header = self.recognition_header pcl.points = points3d return pcl
def testecalcularCentroSolda(scan): iMin,iMax,estado = calcularLimitesSolda(scan) if estado != 3: return -1 centro = PointCloud() centro.header = scan.header centro.points.append(Point(scan.points[int(round((iMin+iMax)/2))].x,0,0)) return centro
def determinarSinal(scan,tanque): dado = PointCloud() dado.header = scan.header for i in range(0,len(scan.points)): ponto_x = scan.points[i].x ponto_z = (scan.points[i].z - tanque.points[i].z) dado.points.append(Point(ponto_x,0,ponto_z)) return dado
def PointCloud2_to_PointCloud(pc): assert isinstance(pc, PointCloud2) xyz = pc2xyz(pc) msg = PointCloud() msg.header = pc.header msg.points = [Point32(x,y,z) for (x,y,z) in xyz[0,:,:]] return msg
def create_point_cloud(points): point_cloud = PointCloud() header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'map' point_cloud.header = header for point in points: point_cloud.points.append(Point32(point[0], point[1], point[2])) return point_cloud
def transform_polygon(self, polygon, frame_target): """ Use TF Listener to transform the points in a PolygonStamped into a different TF frame...via a PointCloud msg, which is the output data type. """ cloud = PointCloud() cloud.header = polygon.header cloud.points = polygon.polygon.points self.listener.waitForTransform(frame_target, cloud.header.frame_id, cloud.header.stamp, rospy.Duration(1.0)) return self.listener.transformPointCloud(frame_target, cloud)
def get_bounding_box(self, points): """ simple axis-aligned bounding box""" world_frame = "odom_combined" # get pointcloud in world frame if not self.listener.frameExists(points.header.frame_id): rospy.logerr("point cloud header frame %s does not exist!", points.header.frame_id) return if not self.listener.frameExists(world_frame): rospy.logerr("world frame %s does not exist!", world_frame) return pc = PointCloud() pc.header = points.header iter_pt = pts.read_points(points) for pt in iter_pt: pc.points.append(Point(pt[0], pt[1], pt[2])) # TODO: technically, want to transform at time image was taken, but thanks # to waiting for human input, will be arbitrary delay ... pc.header.stamp = self.listener.getLatestCommonTime( world_frame, pc.header.frame_id) # TODO: should be wrapped in try/except block pc_world = self.listener.transformPointCloud(world_frame, pc) # get world-axis-aligned bounding box min_x = sys.float_info.max max_x = sys.float_info.min min_y = sys.float_info.max max_y = sys.float_info.min min_z = sys.float_info.max max_z = sys.float_info.min for pt in pc_world.points: min_x = min(min_x, pt.x) max_x = max(max_x, pt.x) min_y = min(min_y, pt.y) max_y = max(max_y, pt.y) min_z = min(min_z, pt.z) max_z = max(max_z, pt.z) # testing whether fudging it helps #min_z = min_z + 0.05 # add bounding box to the planning scene obj_pose = PoseStamped() obj_pose.pose.position.x = (min_x + max_x) / 2 obj_pose.pose.position.y = (min_y + max_y) / 2 obj_pose.pose.position.z = (min_z + max_z) / 2 obj_pose.pose.orientation.z = 1.0 obj_pose.header.frame_id = world_frame obj_dims = Vector3() obj_dims.x = max_x - min_x obj_dims.y = max_y - min_y obj_dims.z = max_z - min_z return (obj_pose, obj_dims)
def GetLocalPC(self, data): pc2_msg = self.lp.projectLaser(data) point_generator = pc2.read_points(pc2_msg) pc_msg = PointCloud() pc_msg.header = pc2_msg.header pc_msg.channels.append(ChannelFloat32()) for p in point_generator: pc_msg.points.append(Point32(p[0], p[1], p[2])) pc_msg.channels[0].values.append(0) return pc_msg
def sub_cb(self, msg): pnt = PointCloud() pnt.header = msg.header angle_step = 1.0 / (msg.range * 100.0) angle = -msg.field_of_view if msg.range < msg.max_range and msg.range > msg.min_range: while angle < msg.field_of_view: pnt.points.append(Point32(msg.range * cos(angle), msg.range * sin(angle), 0)) angle += angle_step self.pub.publish(pnt)
def sub_cb(self, msg): pnt = PointCloud() pnt.header = msg.header angle_step = 1.0/(msg.range*100.0) angle = -msg.spread_angle if msg.range < msg.range_max and msg.range > msg.range_min: while angle < msg.spread_angle: pnt.points.append(Point32(msg.range*cos(angle), msg.range*sin(angle), 0)) angle += angle_step self.pub.publish(pnt)
def create_pointcload(points, res): obscacles_cloud = PointCloud() header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = '/simulator' obscacles_cloud.header = header for p in points: obscacles_cloud.points.append( Point32(p[0] * res, p[1] * res, p[2] * res)) return obscacles_cloud
def get_bounding_box(self, points): """ simple axis-aligned bounding box""" world_frame = "odom_combined" # get pointcloud in world frame if not self.listener.frameExists(points.header.frame_id): rospy.logerr("point cloud header frame %s does not exist!", points.header.frame_id) return if not self.listener.frameExists(world_frame): rospy.logerr("world frame %s does not exist!", world_frame) return pc = PointCloud() pc.header = points.header iter_pt = pts.read_points(points) for pt in iter_pt: pc.points.append(Point(pt[0], pt[1], pt[2])) # TODO: technically, want to transform at time image was taken, but thanks # to waiting for human input, will be arbitrary delay ... pc.header.stamp = self.listener.getLatestCommonTime(world_frame, pc.header.frame_id) # TODO: should be wrapped in try/except block pc_world = self.listener.transformPointCloud(world_frame, pc) # get world-axis-aligned bounding box min_x = sys.float_info.max max_x = sys.float_info.min min_y = sys.float_info.max max_y = sys.float_info.min min_z = sys.float_info.max max_z = sys.float_info.min for pt in pc_world.points: min_x = min(min_x, pt.x) max_x = max(max_x, pt.x) min_y = min(min_y, pt.y) max_y = max(max_y, pt.y) min_z = min(min_z, pt.z) max_z = max(max_z, pt.z) # testing whether fudging it helps min_z = min_z + 0.05 # add bounding box to the planning scene obj_pose = PoseStamped() obj_pose.pose.position.x = (min_x + max_x)/2 obj_pose.pose.position.y = (min_y + max_y)/2 obj_pose.pose.position.z = (min_z + max_z)/2 obj_pose.pose.orientation.z = 1.0 obj_pose.header.frame_id = world_frame obj_dims = Vector3() obj_dims.x = max_x - min_x obj_dims.y = max_y - min_y obj_dims.z = max_z - min_z return (obj_pose, obj_dims)
def process_scan(self, m): pcloud = PointCloud() header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = "map" pcloud.header = header lidar_range_rad = np.deg2rad(270) step = 8 theta = np.deg2rad(self.current_angle) for i in range(0, len(m.ranges), step): d = m.ranges[i] phi = np.deg2rad(Node.translate(i, 0.0, float(len(m.ranges)), 0.0, 270)) - 0.7843 xyz = Node.spherical_to_cartesian(d, theta, phi) pcloud.points.append(Point32(xyz[0], xyz[1], xyz[2])) if self.print_once: if not self.printed: time.sleep(2) xyzs = [] xs = [] ys = [] zs = [] phis = [] for i in range(len(m.ranges)): phi = np.deg2rad(Node.translate(i, 0.0, float(len(m.ranges)), 0.0, 270)) - 0.7843 phis.append(np.rad2deg(phi)) d = m.ranges[i] xyz = Node.spherical_to_cartesian(d, theta, phi) xyzs.append(xyz) xs.append(xyz[0]) ys.append(xyz[1]) zs.append(xyz[2]) f, axarr = plt.subplots(2, 2) axarr[0, 0].plot(phis, m.ranges) axarr[0, 0].set_title("phi vs distance") axarr[0, 1].plot(phis, xs) axarr[0, 1].set_title("phi vs X values") # axarr[0,1].text(0,0,"x = r * np.sin(theta) * np.cos(phi)",fontsize=12) axarr[1, 0].plot(phis, zs) axarr[1, 0].set_title("phi vs Z values") plt.show() self.printed = True print("done plotting") # print Node.pp_list(m.ranges) try: self.publisher.publish(pcloud) except ROSException: pass
def sub_cb(self, msg): pnt = PointCloud() pnt.header = msg.header angle_step = 1.0 / (msg.range * 100.0) angle = -msg.spread_angle if msg.range < msg.range_max and msg.range > msg.range_min: while angle < msg.spread_angle: pnt.points.append( Point32(msg.range * cos(angle), msg.range * sin(angle), 0)) angle += angle_step self.pub.publish(pnt)
def sub_cb(self, msg): pnt = PointCloud() pnt.header = msg.header angle_step = 1.0 / (msg.range * 100.0) angle = -msg.field_of_view if msg.range < msg.max_range and msg.range > msg.min_range: while angle < msg.field_of_view: pnt.points.append( Point32(msg.range * cos(angle), msg.range * sin(angle), 0)) angle += angle_step self.pub.publish(pnt)
def generatePointCloud(obstacleCart): global cloud h = Header() h.stamp = rospy.Time.now() h.frame_id = "laser" numberOfPoints = len(obstacleCart) cloud = PointCloud() cloud.header = h point = [] for i in range(0,numberOfPoints): cloud.points.append(Point32( obstacleCart[i][0], obstacleCart[i][1], 0 )) return cloud
def publish_tracking(points, odom_tracker_key): msg = PointCloud() msg.points = [] msg.header = Header() msg.header.stamp = rospy.Time.now() msg.header.frame_id = 'odom' for p in points: msg.points.append(Point(p.x, p.y, 0)) odom_out_publishers[odom_tracker_key].publish(msg)
def GetUnifiedPointcloud(self, pointcloud_list): pointcloudUnified = PointCloud() pointcloudUnified.points = [] pointcloudUnified.channels = [] if len(pointcloud_list) > 0: pointcloudUnified.header = pointcloud_list[0].header # They all have different headers, but just use the first one. for pointcloud in pointcloud_list: pointcloudUnified.points.extend(pointcloud.points) pointcloudUnified.channels.extend(pointcloud.channels) return pointcloudUnified
def pointToPointcloud( self, x, y, z ): # FROM -> https://answers.ros.org/question/207071/how-to-fill-up-a-pointcloud-message-with-data-in-python/?answer=218951#post-id-218951 my_awesome_pointcloud = PointCloud() # declaring pointcloud #filling pointcloud header header = Header() header.stamp = rospy.Time.now() header.frame_id = 'map' my_awesome_pointcloud.header = header # assign header to pointcloud my_awesome_pointcloud.points.append(Point32(x, y, z)) #filling some points self.pointcloud_publisher.publish(my_awesome_pointcloud) # publish
def determinarDerivada(scan): derivada = PointCloud() derivada.header = scan.header derivada.points = [Point(0,0,0)]*(len(scan.points)-1) for i in range(0,len(derivada.points)): ponto_x = scan.points[i].x ponto_y = 0 if math.fabs(scan.points[i+1].x-scan.points[i].x) > 0.000001: ponto_z = (scan.points[i+1].z-scan.points[i].z)/(scan.points[i+1].x-scan.points[i].x) else: ponto_z = 0 derivada.points[i] = Point(ponto_x,ponto_y,ponto_z) return derivada
def publish_frontiers(self): point_cloud = PointCloud() point_cloud.header = std_msgs.msg.Header() point_cloud.header.stamp = rospy.Time.now() point_cloud.header.frame_id = '/orb_slam/map' off_x, off_y = self.grid.info.origin.position.x, self.grid.info.origin.position.y point_cloud.points = [Point32( (x+.5) * self.grid.info.resolution + off_x, (y+.5) * self.grid.info.resolution + off_y, 0.0 ) for (y, x) in self.frontiers ] publisher = OneTimePublisher("exploration_frontiers", PointCloud, point_cloud) publisher.start()
def lidarinfo(data): p = PointCloud() p.points = [] p.header = data.header current_angle = data.angle_min for i in data.ranges: point = Point32() point.x = i * math.cos(current_angle) point.y = i * math.sin(current_angle) current_angle += data.angle_increment point.z = 0 p.points += [point] pub.publish(p)
def __publishPointCloud(self, x, y, z, publ, step=100): pointcloud = PointCloud() header = std_msgs.msg.Header() header.stamp = self.stamp_now #rospy.Time.now() header.frame_id = 'camera_depth_optical_frame' #header.frame_id = 'camera_link' pointcloud.header = header for i in range(0, len(x), step): pointcloud.points.append(Point32(x[i], y[i], z[i])) publ.publish(pointcloud)
def get_lidar_pcd(self, data): lidar_pcd = PointCloud() header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'lidar' lidar_pcd.header = header for idx in range(data.shape[0]): lidar_pcd.points.append( Point32(data[idx, 0], data[idx, 1], data[idx, 2])) return lidar_pcd
def GetUnifiedPointcloud(self, pointcloud_list): pointcloudUnified = PointCloud() pointcloudUnified.points = [] pointcloudUnified.channels = [] if len(pointcloud_list) > 0: pointcloudUnified.header = pointcloud_list[0].header # They all have different headers, but just use the first one. pointcloudUnified.channels.append(ChannelFloat32(name='intensity', values=[])) for pointcloud in pointcloud_list: pointcloudUnified.points.extend(pointcloud.points) pointcloudUnified.channels[0].values.extend(pointcloud.channels[0].values) return pointcloudUnified
def get_data(self, var): PC = PointCloud( ) # create the new object PointCloud to be published later PC.header = var.header # copy the content of topic to another PointCloud in order to manipulate it PC.channels = var.channels PC.points = var.points for i in range((len(PC.points))): PC.points[i].z = PC.channels[0].values[ i] / 100 # write in the z axis of the points the values of the "intensity" divided by 100 to make it readable on rviz self.pub.publish(PC) # publish it into the new topic
def publish_points(self, pcloud, seq): pc = PointCloud() pc.header = Header(seq, rospy.Time.now(), 'points') for i in range(len(pcloud[:, 0])): # Publishing as (z, x, y) b/c of the way that Z is forward in # triangulation pc.points.append(Point32(pcloud[i, 2], pcloud[i, 0], pcloud[i, 1])) self.point_pub.publish(pc) self.tf_broadcaster.sendTransform((0, 0, 0), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), 'points', 'map')
def gmm_cloud_publish(self, data): h = std_msgs.msg.Header() h.stamp = rospy.Time.now() h.frame_id = "map" pcl = PointCloud() pcl.header = h pcl.points = [] pcl.channels = [] for i in range(0, data.shape[1]): point = Point32() point.x = data[0,i] point.y = data[1,i] point.z = data[2,i] pcl.points.append(point) self.gmmPub.publish(pcl)
def main(): viz_pub = rospy.Publisher('/cluster_boxes', Marker) cloud_pub = rospy.Publisher('/cluster_points', PointCloud) segmentation_service = rospy.ServiceProxy(SEGMENTATION_SERVICE, TabletopSegmentation) rospy.loginfo('Waiting for object recognition service') segmentation_service.wait_for_service() rospy.loginfo('Found segmentation service!') box_service = rospy.ServiceProxy('/find_cluster_bounding_box', FindClusterBoundingBox) rospy.loginfo('Waiting for find cluster bounding box service') box_service.wait_for_service() rospy.loginfo('Found cluster bounding box service!') req = TabletopSegmentationRequest() resp = segmentation_service(req) rospy.loginfo('Found %d clusters', len(resp.clusters)) rospy.loginfo('Table pose is:\n' + str(resp.table.pose)) table = marker_at(resp.table.pose, mtype = Marker.TRIANGLE_LIST, ns = 'table', sx = 1.0, sy = 1.0, sz = 1.0, r = 0.0, g = 1.0, b = 0.0, a = 0.4) for tri in resp.table.convex_hull.triangles: for ind in tri.vertex_indices: table.points.append(resp.table.convex_hull.vertices[ind]) viz_pub.publish(table) pc = PointCloud() pc.header = resp.table.pose.header for (mid, cluster) in enumerate(resp.clusters): box_resp = box_service.call(FindClusterBoundingBoxRequest(cluster = cluster)) marker = marker_at( box_resp.pose, ns = str(mid), mid = mid, mtype = Marker.CUBE, sx = box_resp.box_dims.x, sy = box_resp.box_dims.y, sz = box_resp.box_dims.z, a = 0.5) viz_pub.publish(marker) pose = PoseStamped() pose.header = cluster.header pose.pose.orientation.w = 1.0 marker = marker_at(pose, ns = 'cluster', mid = mid, mtype = Marker.POINTS, sx = 1.0, sy = 1.0, sz = 1.0, r = 1.0, g = 0.0, b = 0.0, a = 1.0) marker.points = cluster.points #viz_pub.publish(marker) pc.points += cluster.points pc.channels += cluster.channels rospy.loginfo('Cluster frame is ' + cluster.header.frame_id) cloud_pub.publish(pc)
#!/usr/bin/env python import rospy import numpy as np import tf import mil_ros_tools from sensor_msgs.msg import PointCloud rospy.init_node("ground_finder") pub = rospy.Publisher("ground_est", PointCloud, queue_size=1) listener = tf.TransformListener() pc = PointCloud() pc.header = mil_ros_tools.make_header(frame="map") pc.points = [] rate = rospy.Rate(1.0) while not rospy.is_shutdown(): try: t = listener.waitForTransform('/map', '/ground', rospy.Time.now(), rospy.Duration(1)) (trans, rot) = listener.lookupTransform('/map', '/ground', rospy.Time(0)) except (tf.Exception, tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.logwarn("TF waitForTransform timeout") continue pc.points.append(mil_ros_tools.numpy_to_point(np.array(trans))) pub.publish(pc) rate.sleep()
while not rospy.is_shutdown(): PC.header.seq = counter PC.header.stamp = rospy.Time.now() counter += 1 pub.publish(PC) br.sendTransform((0, 0, 0), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), 'points', 'map') r.sleep() if __name__ == '__main__': rospy.init_node('talker', anonymous=True) P = PointCloud() P.header = Header(0, 0, 'points') P.points.append(Point32(2, 0, 0)); P.points.append(Point32(3, 0, 0)) P.points.append(Point32(4, 0, 0)); P.points.append(Point32(5, 0, 0)) P.points.append(Point32(0, 3, 0)); P.points.append(Point32(0, 6, 0)) P.points.append(Point32(0, 6.5, 0)); P.points.append(Point32(0, 7, 0)) P.points.append(Point32(0, 0, 2)); P.points.append(Point32(0, 0, -2)) P.points.append(Point32(0, 0, 1)); P.points.append(Point32(0, 0, -1)) # for i in range(2): # P.points.append(Point32(i, i, i)) talker(P)