Branch data Line data Source code
1 : : // Copyright 2018-2020 the Autoware Foundation 2 : : // 3 : : // Licensed under the Apache License, Version 2.0 (the "License"); 4 : : // you may not use this file except in compliance with the License. 5 : : // You may obtain a copy of the License at 6 : : // 7 : : // http://www.apache.org/licenses/LICENSE-2.0 8 : : // 9 : : // Unless required by applicable law or agreed to in writing, software 10 : : // distributed under the License is distributed on an "AS IS" BASIS, 11 : : // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 : : // See the License for the specific language governing permissions and 13 : : // limitations under the License. 14 : : // 15 : : // Co-developed by Tier IV, Inc. and Apex.AI, Inc. 16 : : 17 : : /// \copyright Copyright 2020 the Autoware Foundation 18 : : /// \file 19 : : /// \brief This file defines a driver for Velodyne LiDARs 20 : : 21 : : #ifndef VELODYNE_DRIVER__COMMON_HPP_ 22 : : #define VELODYNE_DRIVER__COMMON_HPP_ 23 : : 24 : : #include <velodyne_driver/visibility_control.hpp> 25 : : #include <geometry_msgs/msg/point32.hpp> 26 : : #include <common/types.hpp> 27 : : 28 : : namespace autoware 29 : : { 30 : : /// \brief Libraries, ROS nodes, and other functionality relating to 31 : : /// sensor drivers or actuation. 32 : : namespace drivers 33 : : { 34 : : /// \brief Classes, types, and definitions specifically relating to 35 : : /// Velodyne LiDARs. In it's current incarnation, we consider Velodyne to be synonymous 36 : : /// with LiDARs. In the future, this namespace will diverge to LiDAR and Velodyne for 37 : : /// general LiDAR point cloud functionality, and specific driver functionality for 38 : : /// velodne LiDARs respectively. 39 : : namespace velodyne_driver 40 : : { 41 : : using autoware::common::types::bool8_t; 42 : : using autoware::common::types::float32_t; 43 : : 44 : : // These constants seem to be shared among velodyne drivers. If a model with different specs is 45 : : // to be supported, they should be refactored. 46 : : 47 : : /// resolution of azimuth angle: number of points in a full rotation 48 : : static constexpr uint32_t AZIMUTH_ROTATION_RESOLUTION = 36000U; 49 : : /// conversion from a degree (vlp) to idx 50 : : static constexpr float32_t DEG2IDX = static_cast<float32_t>(AZIMUTH_ROTATION_RESOLUTION) / 360.0F; 51 : : /// how intensity is quantized: 1 byte = 256 possible values 52 : : static constexpr uint32_t NUM_INTENSITY_VALUES = 256U; 53 : : 54 : : /// number of data blocks per data packet 55 : : static constexpr uint16_t NUM_BLOCKS_PER_PACKET = 12U; 56 : : /// number of points stored in a data block 57 : : static constexpr uint16_t NUM_POINTS_PER_BLOCK = 32U; 58 : : 59 : : /// \brief computes 2 byte representation of two bytes from out of order velodyne packet 60 : : inline uint32_t to_uint32(const uint8_t first, const uint8_t second) 61 : : { 62 : : // probably ok since uint8_t<<8 =>uint32_t, this is to get around 63 : : // warning due to implicit promotion to int 64 : 79596 : const uint32_t ret = static_cast<uint32_t>(first) << 8U; 65 : 79596 : return ret + static_cast<uint32_t>(second); 66 : : } 67 : : 68 : : using autoware::common::types::PointXYZIF; 69 : : } // namespace velodyne_driver 70 : : } // namespace drivers 71 : : } // namespace autoware 72 : : 73 : : #endif // VELODYNE_DRIVER__COMMON_HPP_