def __init__(self): self.pub_out = rospy.Publisher("command", ChannelFloat32, tcp_nodelay=True) self.output = ChannelFloat32() self.input = ChannelFloat32() self.output.name = "output" self.output.values = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] self.input.name = "input" self.input.values = [0, 0, 0, 0] self.fan_temp_on = 60 self.fan_temp_off = 55 self.lights = [LightControl("light_controller", self, [3, 6, 2])] self.motors = [ TrajectoryControl("tray_controller", "tray_joint", self, 0, 0) ] self.fans = [FanControl("fan_controller", self, 7)] rospy.Subscriber("state", ChannelFloat32, self.InputCallback) #reset all motors for m in self.motors: m.move0()
def createPointCloudMsg(points, rgbs): pc_msg = PointCloud() pc_msg.header.frame_id = "aruco_slam_world" r_c = ChannelFloat32() g_c = ChannelFloat32() b_c = ChannelFloat32() r_c.name = "r" g_c.name = "g" b_c.name = "b" for i in range(len(points)): p = points[i] _p = Point32() _p.x = p[0] _p.y = p[1] _p.z = p[2] pc_msg.points.append(_p) rgb = rgbs[i] r = abs(rgb[2] - 255) g = abs(rgb[1] - 255) b = abs(rgb[0] - 255) r_c.values.append(r) g_c.values.append(g) b_c.values.append(b) pc_msg.channels.append(r_c) pc_msg.channels.append(g_c) pc_msg.channels.append(b_c) return pc_msg
def tensor_2d_to_pointcloud(self, ndarray, axis0_size_m, frame="/map_ned"): msg = PointCloud() msg.header.frame_id = frame msg.header.stamp = rospy.get_rostime() # Shift and scale to 0-1 range map_data_np = ndarray[:, :, :3] map_data_np = map_data_np - np.min(map_data_np) map_data_np /= (np.max(map_data_np) + 1e-9) c = np.linspace(0, axis0_size_m, map_data_np.shape[1]) cgrid = np.asarray(np.meshgrid(c, c)) r = ChannelFloat32() r.name = "r" g = ChannelFloat32() g.name = "g" b = ChannelFloat32() b.name = "b" msg.channels.append(r) msg.channels.append(g) msg.channels.append(b) for x in range(map_data_np.shape[0]): for y in range(map_data_np.shape[1]): p = Point32() p.x = cgrid[0, x, y] p.y = cgrid[1, x, y] p.z = 0.05 r.values.append(float(map_data_np[x, y, 0])) g.values.append(float(map_data_np[x, y, 1])) b.values.append(float(map_data_np[x, y, 2])) msg.points.append(p) return msg
def callback(data): global left global right #global frame img = bridge.imgmsg_to_cv2(data) image = process_image(img) image = bridge.cv2_to_imgmsg(image) pub_image.publish(image) left_points, right_points = pub_scatter(left, right) left_msg = PointCloud() left_msg.header.stamp = rospy.Time.now() left_msg.header.frame_id = "footprint" left_msg.points = left_points left_msg_chan = ChannelFloat32() left_msg_chan.name = 'value' left_msg_chan.values = [float(3)] left_msg.channels = [left_msg_chan] right_msg = PointCloud() right_msg.header.stamp = rospy.Time.now() right_msg.header.frame_id = "footprint" right_msg.points = right_points right_msg_chan = ChannelFloat32() right_msg_chan.name = 'value' right_msg_chan.values = [float(3)] right_msg.channels = [right_msg_chan] pub_point_cloud_right.publish(right_msg) pub_point_cloud_left.publish(left_msg)
def local_rand_cloud(): #Create a new cloud object and stuff it ROS_pointcloud = PointCloud() #sensor_msgs.msg.PointCloud() x = random() * 5 #const offset easy to change. #stuff point cloud with random points in a rectangle which drifts over time. #Add an extra intensity channel as well. intensity_channel = ChannelFloat32() intensity_channel.name = 'intensities' for i in range(3000): intensity_channel.values += [random()] for i in range(3000): ROS_pointcloud.points += [ Point32(random() * 5 - x, random(), intensity_channel.values[i]) ] #rgb color has maximum value of 0xFFFFFF rgb_channel = ChannelFloat32() rgb_channel.name = 'rgb' rgb_channel.values = normList(intensity_channel.values, 0xFF) #Separate r,g,b channels range betweeon 0 and 1.0. r, g, b = [], [], [] for pt in ROS_pointcloud.points: b += [pt.y] r += [pt.z] g += [0] r_channel = ChannelFloat32(name='r', values=normList(r)) g_channel = ChannelFloat32(name='g', values=normList(g)) b_channel = ChannelFloat32(name='b', values=b) ROS_pointcloud.channels = (intensity_channel, rgb_channel, r_channel, g_channel, b_channel) return ROS_pointcloud
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 __init__(self): self.kernal = self.make_kernal(11, 'circle') self.kernal1 = self.make_kernal(6, 'rect') self.kernal2 = self.make_kernal(11, 'rect') self.kernal3 = self.make_kernal(25, 'circle') self.kernal4 = self.make_kernal(5, 'circle') self.kernal5 = self.make_kernal(5, 'rect') self.kernal6 = self.make_kernal(25, 'circle') self.kernal_size = 25 self.kernal7 = self.make_kernal(self.kernal_size, 'circle') self.kernal8 = self.make_kernal(2, 'rect') self.kernal9 = self.make_kernal(2, 'rect') self.kernal10 = self.make_kernal(45, 'circle') self.M = np.load('M_gs1_newSensor_427_320.npy') # self.ROImask = np.load('mask_GS2.npy self.previous_slip = False self.previous_u_sum = np.array([0]) self.previous_v_sum = np.array([0]) self.static_flag = False self.index = 0 self.cols, self.rows, self.cha = 320, 427, 3 self.x1_last = [] self.y1_last = [] self.highbar_top = 130 self.lowbar_top = 100 self.highbar_down = 80 self.lowbar_down = 60 self.img_counter = 0 self.thre = 80 self.scale = 1 self.con_flag = False self.refresh = False self.restart = False self.initial_flag = True self.length_flag = True self.marker_refflag = True self.showimage = True self.thre_slip_dis = 4.5 self.thre_slip_num = 7 self.slip_indicator = False self.slip_monitor = ChannelFloat32() self.tran_matrix_msg = ChannelFloat32() self.slip_pub = rospy.Publisher('/raspicam_node1/slip_monitor', ChannelFloat32, queue_size=1) #change to your own rostopic name to subsribe gelslim raw images self.image_sub = rospy.Subscriber("/raspicam_node1/image/compressed", CompressedImage, self.call_back, queue_size=1, buff_size=2**24) self.s = rospy.Service('raspicam_node1/restart_detector', std_srvs.srv.Empty, self.restart_detector_server) self.useSlipDetection = True #True to detect slip, False to detect collision self.collideThre = 1.5
def get_new_pointcloud(self): from sensor_msgs.msg import PointCloud from geometry_msgs.msg import Point32 from sensor_msgs.msg import ChannelFloat32 Points = [Point32(1,2,3), Point32(4,5,6)] Channels = [ChannelFloat32("myname1", [1.23, 4.56]), ChannelFloat32("myname2", [1.231, 4.561])] return PointCloud(None, Points, Channels)
def callback(blobs_msg): puck_array = PuckArray() vis_pucks = PointCloud() vis_pucks.header.frame_id = '/base_link' channel = ChannelFloat32() channel.name = "intensity" for blob in blobs_msg.blobs: try: puck = Puck() puck.xi = blob.x puck.yi = blob.y puck.type = blob.type (puck.position.x, puck.position.y) = corresDict[puck.xi, puck.yi] puck_array.pucks.append(puck) vis_pucks.points.append(puck.position) channel.values.append(0) except: continue puck_publisher.publish(puck_array) vis_pucks.channels.append(channel) vis_pucks_publisher.publish(vis_pucks)
def to_pointcloud(self, frame): """Returns a PointCloud message corresponding to slice. Args: frame: Frame ID. Returns: A sensor_msgs.msg.PointCloud. """ # Construct PointCloud message. cloud = PointCloud() cloud.header.frame_id = frame cloud.header.stamp = self.timestamp # Convert bins to list of Point32 messages. nbins = self.config["nbins"] r_step = self.range / nbins x_unit = math.cos(self.heading) * r_step y_unit = math.sin(self.heading) * r_step cloud.points = [ Point32(x=x_unit * r, y=y_unit * r, z=0.00) for r in range(1, nbins + 1) ] # Set intensity channel. channel = ChannelFloat32() channel.name = "intensity" channel.values = self.bins cloud.channels = [channel] return cloud
def get_point_cloud_as_msg(self): """Return a ROS point cloud sensor message with the points representing the plume's particles and one channel containing the time of creation for each particle. > **Returns** The plume particles as a `sensor_msgs/PointCloud` message or `None` if no particles are found. """ if self._pnts is None or self._time_creation is None: return None pc = PointCloud() pc.header.stamp = rospy.Time.now() pc.header.frame_id = 'world' if self._pnts is None: return None pc.points = [ Point32(x, y, z) for x, y, z in zip(self.x, self.y, self.z) ] channel = ChannelFloat32() channel.name = 'time_creation' if self._time_creation is None: return None channel.values = self._time_creation.tolist() pc.channels.append(channel) return pc
def publish_variables(self, event): t = rospy.get_time() for var in self._variables: pc_msg = PointCloud() pc_msg.header.stamp = rospy.Time.now() pc_msg.header.frame_id = 'world' for name in self._vehicle_pos: value = self._interpolate(var, self._vehicle_pos[name][0], self._vehicle_pos[name][1], self._vehicle_pos[name][2], t) # Updating the point cloud for this variable pc_msg.points.append( Point32(self._vehicle_pos[name][0], self._vehicle_pos[name][1], self._vehicle_pos[name][2])) pc_msg.channels.append(ChannelFloat32()) pc_msg.channels[-1].name = 'intensity' pc_msg.channels[-1].values.append(value) # Create the message objects if 'temperature' in var.lower(): msg = Temperature() msg.header.stamp = rospy.Time.now() msg.header.frame_id = '%s/base_link' % name # TODO Read from the unit of temperature from NC file # Converting to Celsius msg.temperature = value - 273.15 else: msg = FloatStamped() msg.header.stamp = rospy.Time.now() msg.header.frame_id = '%s/base_link' % name msg.data = value self._topics[name][var].publish(msg) self._pc_variables[var].publish(pc_msg) return True
def create_point_cloud(self, sortedRewards, min_len): c = PointCloud() c.header.seq = 1 c.header.stamp = rospy.Time.now() c.header.frame_id = '/map' c.points = [] channel = ChannelFloat32() channel.name = "Values" channel.values = [] c.channels = [channel] for element in sortedRewards[0:min_len]: p = Point() x = self.map_msg.info.origin.orientation.x y = self.map_msg.info.origin.orientation.y z = self.map_msg.info.origin.orientation.z w = self.map_msg.info.origin.orientation.w roll, pitch, yaw = euler_from_quaternion((x, y, z, w)) offset_x = self.map_msg.info.origin.position.x offset_y = self.map_msg.info.origin.position.y p.y = (element[0] * np.cos(yaw) + element[1] * np.sin(yaw)) * self.map_msg.info.resolution + offset_y p.x = (element[1] * np.cos(yaw) - element[0] * np.sin(yaw)) * self.map_msg.info.resolution + offset_x c.points.append(p) channel.values.append(element[2]) return c
def create_point_cloud(self, xs, ys, vals): c = PointCloud() c.header.seq = 1 c.header.stamp = rospy.Time.now() c.header.frame_id = '/map' c.points = [] channel = ChannelFloat32() channel.name = "Values" channel.values = [] c.channels = [channel] for i in range(len(xs)): p = Point() x = self.map_msg.info.origin.orientation.x y = self.map_msg.info.origin.orientation.y z = self.map_msg.info.origin.orientation.z w = self.map_msg.info.origin.orientation.w roll, pitch, yaw = euler_from_quaternion((x, y, z, w)) offset_x = self.map_msg.info.origin.position.x offset_y = self.map_msg.info.origin.position.y p.y = (xs[i] * np.cos(yaw) + ys[i] * np.sin(yaw)) * self.map_msg.info.resolution + offset_y p.x = (ys[i] * np.cos(yaw) - xs[i] * np.sin(yaw)) * self.map_msg.info.resolution + offset_x c.points.append(p) channel.values.append(vals[i]) return c
def cb_shovel(msg): global excav_x, excav_y, _pub data = ChannelFloat32() data.name = "Dist, Angle" data.values = [np.sqrt((msg.x - excav_x)**2 + (msg.y - excav_y)**2), msg.theta] # print("Data: {}".format(data)) if _pub: _pub.publish(data)
def __init__(self, name): self.name = name #name of node and npy file self.pub = rospy.Publisher(name, PointCloud, queue_size=5) self.seg_data = np.load('/home/sun/catkin_ws/src/img_jpg/data/' + self.name + '.npy') #seg result npy self.custom_point = PointCloud() # data to send self.custom_point.header.frame_id = "camera_link" self.channel = ChannelFloat32()
def publish(self): data = map(float, self.intf.get_input()) for a in self.analog_ch: data[a] = self.intf.get_analog(a) / float(1023) ch = ChannelFloat32() ch.name = "avr_measurements" ch.values = data self.pub_marker.publish(ch)
def __init__(self): self._cv_bridge = CvBridge() self._sub = rospy.Subscriber('/map', mp, self.callback, queue_size=1000) self._pub = rospy.Publisher('Semantic_Map', PointCloud, queue_size=1) self._currentframe = Image() self._framequeue = [] self._kp = PointCloud() self.smp = PointCloud() self._mpqueue = [] self._kpqueue = [] self._labelqueue = [] self.smp.channels = [ChannelFloat32(), ChannelFloat32()] self.smp.channels[0].name = 'category' self.smp.channels[1].name = 'rgb' self.temp = 0
def emitPointCloud(points): global cloud_pub head = Header() head.seq = 1 head.stamp = rospy.Time.now() head.frame_id = "/map" points = [ Point32( x, y, 0.0 ) for x, y in points ] pc = PointCloud( head, points, [ ChannelFloat32("", []) for _ in range(len(points))]) cloud_pub.publish( pc )
def __init__(self): rospy.init_node('radar2pcd_pa_node.py', anonymous=True) rospy.Subscriber('radar_messages', radar_msg, self.callback) self.pcpub = rospy.Publisher('radar_pcd', PointCloud, queue_size=10) # PointCloud self.out_Pc = PointCloud() self.out_Pc.header = std_msgs.msg.Header() self.out_Pc.header.stamp = rospy.Time.now() self.out_Pc.header.frame_id = "radar" num_targets = 48 # -- None here represent data type # this data type is point self.out_Pc.points = [None] * num_targets # -- channels must be ChannelFloat32() self.out_Pc.channels = [ChannelFloat32(), ChannelFloat32(), \ ChannelFloat32(), ChannelFloat32(), ChannelFloat32(), \ ChannelFloat32(), ChannelFloat32(), ChannelFloat32(), \ ChannelFloat32(), ChannelFloat32()] self.out_Pc.channels[0].values = [None] * num_targets self.out_Pc.channels[1].values = [None] * num_targets self.out_Pc.channels[2].values = [None] * num_targets self.out_Pc.channels[3].values = [None] * num_targets self.out_Pc.channels[4].values = [None] * num_targets self.out_Pc.channels[5].values = [None] * num_targets self.out_Pc.channels[6].values = [None] * num_targets self.out_Pc.channels[7].values = [None] * num_targets self.out_Pc.channels[8].values = [None] * num_targets self.out_Pc.channels[9].values = [None] * num_targets self.out_Pc.channels[0].name = 'ID' self.out_Pc.channels[1].name = 'distance' self.out_Pc.channels[2].name = 'velocity' self.out_Pc.channels[3].name = 'power' self.out_Pc.channels[4].name = 'angle' self.out_Pc.channels[5].name = 'distance_deviation' self.out_Pc.channels[6].name = 'angle_deviation' self.out_Pc.channels[7].name = 'velocity_deviation' self.out_Pc.channels[8].name = 'proability_target' self.out_Pc.channels[9].name = 'is_target'
def tensor_3d_to_pointcloud(self, ndarray, axis0_size_m, frame="/map_ned"): msg = PointCloud() msg.header.frame_id = frame msg.header.stamp = rospy.get_rostime() # Shift and scale to 0-1 range map_data_np = ndarray[:, :, :, :3] map_data_np = map_data_np - np.min(map_data_np) map_data_np /= (np.max(map_data_np) + 1e-9) c = np.linspace(0, axis0_size_m, map_data_np.shape[1]) cgrid = np.asarray(np.meshgrid(c, c, c)) r = ChannelFloat32() r.name = "r" g = ChannelFloat32() g.name = "g" b = ChannelFloat32() b.name = "b" msg.channels.append(r) msg.channels.append(g) msg.channels.append(b) for x in range(map_data_np.shape[0]): for y in range(map_data_np.shape[1]): for z in range(map_data_np.shape[2]): col = map_data_np[x, y, z, :] # TODO: Clear up this hack if np.linalg.norm(col) < 0.1: continue # Add multiple overlapping points to increase alpha value intensity = int((np.linalg.norm(col)) * 10) for i in range(intensity): p = Point32() p.x = cgrid[0, x, y, z] p.y = cgrid[1, x, y, z] p.z = cgrid[2, x, y, z] r.values.append(min(float(col[0]) * 10, 1.0)) g.values.append(min(float(col[1]) * 10, 1.0)) b.values.append(min(float(col[2]) * 10, 1.0)) msg.points.append(p) print(f"Generated pointcloud with {len(msg.points)} points") return msg
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 PointCloud_Gen(points, frameID='rslidar'): from sensor_msgs.msg import PointCloud, ChannelFloat32 from geometry_msgs.msg import Point32 import numpy as np ##=========PointCloud=============== points = np.array(points, np.float32) point_cloud = points.reshape((-1, 3)) pointx = point_cloud[:, 0].flatten() pointy = point_cloud[:, 1].flatten() pointz = point_cloud[:, 2].flatten() * -1.0 # intensity = point_cloud[:, 3].flatten() intensity = np.ones(pointx.shape, dtype=np.float32) # labels = point_cloud[:,6].flatten() seg_point = PointCloud() seg_point.header.frame_id = frameID channels1 = ChannelFloat32() seg_point.channels.append(channels1) seg_point.channels[0].name = "rgb" channels2 = ChannelFloat32() seg_point.channels.append(channels2) seg_point.channels[1].name = "intensity" for i in range(point_cloud.shape[0]): seg_point.channels[1].values.append(intensity[i]) if True: # labels[i] == 1: seg_point.channels[0].values.append(255) geo_point = Point32(pointx[i], pointy[i], pointz[i]) seg_point.points.append(geo_point) # else: # seg_point.channels[0].values.append(255255255) # geo_point = Point32(pointx[i], pointy[i], pointz[i]) # seg_point.points.append(geo_point) # elif result[i] == 2: # seg_point.channels[0].values.append(255255255) # geo_point = Point32(pointx[i], pointy[i], pointz[i]) # seg_point.points.append(geo_point) # elif result[i] == 3: # seg_point.channels[0].values.append(255000) # geo_point = Point32(pointx[i], pointy[i], pointz[i]) # seg_point.points.append(geo_point) return seg_point
def sub_track(track, begin, end): track2 = TrackMsg() track2.header = track.header track2.id = track.id track2.pose = track.pose[begin:end] for channel in track.channels: ch = ChannelFloat32() ch.name = channel.name ch.values = channel.values[begin:end] track2.channels.append(ch) return track2
def ppl_cb(self, msg): self.p = PointCloud() self.p.header.frame_id = 'map' self.p.header.stamp = rospy.get_rostime() chan = ChannelFloat32() chan.name = "intensity" for n, i in enumerate(msg.persons): self.p.points.append(i.pose.position) self.p.points[n].z = 0.0 chan.values.append(100) self.p.channels.append(chan)
def readtrack(filename): rospy.loginfo("Opening logfile '%s'", filename) msg = TrackMsg() channel_w = None channel_h = None for line in fileinput.input(filename): line = line.strip() # rospy.loginfo("Reading line: '%s'",line) data = line.strip().split() data_float = [float(column) for column in data] if len(data) < 3: continue msg.header.stamp = rospy.get_rostime() msg.header.frame_id = "/" msg.id = 0 pose = Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1)) if len(data) >= 3: pose.position.x = data_float[0] pose.position.y = data_float[1] pose.position.z = data_float[2] if len(data) >= 7: pose.orientation.x = data_float[3] pose.orientation.y = data_float[4] pose.orientation.z = data_float[5] pose.orientation.w = data_float[6] if len(data) >= 9: if channel_w is None or channel_h is None: channel_w = ChannelFloat32('width', []) channel_h = ChannelFloat32('height', []) msg.channels.append(channel_w) msg.channels.append(channel_h) channel_w.values.append(data_float[7]) channel_h.values.append(data_float[8]) msg.pose.append(pose) return msg
def publish_poops_to_cost_map(self, poops): """Always publishes 30 poops. Keeps unused ones at map position 25, 25.""" NUM_POOPS = 30 poops_with_z = [(p[0], p[1], 0) for p in poops] unused_poops = [(25, 25, 25)] * (NUM_POOPS - len(poops)) poop_positions = poops_with_z + unused_poops #print poop_positions point_cloud = PointCloud() # cost map doesn't like the map coordinate frame point_cloud.header = Header(stamp=Time.now(), frame_id='odom_combined') point_cloud.points = [self._map_to_odom_combined(pos) for pos in poop_positions] #print 'new points', point_cloud.points point_cloud.channels = [ ChannelFloat32(name='intensities', values=[2000] * NUM_POOPS), ChannelFloat32(name='index', values=[0] * NUM_POOPS), #values=range(NUM_POOPS)), ChannelFloat32(name='distances', values=[2] * NUM_POOPS), ChannelFloat32(name='stamps', values=[0.002] * NUM_POOPS), ] self.publisher.publish(point_cloud)
def get_lidar_pcd(data): lidar_pcd = PointCloud() header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'lidar' lidar_pcd.header = header channel1 = ChannelFloat32() channel1.name = "intensity" channel2 = ChannelFloat32() channel2.name = "ring" for idx in range(data.shape[0]): lidar_pcd.points.append( Point32(data[idx, 0], data[idx, 1], data[idx, 2])) channel1.values.append(data[idx, 3]) channel2.values.append(data[idx, 4]) lidar_pcd.channels.append(channel1) lidar_pcd.channels.append(channel2) return lidar_pcd
def getMsg_dynamic(lidar_data): gen = point_cloud2.read_points(lidar_data, skip_nans=True) points_list = [] for p in gen: if p[4] == 7: points_list.append([p[0], p[1], p[2], p[3]]) pcl_data = pcl.PointCloud_PointXYZRGB() pcl_data.from_list(points_list) # downsampling 실행 코드 부분 print("Input :", pcl_data) LEAF_SIZE = 0.1 cloud = do_voxel_grid_downssampling(pcl_data, LEAF_SIZE) print("Output :", cloud) # ROI 실행 코드 부분 filter_axis = 'x' axis_min = 0.1 axis_max = 7.0 cloud = do_passthrough(cloud, filter_axis, axis_min, axis_max) filter_axis = 'y' axis_min = -4.0 axis_max = 4.0 cloud = do_passthrough(cloud, filter_axis, axis_min, axis_max) filter_axis = 'z' axis_min = 0.0 axis_max = 2.0 cloud = do_passthrough(cloud, filter_axis, axis_min, axis_max) test = PointCloud() get_in = ChannelFloat32() get_in.name = 'VLP_intensity' test.points = [] for p in cloud: # if p[1] > 0: park = Point32() park.x = p[0] park.y = p[1] park.z = 0 get_in.values.append(p[3]) test.points.append(park) #print("Input :", cnt) test.channels.append(get_in) test.header.frame_id = 'world' test.header.stamp = rospy.Time.now() pub.publish(test)
def callback(self, path): rospy.loginfo("Republishing Path as PointCloud...") self.cloud = PointCloud() self.cloud.header.frame_id = "rtab_init" self.cloud.points = [x.pose.position for x in path.poses] self.cloud.channels = [ChannelFloat32()] self.cloud.channels[0].name = "intensity" self.cloud.channels[0].values = [1.0 for x in path.poses] self.pub.publish(self.cloud)