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 <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>

#include <chrono>
#include <string>
#include <vector>

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

namespace ouster_ros {

bool is_legacy_lidar_profile(const ouster::sdk::core::SensorInfo& info);


size_t get_beams_count(const ouster::sdk::core::SensorInfo& info);

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

std::vector<sensor_msgs::msg::Imu> packet_to_imu_msgs(
    const ouster::sdk::core::ImuPacket& imu_packet,
    const std::string& frame,
    const rclcpp::Time& timestamp,
    const ouster::sdk::core::SensorInfo& sensor_info);

geometry_msgs::msg::TransformStamped transform_to_tf_msg(
    const ouster::sdk::core::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::sdk::core::LidarScan& ls,
    const rclcpp::Time& timestamp,
    const std::string &frame,
    const ouster::sdk::core::LidarMode lidar_mode,
    const uint16_t ring, const std::vector<int>& pixel_shift_by_row,
    const int return_index);

ouster_sensor_msgs::msg::Telemetry lidar_packet_to_telemetry_msg(
    const ouster::sdk::core::LidarPacket& lidar_packet,
    const rclcpp::Time& timestamp,
    const ouster::sdk::core::PacketFormat& pf);

namespace impl {

std::string scan_return(const std::string& input_field, bool second);

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

template <typename T>
inline ouster::sdk::core::img_t<T> get_or_fill_zero(const std::string& field,
                                 const ouster::sdk::core::LidarScan& ls) {
    if (!ls.has_field(field)) {
        return ouster::sdk::core::img_t<T>::Zero(ls.h, ls.w);
    }
    ouster::sdk::core::img_t<T> result{ls.h, ls.w};
    ouster::sdk::core::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;
}

std::set<std::string> parse_tokens(const std::string& input, char delim);

inline bool check_token(const std::set<std::string>& tokens,
                        const std::string& token) {
    return tokens.find(token) != tokens.end();
}

ouster::sdk::core::Version parse_version(const std::string& fw_rev);

template <typename T>
uint64_t ulround(T value) {
    T rounded_value = std::round(value);
    if (rounded_value < 0) return 0ULL;
    if (rounded_value > ULLONG_MAX) return ULLONG_MAX;
    return static_cast<uint64_t>(rounded_value);
}

void warn_mask_resized(int image_cols, int image_rows,
                       int scan_height, int scan_width);

template <typename pixel_type>
ouster::sdk::core::img_t<pixel_type> load_mask(const std::string& mask_path,
                                    size_t height, size_t width) {
    if (mask_path.empty()) return ouster::sdk::core::img_t<pixel_type>();

    cv::Mat image = cv::imread(mask_path, cv::IMREAD_GRAYSCALE);
    if (image.empty()) {
        throw std::runtime_error("Failed to load mask image from path: " + mask_path);
    }

    if (image.rows != static_cast<int>(height) || image.cols != static_cast<int>(width)) {
        warn_mask_resized(image.cols, image.rows, static_cast<int>(height), static_cast<int>(width));
        cv::Mat resized;
        cv::resize(image, resized, cv::Size(width, height), 0, 0, cv::INTER_NEAREST);
        image = resized;
    }
    Eigen::MatrixXi eigen_img(image.rows, image.cols);
    cv::cv2eigen(image, eigen_img);
    Eigen::MatrixXi zero_image = Eigen::MatrixXi::Zero(eigen_img.rows(), eigen_img.cols());
    Eigen::MatrixXi ones_image = Eigen::MatrixXi::Ones(eigen_img.rows(), eigen_img.cols());
    return (eigen_img.array() == 0.0).select(zero_image, ones_image).cast<pixel_type>();
}

} // namespace impl

}  // namespace ouster_ros