Program Listing for File conversions.hpp

↰ Return to documentation for file (include/ros2_ouster/conversions.hpp)

// Copyright 2020, Steve Macenski
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef ROS2_OUSTER__CONVERSIONS_HPP_
#define ROS2_OUSTER__CONVERSIONS_HPP_

#include <cstdint>
#include <string>
#include <vector>
#include <algorithm>

#include "pcl/point_types.h"
#include "pcl/point_cloud.h"
#include "pcl_conversions/pcl_conversions.h"
#include "ros2_ouster/point_os.hpp"
#include "ros2_ouster/image_os.hpp"
#include "ros2_ouster/scan_os.hpp"

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "tf2/LinearMath/Transform.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "ouster_msgs/msg/metadata.hpp"

#include "ros2_ouster/OS1/OS1.hpp"
#include "ros2_ouster/OS1/OS1_packet.hpp"

namespace ros2_ouster
{

static const bool IS_BIGENDIAN = []()
  {
    std::uint16_t dummy = 0x1;
    std::uint8_t * dummy_ptr = reinterpret_cast<std::uint8_t *>(&dummy);
    return dummy_ptr[0] == 0x1 ? false : true;
  } ();

inline std::string toString(const ClientState & state)
{
  switch (state) {
    case TIMEOUT:
      return std::string("timeout");
    case ERROR:
      return std::string("error");
    case EXIT:
      return std::string("exit");
    case IMU_DATA:
      return std::string("lidar data");
    case LIDAR_DATA:
      return std::string("imu data");
    default:
      return std::string("unknown");
  }
}

inline ouster_msgs::msg::Metadata toMsg(const ros2_ouster::Metadata & mdata)
{
  ouster_msgs::msg::Metadata msg;
  msg.hostname = mdata.hostname;
  msg.lidar_mode = mdata.mode;
  msg.timestamp_mode = mdata.timestamp_mode;
  msg.beam_azimuth_angles = mdata.beam_azimuth_angles;
  msg.beam_altitude_angles = mdata.beam_altitude_angles;
  msg.imu_to_sensor_transform = mdata.imu_to_sensor_transform;
  msg.lidar_to_sensor_transform = mdata.lidar_to_sensor_transform;
  msg.serial_no = mdata.sn;
  msg.firmware_rev = mdata.fw_rev;
  msg.imu_port = mdata.imu_port;
  msg.lidar_port = mdata.lidar_port;
  return msg;
}

inline geometry_msgs::msg::TransformStamped toMsg(
  const std::vector<double> & mat, const std::string & frame,
  const std::string & child_frame, const rclcpp::Time & time)
{
  assert(mat.size() == 16);

  tf2::Transform tf;

  tf.setOrigin({mat[3] / 1e3, mat[7] / 1e3, mat[11] / 1e3});
  tf.setBasis(
    {mat[0], mat[1], mat[2], mat[4], mat[5], mat[6], mat[8], mat[9], mat[10]});

  geometry_msgs::msg::TransformStamped msg;
  msg.header.stamp = time;
  msg.header.frame_id = frame;
  msg.child_frame_id = child_frame;
  msg.transform = tf2::toMsg(tf);

  return msg;
}

inline sensor_msgs::msg::Imu toMsg(
  const uint8_t * buf,
  const std::string & frame,
  uint64_t override_ts = 0)
{
  const double standard_g = 9.80665;
  sensor_msgs::msg::Imu m;
  m.orientation.x = 0;
  m.orientation.y = 0;
  m.orientation.z = 0;
  m.orientation.w = 1;

  m.header.stamp = override_ts == 0 ?
    rclcpp::Time(OS1::imu_gyro_ts(buf)) : rclcpp::Time(override_ts);
  m.header.frame_id = frame;

  m.linear_acceleration.x = OS1::imu_la_x(buf) * standard_g;
  m.linear_acceleration.y = OS1::imu_la_y(buf) * standard_g;
  m.linear_acceleration.z = OS1::imu_la_z(buf) * standard_g;

  m.angular_velocity.x = OS1::imu_av_x(buf) * M_PI / 180.0;
  m.angular_velocity.y = OS1::imu_av_y(buf) * M_PI / 180.0;
  m.angular_velocity.z = OS1::imu_av_z(buf) * M_PI / 180.0;

  for (int i = 0; i < 9; i++) {
    m.orientation_covariance[i] = -1;
    m.angular_velocity_covariance[i] = 0;
    m.linear_acceleration_covariance[i] = 0;
  }
  for (int i = 0; i < 9; i += 4) {
    m.linear_acceleration_covariance[i] = 0.01;
    m.angular_velocity_covariance[i] = 6e-4;
  }

  return m;
}

inline sensor_msgs::msg::PointCloud2 toMsg(
  const pcl::PointCloud<point_os::PointOS> & cloud,
  std::chrono::nanoseconds timestamp,
  const std::string & frame)
{
  std::size_t pt_size = sizeof(point_os::PointOS);
  std::size_t data_size = pt_size * cloud.points.size();

  pcl::PCLPointCloud2 cloud2;
  cloud2.height = cloud.height;
  cloud2.width = cloud.width;
  cloud2.fields.clear();
  pcl::for_each_type<typename pcl::traits::fieldList<point_os::PointOS>::type>(
    pcl::detail::FieldAdder<point_os::PointOS>(cloud2.fields));
  cloud2.header = cloud.header;
  cloud2.point_step = pt_size;
  cloud2.row_step = static_cast<std::uint32_t>(pt_size * cloud2.width);
  cloud2.is_dense = cloud.is_dense;
  cloud2.is_bigendian = ros2_ouster::IS_BIGENDIAN;

  cloud2.data.resize(data_size);
  if (data_size) {
    // column-major to row-major conversion
    for (int i = 0; i < cloud.width; ++i) {
      for (int j = 0; j < cloud.height; ++j) {
        std::memcpy(
          &cloud2.data[(j * cloud.width + i) * pt_size],
          &cloud.points[i * cloud.height + j],
          pt_size);
      }
    }
  }

  sensor_msgs::msg::PointCloud2 msg;
  pcl_conversions::moveFromPCL(cloud2, msg);
  msg.header.frame_id = frame;
  rclcpp::Time t(timestamp.count());
  msg.header.stamp = t;
  return msg;
}

inline sensor_msgs::msg::LaserScan toMsg(
  const std::vector<scan_os::ScanOS> & scans,
  std::chrono::nanoseconds timestamp,
  const std::string & frame,
  const ros2_ouster::Metadata & mdata,
  const uint8_t ring_to_use)
{
  sensor_msgs::msg::LaserScan msg;
  rclcpp::Time t(timestamp.count());
  msg.header.stamp = t;
  msg.header.frame_id = frame;
  msg.angle_min = -M_PI;
  msg.angle_max = M_PI;
  msg.range_min = 0.1;
  msg.range_max = 120.0;

  double resolution, rate;
  if (mdata.mode == "512x10") {
    resolution = 512.0;
    rate = 10.0;
  } else if (mdata.mode == "512x20") {
    resolution = 512.0;
    rate = 20.0;
  } else if (mdata.mode == "1024x10") {
    resolution = 1024.0;
    rate = 10.0;
  } else if (mdata.mode == "1024x20") {
    resolution = 1024.0;
    rate = 20.0;
  } else if (mdata.mode == "2048x10") {
    resolution = 2048.0;
    rate = 10.0;
  } else {
    std::cout << "Error: Could not determine lidar mode!" << std::endl;
    resolution = 512.0;
    rate = 10.0;
  }

  msg.scan_time = 1.0 / rate;
  msg.time_increment = 1.0 / rate / resolution;
  msg.angle_increment = 2 * M_PI / resolution;

  for (uint i = 0; i != scans.size(); i++) {
    if (scans[i].ring == ring_to_use) {
      msg.ranges.push_back(scans[i].range * 5e-3);
      msg.intensities.push_back(std::min(scans[i].intensity, 255.0f));
    }
  }

  return msg;
}

}  // namespace ros2_ouster

#endif  // ROS2_OUSTER__CONVERSIONS_HPP_