Program Listing for File OS1_packet.hpp

↰ Return to documentation for file (include/ros2_ouster/OS1/OS1_packet.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__OS1__OS1_PACKET_HPP_
#define ROS2_OUSTER__OS1__OS1_PACKET_HPP_

#include <array>
#include <cmath>
#include <cstdint>
#include <cstring>

namespace OS1
{

const int pixels_per_column = 64;
const int columns_per_buffer = 16;

const int pixel_bytes = 12;
const int column_bytes = 16 + (pixels_per_column * pixel_bytes) + 4;

const int encoder_ticks_per_rev = 90112;

// lidar column fields
inline const uint8_t * nth_col(int n, const uint8_t * udp_buf)
{
  return udp_buf + (n * column_bytes);
}

inline uint64_t col_timestamp(const uint8_t * col_buf)
{
  uint64_t res;
  memcpy(&res, col_buf, sizeof(uint64_t));
  return res;
}

inline float col_h_angle(const uint8_t * col_buf)
{
  uint32_t ticks;
  memcpy(&ticks, col_buf + 12, sizeof(uint32_t));
  return 2.0 * M_PI * ticks / static_cast<float>(encoder_ticks_per_rev);
}

inline uint32_t col_h_encoder_count(const uint8_t * col_buf)
{
  uint32_t res;
  std::memcpy(&res, col_buf + 12, sizeof(uint32_t));
  return res;
}

inline uint16_t col_measurement_id(const uint8_t * col_buf)
{
  uint16_t res;
  memcpy(&res, col_buf + 8, sizeof(uint16_t));
  return res;
}

inline uint16_t col_frame_id(const uint8_t * col_buf)
{
  uint16_t res;
  memcpy(&res, col_buf + 10, sizeof(uint16_t));
  return res;
}

inline uint32_t col_valid(const uint8_t * col_buf)
{
  uint32_t res;
  memcpy(
    &res, col_buf + (16 + pixels_per_column * pixel_bytes), sizeof(uint32_t));
  return res;
}

// lidar pixel fields
inline const uint8_t * nth_px(int n, const uint8_t * col_buf)
{
  return col_buf + 16 + (n * pixel_bytes);
}

inline uint32_t px_range(const uint8_t * px_buf)
{
  uint32_t res;
  memcpy(&res, px_buf, sizeof(uint32_t));
  res &= 0x000fffff;
  return res;
}

inline uint16_t px_reflectivity(const uint8_t * px_buf)
{
  uint16_t res;
  memcpy(&res, px_buf + 4, sizeof(uint16_t));
  return res;
}

inline uint16_t px_signal_photons(const uint8_t * px_buf)
{
  uint16_t res;
  memcpy(&res, px_buf + 6, sizeof(uint16_t));
  return res;
}

inline uint16_t px_noise_photons(const uint8_t * px_buf)
{
  uint16_t res;
  memcpy(&res, px_buf + 8, sizeof(uint16_t));
  return res;
}

// imu packets
inline uint64_t imu_sys_ts(const uint8_t * imu_buf)
{
  uint64_t res;
  memcpy(&res, imu_buf, sizeof(uint64_t));
  return res;
}

inline uint64_t imu_accel_ts(const uint8_t * imu_buf)
{
  uint64_t res;
  memcpy(&res, imu_buf + 8, sizeof(uint64_t));
  return res;
}

inline uint64_t imu_gyro_ts(const uint8_t * imu_buf)
{
  uint64_t res;
  memcpy(&res, imu_buf + 16, sizeof(uint64_t));
  return res;
}

// imu linear acceleration
inline float imu_la_x(const uint8_t * imu_buf)
{
  float res;
  memcpy(&res, imu_buf + 24, sizeof(float));
  return res;
}

inline float imu_la_y(const uint8_t * imu_buf)
{
  float res;
  memcpy(&res, imu_buf + 28, sizeof(float));
  return res;
}

inline float imu_la_z(const uint8_t * imu_buf)
{
  float res;
  memcpy(&res, imu_buf + 32, sizeof(float));
  return res;
}

// imu angular velocity
inline float imu_av_x(const uint8_t * imu_buf)
{
  float res;
  memcpy(&res, imu_buf + 36, sizeof(float));
  return res;
}

inline float imu_av_y(const uint8_t * imu_buf)
{
  float res;
  memcpy(&res, imu_buf + 40, sizeof(float));
  return res;
}

inline float imu_av_z(const uint8_t * imu_buf)
{
  float res;
  memcpy(&res, imu_buf + 44, sizeof(float));
  return res;
}
}  // namespace OS1

#endif  // ROS2_OUSTER__OS1__OS1_PACKET_HPP_