Program Listing for File OS1.hpp

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

#include <unistd.h>
#include <fcntl.h>
#include <netdb.h>

#include <cstdint>
#include <memory>
#include <string>
#include <vector>
#include <algorithm>
#include <array>
#include <cerrno>
#include <chrono>
#include <cstdio>
#include <cstring>
#include <iostream>
#include <sstream>
#include <stdexcept>
#include <utility>

#include "arpa/inet.h"
#include "sys/socket.h"
#include "sys/types.h"

#include "json/json.h"
#include "ros2_ouster/OS1/OS1_packet.hpp"
#include "ros2_ouster/interfaces/metadata.hpp"

namespace OS1
{

using ns = std::chrono::nanoseconds;

struct client
{
  ~client()
  {
    close(lidar_fd);
    close(imu_fd);
  }

  int lidar_fd{7502};
  int imu_fd{7503};
  Json::Value meta;
};

enum lidar_mode
{
  MODE_512x10 = 1,
  MODE_512x20,
  MODE_1024x10,
  MODE_1024x20,
  MODE_2048x10
};

enum timestamp_mode
{
  TIME_FROM_INTERNAL_OSC = 1,
  TIME_FROM_SYNC_PULSE_IN,
  TIME_FROM_PTP_1588,
  TIME_FROM_ROS_RECEPTION = 99
};

struct version
{
  int16_t major;
  int16_t minor;
  int16_t patch;
};

const std::array<std::pair<lidar_mode, std::string>, 5> lidar_mode_strings = {
  {{MODE_512x10, "512x10"},
    {MODE_512x20, "512x20"},
    {MODE_1024x10, "1024x10"},
    {MODE_1024x20, "1024x20"},
    {MODE_2048x10, "2048x10"}}};

const std::array<std::pair<timestamp_mode, std::string>, 4>
timestamp_mode_strings = {
  {{TIME_FROM_INTERNAL_OSC, "TIME_FROM_INTERNAL_OSC"},
    {TIME_FROM_SYNC_PULSE_IN, "TIME_FROM_SYNC_PULSE_IN"},
    {TIME_FROM_PTP_1588, "TIME_FROM_PTP_1588"},
    {TIME_FROM_ROS_RECEPTION, "TIME_FROM_ROS_RECEPTION"}}};

const size_t lidar_packet_bytes = 12608;
const size_t imu_packet_bytes = 48;

const version invalid_version = {0, 0, 0};

const OS1::version min_version = {1, 9, 0};

inline bool operator==(const version & u, const version & v)
{
  return u.major == v.major && u.minor == v.minor && u.patch == v.patch;
}

inline bool operator<(const version & u, const version & v)
{
  return (u.major < v.major) || (u.major == v.major && u.minor < v.minor) ||
         (u.major == v.major && u.minor == v.minor && u.patch < v.patch);
}

inline bool operator<=(const version & u, const version & v)
{
  return u < v || u == v;
}

inline std::string to_string(version v)
{
  if (v == invalid_version) {
    return "UNKNOWN";
  }

  std::stringstream ss{};
  ss << "v" << v.major << "." << v.minor << "." << v.patch;
  return ss.str();
}

inline version version_of_string(const std::string & s)
{
  std::istringstream is{s};
  char c1, c2, c3;
  version v;

  is >> c1 >> v.major >> c2 >> v.minor >> c3 >> v.patch;

  if (is && is.eof() && c1 == 'v' && c2 == '.' && c3 == '.' && v.major >= 0 &&
    v.minor >= 0 && v.patch >= 0)
  {
    return v;
  } else {
    return invalid_version;
  }
}

inline std::string to_string(lidar_mode mode)
{
  auto end = lidar_mode_strings.end();
  auto res = std::find_if(
    lidar_mode_strings.begin(), end,
    [&](const std::pair<lidar_mode, std::string> & p) {
      return p.first == mode;
    });

  return res == end ? "UNKNOWN" : res->second;
}

inline lidar_mode lidar_mode_of_string(const std::string & s)
{
  auto end = lidar_mode_strings.end();
  auto res = std::find_if(
    lidar_mode_strings.begin(), end,
    [&](const std::pair<lidar_mode, std::string> & p) {
      return p.second == s;
    });

  return res == end ? lidar_mode(0) : res->first;
}

inline std::string to_string(timestamp_mode mode)
{
  auto end = timestamp_mode_strings.end();
  auto res = std::find_if(
    timestamp_mode_strings.begin(), end,
    [&](const std::pair<timestamp_mode, std::string> & p) {
      return p.first == mode;
    });

  return res == end ? "UNKNOWN" : res->second;
}

inline timestamp_mode timestamp_mode_of_string(const std::string & s)
{
  auto end = timestamp_mode_strings.end();
  auto res = std::find_if(
    timestamp_mode_strings.begin(), end,
    [&](const std::pair<timestamp_mode, std::string> & p) {
      return p.second == s;
    });

  return res == end ? timestamp_mode(0) : res->first;
}

inline int n_cols_of_lidar_mode(lidar_mode mode)
{
  switch (mode) {
    case MODE_512x10:
    case MODE_512x20:
      return 512;
    case MODE_1024x10:
    case MODE_1024x20:
      return 1024;
    case MODE_2048x10:
      return 2048;
    default:
      throw std::invalid_argument{"n_cols_of_lidar_mode"};
  }
}

inline int udp_data_socket(int port)
{
  struct addrinfo hints, * info_start, * ai;

  memset(&hints, 0, sizeof hints);
  hints.ai_family = AF_INET6;
  hints.ai_socktype = SOCK_DGRAM;
  hints.ai_flags = AI_PASSIVE;

  auto port_s = std::to_string(port);

  int ret = getaddrinfo(NULL, port_s.c_str(), &hints, &info_start);
  if (ret != 0) {
    std::cerr << "getaddrinfo(): " << gai_strerror(ret) << std::endl;
    return -1;
  }
  if (info_start == NULL) {
    std::cerr << "getaddrinfo: empty result" << std::endl;
    return -1;
  }

  int sock_fd;
  for (ai = info_start; ai != NULL; ai = ai->ai_next) {
    sock_fd = socket(ai->ai_family, ai->ai_socktype, ai->ai_protocol);
    if (sock_fd < 0) {
      std::cerr << "udp socket(): " << std::strerror(errno) << std::endl;
      continue;
    }

    if (bind(sock_fd, ai->ai_addr, ai->ai_addrlen) < 0) {
      close(sock_fd);
      std::cerr << "udp bind(): " << std::strerror(errno) << std::endl;
      continue;
    }

    break;
  }

  freeaddrinfo(info_start);
  if (ai == NULL) {
    close(sock_fd);
    return -1;
  }

  if (fcntl(sock_fd, F_SETFL, fcntl(sock_fd, F_GETFL, 0) | O_NONBLOCK) < 0) {
    std::cerr << "udp fcntl(): " << std::strerror(errno) << std::endl;
    return -1;
  }

  return sock_fd;
}

inline std::shared_ptr<client> init_client(int lidar_port = 7502, int imu_port = 7503)
{
  auto cli = std::make_shared<client>();

  int lidar_fd = udp_data_socket(lidar_port);
  int imu_fd = udp_data_socket(imu_port);
  cli->lidar_fd = lidar_fd;
  cli->imu_fd = imu_fd;
  return cli;
}

inline void update_json_obj(Json::Value & dst, const Json::Value & src)
{
  for (const auto & key : src.getMemberNames()) {
    dst[key] = src[key];
  }
}

inline bool do_tcp_cmd(int sock_fd, const std::vector<std::string> & cmd_tokens, std::string & res)
{
  const size_t max_res_len = 16 * 1024;
  auto read_buf = std::unique_ptr<char[]>{new char[max_res_len + 1]};

  std::stringstream ss;
  for (const auto & token : cmd_tokens) {
    ss << token << " ";
  }
  ss << "\n";
  std::string cmd = ss.str();

  ssize_t len = write(sock_fd, cmd.c_str(), cmd.length());
  if (len != (ssize_t)cmd.length()) {
    return false;
  }

  // need to synchronize with server by reading response
  std::stringstream read_ss;
  do {
    len = read(sock_fd, read_buf.get(), max_res_len);
    if (len < 0) {
      return false;
    }
    read_buf.get()[len] = '\0';
    read_ss << read_buf.get();
  } while (len > 0 && read_buf.get()[len - 1] != '\n');

  res = read_ss.str();
  res.erase(res.find_last_not_of(" \r\n\t") + 1);

  return true;
}

inline int cfg_socket(const char * addr)
{
  struct addrinfo hints, * info_start, * ai;

  memset(&hints, 0, sizeof hints);
  hints.ai_family = AF_UNSPEC;
  hints.ai_socktype = SOCK_STREAM;

  int ret = getaddrinfo(addr, "7501", &hints, &info_start);
  if (ret != 0) {
    std::cerr << "getaddrinfo: " << gai_strerror(ret) << std::endl;
    return -1;
  }
  if (info_start == NULL) {
    std::cerr << "getaddrinfo: empty result" << std::endl;
    return -1;
  }

  int sock_fd;
  for (ai = info_start; ai != NULL; ai = ai->ai_next) {
    sock_fd = socket(ai->ai_family, ai->ai_socktype, ai->ai_protocol);
    if (sock_fd < 0) {
      std::cerr << "socket: " << std::strerror(errno) << std::endl;
      continue;
    }

    if (connect(sock_fd, ai->ai_addr, ai->ai_addrlen) == -1) {
      close(sock_fd);
      continue;
    }

    break;
  }

  freeaddrinfo(info_start);
  if (ai == NULL) {
    return -1;
  }

  return sock_fd;
}

inline std::shared_ptr<client> init_client(
  const std::string & hostname,
  const std::string & udp_dest_host,
  lidar_mode mode = MODE_1024x10,
  timestamp_mode ts_mode = TIME_FROM_INTERNAL_OSC,
  int lidar_port = 7502,
  int imu_port = 7503)
{
  auto cli = init_client(lidar_port, imu_port);

  int sock_fd = cfg_socket(hostname.c_str());

  Json::CharReaderBuilder builder{};
  auto reader = std::unique_ptr<Json::CharReader>{builder.newCharReader()};
  Json::Value root{};
  std::string errors{};

  if (sock_fd < 0) {
    return std::shared_ptr<client>();
  }

  std::string res;
  bool success = true;

  success &=
    do_tcp_cmd(sock_fd, {"set_config_param", "udp_ip", udp_dest_host}, res);
  success &= res == "set_config_param";

  success &= do_tcp_cmd(
    sock_fd, {"set_config_param", "udp_port_lidar",
      std::to_string(lidar_port)}, res);
  success &= res == "set_config_param";

  success &= do_tcp_cmd(
    sock_fd, {"set_config_param", "udp_port_imu", std::to_string(imu_port)},
    res);
  success &= res == "set_config_param";

  success &= do_tcp_cmd(
    sock_fd, {"set_config_param", "lidar_mode", to_string(mode)}, res);
  success &= res == "set_config_param";

  if (ts_mode != TIME_FROM_ROS_RECEPTION) {
    success &= do_tcp_cmd(
      sock_fd,
      {"set_config_param", "timestamp_mode", to_string(ts_mode)},
      res);
    success &= res == "set_config_param";
  }

  success &= do_tcp_cmd(sock_fd, {"get_sensor_info"}, res);
  success &= reader->parse(
    res.c_str(), res.c_str() + res.size(), &cli->meta, &errors);

  success &= do_tcp_cmd(sock_fd, {"get_beam_intrinsics"}, res);
  success &=
    reader->parse(res.c_str(), res.c_str() + res.size(), &root, &errors);
  update_json_obj(cli->meta, root);

  success &= do_tcp_cmd(sock_fd, {"get_imu_intrinsics"}, res);
  success &=
    reader->parse(res.c_str(), res.c_str() + res.size(), &root, &errors);
  update_json_obj(cli->meta, root);

  success &= do_tcp_cmd(sock_fd, {"get_lidar_intrinsics"}, res);
  success &=
    reader->parse(res.c_str(), res.c_str() + res.size(), &root, &errors);
  update_json_obj(cli->meta, root);

  success &= do_tcp_cmd(sock_fd, {"reinitialize"}, res);
  success &= res == "reinitialize";

  close(sock_fd);

  // merge extra info into metadata
  cli->meta["hostname"] = hostname;
  cli->meta["lidar_mode"] = to_string(mode);
  cli->meta["timestamp_mode"] = to_string(ts_mode);
  cli->meta["imu_port"] = imu_port;
  cli->meta["lidar_port"] = lidar_port;

  return success ? cli : std::shared_ptr<client>();
}


inline ros2_ouster::ClientState poll_client(const client & c, const int timeout_sec = 1)
{
  fd_set rfds;
  FD_ZERO(&rfds);
  FD_SET(c.lidar_fd, &rfds);
  FD_SET(c.imu_fd, &rfds);

  timeval tv;
  tv.tv_sec = timeout_sec;
  tv.tv_usec = 0;

  int max_fd = std::max(c.lidar_fd, c.imu_fd);

  int retval = select(max_fd + 1, &rfds, NULL, NULL, &tv);

  if (retval == -1 && errno == EINTR) {
    return ros2_ouster::ClientState::EXIT;
  } else if (retval == -1) {
    std::cerr << "select: " << std::strerror(errno) << std::endl;
    return ros2_ouster::ClientState::ERROR;
  } else if (retval) {
    if (FD_ISSET(c.lidar_fd, &rfds)) {
      return ros2_ouster::ClientState::LIDAR_DATA;
    }
    if (FD_ISSET(c.imu_fd, &rfds)) {
      return ros2_ouster::ClientState::IMU_DATA;
    }
  }
  return ros2_ouster::ClientState::TIMEOUT;
}

static bool recv_fixed(int fd, void * buf, size_t len)
{
  ssize_t n = recvfrom(fd, buf, len + 1, 0, NULL, NULL);
  if (n == (ssize_t)len) {
    return true;
  } else if (n == -1) {
    std::cerr << "recvfrom: " << std::strerror(errno) << std::endl;
  } else {
    std::cerr << "Unexpected udp packet length: " << n << std::endl;
  }
  return false;
}

inline bool read_lidar_packet(const client & cli, uint8_t * buf)
{
  return recv_fixed(cli.lidar_fd, buf, lidar_packet_bytes);
}

inline bool read_imu_packet(const client & cli, uint8_t * buf)
{
  return recv_fixed(cli.imu_fd, buf, imu_packet_bytes);
}

inline std::string get_metadata(const client & cli)
{
  Json::StreamWriterBuilder builder;
  builder["enableYAMLCompatibility"] = true;
  builder["precision"] = 6;
  builder["indentation"] = "    ";
  return Json::writeString(builder, cli.meta);
}

inline ros2_ouster::Metadata parse_metadata(const std::string & meta)
{
  Json::Value root{};
  Json::CharReaderBuilder builder{};
  std::string errors{};
  std::stringstream ss{meta};

  if (meta.size()) {
    if (!Json::parseFromStream(builder, ss, &root, &errors)) {
      throw std::runtime_error{errors.c_str()};
    }
  }

  ros2_ouster::Metadata info = {
    "UNKNOWN", "UNKNOWN", "UNNKOWN", "UNNKOWN", "UNKNOWN",
    {}, {}, {}, {}, 7503, 7502};
  info.hostname = root["hostname"].asString();
  info.sn = root["prod_sn"].asString();
  info.fw_rev = root["build_rev"].asString();

  info.mode = root["lidar_mode"].asString();
  info.timestamp_mode = root["timestamp_mode"].asString();
  info.lidar_port = root["lidar_port"].asInt();
  info.imu_port = root["lidar_port"].asInt();

  for (const auto & v : root["beam_altitude_angles"]) {
    info.beam_altitude_angles.push_back(v.asDouble());
  }
  if (info.beam_altitude_angles.size() != OS1::pixels_per_column) {
    info.beam_altitude_angles = {};
  }

  for (const auto & v : root["beam_azimuth_angles"]) {
    info.beam_azimuth_angles.push_back(v.asDouble());
  }
  if (info.beam_azimuth_angles.size() != OS1::pixels_per_column) {
    info.beam_azimuth_angles = {};
  }

  for (const auto & v : root["imu_to_sensor_transform"]) {
    info.imu_to_sensor_transform.push_back(v.asDouble());
  }
  if (info.imu_to_sensor_transform.size() != 16) {
    info.imu_to_sensor_transform = {};
  }

  for (const auto & v : root["lidar_to_sensor_transform"]) {
    info.lidar_to_sensor_transform.push_back(v.asDouble());
  }
  if (info.lidar_to_sensor_transform.size() != 16) {
    info.lidar_to_sensor_transform = {};
  }

  return info;
}

}  // namespace OS1

#endif  // ROS2_OUSTER__OS1__OS1_HPP_