os1_ros.h

Go to the documentation of this file.

#pragma once

#include <chrono>
#include <functional>
#include <string>

#include <pcl/common/common.h>
#include <pcl/point_cloud.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>

#include "ouster/os1.h"
#include "ouster_driver/PacketMsg.h"
#include "ouster_driver/point_os1.h"

namespace ouster_driver {
namespace OS1 {

using CloudOS1 = pcl::PointCloud<PointOS1>;
using CloudOS1XYZ = pcl::PointCloud<pcl::PointXYZ>;
using CloudOS1XYZI = pcl::PointCloud<pcl::PointXYZI>;
using CloudOS1XYZIR = pcl::PointCloud<PointXYZIR>;
using CloudOS1XYZIF = pcl::PointCloud<PointXYZIF>;
using CloudOS1XYZIRF = pcl::PointCloud<PointXYZIRF>;
using CloudOS1XYZIFN = pcl::PointCloud<PointXYZIFN>;
using CloudOS1XYZIRFN = pcl::PointCloud<PointXYZIRFN>;
using ns = std::chrono::nanoseconds;

bool read_imu_packet(const ouster::OS1::client& cli, PacketMsg& pm);

bool read_lidar_packet(const ouster::OS1::client& cli, PacketMsg& pm);

ns timestamp_of_imu_packet(const PacketMsg& pm);

ns timestamp_of_lidar_packet(const PacketMsg& pm);

sensor_msgs::Imu packet_to_imu_msg(const PacketMsg& pm,
                                   const std::string& frame = "os1_imu");

void add_packet_to_cloud(ns scan_start_ts, ns scan_duration,
                         const PacketMsg& pm, CloudOS1& cloud);

sensor_msgs::PointCloud2 cloud_to_cloud_msg(const CloudOS1& cloud, ns timestamp,
                                            const std::string& frame = "os1");
void spin(const ouster::OS1::client& cli,
          const std::function<void(const PacketMsg& pm)>& lidar_handler,
          const std::function<void(const PacketMsg& pm)>& imu_handler);

std::function<void(const PacketMsg&)> batch_packets(
    ns scan_dur, const std::function<void(ns, const CloudOS1&)>& f);

void set_point_mode(std::string mode_xyzir);

void convert2XYZ(const CloudOS1& in, CloudOS1XYZ& out);

void convert2XYZI(const CloudOS1& in, CloudOS1XYZI& out);

void convert2XYZIR(const CloudOS1& in, CloudOS1XYZIR& out);

void convert2XYZIF(const CloudOS1& in, CloudOS1XYZIF& out);

void convert2XYZIRF(const CloudOS1& in, CloudOS1XYZIRF& out);

void convert2XYZIFN(const CloudOS1& in, CloudOS1XYZIFN& out);

void convert2XYZIRFN(const CloudOS1& in, CloudOS1XYZIRFN& out);
}
}