├── MANIFEST.in
├── .clang-format
├── src
    ├── scan_batcher.cpp
    ├── velodyne_decoder
    │   ├── calibrations
    │   │   ├── __init__.py
    │   │   ├── Puck Hi-Res.yml
    │   │   ├── VLP-16.yml
    │   │   ├── HDL-32E.yml
    │   │   ├── VLP-32A.yml
    │   │   ├── VLP-32B.yml
    │   │   ├── VLP-32C.yml
    │   │   ├── Alpha Prime.yml
    │   │   └── HDL-64E_S1-utexas.yml
    │   ├── __init__.py
    │   ├── bag.py
    │   ├── pcap.py
    │   ├── util.py
    │   └── hdl64e.py
    ├── stream_decoder.cpp
    ├── time_conversion.cpp
    ├── scan_decoder.cpp
    ├── types.cpp
    ├── telemetry_packet.cpp
    ├── calibration.cpp
    ├── calib_db.cpp
    └── python.cpp
├── requirements-dev.txt
├── test_package
    ├── CMakeLists.txt
    ├── example.cpp
    └── conanfile.py
├── vcpkg.json
├── include
    └── velodyne_decoder
    │   ├── stream_decoder.h
    │   ├── scan_decoder.h
    │   ├── config.h
    │   ├── time_conversion.h
    │   ├── scan_batcher.h
    │   ├── ros
    │       └── ros_scan_batcher.h
    │   ├── calibration.h
    │   ├── scan_batcher.inl
    │   ├── telemetry_packet.h
    │   ├── packet_decoder.h
    │   └── types.h
├── cmake
    ├── velodyne_decoder-config.cmake.in
    └── third_party.cmake
├── test
    ├── conftest.py
    ├── test_calib.py
    ├── data
    │   └── mock_vls128_dual.py
    ├── test_pcap.py
    └── test_ros.py
├── LICENSE
├── pyproject.toml
├── conanfile.py
├── .github
    └── workflows
    │   ├── publish.yml
    │   └── build.yml
├── CMakeLists.txt
├── .gitignore
├── README.md
└── CHANGELOG.md


/MANIFEST.in:
--------------------------------------------------------------------------------
1 | include src/velodyne_decoder/calibrations/*.yml
2 | 


--------------------------------------------------------------------------------
/.clang-format:
--------------------------------------------------------------------------------
1 | BasedOnStyle: LLVM
2 | ColumnLimit: 100
3 | TabWidth: 2
4 | IndentPPDirectives: AfterHash
5 | AlignConsecutiveAssignments: true
6 | 


--------------------------------------------------------------------------------
/src/scan_batcher.cpp:
--------------------------------------------------------------------------------
 1 | // Copyright (c) 2023, Martin Valgur
 2 | // SPDX-License-Identifier: BSD-3-Clause
 3 | 
 4 | #include "velodyne_decoder/scan_batcher.h"
 5 | 
 6 | namespace velodyne_decoder {
 7 | 
 8 | template class ScanBatcher<VelodynePacket>;
 9 | 
10 | } // namespace velodyne_decoder


--------------------------------------------------------------------------------
/requirements-dev.txt:
--------------------------------------------------------------------------------
 1 | --extra-index-url https://rospypi.github.io/simple/
 2 | pytest
 3 | requests
 4 | rosbag
 5 | pathlib; python_version < '3'
 6 | rospy < 1.15; python_version < '3'
 7 | rosbag < 1.15; python_version < '3'
 8 | numpy>=2; python_version >= "3.9"
 9 | numpy<2; python_version < "3.9"
10 | 


--------------------------------------------------------------------------------
/test_package/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 3.16)
2 | project(PackageTest LANGUAGES CXX)
3 | 
4 | find_package(velodyne_decoder REQUIRED)
5 | 
6 | add_executable(example example.cpp)
7 | target_link_libraries(example PRIVATE velodyne_decoder::velodyne_decoder)
8 | target_compile_features(example PRIVATE cxx_std_17)
9 | 


--------------------------------------------------------------------------------
/test_package/example.cpp:
--------------------------------------------------------------------------------
 1 | #include <velodyne_decoder/config.h>
 2 | #include <velodyne_decoder/scan_decoder.h>
 3 | #include <velodyne_decoder/stream_decoder.h>
 4 | #include <velodyne_decoder/types.h>
 5 | 
 6 | int main() {
 7 |   velodyne_decoder::Config config;
 8 |   velodyne_decoder::StreamDecoder stream_decoder(config);
 9 | }
10 | 


--------------------------------------------------------------------------------
/vcpkg.json:
--------------------------------------------------------------------------------
 1 | {
 2 |   "name": "velodyne-decoder",
 3 |   "description": "Decoder for raw Velodyne packet data",
 4 |   "version-string": "3.0.0",
 5 |   "license": "BSD-3-Clause",
 6 |   "homepage": "https://github.com/valgur/velodyne_decoder",
 7 |   "maintainers": [
 8 |     "Martin Valgur <martin.valgur@gmail.com>"
 9 |   ],
10 |   "builtin-baseline": "c9919121dde6f61c0436adda94624636e041226b",
11 |   "dependencies": [
12 |     "yaml-cpp",
13 |     "ms-gsl"
14 |   ]
15 | }
16 | 


--------------------------------------------------------------------------------
/src/velodyne_decoder/calibrations/__init__.py:
--------------------------------------------------------------------------------
 1 | # Copyright (c) 2023, Martin Valgur
 2 | # SPDX-License-Identifier: BSD-3-Clause
 3 | 
 4 | """
 5 | Except for HDL-64E, the calibration YAML files are for reference only.
 6 | The equivalent data is now stored in src/calib_db.cpp instead.
 7 | """
 8 | 
 9 | import importlib_resources
10 | 
11 | import velodyne_decoder as vd
12 | 
13 | 
14 | def get_bundled_calibration(calib_file):
15 |     """Return a Calibration object for the given calibration file.
16 | 
17 |     E.g. get_bundled_calibration("HDL-64E_S3-VeloView.yml").
18 |     """
19 |     path = importlib_resources.files(__package__) / calib_file
20 |     with importlib_resources.as_file(path) as calib_path:
21 |         return vd.Calibration.read(str(calib_path))
22 | 


--------------------------------------------------------------------------------
/test_package/conanfile.py:
--------------------------------------------------------------------------------
 1 | import os
 2 | 
 3 | from conan import ConanFile
 4 | from conan.tools.build import cross_building
 5 | from conan.tools.cmake import CMake, cmake_layout
 6 | 
 7 | 
 8 | class VelodyneDecoderTestPkg(ConanFile):
 9 |     test_type = "explicit"
10 |     settings = "os", "compiler", "build_type", "arch"
11 |     generators = "CMakeDeps", "CMakeToolchain"
12 | 
13 |     def requirements(self):
14 |         self.requires(self.tested_reference_str)
15 | 
16 |     def build(self):
17 |         cmake = CMake(self)
18 |         cmake.configure()
19 |         cmake.build()
20 | 
21 |     def layout(self):
22 |         cmake_layout(self)
23 | 
24 |     def test(self):
25 |         if not cross_building(self):
26 |             cmd = os.path.join(self.cpp.build.bindirs[0], "example")
27 |             self.run(cmd, env="conanrun")
28 | 


--------------------------------------------------------------------------------
/include/velodyne_decoder/stream_decoder.h:
--------------------------------------------------------------------------------
 1 | // Copyright (c) 2023, Martin Valgur
 2 | // SPDX-License-Identifier: BSD-3-Clause
 3 | 
 4 | #pragma once
 5 | 
 6 | #include "velodyne_decoder/config.h"
 7 | #include "velodyne_decoder/scan_batcher.h"
 8 | #include "velodyne_decoder/scan_decoder.h"
 9 | #include "velodyne_decoder/types.h"
10 | 
11 | #include <optional>
12 | #include <utility>
13 | 
14 | namespace velodyne_decoder {
15 | 
16 | class StreamDecoder {
17 | public:
18 |   explicit StreamDecoder(const Config &config);
19 | 
20 |   std::optional<std::pair<TimePair, PointCloud>> decode(const VelodynePacket &packet);
21 | 
22 |   /**
23 |    * @brief Decodes all remaining packets in the buffer and returns the last scan, if any.
24 |    */
25 |   std::optional<std::pair<TimePair, PointCloud>> finish();
26 | 
27 | private:
28 |   std::pair<TimePair, PointCloud> decodeCollectedPackets();
29 | 
30 | private:
31 |   ScanBatcher<> scan_batcher_;
32 |   ScanDecoder scan_decoder_;
33 | };
34 | 
35 | } // namespace velodyne_decoder
36 | 


--------------------------------------------------------------------------------
/src/velodyne_decoder/__init__.py:
--------------------------------------------------------------------------------
 1 | # Copyright (c) 2021-2023, Martin Valgur
 2 | # SPDX-License-Identifier: BSD-3-Clause
 3 | 
 4 | from enum import IntEnum
 5 | 
 6 | # noinspection PyUnresolvedReferences
 7 | from .velodyne_decoder_pylib import (
 8 |     Config,
 9 |     ScanDecoder,
10 |     StreamDecoder,
11 |     Calibration,
12 |     VelodynePacket,
13 |     TelemetryPacket,
14 |     PacketVector,
15 |     Model,
16 |     ReturnMode,
17 |     PACKET_SIZE,
18 |     TELEMETRY_PACKET_SIZE,
19 |     __version__ as _pylib_version,
20 | )
21 | 
22 | from . import hdl64e
23 | from . import util
24 | from .calibrations import get_bundled_calibration
25 | from .pcap import read_pcap
26 | from .bag import read_bag
27 | 
28 | 
29 | class PointField(IntEnum):
30 |     """Point field indices in the decoded point cloud for contiguous NumPy arrays."""
31 |     x = 0
32 |     y = 1
33 |     z = 2
34 |     intensity = 3
35 |     time = 4
36 |     column = 5
37 |     ring = 6
38 |     return_type = 7
39 | 
40 | 
41 | __version__ = _pylib_version
42 | 


--------------------------------------------------------------------------------
/src/stream_decoder.cpp:
--------------------------------------------------------------------------------
 1 | // Copyright (c) 2023, Martin Valgur
 2 | // SPDX-License-Identifier: BSD-3-Clause
 3 | 
 4 | #include "velodyne_decoder/stream_decoder.h"
 5 | 
 6 | #include <optional>
 7 | #include <utility>
 8 | 
 9 | namespace velodyne_decoder {
10 | 
11 | StreamDecoder::StreamDecoder(const Config &config) : scan_batcher_(config), scan_decoder_(config) {}
12 | 
13 | std::optional<std::pair<TimePair, PointCloud>> //
14 | StreamDecoder::decode(const VelodynePacket &packet) {
15 |   bool scan_complete = scan_batcher_.push(packet);
16 |   if (scan_complete)
17 |     return decodeCollectedPackets();
18 |   return std::nullopt;
19 | }
20 | 
21 | std::pair<TimePair, PointCloud> StreamDecoder::decodeCollectedPackets() {
22 |   auto result = scan_decoder_.decode(*scan_batcher_.scanPackets());
23 |   scan_batcher_.reset();
24 |   return result;
25 | }
26 | 
27 | std::optional<std::pair<TimePair, PointCloud>> StreamDecoder::finish() {
28 |   if (scan_batcher_.empty()) {
29 |     return std::nullopt;
30 |   }
31 |   return decodeCollectedPackets();
32 | }
33 | 
34 | } // namespace velodyne_decoder
35 | 


--------------------------------------------------------------------------------
/include/velodyne_decoder/scan_decoder.h:
--------------------------------------------------------------------------------
 1 | // Copyright (c) 2021-2023, Martin Valgur
 2 | // SPDX-License-Identifier: BSD-3-Clause
 3 | 
 4 | #pragma once
 5 | 
 6 | #include <string>
 7 | 
 8 | #include "velodyne_decoder/config.h"
 9 | #include "velodyne_decoder/packet_decoder.h"
10 | #include "velodyne_decoder/types.h"
11 | 
12 | namespace velodyne_decoder {
13 | 
14 | class ScanDecoder {
15 | public:
16 |   explicit ScanDecoder(const Config &config);
17 | 
18 |   std::pair<TimePair, PointCloud> decode(const std::vector<VelodynePacket> &scan_packets);
19 |   std::pair<TimePair, PointCloud> decode(const std::vector<PacketView> &scan_packets);
20 | 
21 |   /// Detected or configured model ID of the sensor
22 |   [[nodiscard]] std::optional<ModelId> modelId() const;
23 | 
24 |   /// The return mode of the sensor based on the last received packet
25 |   [[nodiscard]] std::optional<ReturnMode> returnMode() const;
26 | 
27 | private:
28 |   velodyne_decoder::PacketDecoder packet_decoder_;
29 |   velodyne_decoder::PointCloud cloud_;
30 | 
31 |   bool timestamp_first_packet_;
32 | };
33 | 
34 | } // namespace velodyne_decoder
35 | 


--------------------------------------------------------------------------------
/src/time_conversion.cpp:
--------------------------------------------------------------------------------
 1 | // Copyright (c) 2019, Matthew Pitropov, Joshua Whitley
 2 | // Copyright (c) 2021-2023, Martin Valgur
 3 | // SPDX-License-Identifier: BSD-3-Clause
 4 | 
 5 | #include "velodyne_decoder/time_conversion.h"
 6 | 
 7 | #include <cmath>
 8 | #include <cstdint>
 9 | 
10 | namespace velodyne_decoder {
11 | 
12 | Time resolveHourAmbiguity(const Time packet_time, const Time reference_time) {
13 |   const double HOUR      = 3600;
14 |   const double HALF_HOUR = 1800;
15 |   if (packet_time > reference_time + HALF_HOUR)
16 |     return packet_time - HOUR;
17 |   if (packet_time < reference_time - HALF_HOUR)
18 |     return packet_time + HOUR;
19 |   return packet_time;
20 | }
21 | 
22 | Time getPacketTimestamp(const uint32_t usec_since_toh, const Time reference_time) {
23 |   const int HOUR_TO_SEC = 3600;
24 |   uint32_t cur_hour     = (uint32_t)std::floor(reference_time) / HOUR_TO_SEC;
25 |   Time packet_time      = cur_hour * HOUR_TO_SEC + usec_since_toh * 1e-6;
26 |   if (reference_time > 0)
27 |     return resolveHourAmbiguity(packet_time, reference_time);
28 |   return packet_time;
29 | }
30 | 
31 | Time getPacketTimestamp(gsl::span<const uint8_t, PACKET_SIZE> pkt_data, Time reference_time) {
32 |   uint32_t usec_since_toh = parseUint32(&pkt_data[PACKET_SIZE - 6]);
33 |   return getPacketTimestamp(usec_since_toh, reference_time);
34 | }
35 | 
36 | } // namespace velodyne_decoder
37 | 


--------------------------------------------------------------------------------
/src/scan_decoder.cpp:
--------------------------------------------------------------------------------
 1 | // Copyright (c) 2021-2023, Martin Valgur
 2 | // SPDX-License-Identifier: BSD-3-Clause
 3 | 
 4 | #include "velodyne_decoder/scan_decoder.h"
 5 | #include "velodyne_decoder/types.h"
 6 | 
 7 | namespace velodyne_decoder {
 8 | 
 9 | ScanDecoder::ScanDecoder(const Config &config)
10 |     : packet_decoder_(config), timestamp_first_packet_(config.timestamp_first_packet) {}
11 | 
12 | std::pair<TimePair, PointCloud> ScanDecoder::decode(const std::vector<PacketView> &scan_packets) {
13 |   cloud_.clear();
14 |   TimePair scan_stamp =
15 |       timestamp_first_packet_ ? scan_packets.front().stamp : scan_packets.back().stamp;
16 |   for (const auto &packet : scan_packets) {
17 |     packet_decoder_.unpack(packet, scan_stamp, cloud_);
18 |   }
19 |   std::pair<TimePair, PointCloud> result;
20 |   result.first = scan_stamp;
21 |   result.second.reserve(cloud_.capacity());
22 |   cloud_.swap(result.second);
23 |   return result;
24 | }
25 | 
26 | std::pair<TimePair, PointCloud>
27 | ScanDecoder::decode(const std::vector<VelodynePacket> &scan_packets) {
28 |   std::vector<PacketView> packet_views;
29 |   packet_views.reserve(scan_packets.size());
30 |   for (PacketView packet : scan_packets)
31 |     packet_views.emplace_back(packet);
32 |   return decode(packet_views);
33 | }
34 | 
35 | std::optional<ModelId> ScanDecoder::modelId() const { return packet_decoder_.modelId(); }
36 | 
37 | std::optional<ReturnMode> ScanDecoder::returnMode() const { return packet_decoder_.returnMode(); }
38 | 
39 | } // namespace velodyne_decoder
40 | 


--------------------------------------------------------------------------------
/cmake/velodyne_decoder-config.cmake.in:
--------------------------------------------------------------------------------
 1 | @PACKAGE_INIT@
 2 | 
 3 | include("${CMAKE_CURRENT_LIST_DIR}/velodyne_decoder-targets.cmake")
 4 | set_and_check(velodyne_decoder_INCLUDE_DIRS "@PACKAGE_CMAKE_INSTALL_INCLUDEDIR@")
 5 | set_and_check(velodyne_decoder_INCLUDE_DIR "@PACKAGE_CMAKE_INSTALL_INCLUDEDIR@")
 6 | set_and_check(velodyne_decoder_LIBRARY_DIR "@PACKAGE_CMAKE_INSTALL_LIBDIR@")
 7 | set(velodyne_decoder_LIBRARIES velodyne_decoder::velodyne_decoder)
 8 | check_required_components(velodyne_decoder)
 9 | 
10 | set(velodyne_decoder_BUNDLED_THIRD_PARTY_LIBS @INSTALL_THIRD_PARTY@)
11 | if(velodyne_decoder_BUNDLED_THIRD_PARTY_LIBS)
12 |     # TODO: This info could be generated with a custom Conan deployer instead
13 |     # e.g. https://github.com/conan-io/conan-extensions/blob/main/extensions/deployers/runtime_zip_deploy.py
14 |     add_library(velodyne_decoder_third_party INTERFACE)
15 |     add_library(yaml-cpp::yaml-cpp ALIAS velodyne_decoder_third_party)
16 |     add_library(Microsoft.GSL::GSL ALIAS velodyne_decoder_third_party)
17 | 
18 |     target_include_directories(velodyne_decoder_third_party INTERFACE "@PACKAGE_THIRD_PARTY_INCLUDEDIR@")
19 |     target_link_directories(velodyne_decoder_third_party INTERFACE "@PACKAGE_THIRD_PARTY_LIBDIR@")
20 |     target_link_libraries(velodyne_decoder_third_party INTERFACE yaml-cpp)
21 |     target_compile_definitions(velodyne_decoder_third_party INTERFACE YAML_CPP_STATIC_DEFINE)
22 | else()
23 |     include(CMakeFindDependencyMacro)
24 |     find_dependency(yaml-cpp REQUIRED CONFIG)
25 |     find_dependency(Microsoft.GSL REQUIRED CONFIG)
26 | endif()
27 | 


--------------------------------------------------------------------------------
/src/velodyne_decoder/calibrations/Puck Hi-Res.yml:
--------------------------------------------------------------------------------
 1 | # converted from https://gitlab.kitware.com/LidarView/velodyneplugin/-/tree/0cddc1f92e69b7b26ffc5619aceb1c00854442e6/CalibrationsFiles
 2 | # using https://wiki.ros.org/velodyne_pointcloud#gen_calibration.py
 3 | distance_resolution: 0.002
 4 | lasers:
 5 |   - laser_id: 0
 6 |     rot_correction: 0
 7 |     vert_correction: -0.1745329
 8 |   - laser_id: 1
 9 |     rot_correction: 0
10 |     vert_correction: 0.01164135
11 |   - laser_id: 2
12 |     rot_correction: 0
13 |     vert_correction: -0.1512677
14 |   - laser_id: 3
15 |     rot_correction: 0
16 |     vert_correction: 0.03490658
17 |   - laser_id: 4
18 |     rot_correction: 0
19 |     vert_correction: -0.127985
20 |   - laser_id: 5
21 |     rot_correction: 0
22 |     vert_correction: 0.05817182
23 |   - laser_id: 6
24 |     rot_correction: 0
25 |     vert_correction: -0.1047198
26 |   - laser_id: 7
27 |     rot_correction: 0
28 |     vert_correction: 0.08145452
29 |   - laser_id: 8
30 |     rot_correction: 0
31 |     vert_correction: -0.08145452
32 |   - laser_id: 9
33 |     rot_correction: 0
34 |     vert_correction: 0.1047198
35 |   - laser_id: 10
36 |     rot_correction: 0
37 |     vert_correction: -0.05817182
38 |   - laser_id: 11
39 |     rot_correction: 0
40 |     vert_correction: 0.127985
41 |   - laser_id: 12
42 |     rot_correction: 0
43 |     vert_correction: -0.03490658
44 |   - laser_id: 13
45 |     rot_correction: 0
46 |     vert_correction: 0.1512677
47 |   - laser_id: 14
48 |     rot_correction: 0
49 |     vert_correction: -0.01164135
50 |   - laser_id: 15
51 |     rot_correction: 0
52 |     vert_correction: 0.1745329
53 | num_lasers: 16


--------------------------------------------------------------------------------
/test/conftest.py:
--------------------------------------------------------------------------------
 1 | # Copyright (c) 2021-2023, Martin Valgur
 2 | # SPDX-License-Identifier: BSD-3-Clause
 3 | 
 4 | from pathlib import Path
 5 | 
 6 | import pytest
 7 | import requests
 8 | 
 9 | # logging with rospy from rosbag hangs in Docker otherwise due to
10 | # https://github.com/ros/ros_comm/blob/842f0f02/tools/rosgraph/src/rosgraph/roslogging.py#L64-L71
11 | # going into an infinite loop for some reason.
12 | import logging
13 | logging.getLogger("rosout").setLevel(logging.CRITICAL)
14 | 
15 | root_dir = Path(__file__).parent.parent
16 | data_dir = Path(__file__).parent / "data"
17 | base_url = "https://github.com/valgur/velodyne_decoder/releases/download/v1.0.1/"
18 | 
19 | 
20 | def fetch(name):
21 |     url = base_url + name
22 |     if not data_dir.exists():
23 |         data_dir.mkdir()
24 |     path = data_dir / name
25 |     if not path.exists():
26 |         with path.open("wb") as f:
27 |             f.write(requests.get(url).content)
28 |     return path
29 | 
30 | 
31 | @pytest.fixture
32 | def sample_pcap_path():
33 |     return fetch("vlp16.pcap")
34 | 
35 | 
36 | @pytest.fixture
37 | def sample_bag_path():
38 |     return fetch("vlp16.bag")
39 | 
40 | 
41 | @pytest.fixture(scope="module")
42 | def pcl_struct_dtype():
43 |     return {
44 |         "names": ["x", "y", "z", "intensity", "time", "column", "ring", "return_type"],
45 |         "formats": ["<f4", "<f4", "<f4", "<f4", "<f4", "<u2", "u1", "u1"],
46 |         "offsets": [0, 4, 8, 12, 16, 20, 22, 23],
47 |         "itemsize": 32,
48 |     }
49 | 
50 | 
51 | @pytest.fixture(scope="module")
52 | def calib_data_dir():
53 |     return root_dir / "src/velodyne_decoder/calibrations"
54 | 


--------------------------------------------------------------------------------
/src/velodyne_decoder/calibrations/VLP-16.yml:
--------------------------------------------------------------------------------
 1 | # aka Puck LITE
 2 | # converted from https://gitlab.kitware.com/LidarView/velodyneplugin/-/tree/0cddc1f92e69b7b26ffc5619aceb1c00854442e6/CalibrationsFiles
 3 | # using https://wiki.ros.org/velodyne_pointcloud#gen_calibration.py
 4 | distance_resolution: 0.002
 5 | lasers:
 6 |   - laser_id: 0
 7 |     rot_correction: 0
 8 |     vert_correction: -0.2617994
 9 |   - laser_id: 1
10 |     rot_correction: 0
11 |     vert_correction: 0.01745329
12 |   - laser_id: 2
13 |     rot_correction: 0
14 |     vert_correction: -0.2268928
15 |   - laser_id: 3
16 |     rot_correction: 0
17 |     vert_correction: 0.05235988
18 |   - laser_id: 4
19 |     rot_correction: 0
20 |     vert_correction: -0.1919862
21 |   - laser_id: 5
22 |     rot_correction: 0
23 |     vert_correction: 0.08726646
24 |   - laser_id: 6
25 |     rot_correction: 0
26 |     vert_correction: -0.1570796
27 |   - laser_id: 7
28 |     rot_correction: 0
29 |     vert_correction: 0.122173
30 |   - laser_id: 8
31 |     rot_correction: 0
32 |     vert_correction: -0.122173
33 |   - laser_id: 9
34 |     rot_correction: 0
35 |     vert_correction: 0.1570796
36 |   - laser_id: 10
37 |     rot_correction: 0
38 |     vert_correction: -0.08726646
39 |   - laser_id: 11
40 |     rot_correction: 0
41 |     vert_correction: 0.1919862
42 |   - laser_id: 12
43 |     rot_correction: 0
44 |     vert_correction: -0.05235988
45 |   - laser_id: 13
46 |     rot_correction: 0
47 |     vert_correction: 0.2268928
48 |   - laser_id: 14
49 |     rot_correction: 0
50 |     vert_correction: -0.01745329
51 |   - laser_id: 15
52 |     rot_correction: 0
53 |     vert_correction: 0.2617994
54 | num_lasers: 16


--------------------------------------------------------------------------------
/include/velodyne_decoder/config.h:
--------------------------------------------------------------------------------
 1 | // Copyright (c) 2021-2023, Martin Valgur
 2 | // SPDX-License-Identifier: BSD-3-Clause
 3 | 
 4 | #pragma once
 5 | 
 6 | #include <cmath>
 7 | #include <optional>
 8 | #include <string>
 9 | #include <utility>
10 | #include <vector>
11 | 
12 | #include "velodyne_decoder/calibration.h"
13 | #include "velodyne_decoder/types.h"
14 | 
15 | namespace velodyne_decoder {
16 | 
17 | struct Config {
18 |   // PacketDecoder params
19 |   float min_range = 0.1; ///< minimum range to publish (m)
20 |   float max_range = 250; ///< maximum range to publish (m)
21 |   float min_angle = 0;   ///< minimum angle to publish (deg)
22 |   float max_angle = 360; ///< maximum angle to publish (deg)
23 |   /// model ID, optional for most models (exceptions: HDL-64E, VLP-32A, VLP-32B)
24 |   std::optional<ModelId> model;
25 |   /// calibration info, optional
26 |   std::optional<Calibration> calibration;
27 | 
28 |   // ScanBatcher params
29 |   /// Azimuth at which to start a new scan (deg).
30 |   /// If unset, the scan is split whenever it covers >= 360 deg at an arbitrary azimuth.
31 |   std::optional<float> cut_angle = std::nullopt;
32 |   /// whether we are timestamping based on the first or last packet in the scan
33 |   bool timestamp_first_packet = false;
34 | 
35 |   static const std::vector<ModelId> SUPPORTED_MODELS;
36 | };
37 | 
38 | inline const std::vector<ModelId> Config::SUPPORTED_MODELS = //
39 |     {ModelId::HDL64E_S1, ModelId::HDL64E_S2, ModelId::HDL64E_S3, ModelId::HDL32E,
40 |      ModelId::VLP32A,    ModelId::VLP32B,    ModelId::VLP32C,    ModelId::VLP16,
41 |      ModelId::PuckHiRes, ModelId::AlphaPrime};
42 | 
43 | } // namespace velodyne_decoder
44 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | BSD 3-Clause License
 2 | 
 3 | Copyright (c) 2021, Martin Valgur
 4 | Copyright (c) 2007-2021 Austin Robot Technology, and others
 5 | All rights reserved.
 6 | 
 7 | Redistribution and use in source and binary forms, with or without
 8 | modification, are permitted provided that the following conditions are met:
 9 | 
10 | 1. Redistributions of source code must retain the above copyright notice, this
11 |    list of conditions and the following disclaimer.
12 | 
13 | 2. Redistributions in binary form must reproduce the above copyright notice,
14 |    this list of conditions and the following disclaimer in the documentation
15 |    and/or other materials provided with the distribution.
16 | 
17 | 3. Neither the name of the copyright holder nor the names of its
18 |    contributors may be used to endorse or promote products derived from
19 |    this software without specific prior written permission.
20 | 
21 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
24 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
25 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
26 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
27 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
29 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
30 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31 | 


--------------------------------------------------------------------------------
/src/types.cpp:
--------------------------------------------------------------------------------
 1 | #include "velodyne_decoder/types.h"
 2 | #include "velodyne_decoder/time_conversion.h"
 3 | 
 4 | #include <algorithm>
 5 | 
 6 | namespace velodyne_decoder {
 7 | 
 8 | TimePair::TimePair(Time host, Time device) : host(host), device(device) {}
 9 | 
10 | TimePair::TimePair(Time host_stamp, gsl::span<const uint8_t, PACKET_SIZE> data)
11 |     : host(host_stamp), device(getPacketTimestamp(data, host_stamp)) {}
12 | 
13 | VelodynePacket::VelodynePacket(TimePair stamp, gsl::span<const uint8_t, PACKET_SIZE> data)
14 |     : stamp(stamp), data() {
15 |   std::copy(data.begin(), data.end(), this->data.begin());
16 | }
17 | 
18 | VelodynePacket::VelodynePacket(Time host_stamp, gsl::span<const uint8_t, PACKET_SIZE> data)
19 |     : VelodynePacket(TimePair(host_stamp, data), data) {}
20 | 
21 | PacketView::PacketView(TimePair stamp, gsl::span<const uint8_t, PACKET_SIZE> data)
22 |     : stamp(stamp), data(data) {}
23 | 
24 | PacketView::PacketView(Time host_stamp, gsl::span<const uint8_t, PACKET_SIZE> data)
25 |     : stamp(host_stamp, data), data(data) {}
26 | 
27 | PacketView::PacketView(const VelodynePacket &packet) : stamp(packet.stamp), data(packet.data) {}
28 | 
29 | VelodynePoint::VelodynePoint(float x, float y, float z, float intensity, float time,
30 |                              uint16_t column, uint8_t ring, ReturnMode return_type)
31 |     : x(x), y(y), z(z), intensity(intensity), time(time), column(column), ring(ring),
32 |       return_type(return_type) {}
33 | 
34 | ReturnMode operator|(ReturnMode lhs, ReturnMode rhs) {
35 |   return static_cast<ReturnMode>(static_cast<uint8_t>(lhs) | static_cast<uint8_t>(rhs));
36 | }
37 | 
38 | ReturnMode operator&(ReturnMode lhs, ReturnMode rhs) {
39 |   return static_cast<ReturnMode>(static_cast<uint8_t>(lhs) & static_cast<uint8_t>(rhs));
40 | }
41 | 
42 | } // namespace velodyne_decoder


--------------------------------------------------------------------------------
/include/velodyne_decoder/time_conversion.h:
--------------------------------------------------------------------------------
 1 | // Copyright (c) 2019, Matthew Pitropov, Joshua Whitley
 2 | // Copyright (c) 2021-2023, Martin Valgur
 3 | // SPDX-License-Identifier: BSD-3-Clause
 4 | 
 5 | #pragma once
 6 | 
 7 | #include "velodyne_decoder/types.h"
 8 | 
 9 | #include <cstdint>
10 | #include <gsl/span>
11 | 
12 | namespace velodyne_decoder {
13 | 
14 | constexpr uint32_t parseUint32(const uint8_t *data) {
15 |   return (((uint32_t)data[3]) << 24u | //
16 |           ((uint32_t)data[2]) << 16u | //
17 |           ((uint32_t)data[1]) << 8u |  //
18 |           ((uint32_t)data[0]));
19 | }
20 | 
21 | /** @brief Function used to check that hour assigned to timestamp in conversion is
22 |  * correct. Velodyne only returns time since the top of the hour, so if the computer clock
23 |  * and the velodyne clock (gps-synchronized) are a little off, there is a chance the wrong
24 |  * hour may be associated with the timestamp
25 |  *
26 |  * @param packet_time timestamp recovered from velodyne
27 |  * @param reference_time time coming from computer's clock
28 |  * @return timestamp from velodyne, possibly shifted by 1 hour if the function arguments
29 |  * disagree by more than a half-hour.
30 |  */
31 | Time resolveHourAmbiguity(Time packet_time, Time reference_time);
32 | 
33 | /**
34 |  * @brief Merges the the top of the hour timestamp from the Velodyne lidar with a
35 |  * reference timestamp, i.e. time from the computer's clock or a GPS receiver.
36 |  *
37 |  * @param usec_since_toh number of microseconds from the top of the hour
38 |  * @param reference_time reference time as a timestamp in seconds since the Unix epoch
39 |  * @return timestamp in seconds since the Unix epoch
40 |  */
41 | Time getPacketTimestamp(uint32_t usec_since_toh, Time reference_time);
42 | 
43 | Time getPacketTimestamp(gsl::span<const uint8_t, PACKET_SIZE> pkt_data, Time reference_time);
44 | 
45 | } // namespace velodyne_decoder
46 | 


--------------------------------------------------------------------------------
/include/velodyne_decoder/scan_batcher.h:
--------------------------------------------------------------------------------
 1 | // Copyright (c) 2023, Martin Valgur
 2 | // SPDX-License-Identifier: BSD-3-Clause
 3 | 
 4 | #pragma once
 5 | 
 6 | #include "velodyne_decoder/config.h"
 7 | #include "velodyne_decoder/types.h"
 8 | 
 9 | #include <gsl/span>
10 | #include <memory>
11 | #include <optional>
12 | #include <vector>
13 | 
14 | namespace velodyne_decoder {
15 | 
16 | template <typename PacketT = VelodynePacket> class ScanBatcher {
17 | public:
18 |   explicit inline ScanBatcher(const Config &config);
19 | 
20 |   /** @brief Pushes a packet into the batcher.
21 |    * @return true if the scan is complete, false otherwise.
22 |    */
23 |   inline bool push(const PacketT &packet);
24 | 
25 |   [[nodiscard]] inline bool empty() const;
26 | 
27 |   [[nodiscard]] inline size_t size() const;
28 | 
29 |   /** @brief True if the current scan is complete.
30 |    */
31 |   [[nodiscard]] inline bool scanComplete() const;
32 | 
33 |   /** @brief The contents of the current scan.
34 |    */
35 |   [[nodiscard]] inline const std::shared_ptr<std::vector<PacketT>> &scanPackets() const;
36 | 
37 |   inline void reset(std::shared_ptr<std::vector<PacketT>> scan_packets);
38 | 
39 |   inline void reset();
40 | 
41 | private:
42 |   std::shared_ptr<std::vector<PacketT>> scan_packets_ = std::make_shared<std::vector<PacketT>>();
43 |   int initial_azimuth_                                = -1;
44 |   int coverage_                                       = 0;
45 |   bool scan_complete_                                 = false;
46 |   std::optional<PacketT> kept_last_packet_            = std::nullopt;
47 | 
48 |   // config
49 |   const bool is_device_time_valid_;
50 |   const int cut_angle_;
51 |   const double duration_threshold_ = 0.3; // max scan duration at ~4 Hz
52 | };
53 | 
54 | inline PacketView toPacketView(const VelodynePacket &packet) { return {packet}; }
55 | 
56 | extern template class ScanBatcher<VelodynePacket>;
57 | 
58 | } // namespace velodyne_decoder
59 | 
60 | #include "scan_batcher.inl"
61 | 


--------------------------------------------------------------------------------
/pyproject.toml:
--------------------------------------------------------------------------------
 1 | [project]
 2 | name = "velodyne-decoder"
 3 | version = "3.1.0"
 4 | description = "Decoder for raw Velodyne packet data"
 5 | readme = "README.md"
 6 | requires-python = ">= 3.8"
 7 | authors = [{name = "Martin Valgur", email = "martin.valgur@gmail.com"}]
 8 | urls."Homepage" = "https://github.com/valgur/velodyne_decoder"
 9 | urls."Bug Reports" = "https://github.com/valgur/velodyne_decoder/issues"
10 | urls."Source" = "https://github.com/valgur/velodyne_decoder"
11 | keywords = [
12 |     "Velodyne",
13 |     "pointcloud",
14 |     "PCL",
15 | ]
16 | license = {text = "BSD-3-Clause"}
17 | classifiers = [
18 |     "License :: OSI Approved :: BSD License",
19 |     "Programming Language :: Python :: 3",
20 |     "Operating System :: OS Independent",
21 | ]
22 | dependencies = [
23 |     "numpy",
24 |     "importlib_resources",
25 |     "dpkt",
26 |     "pyyaml",
27 | ]
28 | 
29 | [project.scripts]
30 | extract-hdl64e-calibration = "velodyne_decoder.hdl64e:cli"
31 | 
32 | [tool.setuptools]
33 | zip-safe = false
34 | include-package-data = true
35 | 
36 | [build-system]
37 | requires = [
38 |     "scikit-build-core >=0.4.3",
39 |     "conan >=2.0.5",
40 |     "nanobind"
41 | ]
42 | build-backend = "scikit_build_core.build"
43 | 
44 | [tool.scikit-build]
45 | cmake.minimum-version = "3.15"
46 | cmake.targets = ["python_bindings"]
47 | install.components = ["python"]
48 | wheel.license-files = ["LICENSE"]
49 | build-dir = "build/{wheel_tag}"
50 | # Build stable ABI wheels for CPython 3.12+
51 | wheel.py-api = "cp312"
52 | 
53 | [tool.scikit-build.cmake.define]
54 | BUILD_PYTHON = true
55 | BUILD_SHARED_LIBS = false
56 | 
57 | [tool.cibuildwheel]
58 | build = "*"
59 | # Disable building of PyPy wheels on all platforms
60 | skip = "pp*"
61 | before-test = "pip install -r {project}/requirements-dev.txt"
62 | test-command = "pytest {project}/test --color=yes -v"
63 | build-verbosity = 1
64 | archs = "auto64"
65 | 
66 | # Needed for full C++17 support
67 | [tool.cibuildwheel.macos.environment]
68 | MACOSX_DEPLOYMENT_TARGET = "10.15"
69 | 


--------------------------------------------------------------------------------
/test/test_calib.py:
--------------------------------------------------------------------------------
 1 | # Copyright (c) 2021-2023, Martin Valgur
 2 | # SPDX-License-Identifier: BSD-3-Clause
 3 | 
 4 | import math
 5 | 
 6 | import pytest
 7 | import velodyne_decoder as vd
 8 | import yaml
 9 | 
10 | MODELS = list(vd.Model.__members__)
11 | 
12 | 
13 | def test_config_constructor(calib_data_dir):
14 |     vd.Config()
15 |     vd.Config(model=vd.Model.VLP16)
16 |     calibration = vd.Calibration.read(str(calib_data_dir / "VLP-16.yml"))
17 |     vd.Config(model=vd.Model.VLP16, calibration=calibration)
18 | 
19 | 
20 | @pytest.mark.parametrize("model_id", MODELS)
21 | def test_bundled_calibrations(model_id):
22 |     model_id = vd.Model.__members__[model_id]
23 |     calib = None
24 |     if model_id.name.startswith("HDL64E"):
25 |         calib_file = {
26 |             vd.Model.HDL64E_S1: "HDL-64E_S1-utexas.yml",
27 |             vd.Model.HDL64E_S2: "HDL-64E_S2.1-sztaki.yml",
28 |             vd.Model.HDL64E_S3: "HDL-64E_S3-VeloView.yml",
29 |         }[model_id]
30 |         calib = vd.get_bundled_calibration(calib_file)
31 |     vd.ScanDecoder(vd.Config(model=model_id, calibration=calib))
32 | 
33 | 
34 | @pytest.mark.parametrize("model_id", [m for m in MODELS if not m.startswith("HDL64E")])
35 | def test_default_calib_vertical_offsets(model_id):
36 |     """Check that the pre-defined vertical offset calibration values are sensible."""
37 |     model_id = vd.Model.__members__[model_id]
38 |     calib = vd.Calibration.default_calibs[model_id]
39 |     lasers = yaml.safe_load(calib.to_string())["lasers"]
40 |     focal_distance = {
41 |         vd.Model.AlphaPrime: 58.63e-3,
42 |         vd.Model.HDL32E: 28.83e-3,
43 |         vd.Model.PuckHiRes: 41.91e-3,
44 |         vd.Model.VLP16: 41.91e-3,
45 |         vd.Model.VLP32A: 42.4e-3,
46 |         vd.Model.VLP32B: 42.4e-3,
47 |         vd.Model.VLP32C: 42.4e-3,
48 |     }[model_id]
49 |     for laser in lasers:
50 |         approx_offset = focal_distance * math.tan(-laser["vert_correction"])
51 |         thr = 0.5e-3 if model_id == vd.Model.PuckHiRes else 0.1e-3
52 |         assert pytest.approx(approx_offset, abs=thr) == laser["vert_offset_correction"]
53 | 


--------------------------------------------------------------------------------
/src/velodyne_decoder/bag.py:
--------------------------------------------------------------------------------
 1 | # Copyright (c) 2021-2023, Martin Valgur
 2 | # SPDX-License-Identifier: BSD-3-Clause
 3 | 
 4 | import sys
 5 | from collections import namedtuple
 6 | 
 7 | import velodyne_decoder as vd
 8 | 
 9 | is_py2 = sys.version_info[0] == 2
10 | 
11 | 
12 | def read_bag(
13 |     bag_file,
14 |     config=None,
15 |     topics=None,
16 |     as_pcl_structs=False,
17 |     time_range=(None, None),
18 | ):
19 |     """Decodes and yields all point clouds stored in a ROS bag file.
20 | 
21 |     `model` parameter must be set in the provided config.
22 | 
23 |     Parameters
24 |     ----------
25 |     bag_file : path or file handle
26 |     config : Config, optional
27 |     topics : str or list of str
28 |     as_pcl_structs : bool
29 |         If False, the returned NumPy arrays will be a contiguous array of floats (default).
30 |         If True, the returned NumPy arrays will contain PCL-compatible structs with dtype
31 |         {'names': ['x', 'y', 'z', 'intensity', 'ring', 'time'],
32 |          'formats': ['<f4', '<f4', '<f4', '<f4', '<u2', '<f4'],
33 |          'offsets': [0, 4, 8, 16, 20, 24], 'itemsize': 32}
34 |     return_frame_id : bool
35 |         If True, includes the frame_id of the messages in the returned tuple. Defaults to False.
36 |     time_range : float, float or rospy.Time, rospy.Time
37 |         Optionally only return scans from given time range.
38 | 
39 |     Yields
40 |     ------
41 |     timestamp: rospy.Time
42 |     point_cloud : numpy.ndarray
43 |     topic : str
44 |     """
45 |     if config is None:
46 |         config = vd.Config()
47 |     decoder = vd.ScanDecoder(config)
48 |     Result = namedtuple("ResultTuple", ("stamp", "points", "topic", "frame_id"))
49 |     msg_types = ["velodyne_msgs/VelodyneScan"]
50 |     for topic, scan_msg, ros_time in vd.util.iter_bag(bag_file, topics, msg_types, time_range):
51 |         if is_py2:
52 |             for packet in scan_msg.packets:
53 |                 packet.data = bytearray(packet.data)
54 |         stamp, points = decoder.decode_message(scan_msg, as_pcl_structs)
55 |         yield Result(stamp, points, topic, scan_msg.header.frame_id)
56 | 


--------------------------------------------------------------------------------
/include/velodyne_decoder/ros/ros_scan_batcher.h:
--------------------------------------------------------------------------------
 1 | // Copyright (c) 2023, Martin Valgur
 2 | // SPDX-License-Identifier: BSD-3-Clause
 3 | 
 4 | #pragma once
 5 | 
 6 | #include <ros/time.h>
 7 | #include <velodyne_decoder/time_conversion.h>
 8 | #include <velodyne_msgs/VelodynePacket.h>
 9 | #include <velodyne_msgs/VelodyneScan.h>
10 | 
11 | #include <memory>
12 | #include <string>
13 | 
14 | namespace velodyne_decoder {
15 | inline PacketView toPacketView(const velodyne_msgs::VelodynePacket &packet_msg) {
16 |   return {packet_msg.stamp.toSec(), gsl::span<const uint8_t, PACKET_SIZE>{packet_msg.data}};
17 | }
18 | } // namespace velodyne_decoder
19 | 
20 | #include <velodyne_decoder/config.h>
21 | #include <velodyne_decoder/scan_batcher.h>
22 | 
23 | namespace velodyne_decoder {
24 | 
25 | class RosScanBatcher {
26 | public:
27 |   using Base = ScanBatcher<velodyne_msgs::VelodynePacket>;
28 | 
29 |   inline explicit RosScanBatcher(const Config &config, const std::string frame_id)
30 |       : frame_id_(frame_id), timestamp_first_packet_(config.timestamp_first_packet),
31 |         scan_batcher_(config) {
32 |     reset();
33 |   }
34 | 
35 |   inline void reset() {
36 |     scan_msg_ = boost::make_shared<velodyne_msgs::VelodyneScan>();
37 |     scan_batcher_.reset({&scan_msg_->packets, [](auto *) {}});
38 |   }
39 | 
40 |   inline bool push(const velodyne_msgs::VelodynePacket &packet) {
41 |     return scan_batcher_.push(packet);
42 |   }
43 | 
44 |   [[nodiscard]] inline bool scanComplete() const { return scan_batcher_.scanComplete(); }
45 | 
46 |   [[nodiscard]] inline const boost::shared_ptr<velodyne_msgs::VelodyneScan> &scanMsg() const {
47 |     scan_msg_->header.frame_id = frame_id_;
48 |     scan_msg_->header.stamp    = timestamp_first_packet_ ? scan_msg_->packets.front().stamp
49 |                                                          : scan_msg_->packets.back().stamp;
50 |     return scan_msg_;
51 |   }
52 | 
53 | private:
54 |   std::string frame_id_;
55 |   bool timestamp_first_packet_;
56 |   ScanBatcher<velodyne_msgs::VelodynePacket> scan_batcher_;
57 |   boost::shared_ptr<velodyne_msgs::VelodyneScan> scan_msg_;
58 | };
59 | 
60 | } // namespace velodyne_decoder
61 | 


--------------------------------------------------------------------------------
/test/data/mock_vls128_dual.py:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python3
 2 | # Copyright (c) 2023, Martin Valgur
 3 | # SPDX-License-Identifier: BSD-3-Clause
 4 | """
 5 | Convert a single-return VLS-128 pcap file into a mock dual-return VLS-128 pcap file.
 6 | All returns are both strongest and last.
 7 | """
 8 | import struct
 9 | from pathlib import Path
10 | 
11 | import dpkt
12 | from tqdm.auto import tqdm
13 | 
14 | pcap_path = Path(__file__).parent / "VLS-128_single/VLS-128_single.pcap"
15 | output_path = Path(__file__).parent / "VLS-128_dual/VLS-128_dual.pcap"
16 | 
17 | PACKET_SIZE = 1206
18 | 
19 | FIRING_DURATION = (8 / 3) * 20 * 1e-6
20 | 
21 | with pcap_path.open("rb") as fin, output_path.open("wb") as fout:
22 |     pcap_out = dpkt.pcap.Writer(fout)
23 |     dummy_block = bytearray(100)
24 |     packet_count = sum(1 for _ in dpkt.pcap.Reader(fin))
25 |     fin.seek(0)
26 |     for pcap_stamp, buf in tqdm(dpkt.pcap.Reader(fin), total=packet_count):
27 |         eth = dpkt.ethernet.Ethernet(buf)
28 |         try:
29 |             data = dpkt.ethernet.Ethernet(buf)
30 |         except dpkt.dpkt.UnpackError:
31 |             continue
32 |         while hasattr(data, "data"):
33 |             data = data.data
34 |         if len(data) != PACKET_SIZE:
35 |             pcap_out.writepkt(buf, stamp)
36 |             continue
37 |         # Parse
38 |         blocks = [data[i * 100 : (i + 1) * 100] for i in range(12)]
39 |         stamp, dual_return_mode, model_id = struct.unpack("<LBB", data[-6:])
40 |         # Convert three sequences in a single-return packet into three dual-return packets
41 |         for seq_idx in range(3):
42 |             new_blocks = []
43 |             for block_idx in range(4):
44 |                 new_blocks.append(blocks[seq_idx * 4 + block_idx])
45 |                 new_blocks.append(blocks[seq_idx * 4 + block_idx])
46 |             dummy_block[2:4] = new_blocks[0][2:4]
47 |             for i in range(4):
48 |                 new_blocks.append(dummy_block)
49 |             new_stamp = stamp + int(round(FIRING_DURATION * 1e6 * seq_idx))
50 |             new_buf = b"".join(new_blocks) + struct.pack("<LBB", new_stamp, 0x39, model_id)
51 | 
52 |             # Re-frame the data and write to PCAP
53 |             ip = eth.data
54 |             udp = ip.data
55 |             udp.data = new_buf
56 |             udp.ulen = len(udp)
57 |             ip.data = udp
58 |             ip.len = len(ip)
59 |             eth = dpkt.ethernet.Ethernet(data=ip)
60 |             pcap_out.writepkt(eth, pcap_stamp + FIRING_DURATION * seq_idx)
61 | 


--------------------------------------------------------------------------------
/conanfile.py:
--------------------------------------------------------------------------------
 1 | import os
 2 | import re
 3 | 
 4 | from conan import ConanFile
 5 | from conan.tools.cmake import CMake, CMakeToolchain, CMakeDeps, cmake_layout
 6 | from conan.tools.files import copy, load, rmdir
 7 | 
 8 | required_conan_version = ">=1.52.0"
 9 | 
10 | 
11 | class VelodyneDecoderConan(ConanFile):
12 |     name = "velodyne_decoder"
13 |     description = "Decoder for raw Velodyne packet data"
14 |     license = "BSD-3-Clause"
15 |     url = "https://github.com/valgur/velodyne_decoder"
16 | 
17 |     package_type = "library"
18 |     settings = "os", "compiler", "build_type", "arch"
19 |     options = {
20 |         "shared": [True, False],
21 |         "fPIC": [True, False],
22 |     }
23 |     default_options = {
24 |         "shared": False,
25 |         "fPIC": True,
26 |     }
27 | 
28 |     exports_sources = ["include/*", "src/*", "test/*", "docs/*", "cmake/*", "CMakeLists.txt", "LICENSE"]
29 | 
30 |     def set_version(self):
31 |         cmakelists = load(self, os.path.join(self.recipe_folder, "CMakeLists.txt"))
32 |         self.version = re.findall(r"project\(\w+\s+VERSION\s+(\S+)", cmakelists)[0]
33 | 
34 |     def config_options(self):
35 |         if self.settings.os == "Windows":
36 |             del self.options.fPIC
37 | 
38 |     def configure(self):
39 |         if self.options.shared:
40 |             self.options.rm_safe("fPIC")
41 | 
42 |     def layout(self):
43 |         cmake_layout(self)
44 | 
45 |     def requirements(self):
46 |         self.requires("yaml-cpp/0.8.0")
47 |         self.requires("ms-gsl/4.0.0", transitive_headers=True)
48 | 
49 |     def generate(self):
50 |         tc = CMakeToolchain(self)
51 |         tc.cache_variables["INSTALL_THIRD_PARTY"] = False
52 |         tc.generate()
53 |         deps = CMakeDeps(self)
54 |         deps.generate()
55 | 
56 |     def build(self):
57 |         cmake = CMake(self)
58 |         cmake.configure()
59 |         cmake.build()
60 | 
61 |     def package(self):
62 |         copy(self, "LICENSE", self.source_folder, os.path.join(self.package_folder, "licenses"))
63 |         cmake = CMake(self)
64 |         cmake.install()
65 |         rmdir(self, os.path.join(self.package_folder, "lib", "cmake"))
66 | 
67 |     def package_info(self):
68 |         self.cpp_info.set_property("cmake_file_name", "velodyne_decoder")
69 |         self.cpp_info.set_property("cmake_target_name", "velodyne_decoder::velodyne_decoder")
70 |         self.cpp_info.set_property("pkg_config_name", "velodyne_decoder")
71 | 
72 |         self.cpp_info.libs = ["velodyne_decoder"]
73 |         self.cpp_info.defines = ["_USE_MATH_DEFINES"]
74 | 


--------------------------------------------------------------------------------
/src/velodyne_decoder/pcap.py:
--------------------------------------------------------------------------------
 1 | # Copyright (c) 2021-2023, Martin Valgur
 2 | # SPDX-License-Identifier: BSD-3-Clause
 3 | 
 4 | import sys
 5 | from collections import namedtuple
 6 | 
 7 | import velodyne_decoder as vd
 8 | 
 9 | is_py2 = sys.version_info[0] == 2
10 | 
11 | 
12 | def read_pcap(
13 |     pcap_file,
14 |     config=None,
15 |     as_pcl_structs=False,
16 |     time_range=(None, None),
17 | ):
18 |     """Decodes and yields all point clouds stored in a PCAP file.
19 | 
20 |     `model` must be set in the provided config.
21 | 
22 |     Parameters
23 |     ----------
24 |     pcap_file : path or file handle
25 |     config : Config, optional
26 |     as_pcl_structs : bool
27 |         If False, the returned NumPy arrays will be a contiguous array of floats (default).
28 |         If True, the returned NumPy arrays will contain PCL-compatible structs with dtype
29 |         {'names': ['x', 'y', 'z', 'intensity', 'ring', 'time'],
30 |          'formats': ['<f4', '<f4', '<f4', '<f4', '<u2', '<f4'],
31 |          'offsets': [0, 4, 8, 16, 20, 24], 'itemsize': 32}
32 |     time_range : float, float
33 |         Optionally only return scans from given time range.
34 | 
35 |     Yields
36 |     ------
37 |     timestamp: float
38 |     point_cloud : numpy.ndarray
39 |     """
40 |     if config is None:
41 |         config = vd.Config()
42 | 
43 |     # Extract model and calibration for HDL-64E S2/S3 from the pcap file if not specified
44 |     if config.model is None or config.calibration is None:
45 |         try:
46 |             model, calib = vd.hdl64e.detect_hdl64e(pcap_file)
47 |         except ValueError as e:
48 |             raise ValueError(
49 |                 "Data is from a HDL-64E, but the exact model and calibration could not be determined: "
50 |                 + str(e)
51 |             )
52 |         config.model = config.model or model
53 |         config.calibration = config.calibration or calib
54 |         if hasattr(pcap_file, "seek"):
55 |             pcap_file.seek(0)
56 | 
57 |     decoder = vd.StreamDecoder(config)
58 |     ResultTuple = namedtuple("StampCloudTuple", ("stamp", "points"))
59 |     for host_stamp, data in vd.util.iter_pcap(pcap_file, time_range):
60 |         if len(data) != vd.PACKET_SIZE:
61 |             continue
62 |         if is_py2:
63 |             data = bytearray(data)
64 |         result = decoder.decode(host_stamp, data, as_pcl_structs)
65 |         if result is not None:
66 |             yield ResultTuple(*result)
67 |     # Decode any remaining packets
68 |     result = decoder.finish(as_pcl_structs)
69 |     if result is not None:
70 |         yield ResultTuple(*result)
71 | 


--------------------------------------------------------------------------------
/test/test_pcap.py:
--------------------------------------------------------------------------------
 1 | # Copyright (c) 2021-2023, Martin Valgur
 2 | # SPDX-License-Identifier: BSD-3-Clause
 3 | 
 4 | import numpy as np
 5 | import velodyne_decoder as vd
 6 | 
 7 | 
 8 | def test_pcap_as_contiguous_array(sample_pcap_path):
 9 |     pcds = list(vd.read_pcap(sample_pcap_path, as_pcl_structs=False))
10 |     assert len(pcds) == 94
11 |     stamp, pcd = pcds[0]
12 |     assert stamp.host == 1427759049.259595
13 |     assert pcd.shape == (27657, 8)
14 |     assert pcd.dtype.name == "float32"
15 |     # Check that the column index is monotonically increasing
16 |     column_idx = pcd[:, vd.PointField.column].astype(int)
17 |     assert np.diff(column_idx).min() == 0
18 |     # Sanity check that the point clouds are actually distinct
19 |     assert not all(len(x) == len(pcd) for _, x in pcds)
20 | 
21 | 
22 | def test_pcap_as_struct_array(sample_pcap_path, pcl_struct_dtype):
23 |     pcds = list(vd.read_pcap(sample_pcap_path, as_pcl_structs=True))
24 |     assert len(pcds) == 94
25 |     stamp, pcd = pcds[0]
26 |     assert stamp.host == 1427759049.259595
27 |     assert pcd.shape == (27657,)
28 |     assert pcd.dtype == pcl_struct_dtype
29 |     # Check that the column index is monotonically increasing
30 |     column_idx = pcd["column"].astype(int)
31 |     assert np.diff(column_idx).min() == 0
32 |     assert not all(len(x) == len(pcd) for _, x in pcds)
33 | 
34 | 
35 | def test_pcap_time_range(sample_pcap_path):
36 |     t0 = 1427759050
37 |     pcds = list(vd.read_pcap(sample_pcap_path, as_pcl_structs=False, time_range=(t0, None)))
38 |     stamp, pcd = pcds[0]
39 |     assert stamp.host >= t0
40 |     assert stamp.device >= t0
41 | 
42 | 
43 | def _struct_pcd_to_contiguous(pcd_struct):
44 |     shape = (len(pcd_struct), len(pcd_struct.dtype.fields))
45 |     converted_pcd = np.empty(shape, dtype=np.float32)
46 |     for i, field in enumerate(pcd_struct.dtype.fields):
47 |         converted_pcd[:, i] = pcd_struct[field]
48 |     return converted_pcd
49 | 
50 | 
51 | def test_pcap_structs_match_contiguous(sample_pcap_path):
52 |     """Check that the contents of as_pcl_structs=True/False match."""
53 |     pcds_struct = list(vd.read_pcap(sample_pcap_path, as_pcl_structs=True))
54 |     pcds_contiguous = list(vd.read_pcap(sample_pcap_path, as_pcl_structs=False))
55 |     assert len(pcds_struct) == len(pcds_contiguous)
56 |     for (stamp_struct, pcd_struct), (stamp_contiguous, pcd_contiguous) in zip(pcds_struct, pcds_contiguous):
57 |         assert stamp_struct.host == stamp_contiguous.host
58 |         assert stamp_struct.device == stamp_contiguous.device
59 |         cast_struct_pcd = _struct_pcd_to_contiguous(pcd_struct)
60 |         assert np.allclose(cast_struct_pcd, pcd_contiguous, rtol=1e-6)
61 | 


--------------------------------------------------------------------------------
/include/velodyne_decoder/calibration.h:
--------------------------------------------------------------------------------
 1 | // Copyright (c) 2012, 2019, Austin Robot Technology, Piyush Khandelwal, Joshua Whitley
 2 | // Copyright (c) 2021-2023, Martin Valgur
 3 | // SPDX-License-Identifier: BSD-3-Clause
 4 | 
 5 | #pragma once
 6 | 
 7 | #include <map>
 8 | #include <string>
 9 | #include <unordered_map>
10 | #include <utility>
11 | #include <vector>
12 | 
13 | #include "velodyne_decoder/types.h"
14 | 
15 | namespace velodyne_decoder {
16 | 
17 | /** \brief correction values for a single laser
18 |  *
19 |  * Correction values for a single laser (as provided by db.xml from
20 |  * Velodyne).  Includes parameters for Velodyne HDL-64E S2.1.
21 |  *
22 |  * http://velodynelidar.com/lidar/products/manual/63-HDL64E%20S2%20Manual_Rev%20D_2011_web.pdf
23 |  */
24 | 
25 | /** \brief Correction information for a single laser. */
26 | struct LaserCorrection {
27 |   /** parameters in db.xml */
28 |   float rot_correction             = 0;
29 |   float vert_correction            = 0;
30 |   float dist_correction            = 0;
31 |   bool two_pt_correction_available = false;
32 |   float dist_correction_x          = 0;
33 |   float dist_correction_y          = 0;
34 |   float vert_offset_correction     = 0;
35 |   float horiz_offset_correction    = 0;
36 |   int max_intensity                = 255;
37 |   int min_intensity                = 0;
38 |   float focal_distance             = 0;
39 |   float focal_slope                = 0;
40 | 
41 |   uint16_t laser_idx  = -1; ///< index of the laser in the measurements block of a packet
42 |   uint16_t laser_ring = -1; ///< ring number for this laser
43 | };
44 | 
45 | /** \brief Calibration information for the entire device. */
46 | class Calibration {
47 | public:
48 |   float distance_resolution_m = 0.002f;
49 |   std::vector<LaserCorrection> laser_corrections;
50 |   int num_lasers = 0;
51 | 
52 | public:
53 |   Calibration() = default;
54 |   explicit Calibration(const std::string &calibration_file);
55 |   Calibration(std::vector<LaserCorrection> laser_corrs, float distance_resolution_m);
56 | 
57 |   bool isAdvancedCalibration() const;
58 | 
59 |   static Calibration read(const std::string &calibration_file);
60 |   void write(const std::string &calibration_file) const;
61 | 
62 |   static Calibration fromString(const std::string &calibration_content);
63 |   std::string toString() const;
64 | 
65 | private:
66 |   void assignRingNumbers();
67 | };
68 | 
69 | class CalibDB {
70 | public:
71 |   CalibDB();
72 | 
73 |   Calibration getDefaultCalibration(ModelId model_id) const;
74 |   std::vector<ModelId> getAvailableModels() const;
75 | 
76 |   const std::unordered_map<ModelId, Calibration> &getAllDefaultCalibrations() const {
77 |     return calibrations_;
78 |   };
79 | 
80 | private:
81 |   std::unordered_map<ModelId, Calibration> calibrations_;
82 | };
83 | 
84 | } // namespace velodyne_decoder
85 | 


--------------------------------------------------------------------------------
/test/test_ros.py:
--------------------------------------------------------------------------------
 1 | # Copyright (c) 2021-2023, Martin Valgur
 2 | # SPDX-License-Identifier: BSD-3-Clause
 3 | import numpy as np
 4 | import velodyne_decoder as vd
 5 | 
 6 | 
 7 | def test_bag_as_contiguous_array(sample_bag_path):
 8 |     pcds = list(vd.read_bag(sample_bag_path, topics="/velodyne_packets", as_pcl_structs=False))
 9 |     assert len(pcds) == 13
10 |     stamp, pcd, topic, frame_id = pcds[0]
11 |     assert topic == "/velodyne_packets"
12 |     assert stamp.host == 1636622716.742135
13 |     assert pcd.shape == (27300, 8)
14 |     assert pcd.dtype.name == "float32"
15 |     assert frame_id == "velodyne"
16 |     # Sanity check that the point clouds are actually distinct
17 |     assert not all(len(x) == len(pcd) for _, x, _, _ in pcds)
18 | 
19 | 
20 | def test_bag_as_struct_array(sample_bag_path, pcl_struct_dtype):
21 |     pcds = list(vd.read_bag(sample_bag_path, topics="/velodyne_packets", as_pcl_structs=True))
22 |     assert len(pcds) == 13
23 |     stamp, pcd, topic, frame_id = pcds[0]
24 |     assert topic == "/velodyne_packets"
25 |     assert stamp.host == 1636622716.742135
26 |     assert pcd.shape == (27300,)
27 |     assert pcd.dtype == pcl_struct_dtype
28 |     assert frame_id == "velodyne"
29 |     assert not all(len(x) == len(pcd) for _, x, _, _ in pcds)
30 | 
31 | 
32 | def test_bag_automatic_topic(sample_bag_path):
33 |     pcds = list(vd.read_bag(sample_bag_path))
34 |     assert len(pcds) == 13
35 |     stamp, pcd, topic, frame_id = pcds[0]
36 |     assert topic == "/velodyne_packets"
37 |     assert stamp.host == 1636622716.742135
38 |     assert pcd.shape == (27300, 8)
39 |     assert frame_id == "velodyne"
40 | 
41 | 
42 | def test_bag_time_range(sample_bag_path):
43 |     t0 = 1636622717
44 |     pcds = list(vd.read_bag(sample_bag_path, time_range=(t0, None)))
45 |     assert pcds[0].stamp.host >= t0
46 | 
47 | 
48 | def _struct_pcd_to_contiguous(pcd_struct):
49 |     shape = (len(pcd_struct), len(pcd_struct.dtype.fields))
50 |     converted_pcd = np.empty(shape, dtype=np.float32)
51 |     for i, field in enumerate(pcd_struct.dtype.fields):
52 |         converted_pcd[:, i] = pcd_struct[field]
53 |     return converted_pcd
54 | 
55 | 
56 | def test_pcap_structs_match_contiguous(sample_bag_path):
57 |     """Check that the contents of as_pcl_structs=True/False match."""
58 |     pcds_struct = list(vd.read_bag(sample_bag_path, topics="/velodyne_packets", as_pcl_structs=True))
59 |     pcds_contiguous = list(vd.read_bag(sample_bag_path, topics="/velodyne_packets", as_pcl_structs=False))
60 |     assert len(pcds_struct) == len(pcds_contiguous)
61 |     for (stamp_struct, pcd_struct, _, _), (stamp_contiguous, pcd_contiguous, _, _) in zip(pcds_struct, pcds_contiguous):
62 |         assert stamp_struct.host == stamp_contiguous.host
63 |         assert stamp_struct.device == stamp_contiguous.device
64 |         cast_struct_pcd = _struct_pcd_to_contiguous(pcd_struct)
65 |         assert np.allclose(cast_struct_pcd, pcd_contiguous, rtol=1e-6)
66 | 


--------------------------------------------------------------------------------
/src/velodyne_decoder/calibrations/HDL-32E.yml:
--------------------------------------------------------------------------------
  1 | # converted from https://gitlab.kitware.com/LidarView/velodyneplugin/-/tree/0cddc1f92e69b7b26ffc5619aceb1c00854442e6/CalibrationsFiles
  2 | # using https://wiki.ros.org/velodyne_pointcloud#gen_calibration.py
  3 | distance_resolution: 0.002
  4 | lasers:
  5 |   - laser_id: 0
  6 |     rot_correction: 0
  7 |     vert_correction: -0.5352925
  8 |   - laser_id: 1
  9 |     rot_correction: 0
 10 |     vert_correction: -0.1628392
 11 |   - laser_id: 2
 12 |     rot_correction: 0
 13 |     vert_correction: -0.5119051
 14 |   - laser_id: 3
 15 |     rot_correction: 0
 16 |     vert_correction: -0.1396263
 17 |   - laser_id: 4
 18 |     rot_correction: 0
 19 |     vert_correction: -0.4886922
 20 |   - laser_id: 5
 21 |     rot_correction: 0
 22 |     vert_correction: -0.1164135
 23 |   - laser_id: 6
 24 |     rot_correction: 0
 25 |     vert_correction: -0.4654793
 26 |   - laser_id: 7
 27 |     rot_correction: 0
 28 |     vert_correction: -0.09302605
 29 |   - laser_id: 8
 30 |     rot_correction: 0
 31 |     vert_correction: -0.4420919
 32 |   - laser_id: 9
 33 |     rot_correction: 0
 34 |     vert_correction: -0.06981317
 35 |   - laser_id: 10
 36 |     rot_correction: 0
 37 |     vert_correction: -0.418879
 38 |   - laser_id: 11
 39 |     rot_correction: 0
 40 |     vert_correction: -0.04660029
 41 |   - laser_id: 12
 42 |     rot_correction: 0
 43 |     vert_correction: -0.3956662
 44 |   - laser_id: 13
 45 |     rot_correction: 0
 46 |     vert_correction: -0.02321288
 47 |   - laser_id: 14
 48 |     rot_correction: 0
 49 |     vert_correction: -0.3722787
 50 |   - laser_id: 15
 51 |     rot_correction: 0
 52 |     vert_correction: 0
 53 |   - laser_id: 16
 54 |     rot_correction: 0
 55 |     vert_correction: -0.3490658
 56 |   - laser_id: 17
 57 |     rot_correction: 0
 58 |     vert_correction: 0.02321288
 59 |   - laser_id: 18
 60 |     rot_correction: 0
 61 |     vert_correction: -0.325853
 62 |   - laser_id: 19
 63 |     rot_correction: 0
 64 |     vert_correction: 0.04660029
 65 |   - laser_id: 20
 66 |     rot_correction: 0
 67 |     vert_correction: -0.3024656
 68 |   - laser_id: 21
 69 |     rot_correction: 0
 70 |     vert_correction: 0.06981317
 71 |   - laser_id: 22
 72 |     rot_correction: 0
 73 |     vert_correction: -0.2792527
 74 |   - laser_id: 23
 75 |     rot_correction: 0
 76 |     vert_correction: 0.09302605
 77 |   - laser_id: 24
 78 |     rot_correction: 0
 79 |     vert_correction: -0.2560398
 80 |   - laser_id: 25
 81 |     rot_correction: 0
 82 |     vert_correction: 0.1164135
 83 |   - laser_id: 26
 84 |     rot_correction: 0
 85 |     vert_correction: -0.2326524
 86 |   - laser_id: 27
 87 |     rot_correction: 0
 88 |     vert_correction: 0.1396263
 89 |   - laser_id: 28
 90 |     rot_correction: 0
 91 |     vert_correction: -0.2094395
 92 |   - laser_id: 29
 93 |     rot_correction: 0
 94 |     vert_correction: 0.1628392
 95 |   - laser_id: 30
 96 |     rot_correction: 0
 97 |     vert_correction: -0.1862266
 98 |   - laser_id: 31
 99 |     rot_correction: 0
100 |     vert_correction: 0.1862266
101 | num_lasers: 32


--------------------------------------------------------------------------------
/src/velodyne_decoder/util.py:
--------------------------------------------------------------------------------
 1 | # Copyright (c) 2021-2023, Martin Valgur
 2 | # SPDX-License-Identifier: BSD-3-Clause
 3 | 
 4 | import struct
 5 | from contextlib import contextmanager
 6 | import warnings
 7 | 
 8 | import dpkt
 9 | import numpy as np
10 | 
11 | 
12 | def iter_pcap(pcap_file, time_range=(None, None)):
13 |     start_time, end_time = time_range
14 |     with _fopen(pcap_file, "rb") as f:
15 |         for stamp, buf in dpkt.pcap.Reader(f):
16 |             if start_time is not None and stamp < start_time:
17 |                 continue
18 |             if end_time is not None and stamp > end_time:
19 |                 break
20 |             # Get the innermost layer of the packet
21 |             # typically Ethernet -> IP -> UDP -> raw Velodyne data
22 |             try:
23 |                 data = dpkt.ethernet.Ethernet(buf)
24 |             except dpkt.dpkt.UnpackError:
25 |                 continue
26 |             while hasattr(data, "data"):
27 |                 data = data.data
28 |             yield stamp, data
29 | 
30 | 
31 | def iter_bag(bag_file, topics=None, default_msg_types=None, time_range=(None, None)):
32 |     with warnings.catch_warnings():
33 |         # Suppress an irrelevant deprecation warning from the Cryptodome package due to 'import imp'
34 |         warnings.filterwarnings("ignore", category=DeprecationWarning)
35 |         from rosbag import Bag
36 |         from rospy import Time
37 | 
38 |     if isinstance(bag_file, Bag):
39 |         bag = bag_file
40 |     else:
41 |         bag = Bag(str(bag_file))
42 | 
43 |     # Find relevant topics automatically, if not specified
44 |     if topics is None and default_msg_types is not None:
45 |         topics = []
46 |         for topic, info in bag.get_type_and_topic_info()[1].items():
47 |             if info.msg_type in default_msg_types:
48 |                 topics.append(topic)
49 |     if not topics:
50 |         return
51 | 
52 |     start_time, end_time = time_range
53 |     if start_time is not None and not isinstance(start_time, Time):
54 |         start_time = Time.from_sec(start_time)
55 |     if end_time is not None and not isinstance(end_time, Time):
56 |         end_time = Time.from_sec(end_time)
57 | 
58 |     try:
59 |         for topic, msg, ros_time in bag.read_messages(topics, start_time, end_time):
60 |             yield topic, msg, ros_time
61 |     finally:
62 |         if not isinstance(bag_file, Bag):
63 |             bag.close()
64 | 
65 | 
66 | @contextmanager
67 | def _fopen(filein, *args, **kwargs):
68 |     if isinstance(filein, str):  # filename
69 |         with open(filein, *args, **kwargs) as f:
70 |             yield f
71 |     elif hasattr(filein, "open"):  # pathlib.Path
72 |         with filein.open(*args, **kwargs) as f:
73 |             yield f
74 |     else:  # file-like object
75 |         yield filein
76 | 
77 | 
78 | def parse_packet(data):
79 |     stamp, dual_return_mode, model_id = struct.unpack("<LBB", data[-6:])
80 |     block_dtype = np.dtype(
81 |         [
82 |             ("bank_id", "<u2"),
83 |             ("azimuth", "<u2"),
84 |             ("measurements", [("distance", "u2"), ("intensity", "u1")], 32),
85 |         ],
86 |         align=False,
87 |     )
88 |     blocks = np.frombuffer(data[:-6], dtype=block_dtype)
89 |     return blocks, stamp, model_id, dual_return_mode
90 | 


--------------------------------------------------------------------------------
/src/velodyne_decoder/calibrations/VLP-32A.yml:
--------------------------------------------------------------------------------
  1 | # converted from https://gitlab.kitware.com/LidarView/velodyneplugin/-/tree/0cddc1f92e69b7b26ffc5619aceb1c00854442e6/CalibrationsFiles
  2 | # using https://wiki.ros.org/velodyne_pointcloud#gen_calibration.py
  3 | distance_resolution: 0.004
  4 | lasers:
  5 |   - laser_id: 0
  6 |     rot_correction: -0.02094395
  7 |     vert_correction: -0.2495821
  8 |   - laser_id: 1
  9 |     rot_correction: 0.02094395
 10 |     vert_correction: 0.01455081
 11 |   - laser_id: 2
 12 |     rot_correction: -0.02094395
 13 |     vert_correction: 0.2181662
 14 |   - laser_id: 3
 15 |     rot_correction: 0.02094395
 16 |     vert_correction: -0.008726646
 17 |   - laser_id: 4
 18 |     rot_correction: -0.02094395
 19 |     vert_correction: -0.1867502
 20 |   - laser_id: 5
 21 |     rot_correction: 0.02094395
 22 |     vert_correction: 0.03782129
 23 |   - laser_id: 6
 24 |     rot_correction: -0.02094395
 25 |     vert_correction: 0.1553343
 26 |   - laser_id: 7
 27 |     rot_correction: 0.02094395
 28 |     vert_correction: -0.03200934
 29 |   - laser_id: 8
 30 |     rot_correction: -0.02094395
 31 |     vert_correction: -0.122173
 32 |   - laser_id: 9
 33 |     rot_correction: 0.06283186
 34 |     vert_correction: 0.02036799
 35 |   - laser_id: 10
 36 |     rot_correction: -0.02094395
 37 |     vert_correction: 0.1047198
 38 |   - laser_id: 11
 39 |     rot_correction: 0.06283186
 40 |     vert_correction: -0.0029147
 41 |   - laser_id: 12
 42 |     rot_correction: -0.02094395
 43 |     vert_correction: -0.08552113
 44 |   - laser_id: 13
 45 |     rot_correction: 0.06283186
 46 |     vert_correction: 0.04363323
 47 |   - laser_id: 14
 48 |     rot_correction: -0.02094395
 49 |     vert_correction: 0.06632251
 50 |   - laser_id: 15
 51 |     rot_correction: 0.06283186
 52 |     vert_correction: -0.02617994
 53 |   - laser_id: 16
 54 |     rot_correction: -0.02094395
 55 |     vert_correction: -0.03782129
 56 |   - laser_id: 17
 57 |     rot_correction: 0.02094395
 58 |     vert_correction: 0.1867502
 59 |   - laser_id: 18
 60 |     rot_correction: -0.02094395
 61 |     vert_correction: 0.03200934
 62 |   - laser_id: 19
 63 |     rot_correction: 0.02094395
 64 |     vert_correction: -0.1553343
 65 |   - laser_id: 20
 66 |     rot_correction: -0.06283186
 67 |     vert_correction: -0.04363323
 68 |   - laser_id: 21
 69 |     rot_correction: 0.02094395
 70 |     vert_correction: 0.08552113
 71 |   - laser_id: 22
 72 |     rot_correction: -0.06283186
 73 |     vert_correction: 0.02617994
 74 |   - laser_id: 23
 75 |     rot_correction: 0.02094395
 76 |     vert_correction: -0.06632251
 77 |   - laser_id: 24
 78 |     rot_correction: -0.02094395
 79 |     vert_correction: -0.01455605
 80 |   - laser_id: 25
 81 |     rot_correction: 0.02094395
 82 |     vert_correction: 0.2495821
 83 |   - laser_id: 26
 84 |     rot_correction: -0.02094395
 85 |     vert_correction: 0.008726646
 86 |   - laser_id: 27
 87 |     rot_correction: 0.02094395
 88 |     vert_correction: -0.2181662
 89 |   - laser_id: 28
 90 |     rot_correction: -0.06283186
 91 |     vert_correction: -0.02036799
 92 |   - laser_id: 29
 93 |     rot_correction: 0.02094395
 94 |     vert_correction: 0.122173
 95 |   - laser_id: 30
 96 |     rot_correction: -0.06283186
 97 |     vert_correction: 0.0029147
 98 |   - laser_id: 31
 99 |     rot_correction: 0.02094395
100 |     vert_correction: -0.1047198
101 | num_lasers: 32


--------------------------------------------------------------------------------
/src/velodyne_decoder/calibrations/VLP-32B.yml:
--------------------------------------------------------------------------------
  1 | # converted from https://gitlab.kitware.com/LidarView/velodyneplugin/-/tree/0cddc1f92e69b7b26ffc5619aceb1c00854442e6/CalibrationsFiles
  2 | # using https://wiki.ros.org/velodyne_pointcloud#gen_calibration.py
  3 | distance_resolution: 0.004
  4 | lasers:
  5 |   - laser_id: 0
  6 |     rot_correction: -0.02443461
  7 |     vert_correction: -0.4363323
  8 |   - laser_id: 1
  9 |     rot_correction: 0.07330383
 10 |     vert_correction: -0.01745329
 11 |   - laser_id: 2
 12 |     rot_correction: -0.02443461
 13 |     vert_correction: -0.02909464
 14 |   - laser_id: 3
 15 |     rot_correction: 0.02443461
 16 |     vert_correction: -0.272952
 17 |   - laser_id: 4
 18 |     rot_correction: -0.02443461
 19 |     vert_correction: -0.1973967
 20 |   - laser_id: 5
 21 |     rot_correction: 0.02443461
 22 |     vert_correction: 0
 23 |   - laser_id: 6
 24 |     rot_correction: -0.07330383
 25 |     vert_correction: -0.01164135
 26 |   - laser_id: 7
 27 |     rot_correction: 0.02443461
 28 |     vert_correction: -0.1543395
 29 |   - laser_id: 8
 30 |     rot_correction: -0.02443461
 31 |     vert_correction: -0.1266062
 32 |   - laser_id: 9
 33 |     rot_correction: 0.07330383
 34 |     vert_correction: 0.005811946
 35 |   - laser_id: 10
 36 |     rot_correction: -0.02443461
 37 |     vert_correction: -0.005811946
 38 |   - laser_id: 11
 39 |     rot_correction: 0.02443461
 40 |     vert_correction: -0.1073028
 41 |   - laser_id: 12
 42 |     rot_correction: -0.07330383
 43 |     vert_correction: -0.09307841
 44 |   - laser_id: 13
 45 |     rot_correction: 0.02443461
 46 |     vert_correction: 0.02326524
 47 |   - laser_id: 14
 48 |     rot_correction: -0.07330383
 49 |     vert_correction: 0.01164135
 50 |   - laser_id: 15
 51 |     rot_correction: 0.02443461
 52 |     vert_correction: -0.06981317
 53 |   - laser_id: 16
 54 |     rot_correction: -0.02443461
 55 |     vert_correction: -0.08145452
 56 |   - laser_id: 17
 57 |     rot_correction: 0.07330383
 58 |     vert_correction: 0.02909464
 59 |   - laser_id: 18
 60 |     rot_correction: -0.02443461
 61 |     vert_correction: 0.01745329
 62 |   - laser_id: 19
 63 |     rot_correction: 0.07330383
 64 |     vert_correction: -0.06400122
 65 |   - laser_id: 20
 66 |     rot_correction: -0.07330383
 67 |     vert_correction: -0.05817182
 68 |   - laser_id: 21
 69 |     rot_correction: 0.02443461
 70 |     vert_correction: 0.05817182
 71 |   - laser_id: 22
 72 |     rot_correction: -0.02443461
 73 |     vert_correction: 0.04071853
 74 |   - laser_id: 23
 75 |     rot_correction: 0.02443461
 76 |     vert_correction: -0.04654793
 77 |   - laser_id: 24
 78 |     rot_correction: -0.02443461
 79 |     vert_correction: -0.05235988
 80 |   - laser_id: 25
 81 |     rot_correction: 0.02443461
 82 |     vert_correction: 0.122173
 83 |   - laser_id: 26
 84 |     rot_correction: -0.02443461
 85 |     vert_correction: 0.08145452
 86 |   - laser_id: 27
 87 |     rot_correction: 0.07330383
 88 |     vert_correction: -0.04071853
 89 |   - laser_id: 28
 90 |     rot_correction: -0.07330383
 91 |     vert_correction: -0.03490658
 92 |   - laser_id: 29
 93 |     rot_correction: 0.02443461
 94 |     vert_correction: 0.2617994
 95 |   - laser_id: 30
 96 |     rot_correction: -0.02443461
 97 |     vert_correction: 0.1803449
 98 |   - laser_id: 31
 99 |     rot_correction: 0.02443461
100 |     vert_correction: -0.02326524
101 | num_lasers: 32


--------------------------------------------------------------------------------
/src/velodyne_decoder/calibrations/VLP-32C.yml:
--------------------------------------------------------------------------------
  1 | # converted from https://gitlab.kitware.com/LidarView/velodyneplugin/-/tree/0cddc1f92e69b7b26ffc5619aceb1c00854442e6/CalibrationsFiles
  2 | # using https://wiki.ros.org/velodyne_pointcloud#gen_calibration.py
  3 | distance_resolution: 0.004
  4 | lasers:
  5 |   - laser_id: 0
  6 |     rot_correction: -0.02443461
  7 |     vert_correction: -0.4363323
  8 |   - laser_id: 1
  9 |     rot_correction: 0.07330383
 10 |     vert_correction: -0.01745329
 11 |   - laser_id: 2
 12 |     rot_correction: -0.02443461
 13 |     vert_correction: -0.02909464
 14 |   - laser_id: 3
 15 |     rot_correction: 0.02443461
 16 |     vert_correction: -0.272952
 17 |   - laser_id: 4
 18 |     rot_correction: -0.02443461
 19 |     vert_correction: -0.1973967
 20 |   - laser_id: 5
 21 |     rot_correction: 0.02443461
 22 |     vert_correction: 0
 23 |   - laser_id: 6
 24 |     rot_correction: -0.07330383
 25 |     vert_correction: -0.01164135
 26 |   - laser_id: 7
 27 |     rot_correction: 0.02443461
 28 |     vert_correction: -0.1543395
 29 |   - laser_id: 8
 30 |     rot_correction: -0.02443461
 31 |     vert_correction: -0.1266062
 32 |   - laser_id: 9
 33 |     rot_correction: 0.07330383
 34 |     vert_correction: 0.005811946
 35 |   - laser_id: 10
 36 |     rot_correction: -0.02443461
 37 |     vert_correction: -0.005811946
 38 |   - laser_id: 11
 39 |     rot_correction: 0.02443461
 40 |     vert_correction: -0.1073028
 41 |   - laser_id: 12
 42 |     rot_correction: -0.07330383
 43 |     vert_correction: -0.09307841
 44 |   - laser_id: 13
 45 |     rot_correction: 0.02443461
 46 |     vert_correction: 0.02326524
 47 |   - laser_id: 14
 48 |     rot_correction: -0.07330383
 49 |     vert_correction: 0.01164135
 50 |   - laser_id: 15
 51 |     rot_correction: 0.02443461
 52 |     vert_correction: -0.06981317
 53 |   - laser_id: 16
 54 |     rot_correction: -0.02443461
 55 |     vert_correction: -0.08145452
 56 |   - laser_id: 17
 57 |     rot_correction: 0.07330383
 58 |     vert_correction: 0.02909464
 59 |   - laser_id: 18
 60 |     rot_correction: -0.02443461
 61 |     vert_correction: 0.01745329
 62 |   - laser_id: 19
 63 |     rot_correction: 0.07330383
 64 |     vert_correction: -0.06400122
 65 |   - laser_id: 20
 66 |     rot_correction: -0.07330383
 67 |     vert_correction: -0.05817182
 68 |   - laser_id: 21
 69 |     rot_correction: 0.02443461
 70 |     vert_correction: 0.05817182
 71 |   - laser_id: 22
 72 |     rot_correction: -0.02443461
 73 |     vert_correction: 0.04071853
 74 |   - laser_id: 23
 75 |     rot_correction: 0.02443461
 76 |     vert_correction: -0.04654793
 77 |   - laser_id: 24
 78 |     rot_correction: -0.02443461
 79 |     vert_correction: -0.05235988
 80 |   - laser_id: 25
 81 |     rot_correction: 0.02443461
 82 |     vert_correction: 0.122173
 83 |   - laser_id: 26
 84 |     rot_correction: -0.02443461
 85 |     vert_correction: 0.08145452
 86 |   - laser_id: 27
 87 |     rot_correction: 0.07330383
 88 |     vert_correction: -0.04071853
 89 |   - laser_id: 28
 90 |     rot_correction: -0.07330383
 91 |     vert_correction: -0.03490658
 92 |   - laser_id: 29
 93 |     rot_correction: 0.02443461
 94 |     vert_correction: 0.2617994
 95 |   - laser_id: 30
 96 |     rot_correction: -0.02443461
 97 |     vert_correction: 0.1803449
 98 |   - laser_id: 31
 99 |     rot_correction: 0.02443461
100 |     vert_correction: -0.02326524
101 | num_lasers: 32


--------------------------------------------------------------------------------
/include/velodyne_decoder/scan_batcher.inl:
--------------------------------------------------------------------------------
 1 | // Copyright (c) 2023, Martin Valgur
 2 | // SPDX-License-Identifier: BSD-3-Clause
 3 | 
 4 | #include "velodyne_decoder/config.h"
 5 | #include "velodyne_decoder/scan_batcher.h"
 6 | #include "velodyne_decoder/time_conversion.h"
 7 | #include "velodyne_decoder/types.h"
 8 | 
 9 | #include <cstring>
10 | #include <memory>
11 | #include <optional>
12 | #include <vector>
13 | 
14 | namespace velodyne_decoder {
15 | 
16 | inline int getAzimuth(const PacketView &packet) {
17 |   // get the azimuth of the last block
18 |   uint16_t azimuth = 0;
19 |   memcpy(&azimuth, &packet.data[(BLOCKS_PER_PACKET - 1) * SIZE_BLOCK + 2], sizeof(azimuth));
20 |   return (int)azimuth;
21 | }
22 | 
23 | template <typename T>
24 | ScanBatcher<T>::ScanBatcher(const Config &config)
25 |     : is_device_time_valid_(!(config.model.has_value() && config.model == ModelId::HDL64E_S1)),
26 |       cut_angle_(!config.cut_angle || config.cut_angle < 0
27 |                      ? -1
28 |                      : (int)(std::lround(*config.cut_angle * 100)) % 36000) {
29 |   if (cut_angle_ >= 0) {
30 |     initial_azimuth_ = cut_angle_;
31 |   }
32 | }
33 | 
34 | template <typename T> bool ScanBatcher<T>::push(const T &packet) {
35 |   PacketView packet_view = toPacketView(packet);
36 |   TimePair stamp         = packet_view.stamp;
37 |   if (!is_device_time_valid_) {
38 |     // HDL-64E S1 does not contain device time info in its packets.
39 |     stamp.device = stamp.host;
40 |   }
41 | 
42 |   if (scan_complete_) {
43 |     reset();
44 |   }
45 | 
46 |   int azimuth = getAzimuth(packet_view);
47 |   if (initial_azimuth_ < 0) {
48 |     initial_azimuth_ = azimuth;
49 |   }
50 | 
51 |   double duration = empty() ? 0.0 : stamp.host - toPacketView(scan_packets_->front()).stamp.host;
52 |   if (duration > duration_threshold_) {
53 |     kept_last_packet_ = packet;
54 |     scan_complete_    = true;
55 |     return true;
56 |   }
57 | 
58 |   scan_packets_->push_back(packet);
59 | 
60 |   const int MAX_ANGLE = 36000;
61 |   int new_coverage    = (azimuth - initial_azimuth_ + MAX_ANGLE) % MAX_ANGLE;
62 |   scan_complete_      = coverage_ > MAX_ANGLE / 2 && coverage_ > new_coverage;
63 |   coverage_           = new_coverage;
64 |   return scan_complete_;
65 | }
66 | 
67 | template <typename T> bool ScanBatcher<T>::empty() const { return scan_packets_->empty(); }
68 | 
69 | template <typename T> size_t ScanBatcher<T>::size() const { return scan_packets_->size(); }
70 | 
71 | template <typename T> bool ScanBatcher<T>::scanComplete() const { return scan_complete_; }
72 | 
73 | template <typename T> const std::shared_ptr<std::vector<T>> &ScanBatcher<T>::scanPackets() const {
74 |   return scan_packets_;
75 | }
76 | 
77 | template <typename T> void ScanBatcher<T>::reset(std::shared_ptr<std::vector<T>> scan_packets) {
78 |   if (cut_angle_ < 0) {
79 |     initial_azimuth_ = empty() ? -1 : getAzimuth(toPacketView(scan_packets_->back()));
80 |   } else {
81 |     initial_azimuth_ = cut_angle_;
82 |   }
83 |   scan_packets->clear();
84 |   scan_packets_  = std::move(scan_packets);
85 |   scan_complete_ = false;
86 |   coverage_      = 0;
87 |   if (kept_last_packet_) {
88 |     push(std::move(*kept_last_packet_));
89 |     kept_last_packet_ = std::nullopt;
90 |   }
91 | }
92 | 
93 | template <typename T> void ScanBatcher<T>::reset() { reset(std::make_shared<std::vector<T>>()); }
94 | 
95 | } // namespace velodyne_decoder
96 | 


--------------------------------------------------------------------------------
/.github/workflows/publish.yml:
--------------------------------------------------------------------------------
  1 | name: Publish
  2 | on:
  3 |   release:
  4 |     types: [ published ]
  5 |   workflow_dispatch:
  6 | jobs:
  7 |   build_sdist:
  8 |     name: Build source distribution
  9 |     runs-on: ubuntu-latest
 10 |     steps:
 11 |       - uses: actions/checkout@v4
 12 | 
 13 |       - uses: actions/setup-python@v5
 14 |         with:
 15 |           python-version: '3.13'
 16 | 
 17 |       - name: Install dependencies
 18 |         run: |
 19 |           python -m pip install --upgrade pip
 20 |           python -m pip install --upgrade build twine
 21 | 
 22 |       - name: Build SDist
 23 |         run: python -m build --sdist
 24 | 
 25 |       - name: Check metadata
 26 |         run: python -m twine check dist/*
 27 | 
 28 |       - name: Test
 29 |         run: |
 30 |           python -m pip install dist/*.tar.gz pytest requests
 31 |           pytest test/ --ignore test/test_ros.py --color=yes -v
 32 | 
 33 |       - uses: actions/upload-artifact@v3
 34 |         with:
 35 |           path: dist/*.tar.gz
 36 | 
 37 |   build_wheels:
 38 |     name: ${{ matrix.platform[0] }} - ${{ matrix.platform[2] }}
 39 |     runs-on: ${{ matrix.platform[1] }}
 40 |     strategy:
 41 |       fail-fast: false
 42 |       matrix:
 43 |         platform:
 44 |           # Build only for 64-bit platforms that have binaries for NumPy
 45 |           - [Linux, ubuntu-latest, x86_64]
 46 |           # - [Linux, ubuntu-latest, i686]
 47 |           - [Linux, ubuntu-latest, aarch64]
 48 |           # - [Linux, ubuntu-latest, ppc64le]
 49 |           # - [Linux, ubuntu-latest, s390x]
 50 |           - [Windows, windows-latest, AMD64]
 51 |           # - [Windows, windows-latest, x86]
 52 |           # - [Windows, windows-latest, ARM64]
 53 |           - [MacOS, macos-latest, x86_64]
 54 |           - [MacOS, macos-latest, arm64]
 55 |           # - [MacOS, macos-latest, universal2]
 56 |     steps:
 57 |       - uses: actions/checkout@v4
 58 | 
 59 |       - name: Set up QEMU
 60 |         if: runner.os == 'Linux'
 61 |         uses: docker/setup-qemu-action@v3
 62 |         with:
 63 |           platforms: all
 64 | 
 65 |       - uses: actions/setup-python@v5
 66 |         with:
 67 |           python-version: '3.13'
 68 | 
 69 |       - name: Download test data
 70 |         run: |
 71 |           python -m pip install pytest requests
 72 |           python -m pytest test --color=yes || true
 73 | 
 74 |       - name: Install cibuildwheel
 75 |         run: python -m pip install cibuildwheel
 76 | 
 77 |       - name: Build wheels
 78 |         run: python -m cibuildwheel --output-dir wheelhouse
 79 |         env:
 80 |           CIBW_ARCHS: ${{ matrix.platform[2] }}
 81 |           # Needed for C++17 support on macOS
 82 |           MACOSX_DEPLOYMENT_TARGET: 10.14
 83 | 
 84 |       - name: Inspect
 85 |         run: ls wheelhouse/
 86 | 
 87 |       - name: Upload wheels
 88 |         uses: actions/upload-artifact@v3
 89 |         with:
 90 |           path: wheelhouse/*.whl
 91 | 
 92 |   upload_all:
 93 |     name: Upload if release
 94 |     needs: [ build_wheels, build_sdist ]
 95 |     runs-on: ubuntu-latest
 96 |     if: github.event_name == 'release' && github.event.action == 'published'
 97 |     steps:
 98 |       - uses: actions/setup-python@v5
 99 |         with:
100 |           python-version: '3.13'
101 | 
102 |       - uses: actions/download-artifact@v3
103 |         with:
104 |           name: artifact
105 |           path: dist
106 | 
107 |       - uses: pypa/gh-action-pypi-publish@release/v1
108 |         with:
109 |           user: __token__
110 |           password: ${{ secrets.PYPI_API_TOKEN }}
111 | 


--------------------------------------------------------------------------------
/include/velodyne_decoder/telemetry_packet.h:
--------------------------------------------------------------------------------
 1 | // Copyright (c) 2023, Martin Valgur
 2 | // SPDX-License-Identifier: BSD-3-Clause
 3 | 
 4 | #pragma once
 5 | 
 6 | #include <cstdint>
 7 | #include <cstring>
 8 | #include <gsl/span>
 9 | #include <optional>
10 | #include <string>
11 | 
12 | #include "velodyne_decoder/types.h"
13 | 
14 | namespace velodyne_decoder {
15 | 
16 | struct NmeaInfo {
17 |   double longitude    = 0;     ///< Longitude in degrees
18 |   double latitude     = 0;     ///< Latitude in degrees
19 |   double altitude     = 0;     ///< Altitude above WGS84 ellipsoid in meters (always 0 for GPRMC)
20 |   uint16_t utc_year   = 0;     ///< UTC year (always 0 for GPGGA)
21 |   uint8_t utc_month   = 0;     ///< UTC month (always 0 for GPGGA)
22 |   uint8_t utc_day     = 0;     ///< UTC day (always 0 for GPGGA)
23 |   uint8_t utc_hours   = 0;     ///< UTC hours
24 |   uint8_t utc_minutes = 0;     ///< UTC minutes
25 |   float utc_seconds   = 0;     ///< UTC seconds
26 |   bool fix_available  = false; ///< Position fix available
27 | 
28 |   /// UTC timestamp as Unix time, or seconds since midnight if date is missing
29 |   [[nodiscard]] Time utcTime() const;
30 | };
31 | 
32 | struct TelemetryPacket {
33 |   /// Reason for the last ADC calibration
34 |   enum class AdcCalibReason : uint8_t {
35 |     NO_CALIBRATION    = 0, ///< No calibration
36 |     POWER_ON          = 1, ///< Power-on calibration performed
37 |     MANUAL            = 2, ///< Manual calibration performed
38 |     DELTA_TEMPERATURE = 3, ///< Delta temperature calibration performed
39 |     PERIODIC          = 4, ///< Periodic calibration performed
40 |   };
41 |   /// Pulse Per Second (PPS) status
42 |   enum class PpsStatus : uint8_t {
43 |     ABSENT        = 0, ///< No PPS detected
44 |     SYNCHRONIZING = 1, ///< Synchronizing to PPS
45 |     LOCKED        = 2, ///< PPS Locked
46 |     ERROR         = 3, ///< Error
47 |   };
48 | 
49 |   // fields set by all firmware versions
50 |   uint32_t usec_since_toh;   ///< Number of microseconds elapsed since the top of the hour
51 |   PpsStatus pps_status;      ///< Pulse Per Second (PPS) status
52 |   std::string nmea_sentence; ///< GPRMC or GPGGA NMEA sentence
53 | 
54 |   // fields set by newer firmware versions only (since mid-2018)
55 |   uint8_t temp_board_top;              ///< Temperature of top board, 0 to 150 °C
56 |   uint8_t temp_board_bottom;           ///< Temperature of bottom board, 0 to 150 °C
57 |   bool thermal_shutdown;               ///< Thermal status, true if thermal shutdown
58 |   uint8_t temp_at_shutdown;            ///< Temperature of unit when thermal shutdown occurred
59 |   uint8_t temp_at_powerup;             ///< Temperature of unit (bottom board) at power up
60 |   uint8_t temp_during_adc_calibration; ///< Temperature when ADC calibration last ran, 0 to 150 °C
61 |   int16_t temp_change_since_adc_calibration; ///< Change in temperature since last ADC calibration,
62 |                                              ///< -150 to 150°C
63 |   uint32_t seconds_since_adc_calibration;    ///< Elapsed seconds since last ADC calibration
64 |   AdcCalibReason adc_calibration_reason;     ///< Reason for the last ADC calibration
65 |   bool adc_calib_in_progress;                ///< ADC calibration in progress
66 |   bool adc_delta_temp_limit_exceeded; ///< ADC calibration: delta temperature limit has been met
67 |   bool adc_period_exceeded;           ///< ADC calibration: periodic time elapsed limit has been met
68 | 
69 |   explicit TelemetryPacket(gsl::span<const uint8_t, TELEMETRY_PACKET_SIZE> raw_data);
70 | 
71 |   [[nodiscard]] std::optional<NmeaInfo> parseNmea() const;
72 | 
73 |   /// Timestamp from the NMEA sentence
74 |   [[nodiscard]] std::optional<Time> nmeaTime() const;
75 | 
76 |   /// Timestamp from the PPS signal combined with the NMEA timestamp
77 |   [[nodiscard]] std::optional<Time> ppsTime() const;
78 | };
79 | 
80 | } // namespace velodyne_decoder
81 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
  1 | cmake_minimum_required(VERSION 3.15)
  2 | include(cmake/third_party.cmake)
  3 | project(velodyne_decoder VERSION 3.0.0 LANGUAGES CXX)
  4 | 
  5 | ## Build flags
  6 | 
  7 | if(NOT DEFINED CMAKE_CXX_STANDARD)
  8 |   set(CMAKE_CXX_STANDARD 17)
  9 |   set(CMAKE_CXX_STANDARD_REQUIRED ON)
 10 |   set(CMAKE_CXX_EXTENSIONS OFF)
 11 | endif()
 12 | 
 13 | if(NOT DEFINED CMAKE_POSITION_INDEPENDENT_CODE)
 14 |   set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 15 | endif()
 16 | 
 17 | set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
 18 | 
 19 | if(MSVC)
 20 |   add_compile_options(/W4 /O2)
 21 | else()
 22 |   add_compile_options(
 23 |     "$<$<CONFIG:Debug>:-ggdb3;-Og>"
 24 |     "$<$<CONFIG:RelWithDebInfo>:-ggdb3;-O3>"
 25 |     "$<$<CONFIG:Release>:-O3>"
 26 |     -Wall
 27 |     -Wno-sign-compare
 28 |     -Wextra
 29 |   )
 30 | endif()
 31 | 
 32 | ## Library
 33 | 
 34 | find_package(yaml-cpp 0.7 REQUIRED)
 35 | find_package(Microsoft.GSL REQUIRED)
 36 | 
 37 | add_library(velodyne_decoder
 38 |   src/calibration.cpp
 39 |   src/calib_db.cpp
 40 |   src/packet_decoder.cpp
 41 |   src/scan_batcher.cpp
 42 |   src/scan_decoder.cpp
 43 |   src/stream_decoder.cpp
 44 |   src/telemetry_packet.cpp
 45 |   src/time_conversion.cpp
 46 |   src/types.cpp
 47 | )
 48 | target_link_libraries(velodyne_decoder
 49 |   PRIVATE yaml-cpp::yaml-cpp
 50 |   PUBLIC Microsoft.GSL::GSL
 51 | )
 52 | target_include_directories(velodyne_decoder
 53 |     PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
 54 |     PUBLIC "$<INSTALL_INTERFACE:include>"
 55 | )
 56 | target_compile_definitions(velodyne_decoder PUBLIC _USE_MATH_DEFINES)
 57 | 
 58 | if(MSVC)
 59 |   copy_third_party_dlls()
 60 | endif()
 61 | 
 62 | ## Python bindings
 63 | option(BUILD_PYTHON "Build velodyne_decoder_pylib Python module" FALSE)
 64 | if(BUILD_PYTHON OR SKBUILD)
 65 |   find_package(Python
 66 |       REQUIRED COMPONENTS Interpreter Development.Module
 67 |       OPTIONAL_COMPONENTS Development.SABIModule)
 68 |   find_package(nanobind CONFIG REQUIRED)
 69 | 
 70 |   nanobind_add_module(python_bindings STABLE_ABI NB_STATIC src/python.cpp)
 71 |   set_target_properties(python_bindings PROPERTIES OUTPUT_NAME velodyne_decoder_pylib)
 72 |   target_include_directories(python_bindings PRIVATE include)
 73 |   target_link_libraries(python_bindings PRIVATE velodyne_decoder)
 74 |   target_compile_definitions(python_bindings PRIVATE VERSION_INFO=${PROJECT_VERSION})
 75 | 
 76 |   if(DEFINED SKBUILD_PROJECT_NAME)
 77 |     set(PYTHON_INSTALL_DIR ${SKBUILD_PROJECT_NAME})
 78 |   elseif(NOT DEFINED PYTHON_INSTALL_DIR)
 79 |     set(PYTHON_INSTALL_DIR lib/python${Python_VERSION_MAJOR}.${Python_VERSION_MINOR}/site-packages/velodyne_decoder)
 80 |   endif()
 81 | 
 82 |   install(TARGETS python_bindings
 83 |       LIBRARY DESTINATION ${PYTHON_INSTALL_DIR}
 84 |       COMPONENT python)
 85 |   install(DIRECTORY src/velodyne_decoder/
 86 |       DESTINATION ${PYTHON_INSTALL_DIR}
 87 |       COMPONENT python)
 88 | endif()
 89 | 
 90 | ## Install
 91 | 
 92 | include(GNUInstallDirs)
 93 | install(TARGETS velodyne_decoder
 94 |     EXPORT velodyne_decoder-targets
 95 |     ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
 96 |     LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
 97 |     RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR})
 98 | install(DIRECTORY ${PROJECT_SOURCE_DIR}/include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR})
 99 | 
100 | set(CMAKE_INSTALL_DIR lib/cmake/velodyne_decoder)
101 | install(EXPORT velodyne_decoder-targets
102 |     NAMESPACE velodyne_decoder::
103 |     DESTINATION "${CMAKE_INSTALL_DIR}"
104 |     FILE velodyne_decoder-targets.cmake
105 |     COMPONENT velodyne_decoder
106 | )
107 | 
108 | set(THIRD_PARTY_INCLUDEDIR "${CMAKE_INSTALL_INCLUDEDIR}/velodyne_decoder/third_party")
109 | set(THIRD_PARTY_LIBDIR "${CMAKE_INSTALL_LIBDIR}/velodyne_decoder/third_party")
110 | set(THIRD_PARTY_BINDIR "${CMAKE_INSTALL_BINDIR}/velodyne_decoder/third_party")
111 | if(INSTALL_THIRD_PARTY)
112 |   install_third_party_libs()
113 | endif()
114 | 
115 | include(CMakePackageConfigHelpers)
116 | configure_package_config_file("${CMAKE_CURRENT_SOURCE_DIR}/cmake/velodyne_decoder-config.cmake.in"
117 |     "${CMAKE_CURRENT_BINARY_DIR}/velodyne_decoder-config.cmake"
118 |     INSTALL_DESTINATION ${CMAKE_INSTALL_DIR}
119 |     PATH_VARS
120 |         CMAKE_INSTALL_INCLUDEDIR
121 |         CMAKE_INSTALL_LIBDIR
122 |         THIRD_PARTY_INCLUDEDIR
123 |         THIRD_PARTY_LIBDIR
124 |         THIRD_PARTY_BINDIR
125 | )
126 | 
127 | write_basic_package_version_file("${CMAKE_CURRENT_BINARY_DIR}/velodyne_decoder-config-version.cmake"
128 |     VERSION ${PROJECT_VERSION}
129 |     COMPATIBILITY SameMajorVersion
130 | )
131 | 
132 | install(FILES "${CMAKE_CURRENT_BINARY_DIR}/velodyne_decoder-config.cmake"
133 |     "${CMAKE_CURRENT_BINARY_DIR}/velodyne_decoder-config-version.cmake"
134 |     DESTINATION "${CMAKE_INSTALL_DIR}"
135 | )
136 | 


--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
  1 | # Created by .ignore support plugin (hsz.mobi)
  2 | ### Python template
  3 | # Byte-compiled / optimized / DLL files
  4 | __pycache__/
  5 | *.py[cod]
  6 | *$py.class
  7 | 
  8 | # C extensions
  9 | *.so
 10 | 
 11 | # Distribution / packaging
 12 | .Python
 13 | build/
 14 | develop-eggs/
 15 | dist/
 16 | downloads/
 17 | eggs/
 18 | .eggs/
 19 | lib/
 20 | lib64/
 21 | parts/
 22 | sdist/
 23 | var/
 24 | wheels/
 25 | share/python-wheels/
 26 | *.egg-info/
 27 | _skbuild/
 28 | .installed.cfg
 29 | *.egg
 30 | MANIFEST
 31 | 
 32 | # PyInstaller
 33 | #  Usually these files are written by a python script from a template
 34 | #  before PyInstaller builds the exe, so as to inject date/other infos into it.
 35 | *.manifest
 36 | *.spec
 37 | 
 38 | # Installer logs
 39 | pip-log.txt
 40 | pip-delete-this-directory.txt
 41 | 
 42 | # Unit test / coverage reports
 43 | htmlcov/
 44 | .tox/
 45 | .nox/
 46 | .coverage
 47 | .coverage.*
 48 | .cache
 49 | nosetests.xml
 50 | coverage.xml
 51 | *.cover
 52 | *.py,cover
 53 | .hypothesis/
 54 | .pytest_cache/
 55 | cover/
 56 | 
 57 | # Translations
 58 | *.mo
 59 | *.pot
 60 | 
 61 | # Django stuff:
 62 | *.log
 63 | local_settings.py
 64 | db.sqlite3
 65 | db.sqlite3-journal
 66 | 
 67 | # Flask stuff:
 68 | instance/
 69 | .webassets-cache
 70 | 
 71 | # Scrapy stuff:
 72 | .scrapy
 73 | 
 74 | # Sphinx documentation
 75 | docs/_build/
 76 | 
 77 | # PyBuilder
 78 | .pybuilder/
 79 | target/
 80 | 
 81 | # Jupyter Notebook
 82 | .ipynb_checkpoints
 83 | 
 84 | # IPython
 85 | profile_default/
 86 | ipython_config.py
 87 | 
 88 | # pyenv
 89 | #   For a library or package, you might want to ignore these files since the code is
 90 | #   intended to run in multiple environments; otherwise, check them in:
 91 | # .python-version
 92 | 
 93 | # pipenv
 94 | #   According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
 95 | #   However, in case of collaboration, if having platform-specific dependencies or dependencies
 96 | #   having no cross-platform support, pipenv may install dependencies that don't work, or not
 97 | #   install all needed dependencies.
 98 | #Pipfile.lock
 99 | 
100 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow
101 | __pypackages__/
102 | 
103 | # Celery stuff
104 | celerybeat-schedule
105 | celerybeat.pid
106 | 
107 | # SageMath parsed files
108 | *.sage.py
109 | 
110 | # Environments
111 | .env
112 | .venv
113 | env/
114 | venv/
115 | ENV/
116 | env.bak/
117 | venv.bak/
118 | 
119 | # Spyder project settings
120 | .spyderproject
121 | .spyproject
122 | 
123 | # Rope project settings
124 | .ropeproject
125 | 
126 | # mkdocs documentation
127 | /site
128 | 
129 | # mypy
130 | .mypy_cache/
131 | .dmypy.json
132 | dmypy.json
133 | 
134 | # Pyre type checker
135 | .pyre/
136 | 
137 | # pytype static type analyzer
138 | .pytype/
139 | 
140 | # Cython debug symbols
141 | cython_debug/
142 | 
143 | ### Linux template
144 | *~
145 | 
146 | # temporary files which can be created if a process still has a handle open of a deleted file
147 | .fuse_hidden*
148 | 
149 | # KDE directory preferences
150 | .directory
151 | 
152 | # Linux trash folder which might appear on any partition or disk
153 | .Trash-*
154 | 
155 | # .nfs files are created when an open file is removed but is still being accessed
156 | .nfs*
157 | 
158 | ### C++ template
159 | # Prerequisites
160 | *.d
161 | 
162 | # Compiled Object files
163 | *.slo
164 | *.lo
165 | *.o
166 | *.obj
167 | 
168 | # Precompiled Headers
169 | *.gch
170 | *.pch
171 | 
172 | # Compiled Dynamic libraries
173 | *.so
174 | *.dylib
175 | *.dll
176 | 
177 | # Fortran module files
178 | *.mod
179 | *.smod
180 | 
181 | # Compiled Static libraries
182 | *.lai
183 | *.la
184 | *.a
185 | *.lib
186 | 
187 | # Executables
188 | *.exe
189 | *.out
190 | *.app
191 | 
192 | ### ROS template
193 | devel/
194 | logs/
195 | build/
196 | bin/
197 | lib/
198 | msg_gen/
199 | srv_gen/
200 | msg/*Action.msg
201 | msg/*ActionFeedback.msg
202 | msg/*ActionGoal.msg
203 | msg/*ActionResult.msg
204 | msg/*Feedback.msg
205 | msg/*Goal.msg
206 | msg/*Result.msg
207 | msg/_*.py
208 | build_isolated/
209 | devel_isolated/
210 | 
211 | # Generated by dynamic reconfigure
212 | *.cfgc
213 | /cfg/cpp/
214 | /cfg/*.py
215 | 
216 | # Ignore generated docs
217 | *.dox
218 | *.wikidoc
219 | 
220 | # eclipse stuff
221 | .project
222 | .cproject
223 | 
224 | # qcreator stuff
225 | CMakeLists.txt.user
226 | 
227 | srv/_*.py
228 | *.pcd
229 | *.pyc
230 | qtcreator-*
231 | *.user
232 | 
233 | /planning/cfg
234 | /planning/docs
235 | /planning/src
236 | 
237 | *~
238 | 
239 | # Emacs
240 | .#*
241 | 
242 | # Catkin custom files
243 | CATKIN_IGNORE
244 | 
245 | ### VisualStudioCode template
246 | .vscode/
247 | 
248 | # Local History for Visual Studio Code
249 | .history/
250 | 
251 | ### JetBrains template
252 | .idea/
253 | cmake-build-*
254 | 
255 | CMakeUserPresets.json
256 | 
257 | /test/data/
258 | !/test/data/*.py
259 | 


--------------------------------------------------------------------------------
/include/velodyne_decoder/packet_decoder.h:
--------------------------------------------------------------------------------
  1 | // Copyright (c) 2007-2019, Yaxin Liu, Patrick Beeson, Jack O'Quin, Joshua Whitley
  2 | // Copyright (c) 2021-2023, Martin Valgur
  3 | // SPDX-License-Identifier: BSD-3-Clause
  4 | 
  5 | /** @file
  6 |  *
  7 |  *  @brief Interfaces for interpreting raw packets from the Velodyne 3D LIDAR.
  8 |  */
  9 | 
 10 | #pragma once
 11 | 
 12 | #include <cstdint>
 13 | #include <gsl/span>
 14 | #include <string>
 15 | #include <vector>
 16 | 
 17 | #include "velodyne_decoder/calibration.h"
 18 | #include "velodyne_decoder/config.h"
 19 | #include "velodyne_decoder/types.h"
 20 | 
 21 | namespace velodyne_decoder {
 22 | 
 23 | /** \brief Velodyne data conversion class */
 24 | class PacketDecoder {
 25 | public:
 26 |   explicit PacketDecoder(const Config &config);
 27 | 
 28 |   void unpack(PacketView pkt, TimePair scan_start_time, PointCloud &cloud);
 29 | 
 30 |   /// Detected or configured model ID of the sensor
 31 |   [[nodiscard]] std::optional<ModelId> modelId() const;
 32 | 
 33 |   /// The return mode of the sensor based on the last received packet
 34 |   [[nodiscard]] std::optional<ReturnMode> returnMode() const;
 35 | 
 36 | private:
 37 |   void unpack(TimePair stamp, const raw_packet_t &pkt_data, PointCloud &cloud,
 38 |               TimePair scan_start_time);
 39 | 
 40 |   void initModel(ModelId model_id);
 41 |   void initModel(PacketModelId packet_model_id);
 42 |   void initCalibration(const Calibration &calibration);
 43 | 
 44 |   void setupSinCosCache();
 45 |   void setupCalibrationCache(const Calibration &calibration);
 46 | 
 47 |   /** \brief setup per-point timing offsets
 48 |    *
 49 |    *  Runs during initialization and determines the firing time for each point in the scan
 50 |    */
 51 |   [[nodiscard]] static std::array<std::array<float, 32>, 12> buildTimings(ModelId model);
 52 | 
 53 |   void correctVls128Timings(uint32_t stamp, bool dual_return);
 54 | 
 55 |   static void verifyPacketModelId(PacketModelId packet_model_id, ModelId model_id);
 56 | 
 57 |   void unpack_16_32_beam(const raw_packet_t &raw, float rel_packet_stamp, PointCloud &cloud);
 58 |   void unpack_hdl64e_s1(const raw_packet_t &raw, float rel_packet_stamp, PointCloud &cloud);
 59 |   void unpack_hdl64e(const raw_packet_t &raw, float rel_packet_stamp, PointCloud &cloud);
 60 |   void unpack_vls128(const raw_packet_t &raw, float rel_packet_stamp, PointCloud &cloud);
 61 | 
 62 |   void unpackPointDual(PointCloud &cloud, int laser_idx, uint16_t azimuth, float time,
 63 |                        uint16_t column, raw_measurement_t last, raw_measurement_t strongest) const;
 64 | 
 65 |   void unpackPoint(PointCloud &cloud, int laser_idx, uint16_t azimuth, float time, uint16_t column,
 66 |                    raw_measurement_t measurement, ReturnMode return_mode) const;
 67 | 
 68 |   /** in-line test whether a point is in range */
 69 |   [[nodiscard]] bool distanceInRange(float range) const;
 70 |   [[nodiscard]] bool azimuthInRange(uint16_t azimuth) const;
 71 | 
 72 |   void setReturnMode(PacketReturnMode packet_return_mode) {
 73 |     switch (packet_return_mode) {
 74 |     case PacketReturnMode::STRONGEST:
 75 |       return_mode_ = ReturnMode::STRONGEST;
 76 |       break;
 77 |     case PacketReturnMode::LAST:
 78 |       return_mode_ = ReturnMode::LAST;
 79 |       break;
 80 |     case PacketReturnMode::DUAL:
 81 |       return_mode_ = ReturnMode::BOTH;
 82 |       break;
 83 |     }
 84 |   }
 85 | 
 86 | private:
 87 |   velodyne_decoder::Calibration calibration_;
 88 |   bool calib_initialized_ = false;
 89 | 
 90 |   std::optional<ModelId> model_id_;
 91 |   std::optional<ReturnMode> return_mode_;
 92 |   float min_range_;
 93 |   float max_range_;
 94 |   uint16_t min_azimuth_;
 95 |   uint16_t max_azimuth_;
 96 | 
 97 |   float sin_rot_table_[ROTATION_MAX_UNITS];
 98 |   float cos_rot_table_[ROTATION_MAX_UNITS];
 99 | 
100 |   // Cache of basic calibration parameters
101 |   bool apply_advanced_calibration_;
102 |   float distance_resolution_;
103 |   std::vector<float> cos_rot_correction_;  ///< cosine of rot_corrections
104 |   std::vector<float> sin_rot_correction_;  ///< sine of rot_corrections
105 |   std::vector<float> cos_vert_correction_; ///< cosine of vert_corrections
106 |   std::vector<float> sin_vert_correction_; ///< sine of vert_corrections
107 |   std::vector<uint8_t> ring_cache_;        ///< cache for ring lookup
108 |   std::vector<float> vert_offset_cache_;   ///< cache for vertical offsets
109 | 
110 |   // timing offset lookup table
111 |   std::array<std::array<float, 32>, 12> timing_offsets_;
112 | 
113 |   // First azimuth in the previous packet.
114 |   // Needed for dual-return mode VLS-128, where only a single column is stored per-packet.
115 |   uint16_t prev_packet_azimuth_ = std::numeric_limits<uint16_t>::max();
116 |   float prev_rotation_rate_     = 0;
117 | 
118 |   // For VLS-128 timing corrections, which differ based on firmware version
119 |   std::optional<uint32_t> prev_packet_stamp_;
120 |   static constexpr double default_vls_128_channel_duration_ = 2.89 * 1e-6; // initial value
121 |   bool vls_128_channel_duration_corrected_                  = false;
122 | };
123 | 
124 | } // namespace velodyne_decoder
125 | 


--------------------------------------------------------------------------------
/cmake/third_party.cmake:
--------------------------------------------------------------------------------
  1 | if(NOT DEFINED CMAKE_BUILD_TYPE)
  2 |     set(CMAKE_BUILD_TYPE Release)
  3 | endif()
  4 | 
  5 | string(FIND "${CMAKE_TOOLCHAIN_FILE}" "conan_toolchain.cmake" _result)
  6 | if(_result GREATER_EQUAL 0)
  7 |     set(FOUND_CONAN_TOOLCHAIN TRUE)
  8 | endif()
  9 | string(FIND "${CMAKE_TOOLCHAIN_FILE}" "vcpkg.cmake" _result)
 10 | if(_result GREATER_EQUAL 0)
 11 |     set(FOUND_VCPKG_TOOLCHAIN TRUE)
 12 | endif()
 13 | 
 14 | if(FOUND_CONAN_TOOLCHAIN)
 15 |     message(NOTICE "Conan toolchain already in use, disabling automatic dependency management")
 16 |     set(USE_CONAN FALSE)
 17 | elseif(FOUND_VCPKG_TOOLCHAIN)
 18 |     message(NOTICE "Vcpkg toolchain already in use, disabling automatic dependency management")
 19 |     set(USE_CONAN FALSE)
 20 | else()
 21 |     option(USE_CONAN "Use Conan to automatically manage dependencies" TRUE)
 22 | endif()
 23 | 
 24 | include(CMakeDependentOption)
 25 | cmake_dependent_option(INSTALL_THIRD_PARTY "Install third-party dependencies alongside the project" TRUE "USE_CONAN" FALSE)
 26 | 
 27 | if(USE_CONAN)
 28 |     if(CMAKE_VERSION GREATER_EQUAL 3.24)
 29 |         set(CONAN_INSTALL_ARGS
 30 |             --build=missing
 31 |             # Deploy the installed dependencies in the build dir for easier installation
 32 |             --deployer=full_deploy --deployer-folder=${CMAKE_BINARY_DIR}
 33 |         )
 34 |         list(APPEND CMAKE_PROJECT_TOP_LEVEL_INCLUDES ${CMAKE_CURRENT_LIST_DIR}/conan_provider.cmake)
 35 |     else()
 36 |         if(NOT EXISTS "${CMAKE_BINARY_DIR}/full_deploy")
 37 |             message(FATAL_ERROR
 38 |                 "CMake 3.24 or greater is required to install Conan dependencies automatically. "
 39 |                 "You will have to run\n"
 40 |                 "'conan install . --build=missing --deployer=full_deploy --deployer-folder=build/Release'\n"
 41 |                 "manually in the repository root instead."
 42 |             )
 43 |         endif()
 44 |         # To use the output from the Conan CMakeDeps generator
 45 |         list(PREPEND CMAKE_PREFIX_PATH
 46 |             ${CMAKE_CURRENT_BINARY_DIR}/${CMAKE_BUILD_TYPE}/generators
 47 |             ${CMAKE_CURRENT_BINARY_DIR}/build/${CMAKE_BUILD_TYPE}/generators
 48 |         )
 49 |     endif()
 50 | endif()
 51 | 
 52 | set(CONAN_DEPLOYER_DIR "${CMAKE_BINARY_DIR}/full_deploy/host")
 53 | 
 54 | function(install_third_party_libs)
 55 |     # Installs all include directories from Conan packages deployed to the build directory by 'conan install --deploy'
 56 |     set(configurations ${CMAKE_CONFIGURATION_TYPES})
 57 |     if(NOT configurations)
 58 |         set(configurations ${CMAKE_BUILD_TYPE})
 59 |     endif()
 60 |     file(GLOB_RECURSE conaninfo_files "${CONAN_DEPLOYER_DIR}/*/conaninfo.txt")
 61 |     foreach(conaninfo_path ${conaninfo_files})
 62 |         # Get relative path
 63 |         file(RELATIVE_PATH rel_conaninfo_path "${CONAN_DEPLOYER_DIR}" "${conaninfo_path}")
 64 |         # Get package name
 65 |         string(REGEX REPLACE "([^/]+)/.*$" "\\1" package_name "${rel_conaninfo_path}")
 66 |         # Get package root dir
 67 |         get_filename_component(package_root "${conaninfo_path}" DIRECTORY)
 68 |         # Copy includes
 69 |         if(EXISTS "${package_root}/include")
 70 |             install(DIRECTORY "${package_root}/include/"
 71 |                     DESTINATION "${THIRD_PARTY_INCLUDEDIR}")
 72 |         endif()
 73 |         foreach(config ${configurations})
 74 |             if(NOT rel_conaninfo_path MATCHES ".+/${config}/.+")
 75 |                 continue()
 76 |             endif()
 77 |             # Copy .a, .so, .lib, .dylib
 78 |             file(GLOB_RECURSE lib_files "${package_root}/lib/*")
 79 |             foreach(lib ${lib_files})
 80 |                 if(lib MATCHES "\\.(a|so[^/]*]|lib|dylib)$")
 81 |                     install(FILES "${lib}"
 82 |                             DESTINATION "${THIRD_PARTY_LIBDIR}"
 83 |                             CONFIGURATIONS ${config})
 84 |                 endif()
 85 |             endforeach()
 86 |             # Copy .dll
 87 |             file(GLOB_RECURSE dll_files "${package_root}/bin/*")
 88 |             foreach(dll ${dll_files})
 89 |                 if(dll MATCHES "\\.dll$")
 90 |                     install(FILES "${dll}"
 91 |                             DESTINATION "${THIRD_PARTY_BINDIR}"
 92 |                             CONFIGURATIONS ${config})
 93 |                 endif()
 94 |             endforeach()
 95 |         endforeach()
 96 |     endforeach()
 97 | endfunction()
 98 | 
 99 | function(copy_third_party_dlls)
100 |     # Copy all third-party DLLs to build/bin for tests and executables
101 |     file(GLOB_RECURSE dll_files "${CONAN_DEPLOYER_DIR}/*/*.dll")
102 |     if(CMAKE_CONFIGURATION_TYPES)
103 |         foreach(config ${CMAKE_CONFIGURATION_TYPES})
104 |             foreach(dll ${dll_files})
105 |                 file(RELATIVE_PATH dll_rel "${CONAN_DEPLOYER_DIR}" "${dll}")
106 |                 if(dll_rel MATCHES ".+/${config}/.+")
107 |                     file(COPY ${dll} DESTINATION "${CMAKE_BINARY_DIR}/bin/${config}")
108 |                 endif()
109 |             endforeach()
110 |         endforeach()
111 |     else()
112 |         file(COPY ${dll_files} DESTINATION "${CMAKE_BINARY_DIR}/bin")
113 |     endif()
114 | endfunction()
115 | 


--------------------------------------------------------------------------------
/include/velodyne_decoder/types.h:
--------------------------------------------------------------------------------
  1 | // Copyright (c) 2007-2019, Yaxin Liu, Patrick Beeson, Jack O'Quin, Joshua Whitley
  2 | // Copyright (c) 2021-2023, Martin Valgur
  3 | // SPDX-License-Identifier: BSD-3-Clause
  4 | 
  5 | #pragma once
  6 | 
  7 | #include <array>
  8 | #include <cstddef>
  9 | #include <cstdint>
 10 | #include <gsl/span>
 11 | #include <utility>
 12 | #include <vector>
 13 | 
 14 | namespace velodyne_decoder {
 15 | 
 16 | constexpr size_t PACKET_SIZE           = 1206;
 17 | constexpr size_t TELEMETRY_PACKET_SIZE = 512;
 18 | 
 19 | using Time = double;
 20 | struct TimePair {
 21 |   /// Time of arrival of the packet at the host machine.
 22 |   Time host;
 23 |   /// Timestamp of the packet as reported by the device.
 24 |   /// Relies on the host time to convert relative top-of-hour timestamps to absolute timestamps.
 25 |   Time device;
 26 | 
 27 |   TimePair() = default;
 28 |   TimePair(Time host, Time device);
 29 |   TimePair(Time host_stamp, gsl::span<const uint8_t, PACKET_SIZE> data);
 30 | };
 31 | 
 32 | using RawPacketData = std::array<uint8_t, PACKET_SIZE>;
 33 | 
 34 | struct VelodynePacket {
 35 |   TimePair stamp;
 36 |   RawPacketData data;
 37 | 
 38 |   VelodynePacket() = default;
 39 |   VelodynePacket(TimePair stamp, gsl::span<const uint8_t, PACKET_SIZE> data);
 40 |   VelodynePacket(Time host_stamp, gsl::span<const uint8_t, PACKET_SIZE> data);
 41 | };
 42 | 
 43 | struct PacketView {
 44 |   TimePair stamp;
 45 |   gsl::span<const uint8_t, PACKET_SIZE> data;
 46 | 
 47 |   PacketView(TimePair stamp, gsl::span<const uint8_t, PACKET_SIZE> data);
 48 |   PacketView(Time host_stamp, gsl::span<const uint8_t, PACKET_SIZE> data);
 49 |   PacketView(const VelodynePacket &packet);
 50 | };
 51 | 
 52 | enum class ReturnMode : uint8_t {
 53 |   STRONGEST = 1,     // the strongest point in firing
 54 |   LAST      = 2,     // the last point in firing
 55 |   BOTH      = 1 | 2, // point is both the last and strongest one
 56 | };
 57 | ReturnMode operator|(ReturnMode lhs, ReturnMode rhs);
 58 | ReturnMode operator&(ReturnMode lhs, ReturnMode rhs);
 59 | 
 60 | struct alignas(16) VelodynePoint {
 61 |   struct alignas(16) {
 62 |     float x;
 63 |     float y;
 64 |     float z;
 65 |     float intensity;
 66 |   };
 67 |   float time;
 68 |   uint16_t column;
 69 |   uint8_t ring;
 70 |   ReturnMode return_type;
 71 | 
 72 |   VelodynePoint() = default;
 73 |   VelodynePoint(float x, float y, float z, float intensity, float time, uint16_t column,
 74 |                 uint8_t ring, ReturnMode return_type);
 75 | };
 76 | static_assert(sizeof(VelodynePoint) == 32, "VelodynePoint is not 32 bytes");
 77 | using PointCloud = std::vector<VelodynePoint>;
 78 | 
 79 | enum class ModelId : uint8_t {
 80 |   HDL64E_S1  = 1,
 81 |   HDL64E_S2  = 2,
 82 |   HDL64E_S3  = 3,
 83 |   HDL32E     = 4,
 84 |   VLP32A     = 5,
 85 |   VLP32B     = 6,
 86 |   VLP32C     = 7,
 87 |   VLP16      = 8,
 88 |   PuckLite   = 8, // = VLP-16
 89 |   PuckHiRes  = 9, // aka VLP-16 Hi-Res
 90 |   VLS128     = 10,
 91 |   AlphaPrime = 10, // = VLS-128
 92 | };
 93 | 
 94 | enum class PacketModelId : uint8_t {
 95 |   HDL32E     = 0x21, // decimal: 33
 96 |   VLP16      = 0x22, // decimal: 34
 97 |   VLP32AB    = 0x23, // decimal: 35
 98 |   VLP16HiRes = 0x24, // decimal: 36
 99 |   VLP32C     = 0x28, // decimal: 40
100 |   Velarray   = 0x31, // decimal: 49
101 |   VLS128     = 0xa1, // decimal: 161
102 | };
103 | 
104 | enum class PacketReturnMode : uint8_t {
105 |   STRONGEST = 0x37, // decimal: 55
106 |   LAST      = 0x38, // decimal: 56
107 |   DUAL      = 0x39, // decimal: 57
108 | };
109 | 
110 | /**
111 |  * Constants and structures specific to raw Velodyne packet structure.
112 |  */
113 | constexpr int SIZE_BLOCK              = 100;
114 | constexpr int SCANS_PER_BLOCK         = 32;
115 | constexpr int BLOCKS_PER_PACKET       = 12;
116 | constexpr float ROTATION_RESOLUTION   = 0.01f;  // [deg]
117 | constexpr uint16_t ROTATION_MAX_UNITS = 36000u; // [deg/100]
118 | 
119 | // These are used to detect which bank of 32 lasers is contained in this block
120 | enum class LaserBankId : uint16_t {
121 |   BANK_0 = 0xeeff, // lasers [0..31], aka upper bank for HDL-64E
122 |   BANK_1 = 0xddff, // lasers [32..63], aka lower bank for HDL-64E
123 |   BANK_2 = 0xccff, // lasers [64..95]
124 |   BANK_3 = 0xbbff, // lasers [96..127]
125 | };
126 | 
127 | #pragma pack(push, 1)
128 | struct raw_measurement_t {
129 |   uint16_t distance;
130 |   uint8_t intensity;
131 | };
132 | 
133 | /** \brief Raw Velodyne data block.
134 |  *
135 |  *  Each block contains data from either the upper or lower laser
136 |  *  bank.  The device returns three times as many upper bank blocks.
137 |  */
138 | struct raw_block_t {
139 |   LaserBankId bank_id; ///< which bank of 32 lasers is contained in this block
140 |   uint16_t rotation;   ///< 0-35999, divide by 100 to get degrees
141 |   std::array<raw_measurement_t, SCANS_PER_BLOCK> data;
142 | };
143 | 
144 | struct raw_packet_t {
145 |   std::array<raw_block_t, BLOCKS_PER_PACKET> blocks;
146 |   uint32_t stamp;
147 |   PacketReturnMode return_mode;
148 |   PacketModelId model_id;
149 |   // In HDL-64E, the last bytes have a different meaning, but we are ignoring these.
150 |   // uint16_t revolution;
151 |   // uint8_t status[4];
152 | };
153 | #pragma pack(pop)
154 | 
155 | } // namespace velodyne_decoder
156 | 


--------------------------------------------------------------------------------
/.github/workflows/build.yml:
--------------------------------------------------------------------------------
  1 | name: Build
  2 | on:
  3 |   push:
  4 |     paths-ignore:
  5 |       - '**.md'
  6 |       - 'LICENSE'
  7 |       - '.gitignore'
  8 |       - 'assets/**'
  9 |       - '.github/workflows/publish.yml'
 10 |   pull_request:
 11 |     paths-ignore:
 12 |       - '**.md'
 13 |       - 'LICENSE'
 14 |       - '.gitignore'
 15 |       - 'assets/**'
 16 |       - '.github/workflows/publish.yml'
 17 | jobs:
 18 |   python:
 19 |     name: Python - ${{ matrix.platform[0] }} - ${{ matrix.platform[2] }}
 20 |     runs-on: ${{ matrix.platform[1] }}
 21 |     strategy:
 22 |       fail-fast: false
 23 |       matrix:
 24 |         platform:
 25 |           - [Linux, ubuntu-latest, x86_64]
 26 |           - [Windows, windows-latest, AMD64]
 27 |           - [Windows, windows-latest, ARM64]
 28 |           - [MacOS, macos-latest, x86_64]
 29 |           - [MacOS, macos-latest, arm64]
 30 |           # universal2 is not compatible with Conan dependencies
 31 |           # - [MacOS, macos-latest, universal2]
 32 |           # aarch64 with QEMU works, but builds 10x slower than the rest
 33 |           # - [Linux, ubuntu-latest, aarch64]
 34 |     steps:
 35 |       - uses: actions/checkout@v4
 36 | 
 37 |       - name: Set up QEMU
 38 |         if: runner.os == 'Linux'
 39 |         uses: docker/setup-qemu-action@v3
 40 |         with:
 41 |           platforms: all
 42 | 
 43 |       - uses: actions/setup-python@v5
 44 |         with:
 45 |           python-version: '3.13'
 46 | 
 47 |       - name: Install cibuildwheel
 48 |         run: python -m pip install --upgrade cibuildwheel
 49 | 
 50 |       - name: Build wheels
 51 |         run: python -m cibuildwheel --output-dir wheelhouse
 52 |         env:
 53 |           CIBW_ARCHS: ${{ matrix.platform[2] }}
 54 |           CIBW_BUILD: cp38* cp312*
 55 |           CIBW_SKIP: "*-musllinux*"
 56 | 
 57 |       - name: Inspect
 58 |         run: ls wheelhouse/
 59 | 
 60 |   conan:
 61 |     name: Conan - ${{ matrix.platform[0] }} - ${{ matrix.platform[2] }}
 62 |     runs-on: ${{ matrix.platform[1] }}
 63 |     strategy:
 64 |       fail-fast: false
 65 |       matrix:
 66 |         platform:
 67 |           - [Linux, ubuntu-latest, gcc]
 68 |           - [Linux, ubuntu-latest, clang]
 69 |           - [Windows, windows-latest, msvc]
 70 |           - [MacOS, macos-latest, apple-clang]
 71 |     steps:
 72 |       - uses: actions/checkout@v4
 73 | 
 74 |       - uses: actions/setup-python@v5
 75 |         with:
 76 |           python-version: '3.13'
 77 | 
 78 |       - name: Install Clang
 79 |         if: matrix.platform[2] == 'clang'
 80 |         uses: KyleMayes/install-llvm-action@v1
 81 |         with:
 82 |           version: "17.0"
 83 | 
 84 |       - name: Set env vars for Clang
 85 |         if: matrix.platform[2] == 'clang'
 86 |         run: echo "CC=clang" >> $GITHUB_ENV && echo "CXX=clang++" >> $GITHUB_ENV
 87 | 
 88 |       - name: Install Conan
 89 |         id: conan
 90 |         uses: turtlebrowser/get-conan@main
 91 | 
 92 |       - name: Create default Conan profile
 93 |         shell: bash
 94 |         run: |
 95 |           conan profile detect
 96 |           if [[ "$RUNNER_OS" == "macOS" ]]; then sed_i='sed -i ""'; else sed_i='sed -i'; fi
 97 |           $sed_i 's/compiler.cppstd=.*/compiler.cppstd=17/' "$(conan config home)/profiles/default"
 98 | 
 99 |       - name: Build Conan package - static
100 |         run: conan create . --build missing
101 | 
102 |       - name: Build Conan package - shared
103 |         run: conan create . --build missing -o "*/*:shared=True"
104 | 
105 |   standalone:
106 |     name: CMake - ${{ matrix.platform[0] }} - ${{ matrix.platform[2] }}
107 |     runs-on: ${{ matrix.platform[1] }}
108 |     strategy:
109 |       fail-fast: false
110 |       matrix:
111 |         platform:
112 |           - [Linux, ubuntu-latest, gcc]
113 |           - [Linux, ubuntu-latest, clang]
114 |           - [Windows, windows-latest, msvc]
115 |           - [MacOS, macos-latest, apple-clang]
116 |     steps:
117 |       - uses: actions/checkout@v4
118 | 
119 |       - uses: actions/setup-python@v5
120 |         with:
121 |           python-version: '3.13'
122 | 
123 |       - name: Install Clang
124 |         if: matrix.platform[2] == 'clang'
125 |         uses: KyleMayes/install-llvm-action@v1
126 |         with:
127 |           version: "17.0"
128 | 
129 |       - name: Set env vars for Clang
130 |         if: matrix.platform[2] == 'clang'
131 |         run: echo "CC=clang" >> $GITHUB_ENV && echo "CXX=clang++" >> $GITHUB_ENV
132 | 
133 |       - uses: lukka/get-cmake@latest
134 |         with:
135 |           cmakeVersion: "~3.24"
136 | 
137 |       - name: Install Conan
138 |         id: conan
139 |         uses: turtlebrowser/get-conan@main
140 | 
141 |       - name: Configure
142 |         run: cmake -S . -B build -DCMAKE_BUILD_TYPE=Release
143 | 
144 |       - name: Build
145 |         run: cmake --build build --parallel --config Release
146 | 
147 |       - name: Test
148 |         run: ctest --test-dir build --build-config Release
149 | 
150 |       - name: Install
151 |         run: cmake --install build --prefix install --config Release
152 | 
153 |       - name: Test installed library
154 |         shell: bash
155 |         run: |
156 |           cmake -S test_package/ -B build/test_install -Dvelodyne_decoder_ROOT=$(pwd)/install/lib/cmake/velodyne_decoder/
157 |           cmake --build build/test_install --config Release
158 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
  1 | # velodyne_decoder
  2 | [![Build](https://github.com/valgur/velodyne_decoder/actions/workflows/build.yml/badge.svg?event=push)](https://github.com/valgur/velodyne_decoder/actions/workflows/build.yml) [![PyPI](https://img.shields.io/pypi/v/velodyne-decoder)](https://pypi.org/project/velodyne-decoder/) [![Conan Center](https://img.shields.io/conan/v/velodyne_decoder)](https://conan.io/center/recipes/velodyne_decoder) [![Vcpkg](https://img.shields.io/vcpkg/v/velodyne-decoder)](https://vcpkg.link/ports/velodyne-decoder) [![PyPI - Downloads](https://img.shields.io/pypi/dm/velodyne-decoder)](https://pypistats.org/packages/velodyne-decoder)
  3 | 
  4 | Python package and C++ library for Velodyne packet decoding. Point cloud extraction from PCAP and ROS bag files is
  5 | supported out of the box.
  6 | 
  7 | All non-solid-state Velodyne lidar models are fully supported. The model type and RPM are detected automatically from the data and no configuration is necessary to start using the library.
  8 | 
  9 | Notably, the library also includes support for dual-return data and decoding of telemetry packets. Precise timing info is available for all models.
 10 | 
 11 | The decoded point clouds have been validated to match the official [VeloView ground truth data](https://gitlab.kitware.com/LidarView/velodyneplugin/-/tree/master/Testing/Data?ref_type=heads) for all models.
 12 | 
 13 | In Python, the decoded point clouds are provided either as a structured NumPy array:
 14 | 
 15 | ```python
 16 | array([(2.6912806, 1.1651788 , -0.47706223,  9., -0.10085896,   0, 16, 1),
 17 |        (2.3603256, 1.021404  , -1.428755  , 85., -0.10085782,   0,  1, 1),
 18 |        (2.6994078, 1.1675802 , -0.4092741 ,  3., -0.10085666,   0, 17, 1),
 19 |        ...,
 20 |        (2.8952641, 0.80728334,  0.48905915,  2.,  0.00054029, 923, 30, 1),
 21 |        (2.8683424, 0.79923725, -0.5555609 ,  2.,  0.00054144, 923, 15, 1),
 22 |        (2.908243 , 0.80980825,  0.56333727,  1.,  0.00054259, 923, 31, 1)],
 23 |       dtype={'names': ['x', 'y', 'z', 'intensity', 'time', 'column', 'ring', 'return_type'], 
 24 |              'formats': ['<f4', '<f4', '<f4', '<f4', '<f4', '<u2', 'u1', 'u1'], 
 25 |              'offsets': [0, 4, 8, 12, 16, 20, 22, 23], 'itemsize': 32})
 26 | ```
 27 | 
 28 | or as a contiguous array of floats (default):
 29 | 
 30 | ```python
 31 | array([[2.691281, 1.165179, -0.477062,  9., -0.100859,   0., 16., 1.],
 32 |        [2.360326, 1.021404, -1.428755, 85., -0.100858,   0.,  1., 1.],
 33 |        [2.699408, 1.16758 , -0.409274,  3., -0.100857,   0., 17., 1.],
 34 |        ...,
 35 |        [2.895264, 0.807283,  0.489059,  2.,  0.00054 , 923., 30., 1.],
 36 |        [2.868342, 0.799237, -0.555561,  2.,  0.000541, 923., 15., 1.],
 37 |        [2.908243, 0.809808,  0.563337,  1.,  0.000543, 923., 31., 1.]], dtype=float32)
 38 | ```
 39 | 
 40 | The decoded point cloud follows ROS conventions for its coordinate axes: x – forward, y – left, z – up.
 41 | 
 42 | ## Installation
 43 | 
 44 | Wheels are available from PyPI for Linux, MacOS and Windows. Python versions 3.8+ are supported.
 45 | 
 46 | ```bash
 47 | pip install velodyne-decoder
 48 | ```
 49 | 
 50 | Alternatively, you can build and install the development version from source.
 51 | 
 52 | ```bash
 53 | sudo apt-get install cmake build-essential python3-dev
 54 | pip install git+https://github.com/valgur/velodyne_decoder.git
 55 | ```
 56 | 
 57 | ## Usage
 58 | 
 59 | ### Decoding Velodyne data from a ROS bag
 60 | 
 61 | ```python
 62 | import velodyne_decoder as vd
 63 | 
 64 | bagfile = 'xyz.bag'
 65 | lidar_topics = ['/velodyne_packets']
 66 | cloud_arrays = []
 67 | for stamp, points, topic in vd.read_bag(bagfile, topics=lidar_topics):
 68 |     cloud_arrays.append(points)
 69 | ```
 70 | 
 71 | The `rosbag` library must be installed. If needed, you can install it without setting up the entire ROS stack with
 72 | 
 73 | ```bash
 74 | pip install rosbag --extra-index-url https://rospypi.github.io/simple/
 75 | ```
 76 | 
 77 | To extract all `VelodyneScan` messages in the bag you can leave the list of topics unspecified.
 78 | 
 79 | The header timestamp from the scan messages will be returned by default. To use the message arrival time instead
 80 | set `use_header_time=False`.
 81 | 
 82 | To return arrays of structs instead of the default contiguous arrays, set `as_pcl_structs=True`.
 83 | 
 84 | ### Decoding Velodyne data from a PCAP file
 85 | 
 86 | ```python
 87 | import velodyne_decoder as vd
 88 | 
 89 | pcap_file = 'vlp16.pcap'
 90 | cloud_arrays = []
 91 | for stamp, points in vd.read_pcap(pcap_file):
 92 |     cloud_arrays.append(points)
 93 | ```
 94 | 
 95 | To return arrays of structs instead of the default contiguous arrays, set `as_pcl_structs=True`.
 96 | 
 97 | ### Configuration
 98 | 
 99 | You can pass a `velodyne_decoder.Config` object to all decoder functions. The following options are available:
100 | 
101 | * `min_range` and `max_range` – only return points between these range values.
102 | * `min_angle` and `max_angle` – only return points between these azimuth angles.
103 | * `timestamp_first_packet` – whether the scan timestamps are set based on the first or last packet in the scan.
104 | * `cut_angle` – when working with a raw packet stream, if unset (by default), the stream is split into a "scan" every time at least 360 degrees have been covered.
105 |   If set, the splitting always occurs at the specified azimuth angle instead. Note that the scan might cover less than 360 degrees in this case.
106 | 
107 | Only required for data from HDL-64E sensors:
108 | * `model` – the sensor model ID. See `velodyne_decoder.Model.__entries` for the possible values.
109 | * `calibration_file` – beam calibration parameters in a YAML format.
110 |   You can either extract the calibration info from a PCAP file with packets using `extract-hdl64e-calibration <pcap_file>` or
111 |   convert a `db.xml` provided with the sensor using [gen_calibration.py](https://wiki.ros.org/velodyne_pointcloud#gen_calibration.py) from the ROS driver.
112 | 
113 | ## Authors
114 | 
115 | * Martin Valgur ([@valgur](https://github.com/valgur))
116 | 
117 | The core functionality has been adapted from the ROS [velodyne driver](https://github.com/ros-drivers/velodyne).
118 | 
119 | ## License
120 | 
121 | [BSD 3-Clause License](LICENSE)
122 | 


--------------------------------------------------------------------------------
/src/telemetry_packet.cpp:
--------------------------------------------------------------------------------
  1 | // Copyright (c) 2023, Martin Valgur
  2 | // SPDX-License-Identifier: BSD-3-Clause
  3 | 
  4 | #include "velodyne_decoder/telemetry_packet.h"
  5 | #include "velodyne_decoder/time_conversion.h"
  6 | 
  7 | #include <algorithm>
  8 | #include <cmath>
  9 | #include <cstdint>
 10 | #include <cstring>
 11 | #include <ctime>
 12 | #include <optional>
 13 | #include <string>
 14 | 
 15 | namespace velodyne_decoder {
 16 | 
 17 | namespace {
 18 | bool validate_nmea_checksum(const std::string &nmea) {
 19 |   if (nmea.size() < 5)
 20 |     return false;
 21 |   size_t body_len  = nmea.size() - 5;
 22 |   uint8_t checksum = 0;
 23 |   for (size_t i = 1; i < body_len; i++)
 24 |     checksum ^= nmea[i];
 25 |   return checksum == strtoul(&nmea[body_len + 1], nullptr, 16);
 26 | }
 27 | 
 28 | /// Parse NMEA sentence
 29 | std::optional<NmeaInfo> parse_nmea(const std::string &nmea) {
 30 |   if (strncmp(nmea.c_str(), "$GPGGA,", 6) == 0) {
 31 |     if (!validate_nmea_checksum(nmea))
 32 |       return std::nullopt;
 33 |     NmeaInfo info = {};
 34 |     // https://gpsd.gitlab.io/gpsd/NMEA.html#_gga_global_positioning_system_fix_data
 35 |     uint8_t lat_deg, lon_deg;
 36 |     double lat_min, lon_min;
 37 |     char lat_sign, lon_sign;
 38 |     uint8_t quality;
 39 |     double altitude_msl, geoid_separation;
 40 |     if (sscanf(nmea.c_str(),
 41 |                "$GPGGA,"       //
 42 |                "%2hhd%2hhd%f," // UTC time
 43 |                "%2hhd%lf,%c,"  // Latitude
 44 |                "%3hhd%lf,%c,"  // Longitude
 45 |                "%hhd,"         // Quality
 46 |                "%*[^,],"       // Number of satellites
 47 |                "%*[^,],"       // Horizontal dilution of precision
 48 |                "%lf,M,"        // Altitude above mean sea level
 49 |                "%lf,M,",       // Height of geoid
 50 |                &info.utc_hours, &info.utc_minutes, &info.utc_seconds, //
 51 |                &lat_deg, &lat_min, &lat_sign,                         //
 52 |                &lon_deg, &lon_min, &lon_sign,                         //
 53 |                &quality,                                              //
 54 |                &altitude_msl, &geoid_separation) != 12)
 55 |       return std::nullopt;
 56 |     info.longitude     = (lon_deg + lon_min / 60.0) * (lon_sign == 'E' ? 1 : -1);
 57 |     info.latitude      = (lat_deg + lat_min / 60.0) * (lat_sign == 'N' ? 1 : -1);
 58 |     info.altitude      = altitude_msl + geoid_separation;
 59 |     info.fix_available = quality > 0;
 60 |     // date info is not available in GPGGA, leaving DMY as 0
 61 |     return info;
 62 | 
 63 |   } else if (strncmp(nmea.c_str(), "$GPRMC,", 6) == 0) {
 64 |     if (!validate_nmea_checksum(nmea))
 65 |       return std::nullopt;
 66 |     NmeaInfo info = {};
 67 |     // https://gpsd.gitlab.io/gpsd/NMEA.html#_rmc_recommended_minimum_navigation_information
 68 |     char status;
 69 |     uint8_t lat_deg, lon_deg;
 70 |     double lat_min, lon_min;
 71 |     char lat_sign, lon_sign;
 72 |     if (sscanf(nmea.c_str(),
 73 |                "$GPRMC,"                                              //
 74 |                "%2hhd%2hhd%f,"                                        // UTC time
 75 |                "%c,"                                                  // Status
 76 |                "%2hhd%lf,%c,"                                         // Latitude
 77 |                "%3hhd%lf,%c,"                                         // Longitude
 78 |                "%*[^,],"                                              // Speed over ground
 79 |                "%*[^,],"                                              // Track made good
 80 |                "%2hhd%2hhd%2hd",                                      // Date
 81 |                &info.utc_hours, &info.utc_minutes, &info.utc_seconds, //
 82 |                &status,                                               //
 83 |                &lat_deg, &lat_min, &lat_sign,                         //
 84 |                &lon_deg, &lon_min, &lon_sign,                         //
 85 |                &info.utc_day, &info.utc_month, &info.utc_year) != 13)
 86 |       return std::nullopt;
 87 |     info.longitude = (lon_deg + lon_min / 60.0) * (lon_sign == 'E' ? 1 : -1);
 88 |     info.latitude  = (lat_deg + lat_min / 60.0) * (lat_sign == 'N' ? 1 : -1);
 89 |     // altitude is not available in GPRMC, leaving it as 0
 90 |     info.fix_available = status == 'A';
 91 |     if (info.utc_year > 0)
 92 |       info.utc_year += 2000;
 93 |     return info;
 94 |   }
 95 |   return std::nullopt;
 96 | }
 97 | 
 98 | } // namespace
 99 | 
100 | TelemetryPacket::TelemetryPacket(gsl::span<const uint8_t, TELEMETRY_PACKET_SIZE> data) {
101 |   // fields set by all firmware versions
102 |   usec_since_toh =
103 |       static_cast<uint32_t>(data[0xC9] << 24u | data[0xC8] << 16u | data[0xC7] << 8u | data[0xC6]);
104 |   pps_status = static_cast<PpsStatus>(data[0xCA]);
105 |   nmea_sentence.assign(&data[0xCE], std::find(&data[0xCE], &data[0xCE] + 128, '\0'));
106 |   // fields set by newer firmware versions only
107 |   temp_board_top                    = data[0xBB];
108 |   temp_board_bottom                 = data[0xBC];
109 |   temp_during_adc_calibration       = data[0xBD];
110 |   temp_change_since_adc_calibration = static_cast<int16_t>(data[0xBF] << 8 | data[0xBE]);
111 |   seconds_since_adc_calibration =
112 |       static_cast<uint32_t>(data[0xC3] << 24u | data[0xC2] << 16u | data[0xC1] << 8u | data[0xC0]);
113 |   adc_calibration_reason        = static_cast<AdcCalibReason>(data[0xC4]);
114 |   adc_calib_in_progress         = data[0xC5] & 1u;
115 |   adc_delta_temp_limit_exceeded = data[0xC5] & 2u;
116 |   adc_period_exceeded           = data[0xC5] & 4u;
117 |   thermal_shutdown              = data[0xCB] == 1;
118 |   temp_at_shutdown              = data[0xCC];
119 |   temp_at_powerup               = data[0xCD];
120 | }
121 | 
122 | std::optional<NmeaInfo> TelemetryPacket::parseNmea() const { return parse_nmea(nmea_sentence); }
123 | 
124 | Time NmeaInfo::utcTime() const {
125 |   std::tm tm = {};
126 |   if (utc_year > 0) {
127 |     tm.tm_year = utc_year - 1900;
128 |     tm.tm_mon  = utc_month - 1;
129 |     tm.tm_mday = utc_day;
130 |   } else { // full datetime not available, return only time of day
131 |     tm.tm_year = 70;
132 |     tm.tm_mon  = 0;
133 |     tm.tm_mday = 1;
134 |   }
135 |   tm.tm_hour = utc_hours;
136 |   tm.tm_min  = utc_minutes;
137 |   tm.tm_sec  = 0;
138 | #ifdef _MSC_VER // timegm is not available on Windows, use _mkgmtime instead
139 | #  define timegm _mkgmtime
140 | #endif
141 |   return (double)timegm(&tm) + utc_seconds;
142 | }
143 | 
144 | std::optional<Time> TelemetryPacket::nmeaTime() const {
145 |   auto nmea_info = parseNmea();
146 |   if (!nmea_info)
147 |     return {};
148 |   return nmea_info->utcTime();
149 | }
150 | 
151 | std::optional<Time> TelemetryPacket::ppsTime() const {
152 |   if (pps_status != TelemetryPacket::PpsStatus::LOCKED)
153 |     return {};
154 |   auto nmea_time = nmeaTime();
155 |   if (!nmea_time)
156 |     return {};
157 |   return getPacketTimestamp(usec_since_toh, *nmea_time);
158 | }
159 | 
160 | } // namespace velodyne_decoder
161 | 


--------------------------------------------------------------------------------
/src/calibration.cpp:
--------------------------------------------------------------------------------
  1 | // Copyright (c) 2012, Austin Robot Technology, Piyush Khandelwal
  2 | // Copyright (c) 2021-2023, Martin Valgur
  3 | // SPDX-License-Identifier: BSD-3-Clause
  4 | 
  5 | #include "velodyne_decoder/calibration.h"
  6 | 
  7 | #include <algorithm>
  8 | #include <cmath>
  9 | #include <fstream>
 10 | #include <numeric>
 11 | #include <optional>
 12 | #include <stdexcept>
 13 | #include <string>
 14 | #include <yaml-cpp/yaml.h>
 15 | 
 16 | namespace YAML {
 17 | 
 18 | namespace {
 19 | constexpr auto NUM_LASERS              = "num_lasers";
 20 | constexpr auto DISTANCE_RESOLUTION     = "distance_resolution";
 21 | constexpr auto LASERS                  = "lasers";
 22 | constexpr auto LASER_ID                = "laser_id";
 23 | constexpr auto ROT_CORRECTION          = "rot_correction";
 24 | constexpr auto VERT_CORRECTION         = "vert_correction";
 25 | constexpr auto DIST_CORRECTION         = "dist_correction";
 26 | constexpr auto DIST_CORRECTION_X       = "dist_correction_x";
 27 | constexpr auto DIST_CORRECTION_Y       = "dist_correction_y";
 28 | constexpr auto VERT_OFFSET_CORRECTION  = "vert_offset_correction";
 29 | constexpr auto HORIZ_OFFSET_CORRECTION = "horiz_offset_correction";
 30 | constexpr auto MAX_INTENSITY           = "max_intensity";
 31 | constexpr auto MIN_INTENSITY           = "min_intensity";
 32 | constexpr auto FOCAL_DISTANCE          = "focal_distance";
 33 | constexpr auto FOCAL_SLOPE             = "focal_slope";
 34 | } // namespace
 35 | 
 36 | template <> struct convert<velodyne_decoder::LaserCorrection> {
 37 |   static bool decode(const YAML::Node &node, velodyne_decoder::LaserCorrection &correction) {
 38 |     correction.laser_idx               = node[LASER_ID].as<int>();
 39 |     correction.rot_correction          = node[ROT_CORRECTION].as<float>();
 40 |     correction.vert_correction         = node[VERT_CORRECTION].as<float>();
 41 |     correction.dist_correction         = node[DIST_CORRECTION].as<float>(0);
 42 |     correction.dist_correction_x       = node[DIST_CORRECTION_X].as<float>(0);
 43 |     correction.dist_correction_y       = node[DIST_CORRECTION_Y].as<float>(0);
 44 |     correction.vert_offset_correction  = node[VERT_OFFSET_CORRECTION].as<float>(0);
 45 |     correction.horiz_offset_correction = node[HORIZ_OFFSET_CORRECTION].as<float>(0);
 46 |     correction.max_intensity           = std::floor(node[MAX_INTENSITY].as<float>(255));
 47 |     correction.min_intensity           = std::floor(node[MIN_INTENSITY].as<float>(0));
 48 |     correction.focal_distance          = node[FOCAL_DISTANCE].as<float>(0);
 49 |     correction.focal_slope             = node[FOCAL_SLOPE].as<float>(0);
 50 |     correction.two_pt_correction_available =
 51 |         correction.dist_correction_x != 0 && correction.dist_correction_y != 0;
 52 |     return true;
 53 |   };
 54 | 
 55 |   static YAML::Node encode(const velodyne_decoder::LaserCorrection &correction) {
 56 |     YAML::Node node;
 57 |     if (correction.dist_correction != 0) {
 58 |       node[DIST_CORRECTION] = correction.dist_correction;
 59 |     }
 60 |     if (correction.dist_correction_x != 0) {
 61 |       node[DIST_CORRECTION_X] = correction.dist_correction_x;
 62 |     }
 63 |     if (correction.dist_correction_y != 0) {
 64 |       node[DIST_CORRECTION_Y] = correction.dist_correction_y;
 65 |     }
 66 |     if (correction.focal_distance != 0) {
 67 |       node[FOCAL_DISTANCE] = correction.focal_distance;
 68 |     }
 69 |     if (correction.focal_slope != 0) {
 70 |       node[FOCAL_SLOPE] = correction.focal_slope;
 71 |     }
 72 |     if (correction.horiz_offset_correction != 0) {
 73 |       node[HORIZ_OFFSET_CORRECTION] = correction.horiz_offset_correction;
 74 |     }
 75 |     node[LASER_ID] = correction.laser_idx;
 76 |     if (correction.min_intensity != 0 || correction.max_intensity != 255) {
 77 |       node[MAX_INTENSITY] = correction.max_intensity;
 78 |       node[MIN_INTENSITY] = correction.min_intensity;
 79 |     }
 80 |     node[ROT_CORRECTION]         = correction.rot_correction;
 81 |     node[VERT_CORRECTION]        = correction.vert_correction;
 82 |     node[VERT_OFFSET_CORRECTION] = correction.vert_offset_correction;
 83 |     return node;
 84 |   }
 85 | };
 86 | 
 87 | template <> struct convert<velodyne_decoder::Calibration> {
 88 |   static bool decode(const YAML::Node &node, velodyne_decoder::Calibration &calibration) {
 89 |     int num_lasers = node[NUM_LASERS].as<int>();
 90 |     std::vector<velodyne_decoder::LaserCorrection> corrections(num_lasers);
 91 |     for (const auto &laser : node[LASERS]) {
 92 |       auto correction = laser.as<velodyne_decoder::LaserCorrection>();
 93 |       if (corrections.size() <= correction.laser_idx) {
 94 |         corrections.resize(correction.laser_idx + 1);
 95 |       }
 96 |       corrections[correction.laser_idx] = correction;
 97 |     }
 98 |     calibration = {corrections, node[DISTANCE_RESOLUTION].as<float>()};
 99 |     return true;
100 |   }
101 | 
102 |   static YAML::Node encode(const velodyne_decoder::Calibration &calibration) {
103 |     YAML::Node node;
104 |     node[DISTANCE_RESOLUTION] = calibration.distance_resolution_m;
105 |     for (const auto &correction : calibration.laser_corrections) {
106 |       node[LASERS][correction.laser_idx] = correction;
107 |     }
108 |     node[NUM_LASERS] = calibration.laser_corrections.size();
109 |     return node;
110 |   }
111 | };
112 | 
113 | } // namespace YAML
114 | 
115 | namespace velodyne_decoder {
116 | 
117 | Calibration::Calibration(const std::string &calibration_file)
118 |     : Calibration(read(calibration_file)) {}
119 | 
120 | Calibration::Calibration(std::vector<LaserCorrection> laser_corrs, float distance_resolution_m)
121 |     : distance_resolution_m(distance_resolution_m), laser_corrections(std::move(laser_corrs)) {
122 |   num_lasers = (int)laser_corrections.size();
123 |   assignRingNumbers();
124 | }
125 | 
126 | void Calibration::assignRingNumbers() {
127 |   // argsort by the vertical angle of the laser to assign ring numbers to lasers
128 |   std::vector<size_t> idx(num_lasers);
129 |   std::iota(idx.begin(), idx.end(), 0);
130 |   std::stable_sort(idx.begin(), idx.end(), [this](size_t i1, size_t i2) {
131 |     return laser_corrections[i1].vert_correction < laser_corrections[i2].vert_correction;
132 |   });
133 |   for (size_t i = 0; i < idx.size(); i++) {
134 |     laser_corrections[idx[i]].laser_ring = i;
135 |   }
136 | }
137 | 
138 | bool Calibration::isAdvancedCalibration() const {
139 |   // Everything besides rot_correction and vert_correction is considered "advanced",
140 |   // since the default calibrations don't really set these (except for the oldest model, HDL-64E).
141 |   for (const auto &corr : laser_corrections) {
142 |     if (corr.dist_correction != 0)
143 |       return true;
144 |     if (corr.two_pt_correction_available)
145 |       return true;
146 |     if (corr.dist_correction_x != 0)
147 |       return true;
148 |     if (corr.dist_correction_y != 0)
149 |       return true;
150 |     if (corr.horiz_offset_correction != 0)
151 |       return true;
152 |     if (corr.max_intensity != 255)
153 |       return true;
154 |     if (corr.min_intensity != 0)
155 |       return true;
156 |     if (corr.focal_distance != 0)
157 |       return true;
158 |     if (corr.focal_slope != 0)
159 |       return true;
160 |   }
161 |   return false;
162 | }
163 | 
164 | Calibration Calibration::read(const std::string &calibration_file) {
165 |   std::ifstream fin{calibration_file};
166 |   if (!fin.is_open()) {
167 |     throw std::runtime_error("Unable to open calibration file: " + calibration_file);
168 |   }
169 |   std::stringstream buffer;
170 |   buffer << fin.rdbuf();
171 |   fin.close();
172 |   return fromString(buffer.str());
173 | }
174 | 
175 | void Calibration::write(const std::string &calibration_file) const {
176 |   std::ofstream fout{calibration_file};
177 |   fout << toString();
178 |   fout.close();
179 | }
180 | 
181 | Calibration Calibration::fromString(const std::string &calibration_content) {
182 |   return YAML::Load(calibration_content).as<Calibration>();
183 | }
184 | 
185 | std::string Calibration::toString() const { return YAML::Dump(YAML::Node(*this)); }
186 | 
187 | } // namespace velodyne_decoder
188 | 


--------------------------------------------------------------------------------
/CHANGELOG.md:
--------------------------------------------------------------------------------
  1 | # Changelog
  2 | 
  3 | ## [3.1.0] – 2024-12-22
  4 | 
  5 | Python-only release. Replaced pybind11 with nanobind for the Python bindings. This allows Python 3.12+ binaries to be
  6 | built against the stable CPython ABI, making the binaries compatible with any future Python versions. 
  7 | 
  8 | ## [3.0.1] – 2024-12-21
  9 | 
 10 | Python-only release. Re-built the PyPI binaries with NumPy 2.x to fix compatibility issues. 
 11 | 
 12 | ## [3.0.0] – 2024-02-25
 13 | 
 14 | This release significantly improves the ease of use of the library and precise timing info capabilities. Dual-return data is now supported as well.
 15 | 
 16 | As a major release, it includes a few breaking changes:
 17 | 
 18 | * All config parameters are now optional and `rpm`, `gps_time` have been dropped.
 19 | * Timestamps are now returned as a pair of `host` and `device` timestamps.
 20 | * Two new fields have been added to the returned point clouds (`return_type` and `column`).
 21 | 
 22 | ### General changes
 23 | 
 24 | * Sensor `model` and `rpm` parameters are detected automatically from the data. This means that only the raw data itself
 25 |   is required to use the decoder without any additional configuration parameters.
 26 | * Added support for dual-return data. A `return_type` field has been added to the decoded points to indicate the type of
 27 |   return:  1 - strongest, 2 - last, 3 - both. This info is also included for single-return data.
 28 | * Added a `column` field to the points, which provides the index of the column of simultaneous firings relative to the
 29 |   start of the scan.
 30 | * Timestamps are now always returned as a pair of `host` and `device` timestamps. This replaces the previous behavior of
 31 |   returning only one based on a config parameter.
 32 | * Added support for parsing of telemetry packets. This provides details about PPS time synchronization status, the time
 33 |   and position of last GNSS message, internal temperatures and more.
 34 | * Precise timing info is now available for all sensor models. All of the logic and parameters related to precise timing
 35 |   info have been thoroughly reviewed to match the official specifications and have been validated against the ground truth
 36 |   dataset provided with VeloView. The existing timing parameters were not modified in the process.
 37 | * Added a correction for a systematic error in the vertical direction for up to 2 cm, dependent on the beam angle. This
 38 |   correction accounts for each laser having a slight vertical offset instead of the previous implicit assumption of them
 39 |   all being located in the optical center. Related to https://github.com/ros-drivers/velodyne/pull/518.
 40 | * Added a `cut_angle` config parameter – when working with a raw packet stream, if unset (by default), the stream is
 41 |   split into a "scan" every time at least 360 degrees have been covered. If set, the splitting always occurs at the
 42 |   specified azimuth angle instead. Note that the scan might cover less than 360 degrees in this case.
 43 | * Fixed packets at the end of a file not being decoded if they didn't form a complete scan.
 44 | * Improved the efficiency of application of calibration parameters. Most of the parameters were only applicable to the
 45 |   legacy HDL-64E model and are handled on-device in all newer models.
 46 | * The calibration parameters for all models besides HDL-64E are now stored in an embedded database. This allows for
 47 |   easier packaging as a C++ library by avoiding the need to access YAML files.
 48 | * Changes specific to HDL-64E:
 49 |   - Added functionality to extract the calibration parameters embedded in a HDL-64E data stream. This is not done
 50 |     automatically and should be run once for each sensor to create a YAML file with the calibration parameters.
 51 |   - Improvements to HDL-64E calibration formulas:
 52 |     - Corrected the focal offset coefficient being off by 100x. https://github.com/ros-drivers/velodyne/issues/499
 53 |     - Fixed two-point correction not being applied for HDL-64E, despite being recommended by the product manual. Related
 54 |       to https://github.com/ros-drivers/velodyne/commit/16be7978ee0fa78818a9ff04faf5efac83ccf378
 55 |     - Fixed two-point correction being applied outside its applicable range.
 56 |     - Horizontal and vertical offset corrections are applied at the last step only.
 57 |     - The vertical-direction correction now uses the mean of the horizontal-direction corrections as its basis instead
 58 |       of just the x-direction correction. This provides a better agreement with the VeloView ground truth dataset.
 59 |   - Note that since HDL-64E does not include model ID info in its packets, the model ID must still be provided for it.
 60 |   - Similarly, the calibration parameters for HDL-64E are not standardized and sensor-specific calibration info always
 61 |     needs to be provided.
 62 |   - The added precise timing parameters for HDL-64E S1 are only approximate, due to them not being provided by the
 63 |     product manual.
 64 | * Changes specific to Alpha Prime (VLS-128):
 65 |   - Account for the precise timing parameters varying depending on the firmware version of the sensor. The correct
 66 |     values are detected automatically from the data.
 67 |   - Increased default max range from 130 to 250.
 68 |   - Fixed `distance_resolution` calibration param being hard-coded.
 69 | 
 70 | ### Python-specific changes
 71 | 
 72 | * More robust PCAP parsing. Do not raise an exception for partial packets.
 73 | * Minimum Python version is now 3.8 instead of 3.7.
 74 | 
 75 | ### C++-specific changes
 76 | 
 77 | * Added support for use as a C++ library via Conan, Vcpkg, or as a standalone system library.
 78 | * `gsl::span` is used for passing data to the decoder, which allows for zero-copy decoding.
 79 | * Renamed `PointXYZIRT` -> `VelodynePoint`.
 80 | * Added `RosScanBatcher` as for use with `velodyne_driver::VelodyneScan` messages. It is only available as a header and
 81 |   not built by default to avoid ROS-specific dependencies.
 82 | 
 83 | ## [2.3.0] – 2022-03-20
 84 | 
 85 | ### Changed
 86 | 
 87 | - `VLS-128` has been replaced by `Alpha Prime` as the model identifier, but is still accepted for backwards compatibility. (@flopie2009 #1)
 88 | - Removed the broken VLS-128 calibration data file. (@flopie2009 #2)
 89 | 
 90 | ## [2.2.0] – 2021-12-10
 91 | 
 92 | ### Added
 93 | 
 94 | - Added `time_range` parameter to `read_bag()` and `read_pcap()`.
 95 | 
 96 | ### Fixed
 97 | 
 98 | - `read_bag()` no longer closes opened `Bag` objects passed to it.
 99 | 
100 | ## [2.1.0] – 2021-12-08
101 | 
102 | ### Added
103 | 
104 | - `read_bag()` now optionally also includes `frame_id`-s from the ROS messages in the returned tuples
105 |   when `return_frame_id=True` is set.
106 | 
107 | ### Fixed
108 | 
109 | - Fixed `Config("model")` constructor not working as expected due to `calibration_file` missing a default value.
110 | - Fixed out-of-scope pointer dereferencing in the calibration parser.
111 | - Fixed a build error on Windows due to yaml-cpp by pinning its version to 0.7.
112 | 
113 | ## [2.0.0] – 2021-11-11
114 | 
115 | ### Added
116 | 
117 | - Support for PCAP file decoding with the `read_pcap()` function in Python and `StreamDecoder` in C++.
118 | - More convenient data extraction from ROS bags with `read_bag()`.
119 | - Standard calibration info provided by Velodyne is now used by default and `config.calibration_file` can be left unset
120 |   in most cases.
121 | - `TIMINGS_AVAILABLE` was added for details about which models support point timing info. The current list is:
122 |   `['HDL-32E', 'VLP-16', 'VLP-32C', 'VLS-128']`.
123 | 
124 | ### Changed
125 | 
126 | - The decoded scans are now returned as a contiguous float32 NumPy arrays instead of PCL-compatible structs by default.
127 |   You can pass `as_pcl_structs=True` to the decoding functions for the old behavior.
128 | - Model ID naming scheme has been updated to better match the typical forms used by Velodyne. <br>
129 |   Before: `['VLP16', '32C', '32E', 'VLS128']`. <br>
130 |   After: `['HDL-32E', 'HDL-64E', 'HDL-64E_S2', 'HDL-64E_S3', 'VLP-16', 'VLP-32C', 'VLS-128']`.
131 | - Conversion to NumPy arrays is now done without any unnecessary data copying.
132 | 
133 | ### Fixed
134 | 
135 | - Fixed a Python 2 ROS message decoding error.
136 | 
137 | ## [1.0.1] – 2021-05-19
138 | 
139 | Initial release.
140 | 
141 | [3.1.0]: https://github.com/valgur/velodyne_decoder/compare/v3.0.1...v3.1.0
142 | 
143 | [3.0.1]: https://github.com/valgur/velodyne_decoder/compare/v3.0.0...v3.0.1
144 | 
145 | [3.0.0]: https://github.com/valgur/velodyne_decoder/compare/v2.3.0...v3.0.0
146 | 
147 | [2.3.0]: https://github.com/valgur/velodyne_decoder/compare/v2.2.0...v2.3.0
148 | 
149 | [2.2.0]: https://github.com/valgur/velodyne_decoder/compare/v2.1.0...v2.2.0
150 | 
151 | [2.1.0]: https://github.com/valgur/velodyne_decoder/compare/v2.0.0...v2.1.0
152 | 
153 | [2.0.0]: https://github.com/valgur/velodyne_decoder/compare/v1.0.1...v2.0.0
154 | 
155 | [1.0.1]: https://github.com/valgur/velodyne_decoder/releases/tag/v1.0.1
156 | 


--------------------------------------------------------------------------------
/src/calib_db.cpp:
--------------------------------------------------------------------------------
  1 | // Copyright (c) 2023, Martin Valgur
  2 | // SPDX-License-Identifier: BSD-3-Clause
  3 | 
  4 | #include <cmath>
  5 | #include <map>
  6 | #include <stdexcept>
  7 | #include <string>
  8 | 
  9 | #include "velodyne_decoder/calibration.h"
 10 | 
 11 | namespace velodyne_decoder {
 12 | 
 13 | namespace {
 14 | struct MinimalCalibData {
 15 |   float rot_corr_deg;
 16 |   float vert_corr_deg;
 17 | };
 18 | } // namespace
 19 | 
 20 | CalibDB::CalibDB() {
 21 |   std::map<ModelId, float> resolutions = {
 22 |       {ModelId::AlphaPrime, 0.004}, {ModelId::HDL32E, 0.002}, {ModelId::PuckHiRes, 0.002},
 23 |       {ModelId::VLP16, 0.002},      {ModelId::VLP32A, 0.004}, {ModelId::VLP32C, 0.004},
 24 |   };
 25 | 
 26 |   // Distance from the sensor center to the focal point where all laser beams intersect.
 27 |   std::map<ModelId, float> focal_distances = {
 28 |       {ModelId::AlphaPrime, 58.63e-3}, {ModelId::HDL32E, 28.83e-3}, {ModelId::PuckHiRes, 41.91e-3},
 29 |       {ModelId::VLP16, 41.91e-3},      {ModelId::VLP32A, 42.4e-3},  {ModelId::VLP32C, 42.4e-3},
 30 |   };
 31 | 
 32 |   std::map<ModelId, std::vector<float>> vertical_offsets;
 33 |   // https://velodynelidar.com/wp-content/uploads/2019/09/86-0101-REV-B1-ENVELOPEVLP-16.pdf#page=3
 34 |   // VLP-16 vertical offsets are documented, but re-deriving them from the focal distance and angles
 35 |   // produces more accurate values.
 36 |   // https://velodynelidar.com/wp-content/uploads/2019/09/86-0129-REV-A-ENVELOPEHI-RESVLP-16.pdf#page=3
 37 |   vertical_offsets[ModelId::PuckHiRes] = {
 38 |       7.4e-3, -0.9e-3, 6.5e-3, -1.8e-3, 5.5e-3, -2.7e-3, 4.6e-3, -3.7e-3,
 39 |       3.7e-3, -4.6e-3, 2.7e-3, -5.5e-3, 1.8e-3, -6.5e-3, 0.9e-3, -7.4e-3,
 40 |   };
 41 |   // https://velodynelidar.com/wp-content/uploads/2019/09/86-0106-REV-BENVELOPEHDL-32E.pdf#page=2
 42 |   vertical_offsets[ModelId::HDL32E] = {
 43 |       17.17e-3, 4.76e-3,  16.27e-3, 4.07e-3,  15.4e-3,  3.38e-3,  14.54e-3, 2.7e-3,
 44 |       13.71e-3, 2.02e-3,  12.89e-3, 1.35e-3,  12.09e-3, 0.67e-3,  11.31e-3, 0,
 45 |       10.54e-3, -0.67e-3, 9.78e-3,  -1.35e-3, 9.04e-3,  -2.02e-3, 8.3e-3,   -2.7e-3,
 46 |       7.58e-3,  -3.38e-3, 6.86e-3,  -4.07e-3, 6.15e-3,  -4.76e-3, 5.45e-3,  -5.45e-3,
 47 |   };
 48 | 
 49 |   std::map<ModelId, std::vector<MinimalCalibData>> raw_calib_data;
 50 |   raw_calib_data[ModelId::AlphaPrime] = {
 51 |       {-6.354, -11.742}, {-4.548, -1.99}, {-2.732, 3.4},     {-0.911, -5.29},  {0.911, -0.78},
 52 |       {2.732, 4.61},     {4.548, -4.08},  {6.354, 1.31},     {-6.354, -6.5},   {-4.548, -1.11},
 53 |       {-2.732, 4.28},    {-0.911, -4.41}, {0.911, 0.1},      {2.732, 6.48},    {4.548, -3.2},
 54 |       {6.354, 2.19},     {-6.354, -3.86}, {-4.548, 1.53},    {-2.732, -9.244}, {-0.911, -1.77},
 55 |       {0.911, 2.74},     {2.732, -5.95},  {4.548, -0.56},    {6.354, 4.83},    {-6.354, -2.98},
 56 |       {-4.548, 2.41},    {-2.732, -6.28}, {-0.911, -0.89},   {0.911, 3.62},    {2.732, -5.07},
 57 |       {4.548, 0.32},     {6.354, 7.58},   {-6.354, -0.34},   {-4.548, 5.18},   {-2.732, -3.64},
 58 |       {-0.911, 1.75},    {0.911, -25},    {2.732, -2.43},    {4.548, 2.96},    {6.354, -5.73},
 59 |       {-6.354, 0.54},    {-4.548, 9.7},   {-2.732, -2.76},   {-0.911, 2.63},   {0.911, -7.65},
 60 |       {2.732, -1.55},    {4.548, 3.84},   {6.354, -4.85},    {-6.354, 3.18},   {-4.548, -5.51},
 61 |       {-2.732, -0.12},   {-0.911, 5.73},  {0.911, -4.3},     {2.732, 1.09},    {4.548, -16.042},
 62 |       {6.354, -2.21},    {-6.354, 4.06},  {-4.548, -4.63},   {-2.732, 0.76},   {-0.911, 15},
 63 |       {0.911, -3.42},    {2.732, 1.97},   {4.548, -6.85},    {6.354, -1.33},   {-6.354, -5.62},
 64 |       {-4.548, -0.23},   {-2.732, 5.43},  {-0.911, -3.53},   {0.911, 0.98},    {2.732, -19.582},
 65 |       {4.548, -2.32},    {6.354, 3.07},   {-6.354, -4.74},   {-4.548, 0.65},   {-2.732, 11.75},
 66 |       {-0.911, -2.65},   {0.911, 1.86},   {2.732, -7.15},    {4.548, -1.44},   {6.354, 3.95},
 67 |       {-6.354, -2.1},    {-4.548, 3.29},  {-2.732, -5.4},    {-0.911, -0.01},  {0.911, 4.5},
 68 |       {2.732, -4.19},    {4.548, 1.2},    {6.354, -13.565},  {-6.354, -1.22},  {-4.548, 4.17},
 69 |       {-2.732, -4.52},   {-0.911, 0.87},  {0.911, 6.08},     {2.732, -3.31},   {4.548, 2.08},
 70 |       {6.354, -6.65},    {-6.354, 1.42},  {-4.548, -10.346}, {-2.732, -1.88},  {-0.911, 3.51},
 71 |       {0.911, -6.06},    {2.732, -0.67},  {4.548, 4.72},     {6.354, -3.97},   {-6.354, 2.3},
 72 |       {-4.548, -6.39},   {-2.732, -1},    {-0.911, 4.39},    {0.911, -5.18},   {2.732, 0.21},
 73 |       {4.548, 6.98},     {6.354, -3.09},  {-6.354, 4.98},    {-4.548, -3.75},  {-2.732, 1.64},
 74 |       {-0.911, -8.352},  {0.911, -2.54},  {2.732, 2.85},     {4.548, -5.84},   {6.354, -0.45},
 75 |       {-6.354, 8.43},    {-4.548, -2.87}, {-2.732, 2.52},    {-0.911, -6.17},  {0.911, -1.66},
 76 |       {2.732, 3.73},     {4.548, -4.96},  {6.354, 0.43},
 77 |   };
 78 |   raw_calib_data[ModelId::HDL32E] = {
 79 |       {0, -30.67}, {0, -9.33},  {0, -29.33}, {0, -8},     {0, -28},    {0, -6.67},  {0, -26.67},
 80 |       {0, -5.33},  {0, -25.33}, {0, -4},     {0, -24},    {0, -2.67},  {0, -22.67}, {0, -1.33},
 81 |       {0, -21.33}, {0, 0},      {0, -20},    {0, 1.33},   {0, -18.67}, {0, 2.67},   {0, -17.33},
 82 |       {0, 4},      {0, -16},    {0, 5.33},   {0, -14.67}, {0, 6.67},   {0, -13.33}, {0, 8},
 83 |       {0, -12},    {0, 9.33},   {0, -10.67}, {0, 10.67},
 84 |   };
 85 |   raw_calib_data[ModelId::PuckHiRes] = {
 86 |       {0, -10}, {0, 0.667}, {0, -8.667}, {0, 2},  {0, -7.333}, {0, 3.333},
 87 |       {0, -6},  {0, 4.667}, {0, -4.667}, {0, 6},  {0, -3.333}, {0, 7.333},
 88 |       {0, -2},  {0, 8.667}, {0, -0.667}, {0, 10},
 89 |   };
 90 |   raw_calib_data[ModelId::VLP16] = {
 91 |       {0, -15}, {0, 1}, {0, -13}, {0, 3},  {0, -11}, {0, 5},  {0, -9}, {0, 7},
 92 |       {0, -7},  {0, 9}, {0, -5},  {0, 11}, {0, -3},  {0, 13}, {0, -1}, {0, 15},
 93 |   };
 94 |   raw_calib_data[ModelId::VLP32A] = {
 95 |       {-1.2, -14.3},  {1.2, 0.8337}, {-1.2, 12.5}, {1.2, -0.5},  {-1.2, -10.7},  {1.2, 2.167},
 96 |       {-1.2, 8.9},    {1.2, -1.834}, {-1.2, -7},   {3.6, 1.167}, {-1.2, 6},      {3.6, -0.167},
 97 |       {-1.2, -4.9},   {3.6, 2.5},    {-1.2, 3.8},  {3.6, -1.5},  {-1.2, -2.167}, {1.2, 10.7},
 98 |       {-1.2, 1.834},  {1.2, -8.9},   {-3.6, -2.5}, {1.2, 4.9},   {-3.6, 1.5},    {1.2, -3.8},
 99 |       {-1.2, -0.834}, {1.2, 14.3},   {-1.2, 0.5},  {1.2, -12.5}, {-3.6, -1.167}, {1.2, 7},
100 |       {-3.6, 0.167},  {1.2, -6},
101 |   };
102 |   raw_calib_data[ModelId::VLP32C] = {
103 |       {-1.4, -25},    {4.2, -1},     {-1.4, -1.667}, {1.4, -15.639}, {-1.4, -11.31}, {1.4, 0},
104 |       {-4.2, -0.667}, {1.4, -8.843}, {-1.4, -7.254}, {4.2, 0.333},   {-1.4, -0.333}, {1.4, -6.148},
105 |       {-4.2, -5.333}, {1.4, 1.333},  {-4.2, 0.667},  {1.4, -4},      {-1.4, -4.667}, {4.2, 1.667},
106 |       {-1.4, 1},      {4.2, -3.667}, {-4.2, -3.333}, {1.4, 3.333},   {-1.4, 2.333},  {1.4, -2.667},
107 |       {-1.4, -3},     {1.4, 7},      {-1.4, 4.667},  {4.2, -2.333},  {-4.2, -2},     {1.4, 15},
108 |       {-1.4, 10.333}, {1.4, -1.333},
109 |   };
110 | 
111 |   for (const auto &[model, data] : raw_calib_data) {
112 |     std::vector<LaserCorrection> laser_corrections(data.size());
113 |     for (size_t i = 0; i < data.size(); i++) {
114 |       auto &corr           = laser_corrections[i];
115 |       corr.rot_correction  = (float)(data[i].rot_corr_deg * M_PI / 180.0);
116 |       corr.vert_correction = (float)(data[i].vert_corr_deg * M_PI / 180.0);
117 |       // The vertical offset correction is not provided for some of the models,
118 |       // but it can be approximated from the beam angle and focal distance.
119 |       // It will still be more accurate than not correcting for the offset at all.
120 |       corr.vert_offset_correction = focal_distances[model] * std::tan(-corr.vert_correction);
121 |       corr.laser_idx              = i;
122 |     }
123 |     // Apply pre-defined vertical offset corrections, if available.
124 |     if (vertical_offsets.find(model) != vertical_offsets.end()) {
125 |       const auto &offsets = vertical_offsets[model];
126 |       for (size_t i = 0; i < data.size(); i++) {
127 |         laser_corrections[i].vert_offset_correction = offsets[i];
128 |       }
129 |     }
130 |     calibrations_.emplace(model, Calibration{laser_corrections, resolutions[model]});
131 |   }
132 |   // VLP-32B is identical to VLP-32C
133 |   calibrations_.emplace(ModelId::VLP32B, calibrations_[ModelId::VLP32C]);
134 | }
135 | 
136 | Calibration CalibDB::getDefaultCalibration(ModelId model_id) const {
137 |   auto it = calibrations_.find(model_id);
138 |   if (it == calibrations_.end()) {
139 |     throw std::runtime_error("No calibration found for model " + std::to_string((int)model_id));
140 |   }
141 |   return it->second;
142 | }
143 | 
144 | std::vector<ModelId> CalibDB::getAvailableModels() const {
145 |   std::vector<ModelId> models;
146 |   for (const auto &[model, calib] : calibrations_) {
147 |     models.push_back(model);
148 |   }
149 |   return models;
150 | }
151 | 
152 | } // namespace velodyne_decoder
153 | 


--------------------------------------------------------------------------------
/src/velodyne_decoder/calibrations/Alpha Prime.yml:
--------------------------------------------------------------------------------
  1 | # aka VLS-128
  2 | # converted from https://gitlab.kitware.com/LidarView/velodyneplugin/-/tree/0cddc1f92e69b7b26ffc5619aceb1c00854442e6/CalibrationsFiles
  3 | # using https://wiki.ros.org/velodyne_pointcloud#gen_calibration.py
  4 | distance_resolution: 0.004
  5 | lasers:
  6 |   - laser_id: 0
  7 |     rot_correction: -0.1108982
  8 |     vert_correction: -0.2049366
  9 |   - laser_id: 1
 10 |     rot_correction: -0.07937758
 11 |     vert_correction: -0.03473205
 12 |   - laser_id: 2
 13 |     rot_correction: -0.04768239
 14 |     vert_correction: 0.0593412
 15 |   - laser_id: 3
 16 |     rot_correction: -0.01589995
 17 |     vert_correction: -0.09232792
 18 |   - laser_id: 4
 19 |     rot_correction: 0.01589995
 20 |     vert_correction: -0.01361357
 21 |   - laser_id: 5
 22 |     rot_correction: 0.04768239
 23 |     vert_correction: 0.08045968
 24 |   - laser_id: 6
 25 |     rot_correction: 0.07937758
 26 |     vert_correction: -0.07120943
 27 |   - laser_id: 7
 28 |     rot_correction: 0.1108982
 29 |     vert_correction: 0.02286381
 30 |   - laser_id: 8
 31 |     rot_correction: -0.1108982
 32 |     vert_correction: -0.1134464
 33 |   - laser_id: 9
 34 |     rot_correction: -0.07937758
 35 |     vert_correction: -0.01937315
 36 |   - laser_id: 10
 37 |     rot_correction: -0.04768239
 38 |     vert_correction: 0.07470009
 39 |   - laser_id: 11
 40 |     rot_correction: -0.01589995
 41 |     vert_correction: -0.07696902
 42 |   - laser_id: 12
 43 |     rot_correction: 0.01589995
 44 |     vert_correction: 0.001745329
 45 |   - laser_id: 13
 46 |     rot_correction: 0.04768239
 47 |     vert_correction: 0.1130973
 48 |   - laser_id: 14
 49 |     rot_correction: 0.07937758
 50 |     vert_correction: -0.05585054
 51 |   - laser_id: 15
 52 |     rot_correction: 0.1108982
 53 |     vert_correction: 0.03822271
 54 |   - laser_id: 16
 55 |     rot_correction: -0.1108982
 56 |     vert_correction: -0.06736971
 57 |   - laser_id: 17
 58 |     rot_correction: -0.07937758
 59 |     vert_correction: 0.02670354
 60 |   - laser_id: 18
 61 |     rot_correction: -0.04768239
 62 |     vert_correction: -0.1613382
 63 |   - laser_id: 19
 64 |     rot_correction: -0.01589995
 65 |     vert_correction: -0.03089233
 66 |   - laser_id: 20
 67 |     rot_correction: 0.01589995
 68 |     vert_correction: 0.04782202
 69 |   - laser_id: 21
 70 |     rot_correction: 0.04768239
 71 |     vert_correction: -0.1038471
 72 |   - laser_id: 22
 73 |     rot_correction: 0.07937758
 74 |     vert_correction: -0.009773844
 75 |   - laser_id: 23
 76 |     rot_correction: 0.1108982
 77 |     vert_correction: 0.0842994
 78 |   - laser_id: 24
 79 |     rot_correction: -0.1108982
 80 |     vert_correction: -0.05201081
 81 |   - laser_id: 25
 82 |     rot_correction: -0.07937758
 83 |     vert_correction: 0.04206244
 84 |   - laser_id: 26
 85 |     rot_correction: -0.04768239
 86 |     vert_correction: -0.1096067
 87 |   - laser_id: 27
 88 |     rot_correction: -0.01589995
 89 |     vert_correction: -0.01553343
 90 |   - laser_id: 28
 91 |     rot_correction: 0.01589995
 92 |     vert_correction: 0.06318092
 93 |   - laser_id: 29
 94 |     rot_correction: 0.04768239
 95 |     vert_correction: -0.08848819
 96 |   - laser_id: 30
 97 |     rot_correction: 0.07937758
 98 |     vert_correction: 0.005585053
 99 |   - laser_id: 31
100 |     rot_correction: 0.1108982
101 |     vert_correction: 0.132296
102 |   - laser_id: 32
103 |     rot_correction: -0.1108982
104 |     vert_correction: -0.005934119
105 |   - laser_id: 33
106 |     rot_correction: -0.07937758
107 |     vert_correction: 0.09040806
108 |   - laser_id: 34
109 |     rot_correction: -0.04768239
110 |     vert_correction: -0.06352998
111 |   - laser_id: 35
112 |     rot_correction: -0.01589995
113 |     vert_correction: 0.03054326
114 |   - laser_id: 36
115 |     rot_correction: 0.01589995
116 |     vert_correction: -0.4363323
117 |   - laser_id: 37
118 |     rot_correction: 0.04768239
119 |     vert_correction: -0.0424115
120 |   - laser_id: 38
121 |     rot_correction: 0.07937758
122 |     vert_correction: 0.05166174
123 |   - laser_id: 39
124 |     rot_correction: 0.1108982
125 |     vert_correction: -0.1000074
126 |   - laser_id: 40
127 |     rot_correction: -0.1108982
128 |     vert_correction: 0.009424778
129 |   - laser_id: 41
130 |     rot_correction: -0.07937758
131 |     vert_correction: 0.1692969
132 |   - laser_id: 42
133 |     rot_correction: -0.04768239
134 |     vert_correction: -0.04817109
135 |   - laser_id: 43
136 |     rot_correction: -0.01589995
137 |     vert_correction: 0.04590216
138 |   - laser_id: 44
139 |     rot_correction: 0.01589995
140 |     vert_correction: -0.1335177
141 |   - laser_id: 45
142 |     rot_correction: 0.04768239
143 |     vert_correction: -0.0270526
144 |   - laser_id: 46
145 |     rot_correction: 0.07937758
146 |     vert_correction: 0.06702064
147 |   - laser_id: 47
148 |     rot_correction: 0.1108982
149 |     vert_correction: -0.08464847
150 |   - laser_id: 48
151 |     rot_correction: -0.1108982
152 |     vert_correction: 0.05550147
153 |   - laser_id: 49
154 |     rot_correction: -0.07937758
155 |     vert_correction: -0.09616764
156 |   - laser_id: 50
157 |     rot_correction: -0.04768239
158 |     vert_correction: -0.002094395
159 |   - laser_id: 51
160 |     rot_correction: -0.01589995
161 |     vert_correction: 0.1000074
162 |   - laser_id: 52
163 |     rot_correction: 0.01589995
164 |     vert_correction: -0.07504915
165 |   - laser_id: 53
166 |     rot_correction: 0.04768239
167 |     vert_correction: 0.01902409
168 |   - laser_id: 54
169 |     rot_correction: 0.07937758
170 |     vert_correction: -0.2799857
171 |   - laser_id: 55
172 |     rot_correction: 0.1108982
173 |     vert_correction: -0.03857177
174 |   - laser_id: 56
175 |     rot_correction: -0.1108982
176 |     vert_correction: 0.07086037
177 |   - laser_id: 57
178 |     rot_correction: -0.07937758
179 |     vert_correction: -0.08080874
180 |   - laser_id: 58
181 |     rot_correction: -0.04768239
182 |     vert_correction: 0.0132645
183 |   - laser_id: 59
184 |     rot_correction: -0.01589995
185 |     vert_correction: 0.2617994
186 |   - laser_id: 60
187 |     rot_correction: 0.01589995
188 |     vert_correction: -0.05969026
189 |   - laser_id: 61
190 |     rot_correction: 0.04768239
191 |     vert_correction: 0.03438299
192 |   - laser_id: 62
193 |     rot_correction: 0.07937758
194 |     vert_correction: -0.1195551
195 |   - laser_id: 63
196 |     rot_correction: 0.1108982
197 |     vert_correction: -0.02321288
198 |   - laser_id: 64
199 |     rot_correction: -0.1108982
200 |     vert_correction: -0.0980875
201 |   - laser_id: 65
202 |     rot_correction: -0.07937758
203 |     vert_correction: -0.004014257
204 |   - laser_id: 66
205 |     rot_correction: -0.04768239
206 |     vert_correction: 0.09477138
207 |   - laser_id: 67
208 |     rot_correction: -0.01589995
209 |     vert_correction: -0.06161012
210 |   - laser_id: 68
211 |     rot_correction: 0.01589995
212 |     vert_correction: 0.01710423
213 |   - laser_id: 69
214 |     rot_correction: 0.04768239
215 |     vert_correction: -0.3417704
216 |   - laser_id: 70
217 |     rot_correction: 0.07937758
218 |     vert_correction: -0.04049164
219 |   - laser_id: 71
220 |     rot_correction: 0.1108982
221 |     vert_correction: 0.05358161
222 |   - laser_id: 72
223 |     rot_correction: -0.1108982
224 |     vert_correction: -0.08272861
225 |   - laser_id: 73
226 |     rot_correction: -0.07937758
227 |     vert_correction: 0.01134464
228 |   - laser_id: 74
229 |     rot_correction: -0.04768239
230 |     vert_correction: 0.2050762
231 |   - laser_id: 75
232 |     rot_correction: -0.01589995
233 |     vert_correction: -0.04625123
234 |   - laser_id: 76
235 |     rot_correction: 0.01589995
236 |     vert_correction: 0.03246313
237 |   - laser_id: 77
238 |     rot_correction: 0.04768239
239 |     vert_correction: -0.124791
240 |   - laser_id: 78
241 |     rot_correction: 0.07937758
242 |     vert_correction: -0.02513274
243 |   - laser_id: 79
244 |     rot_correction: 0.1108982
245 |     vert_correction: 0.06894051
246 |   - laser_id: 80
247 |     rot_correction: -0.1108982
248 |     vert_correction: -0.03665191
249 |   - laser_id: 81
250 |     rot_correction: -0.07937758
251 |     vert_correction: 0.05742133
252 |   - laser_id: 82
253 |     rot_correction: -0.04768239
254 |     vert_correction: -0.09424778
255 |   - laser_id: 83
256 |     rot_correction: -0.01589995
257 |     vert_correction: -0.0001745329
258 |   - laser_id: 84
259 |     rot_correction: 0.01589995
260 |     vert_correction: 0.07853982
261 |   - laser_id: 85
262 |     rot_correction: 0.04768239
263 |     vert_correction: -0.0731293
264 |   - laser_id: 86
265 |     rot_correction: 0.07937758
266 |     vert_correction: 0.02094395
267 |   - laser_id: 87
268 |     rot_correction: 0.1108982
269 |     vert_correction: -0.2367539
270 |   - laser_id: 88
271 |     rot_correction: -0.1108982
272 |     vert_correction: -0.02129302
273 |   - laser_id: 89
274 |     rot_correction: -0.07937758
275 |     vert_correction: 0.07278023
276 |   - laser_id: 90
277 |     rot_correction: -0.04768239
278 |     vert_correction: -0.07888889
279 |   - laser_id: 91
280 |     rot_correction: -0.01589995
281 |     vert_correction: 0.01518436
282 |   - laser_id: 92
283 |     rot_correction: 0.01589995
284 |     vert_correction: 0.106116
285 |   - laser_id: 93
286 |     rot_correction: 0.04768239
287 |     vert_correction: -0.0577704
288 |   - laser_id: 94
289 |     rot_correction: 0.07937758
290 |     vert_correction: 0.03630285
291 |   - laser_id: 95
292 |     rot_correction: 0.1108982
293 |     vert_correction: -0.1160644
294 |   - laser_id: 96
295 |     rot_correction: -0.1108982
296 |     vert_correction: 0.02478367
297 |   - laser_id: 97
298 |     rot_correction: -0.07937758
299 |     vert_correction: -0.1805718
300 |   - laser_id: 98
301 |     rot_correction: -0.04768239
302 |     vert_correction: -0.03281219
303 |   - laser_id: 99
304 |     rot_correction: -0.01589995
305 |     vert_correction: 0.06126106
306 |   - laser_id: 100
307 |     rot_correction: 0.01589995
308 |     vert_correction: -0.105767
309 |   - laser_id: 101
310 |     rot_correction: 0.04768239
311 |     vert_correction: -0.01169371
312 |   - laser_id: 102
313 |     rot_correction: 0.07937758
314 |     vert_correction: 0.08237954
315 |   - laser_id: 103
316 |     rot_correction: 0.1108982
317 |     vert_correction: -0.06928957
318 |   - laser_id: 104
319 |     rot_correction: -0.1108982
320 |     vert_correction: 0.04014257
321 |   - laser_id: 105
322 |     rot_correction: -0.07937758
323 |     vert_correction: -0.1115265
324 |   - laser_id: 106
325 |     rot_correction: -0.04768239
326 |     vert_correction: -0.01745329
327 |   - laser_id: 107
328 |     rot_correction: -0.01589995
329 |     vert_correction: 0.07661995
330 |   - laser_id: 108
331 |     rot_correction: 0.01589995
332 |     vert_correction: -0.09040806
333 |   - laser_id: 109
334 |     rot_correction: 0.04768239
335 |     vert_correction: 0.003665191
336 |   - laser_id: 110
337 |     rot_correction: 0.07937758
338 |     vert_correction: 0.121824
339 |   - laser_id: 111
340 |     rot_correction: 0.1108982
341 |     vert_correction: -0.05393067
342 |   - laser_id: 112
343 |     rot_correction: -0.1108982
344 |     vert_correction: 0.0869174
345 |   - laser_id: 113
346 |     rot_correction: -0.07937758
347 |     vert_correction: -0.06544985
348 |   - laser_id: 114
349 |     rot_correction: -0.04768239
350 |     vert_correction: 0.0286234
351 |   - laser_id: 115
352 |     rot_correction: -0.01589995
353 |     vert_correction: -0.1457699
354 |   - laser_id: 116
355 |     rot_correction: 0.01589995
356 |     vert_correction: -0.04433136
357 |   - laser_id: 117
358 |     rot_correction: 0.04768239
359 |     vert_correction: 0.04974188
360 |   - laser_id: 118
361 |     rot_correction: 0.07937758
362 |     vert_correction: -0.1019272
363 |   - laser_id: 119
364 |     rot_correction: 0.1108982
365 |     vert_correction: -0.007853982
366 |   - laser_id: 120
367 |     rot_correction: -0.1108982
368 |     vert_correction: 0.1471312
369 |   - laser_id: 121
370 |     rot_correction: -0.07937758
371 |     vert_correction: -0.05009095
372 |   - laser_id: 122
373 |     rot_correction: -0.04768239
374 |     vert_correction: 0.0439823
375 |   - laser_id: 123
376 |     rot_correction: -0.01589995
377 |     vert_correction: -0.1076868
378 |   - laser_id: 124
379 |     rot_correction: 0.01589995
380 |     vert_correction: -0.02897247
381 |   - laser_id: 125
382 |     rot_correction: 0.04768239
383 |     vert_correction: 0.06510078
384 |   - laser_id: 126
385 |     rot_correction: 0.07937758
386 |     vert_correction: -0.08656833
387 |   - laser_id: 127
388 |     rot_correction: 0.1108982
389 |     vert_correction: 0.007504916
390 | num_lasers: 128


--------------------------------------------------------------------------------
/src/velodyne_decoder/hdl64e.py:
--------------------------------------------------------------------------------
  1 | # Copyright (c) 2021-2023, Martin Valgur
  2 | # SPDX-License-Identifier: BSD-3-Clause
  3 | 
  4 | """
  5 | Tools to read calibration and status info from HDL-64E S2 and S3 packet streams.
  6 | 
  7 | Usage example:
  8 |     pcap_file = "hdl64e_packets.pcap"
  9 |     model, calib = vd.hdl64e.read_calibration_from_pcap(pcap_file)
 10 |     config = vd.Config(model=model, calibration=calib)
 11 | 
 12 | Saving the calibration to a file (also includes all other parsed metadata under the "status" key):
 13 |     packets = vd.hdl64e.iter_pcap_packets(pcap_file)
 14 |     metadata = vd.hdl64e.read_status_info(packets)
 15 |     calib_dict = vd.hdl64e.status_info_to_calib_dict(metadata)
 16 |     with open("calib.yml", "w") as f:
 17 |         f.write(yaml.dump(calib_dict, sort_keys=False))
 18 | 
 19 | Using the saved calibration:
 20 |     calib = vd.Calibration.read("calib.yml")
 21 |     config = vd.Config(model=vd.Model.HDL64E_S3, calibration=calib)
 22 | 
 23 | YAML output example:
 24 |   distance_resolution: 0.002
 25 |   num_lasers: 64
 26 |   lasers:
 27 |   - laser_id: 0
 28 |     ...
 29 |   - laser_id: 63
 30 |     vert_correction: -0.1534144
 31 |     rot_correction: 0.0483456
 32 |     dist_correction: 1.228
 33 |     dist_correction_x: 1.258
 34 |     dist_correction_y: 1.258
 35 |     vert_offset_correction: 0.144
 36 |     horiz_offset_correction: -0.026
 37 |     focal_distance: 0.25
 38 |     focal_slope: 0.011
 39 |     min_intensity: 40
 40 |     max_intensity: 255
 41 |   status:
 42 |     Model: S3
 43 |     Current Time: 2000-01-01 00:05:54
 44 |     GPS Status: 0
 45 |     Temperature: 20
 46 |     Firmware Version: 227
 47 |     Upper Block Noise Threshold: 24
 48 |     Lower Block Noise Threshold: 24
 49 |     Calibration Time: 2016-11-15 12:47:47
 50 |     Humidity %: 25
 51 |     RPM: 300
 52 |     FOV Min: 0
 53 |     FOV Max: 36000
 54 |     Real Life Hour: 2685
 55 |     IP Source: 192.168.3.43
 56 |     IP Destination: 192.168.3.255
 57 |     Multiple Return Status: Strongest
 58 |     Power Level Status: A0
 59 |     Lens Contaminated: true
 60 |     Unit Too Hot Internally: false
 61 |     Unit Too Cold Internally: false
 62 |     PPS Signal Present: false
 63 |     GPS Time Present: false
 64 | """
 65 | 
 66 | from __future__ import print_function
 67 | 
 68 | import argparse
 69 | import datetime
 70 | import math
 71 | import struct
 72 | import sys
 73 | import warnings
 74 | 
 75 | import numpy as np
 76 | import yaml
 77 | 
 78 | import velodyne_decoder as vd
 79 | 
 80 | __all__ = [
 81 |     "iter_pcap_packets",
 82 |     "iter_bag_packets",
 83 |     "read_calibration_from_pcap",
 84 |     "status_info_to_calib_dict",
 85 |     "read_status_info",
 86 | ]
 87 | 
 88 | 
 89 | def crc16(data):
 90 |     xor_in = 0x0000  # initial value
 91 |     xor_out = 0x0000  # final XOR value
 92 |     poly = 0x8005  # generator polynomial (normal form)
 93 |     reg = xor_in
 94 |     for octet in data:
 95 |         for i in range(8):
 96 |             topbit = reg & 0x8000
 97 |             if octet & (0x80 >> i):
 98 |                 topbit ^= 0x8000
 99 |             reg <<= 1
100 |             if topbit:
101 |                 reg ^= poly
102 |         reg &= 0xFFFF
103 |     return reg ^ xor_out
104 | 
105 | 
106 | def chunks(lst, n):
107 |     """Yield successive n-sized chunks from lst."""
108 |     for i in range(0, len(lst), n):
109 |         yield lst[i : i + n]
110 | 
111 | 
112 | def iter_pcap_packets(pcap_path):
113 |     """Yields raw Velodyne packets from a pcap file."""
114 |     for stamp, data in vd.util.iter_pcap(pcap_path):
115 |         if len(data) == vd.PACKET_SIZE:
116 |             yield data
117 | 
118 | 
119 | def iter_bag_packets(bag_path, topic=None):
120 |     """Yields raw Velodyne packets from a given topic in a bag file."""
121 |     for topic, scan_msg, ros_time in vd.util.iter_bag(bag_path, [topic]):
122 |         for packet in scan_msg.packets:
123 |             yield packet.data
124 | 
125 | 
126 | def read_status_info(raw_packets):
127 |     cur_status = {}
128 |     CALIB_DATA_SIZE = 1820
129 |     values = bytearray(CALIB_DATA_SIZE)
130 |     attempts = 0
131 |     byte_count = 0
132 |     for packet_data in raw_packets:
133 |         stamp, status_type, status_value = struct.unpack("<LBB", packet_data[-6:])
134 |         if chr(status_type) in "HMSDNYGTVW":
135 |             cur_status[chr(status_type)] = status_value
136 |             if chr(status_type) != "W":
137 |                 continue
138 |         values[byte_count] = status_value
139 |         byte_count += 1
140 |         if byte_count == CALIB_DATA_SIZE:
141 |             attempts += 1
142 |             start = values.find(b"UNIT#")
143 |             if start < 0:
144 |                 warnings.warn("Did not find a 'UNIT#' start token in the status data")
145 |                 byte_count = 0
146 |                 continue
147 |             values = values[start:] + values[:start]
148 |             expected_checksum = struct.unpack("<H", values[-2:])[0]
149 |             # S3 uses a checksum, S2 sets only the expected length of data as the last bytes
150 |             is_s3 = "W" in cur_status
151 |             if is_s3:
152 |                 calib_data = b"".join(x[1:21] for x in chunks(values[7 : -3 * 7], 28))
153 |                 checksum = crc16(calib_data)
154 |             else:
155 |                 checksum = CALIB_DATA_SIZE
156 |             if expected_checksum == checksum:
157 |                 return decode_status_bytes(values, cur_status)
158 |             else:
159 |                 warnings.warn(
160 |                     "Checksum validation of calibration data failed: "
161 |                     "calculated {:X} != expected {:X}.".format(checksum, expected_checksum)
162 |                 )
163 |                 byte_count = CALIB_DATA_SIZE - start if start > 0 else 0
164 |                 continue
165 |     if attempts == 0:
166 |         raise ValueError(
167 |             "Not enough metadata bytes in stream: {} < {}".format(byte_count, CALIB_DATA_SIZE)
168 |         )
169 |     else:
170 |         raise ValueError("No valid metadata found in stream")
171 | 
172 | 
173 | def decode_status_bytes(status_bytes, cur_status):
174 |     is_s3 = "W" in cur_status
175 |     tail = status_bytes[-4 * 7 :]
176 |     data = {}
177 |     data["Model"] = "S3" if is_s3 else "S2"
178 |     data["Current Time"] = datetime.datetime(
179 |         2000 + cur_status["Y"],
180 |         cur_status["N"],
181 |         cur_status["D"],
182 |         cur_status["H"],
183 |         cur_status["M"],
184 |         cur_status["S"],
185 |     )
186 |     data["GPS Status"] = cur_status["G"]
187 |     data["Temperature"] = cur_status["T"]
188 |     data["Firmware Version"] = cur_status["V"]
189 |     data["Upper Block Noise Threshold"] = status_bytes[5]
190 |     data["Lower Block Noise Threshold"] = status_bytes[6]
191 |     if all(x != 0xFF for x in tail[:6]):
192 |         data["Calibration Time"] = datetime.datetime(tail[0] + 2000, *tail[1:6])
193 |     else:
194 |         data["Calibration Time"] = None
195 |     data["Humidity %"] = tail[6]
196 |     data["RPM"] = struct.unpack_from("<h", tail, 7)[0]
197 |     data["FOV Min"] = struct.unpack_from("<H", tail, 9)[0]
198 |     data["FOV Max"] = struct.unpack_from("<H", tail, 11)[0]
199 |     data["Real Life Hour"] = struct.unpack_from("<H", tail, 13)[0]
200 |     data["IP Source"] = ".".join(map(str, struct.unpack_from("<BBBB", tail, 15)))
201 |     data["IP Destination"] = ".".join(map(str, struct.unpack_from("<BBBB", tail, 19)))
202 |     data["Multiple Return Status"] = {0: "Strongest", 1: "Last", 2: "Both"}.get(tail[23], tail[23])
203 |     data["Power Level Status"] = hex(tail[25])[2:].upper()
204 |     w = cur_status["W"] if is_s3 else tail[24]
205 |     data["Lens Contaminated"] = bool(w & (1 << 0))
206 |     data["Unit Too Hot Internally"] = bool(w & (1 << 1))
207 |     data["Unit Too Cold Internally"] = bool(w & (1 << 2))
208 |     if not is_s3:
209 |         data["Voltage Too Low"] = bool(w & (1 << 3))
210 |         data["Voltage Too High"] = bool(w & (1 << 4))
211 |     data["PPS Signal Present"] = bool(w & (1 << 5))
212 |     data["GPS Time Present"] = bool(w & (1 << 6))
213 | 
214 |     data["Calibration"] = []
215 |     raw_lasers_data = [x[:21] for x in chunks(status_bytes[7 : -3 * 7], 28)]
216 |     for raw_laser_data in raw_lasers_data:
217 |         raw_values = struct.unpack("<BhhhhhhhhhBB", raw_laser_data)
218 |         c = {}
219 |         c["Laser ID"] = raw_values[0]
220 |         c["Vertical Correction"] = raw_values[1] * 0.01  # deg
221 |         c["Rotational Correction"] = raw_values[2] * 0.01  # deg
222 |         c["Far Distance Correction"] = raw_values[3] * 0.001  # m
223 |         c["Distance Correction X"] = raw_values[4] * 0.001  # m
224 |         c["Distance Correction Y"] = raw_values[5] * 0.001  # m
225 |         c["Vertical Offset Correction"] = raw_values[6] * 0.001  # m
226 |         c["Horizontal Offset Correction"] = raw_values[7] * 0.001  # m
227 |         c["Focal Distance"] = raw_values[8] * 0.001  # m
228 |         c["Focal Slope"] = raw_values[9] * 0.1
229 |         c["Min Intensity"] = raw_values[10]
230 |         c["Max Intensity"] = raw_values[11]
231 |         data["Calibration"].append(c)
232 |     return data
233 | 
234 | 
235 | def status_info_to_calib_dict(metadata):
236 |     calib = {}
237 |     calib["distance_resolution"] = 0.002
238 |     calib["num_lasers"] = 64
239 |     calib["lasers"] = []
240 |     for c in metadata["Calibration"]:
241 |         l = {}
242 |         l["laser_id"] = c["Laser ID"]
243 |         l["vert_correction"] = float(np.round(math.radians(c["Vertical Correction"]), 7))
244 |         l["rot_correction"] = float(np.round(math.radians(c["Rotational Correction"]), 7))
245 |         l["dist_correction"] = float(np.round(c["Far Distance Correction"], 7))
246 |         l["dist_correction_x"] = float(np.round(c["Distance Correction X"], 7))
247 |         l["dist_correction_y"] = float(np.round(c["Distance Correction Y"], 7))
248 |         l["vert_offset_correction"] = float(np.round(c["Vertical Offset Correction"], 7))
249 |         l["horiz_offset_correction"] = float(np.round(c["Horizontal Offset Correction"], 7))
250 |         l["focal_distance"] = float(np.round(c["Focal Distance"], 7))
251 |         l["focal_slope"] = float(np.round(c["Focal Slope"], 7))
252 |         l["min_intensity"] = c["Min Intensity"]
253 |         l["max_intensity"] = c["Max Intensity"]
254 |         calib["lasers"].append(l)
255 |     calib["status"] = {k: v for k, v in metadata.items() if k != "Calibration"}
256 |     return calib
257 | 
258 | 
259 | def _read_calib_dict_from_pcap(pcap_file):
260 |     packets = iter_pcap_packets(pcap_file)
261 |     metadata = read_status_info(packets)
262 |     calib_dict = status_info_to_calib_dict(metadata)
263 |     err = None
264 |     if calib_dict["status"]["Calibration Time"] is None:
265 |         err = "Calibration info not available in the metadata"
266 |     elif [l["laser_id"] for l in calib_dict["lasers"]] != list(range(64)):
267 |         err = "Invalid calibration data"
268 |     return err, calib_dict
269 | 
270 | 
271 | def read_calibration_from_pcap(pcap_file):
272 |     packets = iter_pcap_packets(pcap_file)
273 |     # The last two bytes (return mode and model ID) are constant for newer models,
274 |     # but vary for HDL-64E.
275 |     factory_bytes = [data[-2:] for _, data in zip(range(5), packets)]
276 |     if len(factory_bytes) < 5 or all(b == factory_bytes[0] for b in factory_bytes):
277 |         return None, None
278 |     if hasattr(pcap_file, "seek"):
279 |         pcap_file.seek(0)
280 |     err, calib_dict = _read_calib_dict_from_pcap(pcap_file)
281 |     if err is not None:
282 |         raise ValueError(err)
283 |     model = vd.Model.HDL64E_S2 if calib_dict["status"]["Model"] == "S2" else vd.Model.HDL64E_S3
284 |     return model, vd.Calibration.from_string(yaml.dump(calib_dict))
285 | 
286 | 
287 | def detect_hdl64e(pcap_file):
288 |     packets = iter_pcap_packets(pcap_file)
289 |     # The last two bytes (return mode and model ID) are constant for newer models,
290 |     # but vary for HDL-64E.
291 |     factory_bytes = [data[-2:] for _, data in zip(range(5), packets)]
292 |     if len(factory_bytes) < 5 or all(b == factory_bytes[0] for b in factory_bytes):
293 |         return None, None
294 |     if hasattr(pcap_file, "seek"):
295 |         pcap_file.seek(0)
296 |     return read_calibration_from_pcap(pcap_file)
297 | 
298 | 
299 | def cli(args=None):
300 |     parser = argparse.ArgumentParser(description="Extract HDL-64E calibration from a .pcap file")
301 |     parser.add_argument("pcap_file", help=".pcap file to read")
302 |     args = parser.parse_args(args)
303 |     err, calib_dict = _read_calib_dict_from_pcap(args.pcap_file)
304 |     yaml.dump(calib_dict, sys.stdout, sort_keys=False)
305 |     if err is not None:
306 |         print("Warning: {}".format(err), file=sys.stderr)
307 | 


--------------------------------------------------------------------------------
/src/python.cpp:
--------------------------------------------------------------------------------
  1 | // Copyright (c) 2021-2023, Martin Valgur
  2 | // SPDX-License-Identifier: BSD-3-Clause
  3 | 
  4 | #include <iomanip>
  5 | #include <sstream>
  6 | #include <string>
  7 | #include <utility>
  8 | 
  9 | #include <nanobind/nanobind.h>
 10 | #include <nanobind/ndarray.h>
 11 | #include <nanobind/stl/array.h>
 12 | #include <nanobind/stl/bind_vector.h>
 13 | #include <nanobind/stl/optional.h>
 14 | #include <nanobind/stl/string.h>
 15 | #include <nanobind/stl/unordered_map.h>
 16 | #include <nanobind/stl/vector.h>
 17 | 
 18 | #include "velodyne_decoder/config.h"
 19 | #include "velodyne_decoder/scan_decoder.h"
 20 | #include "velodyne_decoder/stream_decoder.h"
 21 | #include "velodyne_decoder/telemetry_packet.h"
 22 | #include "velodyne_decoder/types.h"
 23 | 
 24 | namespace nb = nanobind;
 25 | using namespace nanobind::literals;
 26 | using namespace velodyne_decoder;
 27 | 
 28 | constexpr int ncols   = 8;
 29 | using NumpyPointCloud = nb::ndarray<float, nb::numpy, nb::shape<-1, ncols>, nb::c_contig>;
 30 | 
 31 | nb::dict get_point_dtype() {
 32 |   nb::dict dtype_dict;
 33 |   dtype_dict["names"] =
 34 |       nb::make_tuple("x", "y", "z", "intensity", "time", "column", "ring", "return_type");
 35 |   dtype_dict["formats"]  = nb::make_tuple("<f4", "<f4", "<f4", "<f4", "<f4", "<u2", "u1", "u1");
 36 |   dtype_dict["offsets"]  = nb::make_tuple(0, 4, 8, 12, 16, 20, 22, 23);
 37 |   dtype_dict["itemsize"] = 32;
 38 |   return dtype_dict;
 39 | }
 40 | 
 41 | nb::object as_recarray(PointCloud &&cloud) {
 42 |   const size_t n = cloud.size();
 43 |   auto tmp       = new PointCloud;
 44 |   tmp->swap(cloud);
 45 |   nb::capsule deleter(tmp, [](void *p) noexcept { delete static_cast<PointCloud *>(p); });
 46 |   nb::object arr    = nb::cast(NumpyPointCloud{tmp->data(), {n, ncols}, deleter});
 47 |   arr.attr("dtype") = get_point_dtype();
 48 |   arr.attr("shape") = nb::make_tuple(n);
 49 |   return arr;
 50 | }
 51 | 
 52 | NumpyPointCloud as_contiguous(const PointCloud &cloud) {
 53 |   const size_t n = cloud.size();
 54 |   float *arr     = new float[n * ncols];
 55 |   nb::capsule deleter(arr, [](void *p) noexcept { delete static_cast<float *>(p); });
 56 |   NumpyPointCloud py_arr = {arr, {n, ncols}, deleter};
 57 |   for (size_t i = 0; i < n; i++) {
 58 |     const auto &p = cloud[i];
 59 |     py_arr(i, 0)  = p.x;
 60 |     py_arr(i, 1)  = p.y;
 61 |     py_arr(i, 2)  = p.z;
 62 |     py_arr(i, 3)  = p.intensity;
 63 |     py_arr(i, 4)  = p.time;
 64 |     py_arr(i, 5)  = static_cast<float>(p.column);
 65 |     py_arr(i, 6)  = static_cast<float>(p.ring);
 66 |     py_arr(i, 7)  = static_cast<float>(p.return_type);
 67 |   }
 68 |   return py_arr;
 69 | }
 70 | 
 71 | nb::object convert(PointCloud &&cloud, bool as_pcl_structs) {
 72 |   if (as_pcl_structs) {
 73 |     return as_recarray(std::move(cloud));
 74 |   }
 75 |   return nb::cast(as_contiguous(cloud));
 76 | }
 77 | 
 78 | nb::object to_datetime(Time time) {
 79 |   static auto fromtimestamp =
 80 |       nb::module_::import_("datetime").attr("datetime").attr("fromtimestamp");
 81 |   return fromtimestamp(time);
 82 | }
 83 | 
 84 | nb::object convert_gps_time(std::optional<Time> time) {
 85 |   if (!time)
 86 |     return nb::none();
 87 |   if (*time < 1e5) // a full datetime is unavailable, only time of day
 88 |     return to_datetime(*time).attr("time")();
 89 |   return to_datetime(*time);
 90 | }
 91 | 
 92 | NB_MAKE_OPAQUE(std::vector<VelodynePacket>);
 93 | 
 94 | NB_MODULE(velodyne_decoder_pylib, m) {
 95 |   m.doc() = "";
 96 | 
 97 |   nb::class_<Config>(m, "Config")
 98 |       .def(
 99 |           "__init__",
100 |           [](Config *t, std::optional<ModelId> model, const std::optional<Calibration> &calibration,
101 |              float min_range, float max_range, float min_angle, float max_angle,
102 |              std::optional<float> cut_angle, bool timestamp_first_packet) {
103 |             auto *cfg                   = new (t) Config;
104 |             cfg->model                  = model;
105 |             cfg->calibration            = calibration;
106 |             cfg->min_range              = min_range;
107 |             cfg->max_range              = max_range;
108 |             cfg->min_angle              = min_angle;
109 |             cfg->max_angle              = max_angle;
110 |             cfg->timestamp_first_packet = timestamp_first_packet;
111 |             cfg->cut_angle              = cut_angle;
112 |           },
113 |           nb::kw_only(),                                  //
114 |           nb::arg("model")                  = nb::none(), //
115 |           nb::arg("calibration")            = nb::none(), //
116 |           nb::arg("min_range")              = 0.1,        //
117 |           nb::arg("max_range")              = 200,        //
118 |           nb::arg("min_angle")              = 0,          //
119 |           nb::arg("max_angle")              = 360,        //
120 |           nb::arg("cut_angle")              = nb::none(), //
121 |           nb::arg("timestamp_first_packet") = false       //
122 |           )
123 |       .def_prop_rw(
124 |           "model", [](const Config &cfg) { return cfg.model; },
125 |           [](Config &cfg, std::optional<ModelId> model) { cfg.model = model; }, "model"_a.none())
126 |       .def_prop_rw(
127 |           "calibration", [](const Config &cfg) { return cfg.calibration; },
128 |           [](Config &cfg, std::optional<Calibration> calib) { cfg.calibration = std::move(calib); },
129 |           "calibration"_a.none())
130 |       .def_rw("min_range", &Config::min_range)
131 |       .def_rw("max_range", &Config::max_range)
132 |       .def_rw("min_angle", &Config::min_angle)
133 |       .def_rw("max_angle", &Config::max_angle)
134 |       .def_rw("timestamp_first_packet", &Config::timestamp_first_packet)
135 |       .def_prop_rw(
136 |           "cut_angle", [](const Config &cfg) { return cfg.cut_angle; },
137 |           [](Config &cfg, std::optional<float> cut_angle) { cfg.cut_angle = cut_angle; },
138 |           "cut_angle"_a.none());
139 | 
140 |   nb::class_<TimePair>(m, "TimePair")
141 |       .def(nb::init<>())
142 |       .def(nb::init<Time, Time>())
143 |       .def_rw("host", &TimePair::host)
144 |       .def_rw("device", &TimePair::device)
145 |       .def("__repr__", [](const TimePair &t) {
146 |         return "TimePair(host=" + std::to_string(t.host) + //
147 |                ", device=" + std::to_string(t.device) + ")";
148 |       });
149 | 
150 |   nb::class_<VelodynePacket>(m, "VelodynePacket")
151 |       .def(nb::init<>())
152 |       .def(nb::init<Time, const RawPacketData &>(), nb::arg("host_stamp"), nb::arg("data"))
153 |       .def(nb::init<TimePair, const RawPacketData &>(), nb::arg("stamp"), nb::arg("data"))
154 |       .def_rw("stamp", &VelodynePacket::stamp)
155 |       .def_rw("data", &VelodynePacket::data);
156 | 
157 |   nb::bind_vector<std::vector<VelodynePacket>>(m, "PacketVector");
158 | 
159 |   nb::class_<ScanDecoder>(m, "ScanDecoder")
160 |       .def(nb::init<const Config &>(), nb::arg("config") = Config())
161 |       .def(
162 |           "decode",
163 |           [](ScanDecoder &decoder, const std::vector<VelodynePacket> &scan_packets,
164 |              bool as_pcl_structs) {
165 |             auto [stamp, cloud] = decoder.decode(scan_packets);
166 |             return std::pair{stamp, convert(std::move(cloud), as_pcl_structs)};
167 |           },
168 |           nb::arg("scan_packets"), nb::arg("as_pcl_structs") = false, //
169 |           nb::rv_policy::move)
170 |       .def(
171 |           "decode_message",
172 |           [](ScanDecoder &decoder, const nb::object &scan_msg, bool as_pcl_structs) {
173 |             std::vector<VelodynePacket> packets;
174 |             nb::iterable packets_py = scan_msg.attr("packets");
175 |             for (const auto &packet_py : packets_py) {
176 |               auto packet = nb::cast<RawPacketData>(packet_py.attr("data"));
177 |               auto stamp  = nb::cast<double>(packet_py.attr("stamp").attr("to_sec")());
178 |               packets.emplace_back(stamp, packet);
179 |             }
180 |             auto [stamp, cloud] = decoder.decode(packets);
181 |             return std::pair{stamp, convert(std::move(cloud), as_pcl_structs)};
182 |           },
183 |           nb::arg("scan_msg"), nb::arg("as_pcl_structs") = false, //
184 |           nb::rv_policy::move)
185 |       .def_prop_ro("model_id", &ScanDecoder::modelId,
186 |                    "Detected or configured model ID of the sensor")
187 |       .def_prop_ro("return_mode", &ScanDecoder::returnMode,
188 |                    "The return mode of the sensor based on the last received packet");
189 | 
190 |   nb::class_<StreamDecoder>(m, "StreamDecoder")
191 |       .def(nb::init<const Config &>(), nb::arg("config") = Config())
192 |       .def(
193 |           "decode",
194 |           [](StreamDecoder &decoder, Time host_stamp, const RawPacketData &packet,
195 |              bool as_pcl_structs) -> std::optional<std::pair<TimePair, nb::object>> {
196 |             auto result = decoder.decode({host_stamp, packet});
197 |             if (result) {
198 |               auto &[scan_stamp, cloud] = *result;
199 |               return std::make_pair(scan_stamp, convert(std::move(cloud), as_pcl_structs));
200 |             }
201 |             return std::nullopt;
202 |           },
203 |           nb::arg("stamp"), nb::arg("packet"), nb::arg("as_pcl_structs") = false, //
204 |           nb::rv_policy::move)
205 |       .def(
206 |           "finish",
207 |           [](StreamDecoder &decoder,
208 |              bool as_pcl_structs) -> std::optional<std::pair<TimePair, nb::object>> {
209 |             auto result = decoder.finish();
210 |             if (result) {
211 |               auto &[scan_stamp, cloud] = *result;
212 |               return std::make_pair(scan_stamp, convert(std::move(cloud), as_pcl_structs));
213 |             }
214 |             return std::nullopt;
215 |           },
216 |           nb::arg("as_pcl_structs") = false, nb::rv_policy::move);
217 | 
218 |   nb::class_<Calibration>(m, "Calibration")
219 |       .def_static("read", &Calibration::read)
220 |       .def("write", &Calibration::write)
221 |       .def_static("from_string", &Calibration::fromString)
222 |       .def("to_string", &Calibration::toString)
223 |       .def("__str__", &Calibration::toString)
224 |       .def_prop_ro_static("default_calibs",
225 |                           [](const nb::object &) { return CalibDB().getAllDefaultCalibrations(); });
226 | 
227 |   auto TelemetryPacket_ =
228 |       nb::class_<TelemetryPacket>(m, "TelemetryPacket")
229 |           .def(nb::init<const std::array<uint8_t, TELEMETRY_PACKET_SIZE> &>(), nb::arg("raw_data"))
230 |           .def_ro("temp_board_top", &TelemetryPacket::temp_board_top,
231 |                   "Temperature of top board, 0 to 150 °C")
232 |           .def_ro("temp_board_bottom", &TelemetryPacket::temp_board_bottom,
233 |                   "Temperature of bottom board, 0 to 150 °C")
234 |           .def_ro("temp_during_adc_calibration", &TelemetryPacket::temp_during_adc_calibration,
235 |                   "Temperature when ADC calibration last ran, 0 to 150 °C")
236 |           .def_ro("temp_change_since_adc_calibration",
237 |                   &TelemetryPacket::temp_change_since_adc_calibration,
238 |                   "Change in temperature since last ADC calibration, -150 to 150 °C")
239 |           .def_ro("seconds_since_adc_calibration", &TelemetryPacket::seconds_since_adc_calibration,
240 |                   "Elapsed seconds since last ADC calibration")
241 |           .def_ro("adc_calibration_reason", &TelemetryPacket::adc_calibration_reason,
242 |                   "Reason for the last ADC calibration")
243 |           .def_ro("adc_calib_in_progress", &TelemetryPacket::adc_calib_in_progress,
244 |                   "ADC calibration in progress")
245 |           .def_ro("adc_delta_temp_limit_exceeded", &TelemetryPacket::adc_delta_temp_limit_exceeded,
246 |                   "ADC calibration: delta temperature limit has been met")
247 |           .def_ro("adc_period_exceeded", &TelemetryPacket::adc_period_exceeded,
248 |                   "ADC calibration: periodic time elapsed limit has been met")
249 |           .def_ro("thermal_shutdown", &TelemetryPacket::thermal_shutdown,
250 |                   "Thermal status, true if thermal shutdown")
251 |           .def_ro("temp_at_shutdown", &TelemetryPacket::temp_at_shutdown,
252 |                   "Temperature of unit when thermal shutdown occurred")
253 |           .def_ro("temp_at_powerup", &TelemetryPacket::temp_at_powerup,
254 |                   "Temperature of unit (bottom board) at power up")
255 |           .def_ro("usec_since_toh", &TelemetryPacket::usec_since_toh,
256 |                   "Number of microseconds elapsed since the top of the hour")
257 |           .def_ro("pps_status", &TelemetryPacket::pps_status, "Pulse Per Second (PPS) status")
258 |           .def_ro("nmea_sentence", &TelemetryPacket::nmea_sentence, "GPRMC or GPGGA NMEA sentence")
259 |           .def("parse_nmea", &TelemetryPacket::parseNmea,
260 |                "Parse the NMEA sentence in the packet, if it exists.")
261 |           .def_prop_ro(
262 |               "gps_position",
263 |               [](const TelemetryPacket &packet) -> nb::object {
264 |                 auto nmea_info = packet.parseNmea();
265 |                 if (!nmea_info || !nmea_info->fix_available)
266 |                   return nb::none();
267 |                 return nb::make_tuple(nmea_info->longitude, nmea_info->latitude,
268 |                                       nmea_info->altitude);
269 |               },
270 |               "GPS position (longitude, latitude, altitude) from the NMEA sentence. "
271 |               "None if no NMEA sentence or no fix available.")
272 |           .def_prop_ro(
273 |               "nmea_time",
274 |               [](const TelemetryPacket &packet) { return convert_gps_time(packet.nmeaTime()); },
275 |               "UTC time from the NMEA sentence. datetime.datetime if date is available, "
276 |               "datetime.time otherwise. None if no valid NMEA sentence in packet.")
277 |           .def_prop_ro(
278 |               "pps_time",
279 |               [](const TelemetryPacket &packet) { return convert_gps_time(packet.ppsTime()); },
280 |               "UTC time from the PPS signal + NMEA sentence. datetime.datetime if date is "
281 |               "available in NMEA, datetime.time otherwise. None if no valid PPS or NMEA.")
282 |           .def("__repr__", [](const TelemetryPacket &packet) {
283 |             std::stringstream ss;
284 |             std::string nmea = packet.nmea_sentence;
285 |             if (size_t pos = nmea.find("\r\n"); pos != std::string::npos)
286 |               nmea.replace(pos, 2, "\\r\\n");
287 |             ss << "TelemetryPacket(";
288 |             // Some fields are only set in newer firmware versions. Only print them if they are set.
289 |             if (packet.temp_board_top != 0 || packet.temp_board_bottom != 0 ||
290 |                 packet.temp_during_adc_calibration != 0 ||
291 |                 packet.temp_change_since_adc_calibration != 0 ||
292 |                 packet.seconds_since_adc_calibration != 0 ||
293 |                 (int)packet.adc_calibration_reason != 0 || packet.adc_calib_in_progress != 0 ||
294 |                 packet.adc_delta_temp_limit_exceeded != 0 || packet.adc_period_exceeded != 0 ||
295 |                 packet.thermal_shutdown != 0 || packet.temp_at_shutdown != 0 ||
296 |                 packet.temp_at_powerup != 0) {
297 |               ss << "temp_board_top=" << (int)packet.temp_board_top                             //
298 |                  << ", temp_board_bottom=" << (int)packet.temp_board_bottom                     //
299 |                  << ", temp_during_adc_calibration=" << (int)packet.temp_during_adc_calibration //
300 |                  << ", temp_change_since_adc_calibration="
301 |                  << packet.temp_change_since_adc_calibration                                   //
302 |                  << ", seconds_since_adc_calibration=" << packet.seconds_since_adc_calibration //
303 |                  << ", adc_calibration_reason=" << (int)packet.adc_calibration_reason          //
304 |                  << ", adc_calib_in_progress=" << packet.adc_calib_in_progress                 //
305 |                  << ", adc_delta_temp_limit_exceeded=" << packet.adc_delta_temp_limit_exceeded //
306 |                  << ", adc_period_exceeded=" << packet.adc_period_exceeded                     //
307 |                  << ", thermal_shutdown=" << packet.thermal_shutdown                           //
308 |                  << ", temp_at_shutdown=" << (int)packet.temp_at_shutdown                      //
309 |                  << ", temp_at_powerup=" << (int)packet.temp_at_powerup                        //
310 |                  << ", ";
311 |             }
312 |             ss << "usec_since_toh=" << packet.usec_since_toh //
313 |                << ", pps_status=" << (int)packet.pps_status  //
314 |                << ", nmea_sentence='" << nmea << "')";
315 |             return ss.str();
316 |           });
317 | 
318 |   nb::enum_<TelemetryPacket::AdcCalibReason>(TelemetryPacket_, "AdcCalibrationReason",
319 |                                              "Reason for the last ADC calibration")
320 |       .value("NO_CALIBRATION", TelemetryPacket::AdcCalibReason::NO_CALIBRATION, "No calibration")
321 |       .value("POWER_ON", TelemetryPacket::AdcCalibReason::POWER_ON,
322 |              "Power-on calibration performed")
323 |       .value("MANUAL", TelemetryPacket::AdcCalibReason::MANUAL, "Manual calibration performed")
324 |       .value("DELTA_TEMPERATURE", TelemetryPacket::AdcCalibReason::DELTA_TEMPERATURE,
325 |              "Delta temperature calibration performed")
326 |       .value("PERIODIC", TelemetryPacket::AdcCalibReason::PERIODIC,
327 |              "Periodic calibration performed");
328 | 
329 |   nb::enum_<TelemetryPacket::PpsStatus>(TelemetryPacket_, "PpsStatus",
330 |                                         "Pulse Per Second (PPS) status")
331 |       .value("ABSENT", TelemetryPacket::PpsStatus::ABSENT, "No PPS detected")
332 |       .value("SYNCHRONIZING", TelemetryPacket::PpsStatus::SYNCHRONIZING, "Synchronizing to PPS")
333 |       .value("LOCKED", TelemetryPacket::PpsStatus::LOCKED, "PPS Locked")
334 |       .value("ERROR", TelemetryPacket::PpsStatus::ERROR, "Error");
335 | 
336 |   nb::class_<NmeaInfo>(TelemetryPacket_, "NmeaInfo")
337 |       .def_ro("longitude", &NmeaInfo::longitude, "Longitude in degrees")
338 |       .def_ro("latitude", &NmeaInfo::latitude, "Latitude in degrees")
339 |       .def_ro("altitude", &NmeaInfo::altitude,
340 |               "Altitude above WGS84 ellipsoid in meters (always 0 for GPRMC)")
341 |       .def_ro("utc_year", &NmeaInfo::utc_year, "UTC year (always 0 for GPGGA)")
342 |       .def_ro("utc_month", &NmeaInfo::utc_month, "UTC month (always 0 for GPGGA)")
343 |       .def_ro("utc_day", &NmeaInfo::utc_day, "UTC day (always 0 for GPGGA)")
344 |       .def_ro("utc_hours", &NmeaInfo::utc_hours, "UTC hours")
345 |       .def_ro("utc_minutes", &NmeaInfo::utc_minutes, "UTC minutes")
346 |       .def_ro("utc_seconds", &NmeaInfo::utc_seconds, "UTC seconds")
347 |       .def_ro("fix_available", &NmeaInfo::fix_available, "Position fix available")
348 |       .def("__repr__", [](const NmeaInfo &info) {
349 |         std::ostringstream os;
350 |         os << std::setprecision(9) << std::fixed;
351 |         os << "NmeaInfo(longitude=" << info.longitude                      //
352 |            << ", latitude=" << info.latitude                               //
353 |            << ", altitude=" << info.altitude                               //
354 |            << ", utc_year=" << info.utc_year                               //
355 |            << ", utc_month=" << (int)info.utc_month                        //
356 |            << ", utc_day=" << (int)info.utc_day                            //
357 |            << ", utc_hours=" << (int)info.utc_hours                        //
358 |            << ", utc_minutes=" << (int)info.utc_minutes                    //
359 |            << ", utc_seconds=" << std::setprecision(3) << info.utc_seconds //
360 |            << ", fix_available=" << info.fix_available                     //
361 |            << ")";
362 |         return os.str();
363 |       });
364 | 
365 |   nb::enum_<ModelId>(m, "Model", nb::is_arithmetic())
366 |       .value("HDL64E_S1", ModelId::HDL64E_S1, "HDL-64E S1")
367 |       .value("HDL64E_S2", ModelId::HDL64E_S2, "HDL-64E S2 and S2.1")
368 |       .value("HDL64E_S3", ModelId::HDL64E_S3, "HDL-64E S3")
369 |       .value("HDL32E", ModelId::HDL32E, "HDL-32E")
370 |       .value("VLP32A", ModelId::VLP32A, "VLP-32A")
371 |       .value("VLP32B", ModelId::VLP32B, "VLP-32B")
372 |       .value("VLP32C", ModelId::VLP32C, "VLP-32C")
373 |       .value("VLP16", ModelId::VLP16, "VLP-16")
374 |       .value("PuckLite", ModelId::PuckLite, "Puck Lite (aka VLP-16)")
375 |       .value("PuckHiRes", ModelId::PuckHiRes, "Puck Hi-Res (aka VLP-16 Hi-Res)")
376 |       .value("VLS128", ModelId::VLS128, "VLS-128 (aka Alpha Prime)")
377 |       .value("AlphaPrime", ModelId::AlphaPrime, "Alpha Prime (aka VLS-128)");
378 | 
379 |   nb::enum_<ReturnMode>(m, "ReturnMode",
380 |                         "Flag added to the 'ring' field of a point depending on its return type.")
381 |       .value("STRONGEST", ReturnMode::STRONGEST, "The strongest reflection in a firing")
382 |       .value("LAST", ReturnMode::LAST, "The last reflection in a firing")
383 |       .value("BOTH", ReturnMode::BOTH,
384 |              "Point is both the last and the strongest reflection in a firing");
385 | 
386 |   m.attr("PACKET_SIZE")           = PACKET_SIZE;
387 |   m.attr("TELEMETRY_PACKET_SIZE") = TELEMETRY_PACKET_SIZE;
388 | 
389 | #define STRINGIFY(x) #x
390 | #define MACRO_STRINGIFY(x) STRINGIFY(x)
391 | #ifdef VERSION_INFO
392 |   m.attr("__version__") = MACRO_STRINGIFY(VERSION_INFO);
393 | #else
394 |   m.attr("__version__") = "dev";
395 | #endif
396 | }
397 | 


--------------------------------------------------------------------------------
/src/velodyne_decoder/calibrations/HDL-64E_S1-utexas.yml:
--------------------------------------------------------------------------------
  1 | # Source: https://github.com/ros-drivers/velodyne/blob/master/velodyne_pointcloud/params/64e_utexas.yaml
  2 | # Added by https://github.com/jack-oquin, University of Texas, on 2012-04-09
  3 | distance_resolution: 0.002
  4 | lasers:
  5 |   - dist_correction: 0.100000001490116
  6 |     dist_correction_x: 0
  7 |     dist_correction_y: 0
  8 |     focal_distance: 0
  9 |     focal_slope: 0
 10 |     laser_id: 0
 11 |     max_intensity: 255
 12 |     min_intensity: 0
 13 |     rot_correction: -0.0698131695389748
 14 |     vert_correction: -0.124932751059532
 15 |     vert_offset_correction: 0
 16 |   - dist_correction: 0.280000001192093
 17 |     dist_correction_x: 0
 18 |     dist_correction_y: 0
 19 |     focal_distance: 0
 20 |     focal_slope: 0
 21 |     laser_id: 1
 22 |     max_intensity: 255
 23 |     min_intensity: 0
 24 |     rot_correction: -0.0392699092626572
 25 |     vert_correction: -0.118993431329727
 26 |     vert_offset_correction: 0
 27 |   - dist_correction: 0.319999992847443
 28 |     dist_correction_x: 0
 29 |     dist_correction_y: 0
 30 |     focal_distance: 0
 31 |     focal_slope: 0
 32 |     laser_id: 2
 33 |     max_intensity: 255
 34 |     min_intensity: 0
 35 |     rot_correction: 0.0698131695389748
 36 |     vert_correction: 0.0055470340885222
 37 |     vert_offset_correction: 0
 38 |   - dist_correction: 0.230000004172325
 39 |     dist_correction_x: 0
 40 |     dist_correction_y: 0
 41 |     focal_distance: 0
 42 |     focal_slope: 0
 43 |     laser_id: 3
 44 |     max_intensity: 255
 45 |     min_intensity: 0
 46 |     rot_correction: 0.104719758033752
 47 |     vert_correction: 0.0114863449707627
 48 |     vert_offset_correction: 0
 49 |   - dist_correction: 0.0700000002980232
 50 |     dist_correction_x: 0
 51 |     dist_correction_y: 0
 52 |     focal_distance: 0
 53 |     focal_slope: 0
 54 |     laser_id: 4
 55 |     max_intensity: 255
 56 |     min_intensity: 0
 57 |     rot_correction: 0.013962633907795
 58 |     vert_correction: -0.113056324422359
 59 |     vert_offset_correction: 0
 60 |   - dist_correction: 0.0900000035762787
 61 |     dist_correction_x: 0
 62 |     dist_correction_y: 0
 63 |     focal_distance: 0
 64 |     focal_slope: 0
 65 |     laser_id: 5
 66 |     max_intensity: 255
 67 |     min_intensity: 0
 68 |     rot_correction: 0.0392699092626572
 69 |     vert_correction: -0.107121199369431
 70 |     vert_offset_correction: 0
 71 |   - dist_correction: 0.119999997317791
 72 |     dist_correction_x: 0
 73 |     dist_correction_y: 0
 74 |     focal_distance: 0
 75 |     focal_slope: 0
 76 |     laser_id: 6
 77 |     max_intensity: 255
 78 |     min_intensity: 0
 79 |     rot_correction: 0
 80 |     vert_correction: -0.148716226220131
 81 |     vert_offset_correction: 0
 82 |   - dist_correction: 0.200000002980232
 83 |     dist_correction_x: 0
 84 |     dist_correction_y: 0
 85 |     focal_distance: 0
 86 |     focal_slope: 0
 87 |     laser_id: 7
 88 |     max_intensity: 255
 89 |     min_intensity: 0
 90 |     rot_correction: 0.0226892791688442
 91 |     vert_correction: -0.142765983939171
 92 |     vert_offset_correction: 0
 93 |   - dist_correction: 0.129999995231628
 94 |     dist_correction_x: 0
 95 |     dist_correction_y: 0
 96 |     focal_distance: 0
 97 |     focal_slope: 0
 98 |     laser_id: 8
 99 |     max_intensity: 255
100 |     min_intensity: 0
101 |     rot_correction: 0.0820304751396179
102 |     vert_correction: -0.101187855005264
103 |     vert_offset_correction: 0
104 |   - dist_correction: 0.159999996423721
105 |     dist_correction_x: 0
106 |     dist_correction_y: 0
107 |     focal_distance: 0
108 |     focal_slope: 0
109 |     laser_id: 9
110 |     max_intensity: 255
111 |     min_intensity: 0
112 |     rot_correction: 0.113446399569511
113 |     vert_correction: -0.0952560678124428
114 |     vert_offset_correction: 0
115 |   - dist_correction: 0.159999996423721
116 |     dist_correction_x: 0
117 |     dist_correction_y: 0
118 |     focal_distance: 0
119 |     focal_slope: 0
120 |     laser_id: 10
121 |     max_intensity: 255
122 |     min_intensity: 0
123 |     rot_correction: 0.0698131695389748
124 |     vert_correction: -0.136818811297417
125 |     vert_offset_correction: 0
126 |   - dist_correction: 0.159999996423721
127 |     dist_correction_x: 0
128 |     dist_correction_y: 0
129 |     focal_distance: 0
130 |     focal_slope: 0
131 |     laser_id: 11
132 |     max_intensity: 255
133 |     min_intensity: 0
134 |     rot_correction: 0.104719758033752
135 |     vert_correction: -0.130874469876289
136 |     vert_offset_correction: 0
137 |   - dist_correction: 0.129999995231628
138 |     dist_correction_x: 0
139 |     dist_correction_y: 0
140 |     focal_distance: 0
141 |     focal_slope: 0
142 |     laser_id: 12
143 |     max_intensity: 255
144 |     min_intensity: 0
145 |     rot_correction: -0.068067841231823
146 |     vert_correction: -0.05375986546278
147 |     vert_offset_correction: 0
148 |   - dist_correction: 0.200000002980232
149 |     dist_correction_x: 0
150 |     dist_correction_y: 0
151 |     focal_distance: 0
152 |     focal_slope: 0
153 |     laser_id: 13
154 |     max_intensity: 255
155 |     min_intensity: 0
156 |     rot_correction: -0.0349065847694874
157 |     vert_correction: -0.0478330813348293
158 |     vert_offset_correction: 0
159 |   - dist_correction: 0.170000001788139
160 |     dist_correction_x: 0
161 |     dist_correction_y: 0
162 |     focal_distance: 0
163 |     focal_slope: 0
164 |     laser_id: 14
165 |     max_intensity: 255
166 |     min_intensity: 0
167 |     rot_correction: -0.0837758108973503
168 |     vert_correction: -0.0893256440758705
169 |     vert_offset_correction: 0
170 |   - dist_correction: 0.239999994635582
171 |     dist_correction_x: 0
172 |     dist_correction_y: 0
173 |     focal_distance: 0
174 |     focal_slope: 0
175 |     laser_id: 15
176 |     max_intensity: 255
177 |     min_intensity: 0
178 |     rot_correction: -0.0479965545237064
179 |     vert_correction: -0.0833963677287102
180 |     vert_offset_correction: 0
181 |   - dist_correction: 0.180000007152557
182 |     dist_correction_x: 0
183 |     dist_correction_y: 0
184 |     focal_distance: 0
185 |     focal_slope: 0
186 |     laser_id: 16
187 |     max_intensity: 255
188 |     min_intensity: 0
189 |     rot_correction: 0.00872664619237185
190 |     vert_correction: -0.0419059917330742
191 |     vert_offset_correction: 0
192 |   - dist_correction: 0.0599999986588955
193 |     dist_correction_x: 0
194 |     dist_correction_y: 0
195 |     focal_distance: 0
196 |     focal_slope: 0
197 |     laser_id: 17
198 |     max_intensity: 255
199 |     min_intensity: 0
200 |     rot_correction: 0.0392699092626572
201 |     vert_correction: -0.035978376865387
202 |     vert_offset_correction: 0
203 |   - dist_correction: 0.140000000596046
204 |     dist_correction_x: 0
205 |     dist_correction_y: 0
206 |     focal_distance: 0
207 |     focal_slope: 0
208 |     laser_id: 18
209 |     max_intensity: 255
210 |     min_intensity: 0
211 |     rot_correction: 0
212 |     vert_correction: -0.0774680152535439
213 |     vert_offset_correction: 0
214 |   - dist_correction: 0.150000005960464
215 |     dist_correction_x: 0
216 |     dist_correction_y: 0
217 |     focal_distance: 0
218 |     focal_slope: 0
219 |     laser_id: 19
220 |     max_intensity: 255
221 |     min_intensity: 0
222 |     rot_correction: 0.0305432621389627
223 |     vert_correction: -0.0715404152870178
224 |     vert_offset_correction: 0
225 |   - dist_correction: 0.219999998807907
226 |     dist_correction_x: 0
227 |     dist_correction_y: 0
228 |     focal_distance: 0
229 |     focal_slope: 0
230 |     laser_id: 20
231 |     max_intensity: 255
232 |     min_intensity: 0
233 |     rot_correction: 0.0855211317539215
234 |     vert_correction: -0.0300500374287367
235 |     vert_offset_correction: 0
236 |   - dist_correction: 0.140000000596046
237 |     dist_correction_x: 0
238 |     dist_correction_y: 0
239 |     focal_distance: 0
240 |     focal_slope: 0
241 |     laser_id: 21
242 |     max_intensity: 255
243 |     min_intensity: 0
244 |     rot_correction: 0.109083078801632
245 |     vert_correction: -0.024120757356286
246 |     vert_offset_correction: 0
247 |   - dist_correction: 0.0799999982118607
248 |     dist_correction_x: 0
249 |     dist_correction_y: 0
250 |     focal_distance: 0
251 |     focal_slope: 0
252 |     laser_id: 22
253 |     max_intensity: 255
254 |     min_intensity: 0
255 |     rot_correction: 0.0698131695389748
256 |     vert_correction: -0.0656133219599724
257 |     vert_offset_correction: 0
258 |   - dist_correction: 0.159999996423721
259 |     dist_correction_x: 0
260 |     dist_correction_y: 0
261 |     focal_distance: 0
262 |     focal_slope: 0
263 |     laser_id: 23
264 |     max_intensity: 255
265 |     min_intensity: 0
266 |     rot_correction: 0.104719758033752
267 |     vert_correction: -0.0596865378320217
268 |     vert_offset_correction: 0
269 |   - dist_correction: 0.119999997317791
270 |     dist_correction_x: 0
271 |     dist_correction_y: 0
272 |     focal_distance: 0
273 |     focal_slope: 0
274 |     laser_id: 24
275 |     max_intensity: 255
276 |     min_intensity: 0
277 |     rot_correction: -0.0610865242779255
278 |     vert_correction: 0.0174280721694231
279 |     vert_offset_correction: 0
280 |   - dist_correction: 0.219999998807907
281 |     dist_correction_x: 0
282 |     dist_correction_y: 0
283 |     focal_distance: 0
284 |     focal_slope: 0
285 |     laser_id: 25
286 |     max_intensity: 255
287 |     min_intensity: 0
288 |     rot_correction: -0.0349065847694874
289 |     vert_correction: 0.0233724191784859
290 |     vert_offset_correction: 0
291 |   - dist_correction: 0.159999996423721
292 |     dist_correction_x: 0
293 |     dist_correction_y: 0
294 |     focal_distance: 0
295 |     focal_slope: 0
296 |     laser_id: 26
297 |     max_intensity: 255
298 |     min_intensity: 0
299 |     rot_correction: -0.0785398185253143
300 |     vert_correction: -0.0181903336197138
301 |     vert_offset_correction: 0
302 |   - dist_correction: 0.259999990463257
303 |     dist_correction_x: 0
304 |     dist_correction_y: 0
305 |     focal_distance: 0
306 |     focal_slope: 0
307 |     laser_id: 27
308 |     max_intensity: 255
309 |     min_intensity: 0
310 |     rot_correction: -0.0471238903701305
311 |     vert_correction: -0.0122585473582149
312 |     vert_offset_correction: 0
313 |   - dist_correction: 0.140000000596046
314 |     dist_correction_x: 0
315 |     dist_correction_y: 0
316 |     focal_distance: 0
317 |     focal_slope: 0
318 |     laser_id: 28
319 |     max_intensity: 255
320 |     min_intensity: 0
321 |     rot_correction: 0.0104719763621688
322 |     vert_correction: 0.0293195936828852
323 |     vert_offset_correction: 0
324 |   - dist_correction: 0.219999998807907
325 |     dist_correction_x: 0
326 |     dist_correction_y: 0
327 |     focal_distance: 0
328 |     focal_slope: 0
329 |     laser_id: 29
330 |     max_intensity: 255
331 |     min_intensity: 0
332 |     rot_correction: 0.0349065847694874
333 |     vert_correction: 0.0352698266506195
334 |     vert_offset_correction: 0
335 |   - dist_correction: 0.209999993443489
336 |     dist_correction_x: 0
337 |     dist_correction_y: 0
338 |     focal_distance: 0
339 |     focal_slope: 0
340 |     laser_id: 30
341 |     max_intensity: 255
342 |     min_intensity: 0
343 |     rot_correction: -0.00436332309618592
344 |     vert_correction: -0.00632520206272602
345 |     vert_offset_correction: 0
346 |   - dist_correction: 0.140000000596046
347 |     dist_correction_x: 0
348 |     dist_correction_y: 0
349 |     focal_distance: 0
350 |     focal_slope: 0
351 |     laser_id: 31
352 |     max_intensity: 255
353 |     min_intensity: 0
354 |     rot_correction: 0.0296705979853868
355 |     vert_correction: -0.000390077300835401
356 |     vert_offset_correction: 0
357 |   - dist_correction: 0.119999997317791
358 |     dist_correction_x: 0
359 |     dist_correction_y: 0
360 |     focal_distance: 0
361 |     focal_slope: 0
362 |     laser_id: 32
363 |     max_intensity: 255
364 |     min_intensity: 0
365 |     rot_correction: -0.122173048555851
366 |     vert_correction: -0.396850973367691
367 |     vert_offset_correction: 0
368 |   - dist_correction: 0.0199999995529652
369 |     dist_correction_x: 0
370 |     dist_correction_y: 0
371 |     focal_distance: 0
372 |     focal_slope: 0
373 |     laser_id: 33
374 |     max_intensity: 255
375 |     min_intensity: 0
376 |     rot_correction: -0.0610865242779255
377 |     vert_correction: -0.387918144464493
378 |     vert_offset_correction: 0
379 |   - dist_correction: 0.100000001490116
380 |     dist_correction_x: 0
381 |     dist_correction_y: 0
382 |     focal_distance: 0
383 |     focal_slope: 0
384 |     laser_id: 34
385 |     max_intensity: 255
386 |     min_intensity: 0
387 |     rot_correction: 0.0959931090474129
388 |     vert_correction: -0.200955957174301
389 |     vert_offset_correction: 0
390 |   - dist_correction: 0.230000004172325
391 |     dist_correction_x: 0
392 |     dist_correction_y: 0
393 |     focal_distance: 0
394 |     focal_slope: 0
395 |     laser_id: 35
396 |     max_intensity: 255
397 |     min_intensity: 0
398 |     rot_correction: 0.148352980613708
399 |     vert_correction: -0.192023113369942
400 |     vert_offset_correction: 0
401 |   - dist_correction: 0.170000001788139
402 |     dist_correction_x: 0
403 |     dist_correction_y: 0
404 |     focal_distance: 0
405 |     focal_slope: 0
406 |     laser_id: 36
407 |     max_intensity: 255
408 |     min_intensity: 0
409 |     rot_correction: -0.00872664619237185
410 |     vert_correction: -0.378992766141891
411 |     vert_offset_correction: 0
412 |   - dist_correction: 0.150000005960464
413 |     dist_correction_x: 0
414 |     dist_correction_y: 0
415 |     focal_distance: 0
416 |     focal_slope: 0
417 |     laser_id: 37
418 |     max_intensity: 255
419 |     min_intensity: 0
420 |     rot_correction: 0.0610865242779255
421 |     vert_correction: -0.370074152946472
422 |     vert_offset_correction: 0
423 |   - dist_correction: 0.0500000007450581
424 |     dist_correction_x: 0
425 |     dist_correction_y: 0
426 |     focal_distance: 0
427 |     focal_slope: 0
428 |     laser_id: 38
429 |     max_intensity: 255
430 |     min_intensity: 0
431 |     rot_correction: -0.0174532923847437
432 |     vert_correction: -0.43128889799118
433 |     vert_offset_correction: 0
434 |   - dist_correction: 0.270000010728836
435 |     dist_correction_x: 0
436 |     dist_correction_y: 0
437 |     focal_distance: 0
438 |     focal_slope: 0
439 |     laser_id: 39
440 |     max_intensity: 255
441 |     min_intensity: 0
442 |     rot_correction: 0.0349065847694874
443 |     vert_correction: -0.423701733350754
444 |     vert_offset_correction: 0
445 |   - dist_correction: 0.180000007152557
446 |     dist_correction_x: 0
447 |     dist_correction_y: 0
448 |     focal_distance: 0
449 |     focal_slope: 0
450 |     laser_id: 40
451 |     max_intensity: 255
452 |     min_intensity: 0
453 |     rot_correction: 0.122173048555851
454 |     vert_correction: -0.361161530017853
455 |     vert_offset_correction: 0
456 |   - dist_correction: 0.189999997615814
457 |     dist_correction_x: 0
458 |     dist_correction_y: 0
459 |     focal_distance: 0
460 |     focal_slope: 0
461 |     laser_id: 41
462 |     max_intensity: 255
463 |     min_intensity: 0
464 |     rot_correction: 0.174532920122147
465 |     vert_correction: -0.352254241704941
466 |     vert_offset_correction: 0
467 |   - dist_correction: 0.100000001490116
468 |     dist_correction_x: 0
469 |     dist_correction_y: 0
470 |     focal_distance: 0
471 |     focal_slope: 0
472 |     laser_id: 42
473 |     max_intensity: 255
474 |     min_intensity: 0
475 |     rot_correction: 0.104719758033752
476 |     vert_correction: -0.414742022752762
477 |     vert_offset_correction: 0
478 |   - dist_correction: 0.200000002980232
479 |     dist_correction_x: 0
480 |     dist_correction_y: 0
481 |     focal_distance: 0
482 |     focal_slope: 0
483 |     laser_id: 43
484 |     max_intensity: 255
485 |     min_intensity: 0
486 |     rot_correction: 0.165806278586388
487 |     vert_correction: -0.405792057514191
488 |     vert_offset_correction: 0
489 |   - dist_correction: 0.219999998807907
490 |     dist_correction_x: 0
491 |     dist_correction_y: 0
492 |     focal_distance: 0
493 |     focal_slope: 0
494 |     laser_id: 44
495 |     max_intensity: 255
496 |     min_intensity: 0
497 |     rot_correction: -0.113446399569511
498 |     vert_correction: -0.28999200463295
499 |     vert_offset_correction: 0
500 |   - dist_correction: 0.200000002980232
501 |     dist_correction_x: 0
502 |     dist_correction_y: 0
503 |     focal_distance: 0
504 |     focal_slope: 0
505 |     laser_id: 45
506 |     max_intensity: 255
507 |     min_intensity: 0
508 |     rot_correction: -0.0567231997847557
509 |     vert_correction: -0.281101644039154
510 |     vert_offset_correction: 0
511 |   - dist_correction: 0.150000005960464
512 |     dist_correction_x: 0
513 |     dist_correction_y: 0
514 |     focal_distance: 0
515 |     focal_slope: 0
516 |     laser_id: 46
517 |     max_intensity: 255
518 |     min_intensity: 0
519 |     rot_correction: -0.13962633907795
520 |     vert_correction: -0.343351542949677
521 |     vert_offset_correction: 0
522 |   - dist_correction: 0.219999998807907
523 |     dist_correction_x: 0
524 |     dist_correction_y: 0
525 |     focal_distance: 0
526 |     focal_slope: 0
527 |     laser_id: 47
528 |     max_intensity: 255
529 |     min_intensity: 0
530 |     rot_correction: -0.0872664600610733
531 |     vert_correction: -0.334452718496323
532 |     vert_offset_correction: 0
533 |   - dist_correction: 0.0599999986588955
534 |     dist_correction_x: 0
535 |     dist_correction_y: 0
536 |     focal_distance: 0
537 |     focal_slope: 0
538 |     laser_id: 48
539 |     max_intensity: 255
540 |     min_intensity: 0
541 |     rot_correction: 0
542 |     vert_correction: -0.272210210561752
543 |     vert_offset_correction: 0
544 |   - dist_correction: 0.129999995231628
545 |     dist_correction_x: 0
546 |     dist_correction_y: 0
547 |     focal_distance: 0
548 |     focal_slope: 0
549 |     laser_id: 49
550 |     max_intensity: 255
551 |     min_intensity: 0
552 |     rot_correction: 0.0558505356311798
553 |     vert_correction: -0.26331701874733
554 |     vert_offset_correction: 0
555 |   - dist_correction: 0.00999999977648258
556 |     dist_correction_x: 0
557 |     dist_correction_y: 0
558 |     focal_distance: 0
559 |     focal_slope: 0
560 |     laser_id: 50
561 |     max_intensity: 255
562 |     min_intensity: 0
563 |     rot_correction: -0.0174532923847437
564 |     vert_correction: -0.323895305395126
565 |     vert_offset_correction: 0
566 |   - dist_correction: 0.150000005960464
567 |     dist_correction_x: 0
568 |     dist_correction_y: 0
569 |     focal_distance: 0
570 |     focal_slope: 0
571 |     laser_id: 51
572 |     max_intensity: 255
573 |     min_intensity: 0
574 |     rot_correction: 0.0349065847694874
575 |     vert_correction: -0.316663861274719
576 |     vert_offset_correction: 0
577 |   - dist_correction: 0.200000002980232
578 |     dist_correction_x: 0
579 |     dist_correction_y: 0
580 |     focal_distance: 0
581 |     focal_slope: 0
582 |     laser_id: 52
583 |     max_intensity: 255
584 |     min_intensity: 0
585 |     rot_correction: 0.122173048555851
586 |     vert_correction: -0.254421383142471
587 |     vert_offset_correction: 0
588 |   - dist_correction: 0.239999994635582
589 |     dist_correction_x: 0
590 |     dist_correction_y: 0
591 |     focal_distance: 0
592 |     focal_slope: 0
593 |     laser_id: 53
594 |     max_intensity: 255
595 |     min_intensity: 0
596 |     rot_correction: 0.165806278586388
597 |     vert_correction: -0.245522528886795
598 |     vert_offset_correction: 0
599 |   - dist_correction: 0.180000007152557
600 |     dist_correction_x: 0
601 |     dist_correction_y: 0
602 |     focal_distance: 0
603 |     focal_slope: 0
604 |     laser_id: 54
605 |     max_intensity: 255
606 |     min_intensity: 0
607 |     rot_correction: 0.104719758033752
608 |     vert_correction: -0.307772427797318
609 |     vert_offset_correction: 0
610 |   - dist_correction: 0.219999998807907
611 |     dist_correction_x: 0
612 |     dist_correction_y: 0
613 |     focal_distance: 0
614 |     focal_slope: 0
615 |     laser_id: 55
616 |     max_intensity: 255
617 |     min_intensity: 0
618 |     rot_correction: 0.157079637050629
619 |     vert_correction: -0.298882067203522
620 |     vert_offset_correction: 0
621 |   - dist_correction: 0.140000000596046
622 |     dist_correction_x: 0
623 |     dist_correction_y: 0
624 |     focal_distance: 0
625 |     focal_slope: 0
626 |     laser_id: 56
627 |     max_intensity: 255
628 |     min_intensity: 0
629 |     rot_correction: -0.104719758033752
630 |     vert_correction: -0.183082059025764
631 |     vert_offset_correction: 0
632 |   - dist_correction: 0.319999992847443
633 |     dist_correction_x: 0
634 |     dist_correction_y: 0
635 |     focal_distance: 0
636 |     focal_slope: 0
637 |     laser_id: 57
638 |     max_intensity: 255
639 |     min_intensity: 0
640 |     rot_correction: -0.0610865242779255
641 |     vert_correction: -0.17413204908371
642 |     vert_offset_correction: 0
643 |   - dist_correction: 0.200000002980232
644 |     dist_correction_x: 0
645 |     dist_correction_y: 0
646 |     focal_distance: 0
647 |     focal_slope: 0
648 |     laser_id: 58
649 |     max_intensity: 255
650 |     min_intensity: 0
651 |     rot_correction: -0.13962633907795
652 |     vert_correction: -0.236619830131531
653 |     vert_offset_correction: 0
654 |   - dist_correction: 0.25
655 |     dist_correction_x: 0
656 |     dist_correction_y: 0
657 |     focal_distance: 0
658 |     focal_slope: 0
659 |     laser_id: 59
660 |     max_intensity: 255
661 |     min_intensity: 0
662 |     rot_correction: -0.0785398185253143
663 |     vert_correction: -0.22771255671978
664 |     vert_offset_correction: 0
665 |   - dist_correction: 0.170000001788139
666 |     dist_correction_x: 0
667 |     dist_correction_y: 0
668 |     focal_distance: 0
669 |     focal_slope: 0
670 |     laser_id: 60
671 |     max_intensity: 255
672 |     min_intensity: 0
673 |     rot_correction: 0
674 |     vert_correction: -0.16517236828804
675 |     vert_offset_correction: 0
676 |   - dist_correction: 0.230000004172325
677 |     dist_correction_x: 0
678 |     dist_correction_y: 0
679 |     focal_distance: 0
680 |     focal_slope: 0
681 |     laser_id: 61
682 |     max_intensity: 255
683 |     min_intensity: 0
684 |     rot_correction: 0.0436332300305367
685 |     vert_correction: -0.156202226877213
686 |     vert_offset_correction: 0
687 |   - dist_correction: 0.150000005960464
688 |     dist_correction_x: 0
689 |     dist_correction_y: 0
690 |     focal_distance: 0
691 |     focal_slope: 0
692 |     laser_id: 62
693 |     max_intensity: 255
694 |     min_intensity: 0
695 |     rot_correction: -0.0174532923847437
696 |     vert_correction: -0.218799933791161
697 |     vert_offset_correction: 0
698 |   - dist_correction: 0.230000004172325
699 |     dist_correction_x: 0
700 |     dist_correction_y: 0
701 |     focal_distance: 0
702 |     focal_slope: 0
703 |     laser_id: 63
704 |     max_intensity: 255
705 |     min_intensity: 0
706 |     rot_correction: 0.0314159244298935
707 |     vert_correction: -0.209881335496902
708 |     vert_offset_correction: 0
709 | num_lasers: 64
710 | 


--------------------------------------------------------------------------------