.. _program_listing_file_include_ros2_ouster_OS1_OS1.hpp:

Program Listing for File OS1.hpp
================================

|exhale_lsh| :ref:`Return to documentation for file <file_include_ros2_ouster_OS1_OS1.hpp>` (``include/ros2_ouster/OS1/OS1.hpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   // 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_