def add_ros_cloud_template(self): # XYZ RGB point cloud2 self.rgb_cloud = PointCloud2() self.rgb_cloud.fields = [ PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1), PointField('rgb', 12, PointField.UINT32, 1) ] self.rgb_cloud.point_step = 16 self.rgb_cloud.is_bigendian = False self.rgb_cloud.height = 1 self.rgb_cloud.is_dense = True # XYZ point cloud 2 self.cloud = PointCloud2() self.cloud.fields = [ PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1) ] self.cloud.point_step = 12 self.cloud.is_bigendian = False self.cloud.height = 1 self.cloud.is_dense = True
def objDetection(req): print('[Object Detection] Receive object name: ' + req.obj_name) rep = ObjDetectionWithNameResponse() rep.input_pc = PointCloud2() pt1 = Point() pt1.x = 0 pt1.y = 0 pt1.z = 0 pt2 = Point() pt2.x = 50 pt2.y = 50 pt2.z = 0 rep.bbox_corner1 = pt1 rep.bbox_corner2 = pt2 pc = PointCloud2() pc.height = 480 pc.width = 640 rep.input_pc = pc rospy.sleep(1) print('[Object Detection] Return pointcloud and bounding boxes:') print(' pointcloud = (width, height) = ({}, {})'.format( rep.input_pc.width, rep.input_pc.height)) print(' bbox_corner1 = (x, y, z) = ({}, {}, {})'.format( rep.bbox_corner1.x, rep.bbox_corner1.y, rep.bbox_corner1.z)) print(' bbox_corner2 = (x, y, z) = ({}, {}, {})'.format( rep.bbox_corner2.x, rep.bbox_corner2.y, rep.bbox_corner2.z)) return rep
def load_visible_vg(self, filename): if self.scene.scene_type == SceneType.LIVE: # raise RuntimeError("Live scene not updated after multiobject refactor") try: pt_msg = PointCloud2() with (self.scene.get_save_path() / filename).open('rb') as f: pt_msg.deserialize(f.read()) if len(self.robot_views) != 1: raise RuntimeError( "Live scene with multiple objects, yet single latest segmented point found" ) self.robot_views[0].voxelize_visible_element(pt_msg) except FileNotFoundError as e: print( "Single latest segmeented points not found. You are probably doing this after the refactor" ) for i in range(self.scene.num_objects): filename = f"{self.scene.name}_latest_segmented_pts_{i}.msg" pt_msg = PointCloud2() with (self.scene.get_save_path() / filename).open('rb') as f: pt_msg.deserialize(f.read()) self.robot_views[i].voxelize_visible_element(pt_msg) # self.robot_views[i].get_visible_element(save_file=save_path) elif self.scene.scene_type == SceneType.SIMULATION: raise RuntimeError( "What are you doing loading the visible_vg from a simulation scene?" ) else: raise RuntimeError("Unknown scene type")
def array_to_pointcloud2(cloud_arr, stamp=None, frame_id=None, merge_rgb=False): '''Converts a numpy record array to a sensor_msgs.msg.PointCloud2. ''' if merge_rgb: cloud_arr = merge_rgb_fields(cloud_arr) # make it 2d (even if height will be 1) cloud_arr = np.atleast_2d(cloud_arr) cloud_msg = PointCloud2() if stamp is not None: cloud_msg.header.stamp = stamp if frame_id is not None: cloud_msg.header.frame_id = frame_id cloud_msg.height = cloud_arr.shape[0] cloud_msg.width = cloud_arr.shape[1] cloud_msg.fields = arr_to_fields(cloud_arr) cloud_msg.is_bigendian = False # assumption cloud_msg.point_step = cloud_arr.dtype.itemsize cloud_msg.row_step = cloud_msg.point_step * cloud_arr.shape[1] cloud_msg.is_dense = all([ np.isfinite(cloud_arr[fname]).all() for fname in cloud_arr.dtype.names ]) cloud_msg.data = cloud_arr.tostring() return cloud_msg
def xyz_array_to_pointcloud2(points_sum, stamp=None, frame_id=None): ''' Create a sensor_msgs.PointCloud2 from an array of points. ''' msg = PointCloud2() if stamp: msg.header.stamp = stamp if frame_id: msg.header.frame_id = frame_id print("cluster points shape:", points_sum.shape) # if len(points_sum.shape) == 3: msg.height = 1 msg.width = points_sum.shape[1] * points_sum.shape[0] # msg.width = points_sum.shape[1] # else: # msg.height = 1 # msg.width = len(points_sum) msg.fields = [ PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1) # PointField('i', 12, PointField.FLOAT32, 1) ] msg.is_bigendian = False msg.point_step = 4 msg.row_step = points_sum.shape[1] msg.is_dense = int(np.isfinite(points_sum).all()) # msg.is_dense = 0 print("msg.is_dense:", msg.is_dense) # msg.data = np.asarray(points_sum, np.float32).tostring() msg.data = points_sum.astype(np.float32).tobytes() # print("convert msg.data:", msg.data) return msg
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 publish_filtered_rscan(self, pts_filtered, radar_msg): rscan_filtered = PointCloud2() rscan_filtered.header = radar_msg.header rscan_filtered.height = 1 rscan_filtered.width = pts_filtered.shape[0] rscan_filtered.is_bigendian = False rscan_filtered.point_step = 24 rscan_filtered.row_step = rscan_filtered.width * rscan_filtered.point_step rscan_filtered.is_dense = True ## this is not being done correctly... rscan_filtered.fields = [ PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1), PointField('intensity', 12, PointField.FLOAT32, 1), PointField('range', 16, PointField.FLOAT32, 1), PointField('doppler', 20, PointField.FLOAT32, 1), ] data = np.reshape(pts_filtered, (pts_filtered.shape[0] * pts_filtered.shape[1], )) rscan_filtered.data = data.tostring() self.pc_pub.publish(rscan_filtered)
def pcl_to_ros(pcl_array): """ Converts a pcl PointXYZRGB to a ROS PointCloud2 message Args: pcl_array (PointCloud_PointXYZRGB): A PCL XYZRGB point cloud Returns: PointCloud2: A ROS point cloud """ ros_msg = PointCloud2() ros_msg.header.stamp = rospy.Time.now() ros_msg.header.frame_id = "world" ros_msg.height = 1 ros_msg.width = pcl_array.size ros_msg.fields.append(PointField( name="x", offset=0, datatype=PointField.FLOAT32, count=1)) ros_msg.fields.append(PointField( name="y", offset=4, datatype=PointField.FLOAT32, count=1)) ros_msg.fields.append(PointField( name="z", offset=8, datatype=PointField.FLOAT32, count=1)) ros_msg.fields.append(PointField( name="rgb", offset=16, datatype=PointField.FLOAT32, count=1)) ros_msg.is_bigendian = False ros_msg.point_step = 32 ros_msg.row_step = ros_msg.point_step * ros_msg.width * ros_msg.height ros_msg.is_dense = False buffer = [] for data in pcl_array: s = struct.pack('>f', data[3]) i = struct.unpack('>l', s)[0] pack = ctypes.c_uint32(i).value r = (pack & 0x00FF0000) >> 16 g = (pack & 0x0000FF00) >> 8 b = (pack & 0x000000FF) buffer.append(struct.pack('ffffBBBBIII', data[0], data[1], data[2], 1.0, b, g, r, 0, 0, 0, 0)) # print(buffer) ros_msg.data = b"".join(buffer) # ros_msg.data = str.join(buffer) # buffer = np.atleast_2d(buffer) # ros_msg.data = buffer.tostring() # ros_msg.data = str(buffer) # print(ros_msg.data) return ros_msg
def __init__(self): rospy.init_node("PC") self._pub = rospy.Publisher("PointCloud", PointCloud2, queue_size=1) self._sub = rospy.Subscriber("/angles", Int16, self.callback) self.angle = Int16() self.data = PointCloud2() self.once = True
def npz_pcl_dense(npz=None, ts=None, frame_id=None): points = npz["pcloud_points"] reflectance = npz["pcloud_attr.reflectance"] colors = np.stack([reflectance, reflectance, reflectance], axis=1) assert (points.shape == colors.shape) msg = PointCloud2() msg.header.frame_id = frame_id msg.header.stamp.secs = divmod(ts, 1000000)[0] #stamp in micro secs msg.header.stamp.nsecs = divmod(ts, 1000000)[1] * 1000 # nano secs msg.width = points.shape[0] msg.height = 1 msg.fields = [ PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1), PointField('r', 12, PointField.FLOAT32, 1), PointField('g', 16, PointField.FLOAT32, 1), PointField('b', 20, PointField.FLOAT32, 1) ] msg.is_bigendian = False msg.point_step = 24 msg.row_step = msg.point_step * msg.width msg.is_dense = True data_array = np.array(np.hstack([points, colors]), dtype=np.float32) msg.data = data_array.tostring() return msg
def make_points_msg(header, xyz): pts = PointCloud2() pts.header = header pts.height = SENSOR_HEIGHT_PIXELS pts.width = SENSOR_WIDTH_PIXELS pts.fields = [ PointField(name="x", offset=0, datatype=PointField.FLOAT32, count=1), PointField(name="y", offset=4, datatype=PointField.FLOAT32, count=1), PointField(name="z", offset=8, datatype=PointField.FLOAT32, count=1), ] pts.is_bigendian = False pts.point_step = 20 pts.row_step = 4480 xyz32 = xyz.astype(np.float32) xyz32[np.isnan(xyz32)] = 0.0 buf = io.BytesIO() for row in xyz32: # write xyz values (12 bytes) buf.write(struct.pack("<fff", *row)) # pad with 8 bytes of 0 to replicate original point_step of 20 buf.write(struct.pack("<q", 0)) pts.data = buf.getvalue() pts.is_dense = True return pts
def publish_points(points, publisher, frame_id=None, stamp=None): """ Publish point cloud. :param points: :param publisher: :param frame_id: :param stamp: :return: """ if not points.dtype == np.float32: rospy.logwarn( "Point cloud type not float32, may not be visualized by Rviz.") msg = PointCloud2() if stamp: msg.header.stamp = stamp if frame_id: msg.header.frame_id = frame_id msg.height = 1 msg.width = len(points) msg.fields = [ PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1) ] msg.is_bigendian = False msg.point_step = 12 msg.row_step = 12 * len(points) msg.is_dense = int(np.isfinite(points).all()) msg.data = points.tostring() publisher.publish(msg)
def xyzi_array_to_pointcloud2(points, stamp=None, frame_id=None, seq=None): ''' Create a sensor_msgs.PointCloud2 from an array of points. ''' msg = PointCloud2() if stamp: msg.header.stamp = stamp if frame_id: msg.header.frame_id = frame_id if seq: msg.header.seq = seq else: N = len(points) xyzi = np.array(np.hstack([points]), dtype=np.float32) msg.height = 1 msg.width = N msg.fields = [ PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1), PointField('intensity', 12, PointField.FLOAT32, 1) ] msg.is_bigendian = False msg.point_step = 16 msg.row_step = msg.point_step * N msg.is_dense = True msg.data = xyzi.tostring() return msg
def publisher(): global g_pub, g_msg sys = CameraSystem() dev = sys.scan() cam = sys.connect(dev[0]) if cam.isInitialized() == False: print('Fail init') # g_pub = rospy.Publisher('cloud_point', PointCloud2, queue_size=10) rospy.init_node('voxel_publisher', anonymous=True) g_msg = PointCloud2() g_msg.header.frame_id = 'base_link' g_msg.height = 240 g_msg.width = 320 g_msg.fields.append(PointField(name='x', offset=0, datatype=7, count=1)) g_msg.fields.append(PointField(name='y', offset=4, datatype=7, count=1)) g_msg.fields.append(PointField(name='z', offset=8, datatype=7, count=1)) g_msg.point_step = 12 g_msg.row_step = 320 * 12 # cam.registerCallback(3, cb) ret, rate = cam.getFrameRate() if ret == False: print("Fail") print("fr:{}".format(rate.numerator)) time.sleep(2) # necessory cam.start() rospy.spin()
def pcl_to_ros(pcl_array): ros_msg = PointCloud2() ros_msg.header.stamp = rospy.Time.now() ros_msg.header.frame_id = "world" ros_msg.height = 1 ros_msg.width = pcl_array.size ros_msg.fields.append( PointField(name="x", offset=0, datatype=PointField.FLOAT32, count=1)) ros_msg.fields.append( PointField(name="y", offset=4, datatype=PointField.FLOAT32, count=1)) ros_msg.fields.append( PointField(name="z", offset=8, datatype=PointField.FLOAT32, count=1)) ros_msg.is_bigendian = False ros_msg.point_step = 12 ros_msg.row_step = ros_msg.point_step * ros_msg.width * ros_msg.height ros_msg.is_dense = False buffer = [] for data in pcl_array: float_rgb = struct.unpack('f', struct.pack('i', 0xff0000))[0] try: buffer.append(struct.pack('fff', data[0], data[1], data[2])) except Exception as e: print(e) else: pass ros_msg.data = "".join(buffer) ground_points_pub.publish(ros_msg)
def pcl_dense_msg(cls, points=None, reflectance=None, ts=None, frame_id=None): colors = np.stack([reflectance, reflectance, reflectance], axis=1) assert (points.shape == colors.shape) msg = PointCloud2() cls.set_ros_msg_header(ros_msg=msg, ts=ts, frame_id=frame_id) msg.width = points.shape[0] msg.height = 1 msg.fields = msg.fields = cls.get_pcl_fields() msg.is_bigendian = False msg.point_step = 24 msg.row_step = msg.point_step * msg.width msg.is_dense = True data_array = np.array(np.hstack([points, colors]), dtype=np.float32) msg.data = data_array.tostring() return msg
def custom_publish(self, points, stamp=None, frame_id=None): msg = PointCloud2() if stamp: msg.header.stamp = stamp if frame_id: msg.header.frame_id = frame_id if len(points.shape) == 3: msg.height = points.shape[1] msg.width = points.shape[0] else: msg.height = 1 msg.width = len(points) msg.fields = [ PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1) ] msg.is_bigendian = False msg.point_step = 12 #addPointField(msg_pointcloud, format_str.c_str(), 1, sensor_msgs::PointField::UINT32, msg_pointcloud.point_step); msg.row_step = points.shape[0] * points.shape[1] #*msg.point_step msg.is_dense = int(np.isfinite(points).all()) msg.data = np.asarray(points, np.float32).tostring() self.publish(msg) """
def xyz_array_to_pointcloud2(self, points, limit): ''' Create a sensor_msgs.PointCloud2 from an array of points. ''' if limit > 0: points = points[-limit:] msg = PointCloud2() msg.header.stamp = rospy.Time.now() msg.header.frame_id = "base_link" if len(points.shape) == 3: msg.height = points.shape[1] msg.width = points.shape[0] else: msg.height = 1 msg.width = len(points) msg.fields = [ PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1) ] msg.is_bigendian = False msg.point_step = 12 msg.row_step = 12 * points.shape[0] msg.is_dense = int(np.isfinite(points).all()) msg.data = np.asarray(points, np.float32).tostring() return msg
def xyz_array_to_pointcloud2( self, points, frame_id=None, stamp=None, ): ''' Create a sensor_msgs.PointCloud2 from an array of points. ''' msg = PointCloud2() if stamp: msg.header.stamp = stamp if frame_id: msg.header.frame_id = frame_id if len(points.shape) == 3: msg.height = points.shape[1] msg.width = points.shape[0] else: msg.height = 1 msg.width = len(points) msg.fields = [ PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1) ] msg.is_bigendian = False msg.point_step = 12 msg.row_step = 12 * points.shape[0] msg.is_dense = int(np.isfinite(points).all()) msg.data = np.asarray(points, np.float32).tostring() return msg
def run(self): """ The run function for this step Args: Yields: chuck_approach_pose (geometry_msgs/PoseStamped) : approach pose for impedance control with the chuck .. seealso:: :meth:`task_executor.abstract_step.AbstractStep.run` """ rospy.loginfo("Action {}: Detecting schunk.".format(self.name)) self._stopped = False # Ask for the schunk detector stub_tf = Transform() stub_pcl = PointCloud2() stub = TemplateMatchRequest() stub.initial_estimate = stub_tf stub.target_cloud = stub_pcl chuck_approach_pose = self._detect_schunk_srv(stub).template_pose self.notify_service_called( DetectSchunkAction.DETECT_SCHUNK_SERVICE_NAME) yield self.set_running() # Check the status of the server if self._stopped: yield self.set_preempted(action=self.name, srv=self._detect_schunk_srv.resolved_name, chuck_approach_pose=chuck_approach_pose) else: yield self.set_succeeded(chuck_approach_pose=chuck_approach_pose)
def default(self, ci='unused'): if not self.component_instance.capturing: return # press [Space] key to enable capturing points = self.data['points'] width = self.component_instance.image_width * self.component_instance.image_height size = len(points) pc2 = PointCloud2() pc2.header = self.get_ros_header() pc2.height = 1 pc2.width = width pc2.is_dense = False pc2.is_bigendian = False pc2.fields = [PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1)] pc2.point_step = int(size / width) pc2.row_step = size # memoryview from PyMemoryView_FromMemory() implements the buffer interface pc2.data = bytes(points) self.publish(pc2) self.send_transform_robot()
def processing_xyzi(self): for i in range(self.len_files): print("[+] {} th file name : {} ".format(i, self.npy_files[i])) bin_points = np.load(os.path.join(self.npy_path, self.npy_files[i])) bin_points_pred = np.load( os.path.join(self.npy_path_pred, self.npy_files_pred[i])) pc2_msg = PointCloud2() header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'velodyne_link' h, w, _ = bin_points.shape # 64, 512 ,6 cloud_points = [] for j in range(h): # 64 for k in range(w): # 512 cloud_points.append( list( np.append(bin_points[j, :, :3][k], bin_points_pred[j][k]))) pc2_msg = self.create_cloud_xyzi32(header, cloud_points) # ed: /velodyne_points_npy publish self.velo_pub.publish(pc2_msg) self.loop_rate.sleep() if rospy.is_shutdown(): return
def to_cloud_msg(points, intensities=None, frame=None, stamp=None): """Convert list of unstructured points to a PointCloud2 message. Args: points: Point coordinates as array of shape (N,3). colors: Colors as array of shape (N,3). frame stamp """ msg = PointCloud2() msg.header.frame_id = frame msg.header.stamp = stamp or rospy.Time.now() msg.height = 1 msg.width = points.shape[0] msg.is_bigendian = False msg.is_dense = False msg.fields = [ PointField("x", 0, PointField.FLOAT32, 1), PointField("y", 4, PointField.FLOAT32, 1), PointField("z", 8, PointField.FLOAT32, 1), ] msg.point_step = 12 data = points if intensities is not None: msg.fields.append(PointField("intensity", 12, PointField.FLOAT32, 1)) msg.point_step += 4 data = np.hstack([points, intensities]) msg.row_step = msg.point_step * points.shape[0] msg.data = data.astype(np.float32).tostring() return msg
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 xyzarray_to_pc2(points, parent_frame): """ Creates a point cloud message. Args: points: Nx7 array of xyz positions (m) and rgba colors (0..1) parent_frame: frame in which the point cloud is defined Returns: sensor_msgs/PointCloud2 message """ ros_dtype = PointField.FLOAT32 dtype = np.float32 itemsize = np.dtype(dtype).itemsize data = points.astype(dtype).tobytes() fields = [ PointField(name=n, offset=i * itemsize, datatype=ros_dtype, count=1) for i, n in enumerate('xyz') ] header = parent_frame.header #std_msgs.Header(frame_id=parent_frame, stamp=rospy.Time.now()) return PointCloud2(header=header, height=1, width=points.shape[0], is_dense=False, is_bigendian=False, fields=fields, point_step=(itemsize * 3), row_step=(itemsize * 3 * points.shape[0]), data=data)
def __publish_point_cloud_data(self, stamp): data = self._wb_device.getPointCloud(data_type='buffer') if data: msg = PointCloud2() msg.header.stamp = stamp msg.header.frame_id = self._frame_id msg.height = 1 msg.width = self._wb_device.getNumberOfPoints() msg.point_step = 20 msg.row_step = 20 * self._wb_device.getNumberOfPoints() msg.is_dense = False msg.fields = [ PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1), PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1), PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1) ] msg.is_bigendian = False # We pass `data` directly to we avoid using `data` setter. # Otherwise ROS2 converts data to `array.array` which slows down the simulation as it copies memory internally. # Both, `bytearray` and `array.array`, implement Python buffer protocol, so we should not see unpredictable # behavior. # deepcode ignore W0212: Avoid conversion from `bytearray` to `array.array`. msg._data = data self.__publisher.publish(msg)
def create_cloud(header, fields, points): """ Create a L{sensor_msgs.msg.PointCloud2} message. @param header: The point cloud header. @type header: L{std_msgs.msg.Header} @param fields: The point cloud fields. @type fields: iterable of L{sensor_msgs.msg.PointField} @param points: The point cloud points. @type points: list of iterables, i.e. one iterable for each point, with the elements of each iterable being the values of the fields for that point (in the same order as the fields parameter) @return: The point cloud. @rtype: L{sensor_msgs.msg.PointCloud2} """ cloud_struct = struct.Struct(_get_struct_fmt(False, fields)) buff = ctypes.create_string_buffer(cloud_struct.size * len(points)) point_step, pack_into = cloud_struct.size, cloud_struct.pack_into offset = 0 for p in points: pack_into(buff, offset, *p) offset += point_step return PointCloud2(header=header, height=1, width=len(points), is_dense=False, is_bigendian=False, fields=fields, point_step=cloud_struct.size, row_step=cloud_struct.size * len(points), data=buff.raw)
def __init__(self): self.pointcloud = PointCloud2() self.rate = 1 self.start = 0 return
def simulate_mbes(self, mbes_ac): # Find particle's mbes pose without broadcasting/listening to tf transforms particle_tf = Transform() particle_tf.translation = self.pose.position particle_tf.rotation = self.pose.orientation mat_part = matrix_from_tf(particle_tf) trans_mat = np.dot(mat_part, self.mbes_tf_mat) trans = TransformStamped() trans.transform.translation.x = translation_from_matrix(trans_mat)[0] trans.transform.translation.y = translation_from_matrix(trans_mat)[1] trans.transform.translation.z = translation_from_matrix(trans_mat)[2] trans.transform.rotation = Quaternion( *quaternion_from_matrix(trans_mat)) # Build MbesSimGoal to send to action server mbes_goal = MbesSimGoal() mbes_goal.mbes_pose.header.frame_id = self.map_frame # mbes_goal.mbes_pose.child_frame_id = self.mbes_frame_id # The particles will be in a child frame to the map mbes_goal.mbes_pose.header.stamp = rospy.Time.now() mbes_goal.mbes_pose.transform = trans.transform # Get result from action server mbes_ac.send_goal(mbes_goal) mbes_ac.wait_for_result() mbes_res = mbes_ac.get_result() # Pack result into PointCloud2 mbes_pcloud = PointCloud2() mbes_pcloud = mbes_res.sim_mbes mbes_pcloud.header.frame_id = self.map_frame return mbes_pcloud
def publish_image_as_pointcloud(point_cloud_publisher: rospy.Publisher, message: PointCloud2): h = std_msgs.msg.Header() h.stamp = rospy.Time.now() message.header = h message.header.frame_id = 'img2rviz' point_cloud_publisher.publish(message)