Program Listing for File os_ros.h

↰ Return to documentation for file (include/ouster_ros/os_ros.h)

#pragma once

#define PCL_NO_PRECOMPILE
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <ouster/client.h>
#include <ouster/lidar_scan.h>
#include <ouster/types.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include <sensor_msgs/msg/imu.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>

#include <chrono>
#include <string>

#include "ouster_sensor_msgs/msg/packet_msg.hpp"
#include "ouster_ros/os_point.h"

namespace ouster_ros {

namespace sensor = ouster::sensor;
using Cloud = pcl::PointCloud<Point>;
using ns = std::chrono::nanoseconds;

bool is_legacy_lidar_profile(const sensor::sensor_info& info);

int get_n_returns(const sensor::sensor_info& info);


size_t get_beams_count(const sensor::sensor_info& info);

std::string topic_for_return(const std::string& topic_base, int return_idx);

sensor_msgs::msg::Imu packet_to_imu_msg(const ouster::sensor::packet_format& pf,
                                        const rclcpp::Time& timestamp,
                                        const std::string& frame,
                                        const uint8_t* buf);

sensor_msgs::msg::Imu packet_to_imu_msg(const ouster_sensor_msgs::msg::PacketMsg& pm,
                                        const rclcpp::Time& timestamp,
                                        const std::string& frame,
                                        const sensor::packet_format& pf);

[[deprecated("use the 2nd version of scan_to_cloud_f")]] void scan_to_cloud_f(
    ouster::PointsF& points, const ouster::PointsF& lut_direction,
    const ouster::PointsF& lut_offset, std::chrono::nanoseconds scan_ts,
    const ouster::LidarScan& lidar_scan, ouster_ros::Cloud& cloud,
    int return_index);

void scan_to_cloud_f(ouster::PointsF& points,
                     const ouster::PointsF& lut_direction,
                     const ouster::PointsF& lut_offset, uint64_t scan_ts,
                     const ouster::LidarScan& lidar_scan,
                     ouster_ros::Cloud& cloud, int return_index);

void scan_to_cloud_f_destaggered(ouster_ros::Cloud& cloud,
                     ouster::PointsF& points,
                     const ouster::PointsF& lut_direction,
                     const ouster::PointsF& lut_offset, uint64_t scan_ts,
                     const ouster::LidarScan& ls,
                     const std::vector<int>& pixel_shift_by_row,
                     int return_index);

sensor_msgs::msg::PointCloud2 cloud_to_cloud_msg(const Cloud& cloud,
                                                 const rclcpp::Time& timestamp,
                                                 const std::string& frame);

geometry_msgs::msg::TransformStamped transform_to_tf_msg(
    const ouster::mat4d& mat, const std::string& frame,
    const std::string& child_frame, rclcpp::Time timestamp);


sensor_msgs::msg::LaserScan lidar_scan_to_laser_scan_msg(
    const ouster::LidarScan& ls,
    const rclcpp::Time& timestamp,
    const std::string &frame,
    const ouster::sensor::lidar_mode lidar_mode,
    const uint16_t ring,
    const int return_index);

namespace impl {
sensor::ChanField suitable_return(sensor::ChanField input_field, bool second);

struct read_and_cast {
    template <typename T, typename U>
    void operator()(Eigen::Ref<const ouster::img_t<T>> field,
                    ouster::img_t<U>& dest) {
        dest = field.template cast<U>();
    }
};

template <typename T>
inline ouster::img_t<T> get_or_fill_zero(sensor::ChanField field,
                                         const ouster::LidarScan& ls) {
    if (!ls.field_type(field)) {
        return ouster::img_t<T>::Zero(ls.h, ls.w);
    }

    ouster::img_t<T> result{ls.h, ls.w};
    ouster::impl::visit_field(ls, field, read_and_cast(), result);
    return result;
}

inline uint64_t ts_safe_offset_add(uint64_t ts, int64_t offset) {
    return offset < 0 && ts < static_cast<uint64_t>(std::abs(offset)) ? 0 : ts + offset;
}


} // namespace impl

}  // namespace ouster_ros