├── .gitignore ├── .travis.yml ├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE ├── include ├── ouster │ ├── os1.h │ └── os1_packet.h └── ouster_driver │ ├── os1_ros.h │ └── point_os1.h ├── launch └── os1.launch ├── msg └── PacketMsg.msg ├── package.xml ├── readme.md ├── rviz └── viz.rviz └── src ├── os1.cpp ├── os1_node.cpp └── os1_ros.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # GNU global tags 2 | GTAGS 3 | GRTAGS 4 | GSYMS 5 | GPATH 6 | 7 | # Python byte-compile file 8 | __pycache__/ 9 | *.py[co] 10 | .ycm_extra_conf.py 11 | # editor backup files 12 | *~ 13 | \#*\# 14 | 15 | # backup files 16 | *.bak 17 | 18 | # Eclipse 19 | .cproject 20 | .project 21 | .pydevproject 22 | .settings/ 23 | 24 | # Visual Studio Code 25 | .vscode/ 26 | 27 | # CLion 28 | .idea/ 29 | cmake-build-debug/ 30 | 31 | # clang-format 32 | #.clang-format 33 | 34 | # ROSBag files 35 | *.bag 36 | 37 | # Environments 38 | .env 39 | .venv 40 | env/ 41 | venv/ 42 | ENV/ 43 | 44 | # Autoware Resources 45 | *.pcap 46 | *.pcd 47 | *.bag 48 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | sudo: required 2 | dist: trusty 3 | language: generic 4 | compiler: 5 | - gcc 6 | env: 7 | matrix: 8 | - ROS_DISTRO="kinetic" ROS_REPO=ros CATKIN_CONFIG="-DCMAKE_BUILD_TYPE=Release" DOCKER_BASE_IMAGE=ros:kinetic-perception 9 | - ROS_DISTRO="melodic" ROS_REPO=ros CATKIN_CONFIG="-DCMAKE_BUILD_TYPE=Release" DOCKER_BASE_IMAGE=ros:melodic-perception 10 | 11 | install: 12 | git clone https://github.com/ros-industrial/industrial_ci.git .ci_config 13 | script: 14 | source .ci_config/travis.sh -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package ouster 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.1.7 (2019-03-28) 6 | ------------------ 7 | * Sensor model and firmware version validation 8 | * Added sensor model and firmware version validation. The driver currently only works for OS-1-64 and on firmware version 1.10 some configuration parameters disappeared. 9 | * Parameter validation and new pointcloud types 10 | * Added parameter validation to avoid reinitializing the LiDAR. Added new pointcloud types XYZIRF and XYZIRFN. Changed pulse_mode parameter from integer to string for readability. Updated the readme file. 11 | * Contributors: Alexander Carballo, alexandrx 12 | 13 | 0.1.6 (2019-03-13) 14 | ------------------ 15 | * Renamed ouster_ros to ouster_driver 16 | 17 | 0.1.5 (2019-03-13) 18 | ------------------ 19 | * updated package.xml 20 | 21 | 0.1.4 (2019-03-13) 22 | ------------------ 23 | * Updated project name to follow ros standards 24 | 25 | 0.1.3 (2019-03-06) 26 | ------------------ 27 | * Updated LICENSE 28 | 29 | 0.1.2 (2019-03-06) 30 | ------------------ 31 | * Changed operation mode from integer to string to ease reading. 32 | * New pointcloud mode format added 33 | * Improved parameter documentation 34 | * removed the old bool pointcloud_mode_xyzir parameter (to match Velodyne's mode) and replaced with a way to specify the desired pointcloud format, several options added including Ouster's native pointcloud format. 35 | * Added install commands 36 | * run CI on kinetic and melodic only 37 | * Fixed launch file to deal with hidden parameters, fixed readme.md to update the TODO, fixed os1_node.cpp to reduce by half the scan duration variable in case of 20Hz operation. 38 | * Updated the driver to support advanced parameter configuration (lidar-mode, pulse-mode and window-rejection-enable) 39 | * Eliminated the ouster-ros static library 40 | * Updated readme.md incorrect info for lidar address configuration. 41 | * Switched to industrial-ci 42 | * First commit for Autoware version branch. Modified the original Ouster ROS code to suit Autoware needs. Code structure is more simple, only ROS driver code is kept, and the readme.md file has more details on how to configure the sensor and use this driver. To further ease Autoware integration, Velodyne compatibility options were added and tested with nd-_matching, ray-ground-filter and euclidean-cluster nodes. 43 | * Improve positional argument parsing in viz 44 | * Use allocate-shared for Eigen compatibility 45 | * Support building visualizer with newer VTK 46 | * Readme tweaks and new example data 47 | * Update readme.md 48 | * updated the readme to include viz in the contents 49 | * Initial commit of the example visualizer 50 | * Uses VTK6 to display point clouds and range/intensity/noise images 51 | * No dependencies other than VTK6, Eigen3, and a C++11 compiler 52 | * Currently only supports linux; tested on multiple distributions and platforms 53 | * Use correct units in sensor-msgs::Imu 54 | * Deal with time going backwards in replays 55 | * Contributors: Alexander Carballo, Dima Garbuzov, Roy Rinberg, alexandrx, amc-nu, hooic, rafficm, rrinberg 56 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ouster_driver) 3 | 4 | add_compile_options(-std=c++11 -Wall) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | message_generation 8 | std_msgs 9 | sensor_msgs 10 | pcl_ros 11 | pcl_conversions 12 | roscpp 13 | ) 14 | 15 | add_message_files(DIRECTORY msg FILES PacketMsg.msg) 16 | generate_messages(DEPENDENCIES std_msgs sensor_msgs) 17 | 18 | include_directories( 19 | include 20 | ${catkin_INCLUDE_DIRS} 21 | ) 22 | 23 | catkin_package( 24 | INCLUDE_DIRS include 25 | LIBRARIES ouster_driver 26 | CATKIN_DEPENDS pcl_ros pcl_conversions roscpp message_runtime std_msgs sensor_msgs 27 | ) 28 | 29 | add_executable(${PROJECT_NAME} src/os1_node.cpp src/os1_ros.cpp src/os1.cpp) 30 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 31 | add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencpp) 32 | 33 | install(TARGETS 34 | ${PROJECT_NAME} 35 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 36 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 37 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 38 | ) 39 | 40 | install(DIRECTORY launch/ 41 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 42 | PATTERN ".svn" EXCLUDE 43 | ) 44 | 45 | install(DIRECTORY rviz/ 46 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/rviz 47 | PATTERN ".svn" EXCLUDE 48 | ) 49 | 50 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2018, ouster-lidar 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /include/ouster/os1.h: -------------------------------------------------------------------------------- 1 | /** 2 | * OS1 sample client 3 | */ 4 | 5 | #pragma once 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | namespace ouster { 12 | namespace OS1 { 13 | 14 | const size_t lidar_packet_bytes = 12608; 15 | const size_t imu_packet_bytes = 48; 16 | 17 | struct client; 18 | 19 | enum client_state { ERROR = 1, LIDAR_DATA = 2, IMU_DATA = 4 }; 20 | 21 | /** 22 | * Connect to the sensor and start listening for data 23 | * @param hostname hostname or ip of the sensor 24 | * @param udp_dest_host hostname or ip where the sensor should send data 25 | * @param lidar_port port on which the sensor will send lidar data 26 | * @param imu_port port on which the sensor will send imu data 27 | * @return pointer owning the resources associated with the connection 28 | */ 29 | std::shared_ptr init_client(const std::string& hostname, 30 | const std::string& udp_dest_host, 31 | int lidar_port, int imu_port); 32 | 33 | /** 34 | * Block for up to a second until either data is ready or an error occurs. 35 | * @param cli client returned by init_client associated with the connection 36 | * @return client_state s where (s & ERROR) is true if an error occured, (s & 37 | * LIDAR_DATA) is true if lidar data is ready to read, and (s & IMU_DATA) is 38 | * true if imu data is ready to read 39 | */ 40 | client_state poll_client(const client& cli); 41 | 42 | /** 43 | * Read lidar data from the sensor. Will block for up to a second if no data is 44 | * available. 45 | * @param cli client returned by init_client associated with the connection 46 | * @param buf buffer to which to write lidar data. Must be at least 47 | * lidar_packet_bytes + 1 bytes 48 | * @returns true if a lidar packet was successfully read 49 | */ 50 | bool read_lidar_packet(const client& cli, uint8_t* buf); 51 | 52 | /** 53 | * Read imu data from the sensor. Will block for up to a second if no data is 54 | * available. 55 | * @param cli client returned by init_client associated with the connection 56 | * @param buf buffer to which to write imu data. Must be at least 57 | * imu_packet_bytes + 1 bytes 58 | * @returns true if an imu packet was successfully read 59 | */ 60 | bool read_imu_packet(const client& cli, uint8_t* buf); 61 | 62 | /** 63 | * Operation modes (horizontal resolution and scan rate) supported by the OS1 LiDAR 64 | */ 65 | typedef enum { 66 | MODE_512x10=0, 67 | MODE_1024x10=1, 68 | MODE_2048x10=2, 69 | MODE_512x20=3, 70 | MODE_1024x20=4 71 | } operation_mode_t; 72 | using OperationMode = operation_mode_t; 73 | 74 | /** 75 | * Laser pulse modes supported by the OS1 LiDAR 76 | */ 77 | typedef enum { 78 | PULSE_STANDARD=0, 79 | PULSE_NARROW=1, 80 | } pulse_mode_t; 81 | using PulseMode = pulse_mode_t; 82 | /** 83 | * Define the pointcloud type to use 84 | * @param operation_mode defines the resolution and frame rate 85 | * @param pulse_mode is the width of the laser pulse (standard or narrow) 86 | * @param window_rejection to reject short range data (true), or to accept short range data (false) 87 | * @note This function was added to configure advanced operation modes for Autoware 88 | */ 89 | void set_advanced_params(std::string operation_mode_str, std::string pulse_mode_str, bool window_rejection); 90 | 91 | } 92 | } 93 | -------------------------------------------------------------------------------- /include/ouster/os1_packet.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Utilities to parse lidar and imu packets 3 | */ 4 | 5 | #pragma once 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace ouster { 13 | namespace OS1 { 14 | 15 | namespace { 16 | 17 | const int pixels_per_column = 64; 18 | const int columns_per_buffer = 16; 19 | 20 | const int pixel_bytes = 12; 21 | const int column_bytes = 16 + (pixels_per_column * pixel_bytes) + 4; 22 | 23 | const int encoder_ticks_per_rev = 90112; 24 | 25 | const std::array beam_altitude_angles = { 26 | 16.611, 16.084, 15.557, 15.029, 14.502, 13.975, 13.447, 12.920, 27 | 12.393, 11.865, 11.338, 10.811, 10.283, 9.756, 9.229, 8.701, 28 | 8.174, 7.646, 7.119, 6.592, 6.064, 5.537, 5.010, 4.482, 29 | 3.955, 3.428, 2.900, 2.373, 1.846, 1.318, 0.791, 0.264, 30 | -0.264, -0.791, -1.318, -1.846, -2.373, -2.900, -3.428, -3.955, 31 | -4.482, -5.010, -5.537, -6.064, -6.592, -7.119, -7.646, -8.174, 32 | -8.701, -9.229, -9.756, -10.283, -10.811, -11.338, -11.865, -12.393, 33 | -12.920, -13.447, -13.975, -14.502, -15.029, -15.557, -16.084, -16.611, 34 | }; 35 | const std::array beam_azimuth_angles = { 36 | 3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164, 37 | 3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164, 38 | 3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164, 39 | 3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164, 40 | 3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164, 41 | 3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164, 42 | 3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164, 43 | 3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164, 44 | }; 45 | 46 | struct trig_table_entry { 47 | float sin_beam_altitude_angles; 48 | float cos_beam_altitude_angles; 49 | float beam_azimuth_angles; 50 | }; 51 | 52 | // table of vertical angle cos, sin, and horizontal offset of each pixel 53 | static trig_table_entry trig_table[pixels_per_column]; 54 | } 55 | 56 | static bool init_tables() { 57 | for (int i = 0; i < pixels_per_column; i++) { 58 | trig_table[i] = {sinf(beam_altitude_angles[i] * 2 * M_PI / 360.0f), 59 | cosf(beam_altitude_angles[i] * 2 * M_PI / 360.0f), 60 | beam_azimuth_angles[i] * 2 * (float)M_PI / 360.0f}; 61 | } 62 | return true; 63 | } 64 | 65 | static bool tables_initialized = init_tables(); 66 | 67 | // lidar column fields 68 | inline const uint8_t* nth_col(int n, const uint8_t* udp_buf) { 69 | return udp_buf + (n * column_bytes); 70 | } 71 | 72 | inline uint64_t col_timestamp(const uint8_t* col_buf) { 73 | uint64_t res; 74 | memcpy(&res, col_buf, sizeof(uint64_t)); 75 | return res; 76 | } 77 | 78 | inline float col_h_angle(const uint8_t* col_buf) { 79 | uint32_t ticks; 80 | memcpy(&ticks, col_buf + (3 * 4), sizeof(uint32_t)); 81 | return (2.0 * M_PI * ticks / (float)encoder_ticks_per_rev); 82 | } 83 | 84 | inline float col_h_encoder_count(const uint8_t* col_buf) { 85 | uint32_t res; 86 | std::memcpy(&res, col_buf + (3 * 4), sizeof(uint32_t)); 87 | return res; 88 | } 89 | 90 | inline uint32_t col_measurement_id(const uint8_t* col_buf) { 91 | uint32_t res; 92 | memcpy(&res, col_buf + (2 * 4), sizeof(uint32_t)); 93 | return res; 94 | } 95 | 96 | inline uint32_t col_valid(const uint8_t* col_buf) { 97 | uint32_t res; 98 | memcpy(&res, col_buf + (196 * 4), sizeof(uint32_t)); 99 | return res; 100 | } 101 | 102 | // lidar pixel fields 103 | inline const uint8_t* nth_px(int n, const uint8_t* col_buf) { 104 | return col_buf + 16 + (n * pixel_bytes); 105 | } 106 | 107 | inline uint32_t px_range(const uint8_t* px_buf) { 108 | uint32_t res; 109 | memcpy(&res, px_buf, sizeof(uint32_t)); 110 | res &= 0x000fffff; 111 | return res; 112 | } 113 | 114 | inline uint16_t px_reflectivity(const uint8_t* px_buf) { 115 | uint16_t res; 116 | memcpy(&res, px_buf + 4, sizeof(uint16_t)); 117 | return res; 118 | } 119 | 120 | inline uint16_t px_signal_photons(const uint8_t* px_buf) { 121 | uint16_t res; 122 | memcpy(&res, px_buf + 6, sizeof(uint16_t)); 123 | return res; 124 | } 125 | 126 | inline uint16_t px_noise_photons(const uint8_t* px_buf) { 127 | uint16_t res; 128 | memcpy(&res, px_buf + 8, sizeof(uint16_t)); 129 | return res; 130 | } 131 | 132 | // imu packets 133 | inline uint64_t imu_sys_ts(const uint8_t* imu_buf) { 134 | uint64_t res; 135 | memcpy(&res, imu_buf, sizeof(uint64_t)); 136 | return res; 137 | } 138 | 139 | inline uint64_t imu_accel_ts(const uint8_t* imu_buf) { 140 | uint64_t res; 141 | memcpy(&res, imu_buf + 8, sizeof(uint64_t)); 142 | return res; 143 | } 144 | 145 | inline uint64_t imu_gyro_ts(const uint8_t* imu_buf) { 146 | uint64_t res; 147 | memcpy(&res, imu_buf + 16, sizeof(uint64_t)); 148 | return res; 149 | } 150 | 151 | // imu linear acceleration 152 | inline float imu_la_x(const uint8_t* imu_buf) { 153 | float res; 154 | memcpy(&res, imu_buf + 24, sizeof(float)); 155 | return res; 156 | } 157 | 158 | inline float imu_la_y(const uint8_t* imu_buf) { 159 | float res; 160 | memcpy(&res, imu_buf + 28, sizeof(float)); 161 | return res; 162 | } 163 | 164 | inline float imu_la_z(const uint8_t* imu_buf) { 165 | float res; 166 | memcpy(&res, imu_buf + 32, sizeof(float)); 167 | return res; 168 | } 169 | 170 | // imu angular velocity 171 | inline float imu_av_x(const uint8_t* imu_buf) { 172 | float res; 173 | memcpy(&res, imu_buf + 36, sizeof(float)); 174 | return res; 175 | } 176 | 177 | inline float imu_av_y(const uint8_t* imu_buf) { 178 | float res; 179 | memcpy(&res, imu_buf + 40, sizeof(float)); 180 | return res; 181 | } 182 | 183 | inline float imu_av_z(const uint8_t* imu_buf) { 184 | float res; 185 | memcpy(&res, imu_buf + 44, sizeof(float)); 186 | return res; 187 | } 188 | } 189 | } 190 | -------------------------------------------------------------------------------- /include/ouster_driver/os1_ros.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Higher-level functions to read data from the OS1 as ROS messages 3 | */ 4 | 5 | #pragma once 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include "ouster/os1.h" 17 | #include "ouster_driver/PacketMsg.h" 18 | #include "ouster_driver/point_os1.h" 19 | 20 | namespace ouster_driver { 21 | namespace OS1 { 22 | 23 | using CloudOS1 = pcl::PointCloud; 24 | using CloudOS1XYZ = pcl::PointCloud; 25 | using CloudOS1XYZI = pcl::PointCloud; 26 | using CloudOS1XYZIR = pcl::PointCloud; 27 | using CloudOS1XYZIF = pcl::PointCloud; 28 | using CloudOS1XYZIRF = pcl::PointCloud; 29 | using CloudOS1XYZIFN = pcl::PointCloud; 30 | using CloudOS1XYZIRFN = pcl::PointCloud; 31 | using ns = std::chrono::nanoseconds; 32 | 33 | /** 34 | * Read an imu packet into a ROS message. Blocks for up to a second if no data 35 | * is available. 36 | * @param cli the OS1 client 37 | * @param pm the destination packet message 38 | * @return whether reading was successful 39 | */ 40 | bool read_imu_packet(const ouster::OS1::client& cli, PacketMsg& pm); 41 | 42 | /** 43 | * Read a lidar packet into a ROS message. Blocks for up to a second if no data 44 | * is available. 45 | * @param cli the OS1 client 46 | * @param pm the destination packet message 47 | * @return whether reading was successful 48 | */ 49 | bool read_lidar_packet(const ouster::OS1::client& cli, PacketMsg& pm); 50 | 51 | /** 52 | * Read a timestamp from an imu packet message 53 | * @param cli the OS1 client 54 | * @param pm packet message populated by read_imu_packet 55 | * @returns timestamp in nanoseconds 56 | */ 57 | ns timestamp_of_imu_packet(const PacketMsg& pm); 58 | 59 | /** 60 | * Read a timestamp from a lidar packet message 61 | * @param pm packet message populated by read_lidar_packet 62 | * @returns timestamp in nanoseconds 63 | */ 64 | ns timestamp_of_lidar_packet(const PacketMsg& pm); 65 | 66 | /** 67 | * Parse an imu packet message into a ROS imu message 68 | * @param pm packet message populated by read_imu_packet 69 | * @param frame the frame to set in the resulting ROS message 70 | * @returns ROS sensor message with fields populated from the OS1 packet 71 | * 72 | * @note Modified to support custom message frame name 73 | */ 74 | sensor_msgs::Imu packet_to_imu_msg(const PacketMsg& pm, 75 | const std::string& frame = "os1_imu"); 76 | 77 | /** 78 | * Accumulate points from a lidar packet message into a PCL point cloud. All 79 | * points are timestamped relative to scan_start_ts as a fraction of 80 | * scan_duration, a float usually [0.0, 1.0) 81 | * @param scan_start_ts the timestamp of the beginning of the batch 82 | * @param scan_duration length of a scan used to compute point timestamps 83 | * @param pm packet message populated by read_lidar_packet 84 | * @param cloud PCL point cloud of PointOS1s to accumulate 85 | */ 86 | void add_packet_to_cloud(ns scan_start_ts, ns scan_duration, 87 | const PacketMsg& pm, CloudOS1& cloud); 88 | 89 | /** 90 | * Serialize a PCL point cloud to a ROS message 91 | * @param cloud the PCL point cloud to convert 92 | * @param timestamp the timestamp to give the resulting ROS message 93 | * @param frame the frame to set in the resulting ROS message 94 | * @returns a ROS message containing the point cloud. Can be deserialized with 95 | * fromROSMsg in pcl_conversions 96 | */ 97 | sensor_msgs::PointCloud2 cloud_to_cloud_msg(const CloudOS1& cloud, ns timestamp, 98 | const std::string& frame = "os1"); 99 | /** 100 | * Loop reading from the OS1 client and invoking callbacks with packet messages. 101 | * Returns when ROS exits. Also runs the ROS event loop via ros::spinOnce(). 102 | * @param cli the OS1 client 103 | * @param lidar_handler callback invoked with messages populated by 104 | * read_lidar_packet 105 | * @param imu_handler callback invoked with messages populated by 106 | * read_imu_packet 107 | */ 108 | void spin(const ouster::OS1::client& cli, 109 | const std::function& lidar_handler, 110 | const std::function& imu_handler); 111 | 112 | /** 113 | * Construct a function that will batch packets into a PCL point cloud for the 114 | * given duration, then invoke f with the complete cloud. 115 | * @param scan_dur duration to batch packets in nanoseconds 116 | * @param f a callback taking a PCL pointcloud to invoke on a complete batch 117 | * @returns a function that batches packet messages populated by 118 | * read_lidar_packet and invokes f on the result 119 | */ 120 | std::function batch_packets( 121 | ns scan_dur, const std::function& f); 122 | 123 | /** 124 | * Define the pointcloud type to use 125 | * @param mode_xyzir specifies the point cloud type to publish 126 | * supported values: NATIVE, XYZ, XYZI, XYZIR, XYZIF, XYZIFN 127 | */ 128 | void set_point_mode(std::string mode_xyzir); 129 | 130 | /** 131 | * Converts the OS1 native point format to XYZ 132 | */ 133 | void convert2XYZ(const CloudOS1& in, CloudOS1XYZ& out); 134 | 135 | /** 136 | * Converts the OS1 native point format to XYZI 137 | */ 138 | void convert2XYZI(const CloudOS1& in, CloudOS1XYZI& out); 139 | 140 | /** 141 | * Converts the OS1 native point format to XYZIR (Velodyne like) 142 | */ 143 | void convert2XYZIR(const CloudOS1& in, CloudOS1XYZIR& out); 144 | 145 | /** 146 | * Converts the OS1 native point format to XYZIF (with intensity and reflectivity) 147 | */ 148 | void convert2XYZIF(const CloudOS1& in, CloudOS1XYZIF& out); 149 | 150 | /** 151 | * Converts the OS1 native point format to XYZIRF (with intensity, ring and reflectivity) 152 | */ 153 | void convert2XYZIRF(const CloudOS1& in, CloudOS1XYZIRF& out); 154 | 155 | /** 156 | * Converts the OS1 native point format to XYZIFN (with intensity, reflectivity and ambient noise) 157 | */ 158 | void convert2XYZIFN(const CloudOS1& in, CloudOS1XYZIFN& out); 159 | 160 | /** 161 | * Converts the OS1 native point format to XYZIRFN (with intensity, ring, reflectivity and ambient noise) 162 | */ 163 | void convert2XYZIRFN(const CloudOS1& in, CloudOS1XYZIRFN& out); 164 | } 165 | } 166 | -------------------------------------------------------------------------------- /include/ouster_driver/point_os1.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #define PCL_NO_PRECOMPILE 3 | #include 4 | 5 | namespace ouster_driver { 6 | namespace OS1 { 7 | 8 | struct EIGEN_ALIGN16 PointOS1 { 9 | PCL_ADD_POINT4D; 10 | float t; 11 | uint16_t intensity; 12 | uint16_t reflectivity; 13 | uint16_t noise; 14 | uint8_t ring; 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 16 | }; 17 | 18 | struct EIGEN_ALIGN16 PointXYZIR { 19 | PCL_ADD_POINT4D; // quad-word XYZ 20 | float intensity; ///< laser intensity reading 21 | uint16_t ring; ///< laser ring number 22 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // ensure proper alignment 23 | }; 24 | 25 | struct EIGEN_ALIGN16 PointXYZIF { 26 | PCL_ADD_POINT4D; 27 | uint16_t intensity; 28 | uint16_t reflectivity; 29 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 30 | }; 31 | 32 | struct EIGEN_ALIGN16 PointXYZIRF { 33 | PCL_ADD_POINT4D; 34 | uint16_t intensity; 35 | uint16_t ring; 36 | uint16_t reflectivity; 37 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 38 | }; 39 | 40 | struct EIGEN_ALIGN16 PointXYZIFN { 41 | PCL_ADD_POINT4D; 42 | uint16_t intensity; 43 | uint16_t reflectivity; 44 | uint16_t noise; 45 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 46 | }; 47 | 48 | struct EIGEN_ALIGN16 PointXYZIRFN { 49 | PCL_ADD_POINT4D; 50 | uint16_t intensity; 51 | uint16_t ring; 52 | uint16_t reflectivity; 53 | uint16_t noise; 54 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 55 | }; 56 | 57 | } 58 | } 59 | 60 | POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_driver::OS1::PointOS1, 61 | (float, x, x) 62 | (float, y, y) 63 | (float, z, z) 64 | (float, t, t) 65 | (uint16_t, intensity, intensity) 66 | (uint16_t, reflectivity, reflectivity) 67 | (uint8_t, ring, ring) 68 | ) 69 | 70 | POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_driver::OS1::PointXYZIR, 71 | (float, x, x) 72 | (float, y, y) 73 | (float, z, z) 74 | (float, intensity, intensity) 75 | (uint16_t, ring, ring) 76 | ) 77 | 78 | POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_driver::OS1::PointXYZIF, 79 | (float, x, x) 80 | (float, y, y) 81 | (float, z, z) 82 | (uint16_t, intensity, intensity) 83 | (uint16_t, reflectivity, reflectivity) 84 | ) 85 | 86 | POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_driver::OS1::PointXYZIRF, 87 | (float, x, x) 88 | (float, y, y) 89 | (float, z, z) 90 | (uint16_t, intensity, intensity) 91 | (uint16_t, ring, ring) 92 | (uint16_t, reflectivity, reflectivity) 93 | ) 94 | 95 | POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_driver::OS1::PointXYZIFN, 96 | (float, x, x) 97 | (float, y, y) 98 | (float, z, z) 99 | (uint16_t, intensity, intensity) 100 | (uint16_t, reflectivity, reflectivity) 101 | (uint16_t, noise, noise) 102 | ) 103 | 104 | POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_driver::OS1::PointXYZIRFN, 105 | (float, x, x) 106 | (float, y, y) 107 | (float, z, z) 108 | (uint16_t, intensity, intensity) 109 | (uint16_t, ring, ring) 110 | (uint16_t, reflectivity, reflectivity) 111 | (uint16_t, noise, noise) 112 | ) 113 | 114 | -------------------------------------------------------------------------------- /launch/os1.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /msg/PacketMsg.msg: -------------------------------------------------------------------------------- 1 | uint8[] buf 2 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ouster_driver 4 | 0.1.7 5 | OS1 ROS client 6 | ouster developers 7 | Alexander Carballo 8 | Abraham Monrroy 9 | Apache 2 10 | catkin 11 | 12 | roscpp 13 | message_generation 14 | std_msgs 15 | sensor_msgs 16 | pcl_ros 17 | pcl_conversions 18 | 19 | roscpp 20 | message_runtime 21 | std_msgs 22 | sensor_msgs 23 | pcl_ros 24 | pcl_conversions 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /readme.md: -------------------------------------------------------------------------------- 1 | [![Build Status](https://travis-ci.org/CPFL/ouster.svg?branch=autoware_branch)](https://travis-ci.org/CPFL/ouster) 2 | 3 | # OS1 ROS Node 4 | 5 | ## Building the ROS Node 6 | * Supports Ubuntu 16.04 with ROS Kinetic (for ouster_driver) 7 | * ROS installation instructions can be found [here](http://wiki.ros.org/kinetic/Installation/Ubuntu) 8 | * Additionally requires ros-kinetic-pcl-ros and, optionally, ros-kinetic-rviz for visualization 9 | * Be sure to source the ROS setup script before building. For example:`source /opt/ros/kinetic/setup.bash` 10 | * Both packages can be built by catkin by moving them into a catkin workspace 11 | * Build with `mkdir myworkspace && cd myworkspace && ln -s /path/to/ouster_driver ./src && catkin_make` 12 | 13 | ## Running the ROS Node 14 | * Set up the ROS environment with `source /path/to/myworkspace/devel/setup.bash` in a new terminal for each command below 15 | * For use with a running sensor: 16 | - To publish OS1 data as ROS topic `roslaunch ouster_driver os1.launch lidar_address:= pc_address:=` where `` can be the hostname or IP address of the OS1 device, and `` is the hostname or IP address to which the sensor should send data to (i.e., your computer's IP address on the interface connected to the OS1) 17 | - To record raw sensor output, run `rosbag record /os1_node/imu_packets /os1_node/lidar_packets` in another terminal 18 | - *Note: `os1_node/lidar_packets` and `os1_node/imu_packets` are the "raw data" topics, while `os1_node/points` is the ROS compatible XYZ topic and `os1_node/imu` is the ROS compatible IMU topic* 19 | - To visualize output, run `rviz -d /path/to/ouster_driver/rviz/viz.rviz` in another terminal 20 | * For use with recorded sensor data: 21 | - To replay raw sensor output, run `roslaunch ouster_driver os1.launch replay:=true` 22 | - In a second terminal, run `rosbag play --clock ` 23 | - To visualize output, run `rviz -d /path/to/ouster_driver/rviz/viz.rviz` in another terminal 24 | * Sample raw sensor output is available [here](https://data.ouster.io/sample-data-2018-08-29) 25 | 26 | ## OS1 address configuration 27 | * By default, the OS1 uses DHCP to obtain its IP address (the `` parameter above). The **"OS-1-64/16 High Resolution Imaging LIDAR: Software User Guide"** by Ouster Inc. recommends using `dnsmasq`. 28 | * Setting IP address dynamically (default) 29 | - Install `dnsmasq` (when required), as `sudo apt install dnsmasq dnsmasq-utils` 30 | - Identify the interface name the OS1 is attached to. If this interface is not configured yet, setup its IP address and network mask. Use the NetworkManager and use "manual" in IPv4 settings, or use the `ifconfig` command (ex., `ifconfig 192.168.2.1 netmask 255.255.255.0 up`) 31 | - Modify the configuration file `/etc/dnsmasq.conf`, define/modify the `interface=` line with the interface name above. Also define the `dhcp-range=` value according to your interface (ex., `dhcp-range=192.168.2.50,192.168.2.150,12h`, in accordance to the `ifconfig` example above) 32 | - Start (stop) `dnsmasq` system service (`sudo systemctl stop dnsmasq` to stop, `sudo systemctl start dnsmasq` to start) 33 | - Identify the address assigned to the OS1 using `journalctl -fu dnsmasq`. Example: 34 | ``` 35 | Oct 23 09:45:56 dnsmasq-dhcp[30010]: DHCPREQUEST() 192.168.2.*** bc:0f:**:**:**:** 36 | Oct 23 09:45:56 dnsmasq-dhcp[30010]: DHCPACK() 192.168.2.*** bc:0f:**:**:**:** os1-XXXXXXXXXXXX 37 | ``` 38 | where `` is your computer's assigned hostname, `` is the interface configured above, `192.168.2.***` is the IP address assigned to the sensor, `bc:0f:**:**:**:**` is the OS1 MAC address and `os1-XXXXXXXXXXXX` is the sensor's hostname (`XXXXXXXXXXXX` is replaced by the sensor's serial number). *Note: instead of "\*" an actual number will be shown.* 39 | 40 | - You can set `` as either the assigned IP address (ex., `192.168.2.***`) or the sensor's hostname (ex., `os1-XXXXXXXXXXXX`). 41 | 42 | ## OS1 mode configuration 43 | * Setting the LiDAR horizontal resolution and scan rate 44 | - Please identify the current sensor IP address or hostname using the steps above 45 | - The supported values for `lidar_mode` are: 512x10, 1024x10, 2048x10, 512x20, 1024x20. The first value is horizontal resolution and the second is scan rate. 46 | - Connect to the sensor and reconfigure the `lidar_mode` parameter, storing the modification internally so to use it at next boot. You can skip the `write_config_txt` command to use default value at next boot. `os1-XXXXXXXXXXXX` is the sensor's hostname (`XXXXXXXXXXXX` is replaced by the sensor's serial number). 47 | ```bash 48 | nc os1-XXXXXXXXXXXX 7501 49 | get_config_param active lidar_mode 50 | set_config_param lidar_mode 2048x10 51 | write_config_txt 52 | reinitialize 53 | ``` 54 | 55 | ##Todo 56 | - [x] Velodyne compatibility mode. 57 | - [x] Configure active sensor parameters from driver code, in particular `lidar_mode`. 58 | - [ ] Fix `replay` option currently not working. 59 | - [x] Verify the sensor operation information and attempt software reset on error condition, and current parameter values to avoid unnecessary rewrites/reinitialization. 60 | 61 | -------------------------------------------------------------------------------- /rviz/viz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 263 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /PointCloud21 10 | Splitter Ratio: 0.5 11 | Tree Height: 585 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.588679016 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: PointCloud2 31 | Visualization Manager: 32 | Class: "" 33 | Displays: 34 | - Alpha: 0.5 35 | Cell Size: 1 36 | Class: rviz/Grid 37 | Color: 160; 160; 164 38 | Enabled: true 39 | Line Style: 40 | Line Width: 0.0299999993 41 | Value: Lines 42 | Name: Grid 43 | Normal Cell Count: 0 44 | Offset: 45 | X: 0 46 | Y: 0 47 | Z: 0 48 | Plane: XY 49 | Plane Cell Count: 10 50 | Reference Frame: 51 | Value: true 52 | - Alpha: 1 53 | Autocompute Intensity Bounds: true 54 | Autocompute Value Bounds: 55 | Max Value: 10 56 | Min Value: -10 57 | Value: true 58 | Axis: Z 59 | Channel Name: intensity 60 | Class: rviz/PointCloud2 61 | Color: 255; 255; 255 62 | Color Transformer: Intensity 63 | Decay Time: 0 64 | Enabled: true 65 | Invert Rainbow: false 66 | Max Color: 255; 255; 255 67 | Max Intensity: 1153 68 | Min Color: 0; 0; 0 69 | Min Intensity: 4 70 | Name: PointCloud2 71 | Position Transformer: XYZ 72 | Queue Size: 10 73 | Selectable: true 74 | Size (Pixels): 3 75 | Size (m): 0.00999999978 76 | Style: Points 77 | Topic: /os1_node/points 78 | Unreliable: false 79 | Use Fixed Frame: true 80 | Use rainbow: true 81 | Value: true 82 | Enabled: true 83 | Global Options: 84 | Background Color: 48; 48; 48 85 | Fixed Frame: os1 86 | Frame Rate: 30 87 | Name: root 88 | Tools: 89 | - Class: rviz/Interact 90 | Hide Inactive Objects: true 91 | - Class: rviz/MoveCamera 92 | - Class: rviz/Select 93 | - Class: rviz/FocusCamera 94 | - Class: rviz/Measure 95 | - Class: rviz/SetInitialPose 96 | Topic: /initialpose 97 | - Class: rviz/SetGoal 98 | Topic: /move_base_simple/goal 99 | - Class: rviz/PublishPoint 100 | Single click: true 101 | Topic: /clicked_point 102 | Value: true 103 | Views: 104 | Current: 105 | Class: rviz/Orbit 106 | Distance: 44.8919868 107 | Enable Stereo Rendering: 108 | Stereo Eye Separation: 0.0599999987 109 | Stereo Focal Distance: 1 110 | Swap Stereo Eyes: false 111 | Value: false 112 | Focal Point: 113 | X: 0 114 | Y: 0 115 | Z: 0 116 | Focal Shape Fixed Size: true 117 | Focal Shape Size: 0.0500000007 118 | Invert Z Axis: false 119 | Name: Current View 120 | Near Clip Distance: 0.00999999978 121 | Pitch: 0.565398574 122 | Target Frame: 123 | Value: Orbit (rviz) 124 | Yaw: 0.725398004 125 | Saved: ~ 126 | Window Geometry: 127 | Displays: 128 | collapsed: false 129 | Height: 1051 130 | Hide Left Dock: false 131 | Hide Right Dock: false 132 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000391fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000391000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000391fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000391000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004fb0000039100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 133 | Selection: 134 | collapsed: false 135 | Time: 136 | collapsed: false 137 | Tool Properties: 138 | collapsed: false 139 | Views: 140 | collapsed: false 141 | Width: 1920 142 | X: 0 143 | Y: 0 144 | -------------------------------------------------------------------------------- /src/os1.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include "ouster/os1.h" 17 | 18 | namespace ouster { 19 | namespace OS1 { 20 | 21 | using ns = std::chrono::nanoseconds; 22 | 23 | /** 24 | * @note Added to support advanced mode parameters configuration for Autoware 25 | */ 26 | static OperationMode _operation_mode = ouster::OS1::MODE_1024x10; 27 | static PulseMode _pulse_mode = ouster::OS1::PULSE_STANDARD; 28 | static bool _window_rejection = true; 29 | static std::string _operation_mode_str = ""; 30 | static std::string _pulse_mode_str = ""; 31 | static std::string _window_rejection_str = ""; 32 | //---------------- 33 | 34 | struct client { 35 | int lidar_fd; 36 | int imu_fd; 37 | ~client() { 38 | close(lidar_fd); 39 | close(imu_fd); 40 | } 41 | }; 42 | 43 | static int udp_data_socket(int port) { 44 | int sock_fd = socket(PF_INET, SOCK_DGRAM, 0); 45 | if (sock_fd < 0) { 46 | std::cerr << "socket: " << std::strerror(errno) << std::endl; 47 | return -1; 48 | } 49 | 50 | struct sockaddr_in my_addr; 51 | memset((char*)&my_addr, 0, sizeof(my_addr)); 52 | my_addr.sin_family = AF_INET; 53 | my_addr.sin_port = htons(port); 54 | my_addr.sin_addr.s_addr = INADDR_ANY; 55 | 56 | if (bind(sock_fd, (struct sockaddr*)&my_addr, sizeof(my_addr)) < 0) { 57 | std::cerr << "bind: " << std::strerror(errno) << std::endl; 58 | return -1; 59 | } 60 | 61 | struct timeval timeout; 62 | timeout.tv_sec = 1; 63 | timeout.tv_usec = 0; 64 | if (setsockopt(sock_fd, SOL_SOCKET, SO_RCVTIMEO, &timeout, 65 | sizeof(timeout)) < 0) { 66 | std::cerr << "setsockopt: " << std::strerror(errno) << std::endl; 67 | return -1; 68 | } 69 | 70 | return sock_fd; 71 | } 72 | 73 | static int cfg_socket(const char* addr) { 74 | struct addrinfo hints, *info_start, *ai; 75 | 76 | memset(&hints, 0, sizeof hints); 77 | hints.ai_family = AF_UNSPEC; 78 | hints.ai_socktype = SOCK_STREAM; 79 | 80 | int ret = getaddrinfo(addr, "7501", &hints, &info_start); 81 | if (ret != 0) { 82 | std::cerr << "getaddrinfo: " << gai_strerror(ret) << std::endl; 83 | return -1; 84 | } 85 | if (info_start == NULL) { 86 | std::cerr << "getaddrinfo: empty result" << std::endl; 87 | return -1; 88 | } 89 | 90 | int sock_fd; 91 | for (ai = info_start; ai != NULL; ai = ai->ai_next) { 92 | sock_fd = socket(ai->ai_family, ai->ai_socktype, ai->ai_protocol); 93 | if (sock_fd < 0) { 94 | std::cerr << "socket: " << std::strerror(errno) << std::endl; 95 | continue; 96 | } 97 | 98 | if (connect(sock_fd, ai->ai_addr, ai->ai_addrlen) == -1) { 99 | close(sock_fd); 100 | continue; 101 | } 102 | 103 | break; 104 | } 105 | 106 | freeaddrinfo(info_start); 107 | if (ai == NULL) { 108 | return -1; 109 | } 110 | 111 | return sock_fd; 112 | } 113 | 114 | std::shared_ptr init_client(const std::string& hostname, 115 | const std::string& udp_dest_host, 116 | int lidar_port, int imu_port) { 117 | int sock_fd = cfg_socket(hostname.c_str()); 118 | 119 | if (sock_fd < 0) return std::shared_ptr(); 120 | 121 | std::string cmd; 122 | 123 | auto do_cmd = [&](const std::string& op, const std::string& val) { 124 | const size_t max_res_len = 4095; 125 | char read_buf[max_res_len + 1]; 126 | 127 | ssize_t len; 128 | std::string cmd = op + " " + val + "\n"; 129 | 130 | len = write(sock_fd, cmd.c_str(), cmd.length()); 131 | if (len != (ssize_t)cmd.length()) { 132 | //std::cerr << "init_client: failed to send command" << std::endl; 133 | return std::string("[error] init_client: failed to send command"); 134 | } 135 | 136 | len = read(sock_fd, read_buf, max_res_len); 137 | if (len < 0) { 138 | //std::cerr << "read: " << std::strerror(errno) << std::endl; 139 | return std::string("[error] read: ") + std::strerror(errno); 140 | } 141 | read_buf[len] = '\0'; 142 | 143 | auto res = std::string(read_buf); 144 | res.erase(res.find_last_not_of(" \r\n\t") + 1); 145 | 146 | return res; 147 | }; 148 | 149 | auto do_cmd_chk = [&](const std::string& op, const std::string& val) { 150 | auto res = do_cmd(op, val); 151 | if (res.find("[error]") != std::string::npos) { 152 | std::cerr << res << std::endl; 153 | return false; 154 | } else if (res != op) { 155 | std::cerr << "init_client: command \"" << op << " " << val << "\" failed with \"" 156 | << res << "\"" << std::endl; 157 | return false; 158 | } 159 | return true; 160 | }; 161 | 162 | auto get_cmd = [&](const std::string& op, const std::string& val) { 163 | auto res = do_cmd(op, val); 164 | if (res.find("error") != std::string::npos) { 165 | std::cerr << res << std::endl; 166 | return std::string(""); 167 | } 168 | return res; 169 | }; 170 | 171 | auto str_tok = [&](const std::string &str, const std::string delim) { 172 | auto start = str.find_first_not_of(delim); 173 | auto end = start; 174 | std::vector tokens; 175 | 176 | while (start != std::string::npos){ 177 | end = str.find_first_of(delim, start); 178 | tokens.push_back(str.substr(start, end-start)); 179 | start = str.find_first_not_of(delim, end); 180 | } 181 | 182 | return tokens; 183 | }; 184 | 185 | bool success = true; 186 | success &= do_cmd_chk("set_udp_port_lidar", std::to_string(lidar_port)); 187 | success &= do_cmd_chk("set_udp_port_imu", std::to_string(imu_port)); 188 | success &= do_cmd_chk("set_udp_ip", udp_dest_host); 189 | 190 | if (!success) return std::shared_ptr(); 191 | 192 | /** 193 | * @note Added to support advanced mode parameters configuration 194 | */ 195 | //read the sensor information 196 | std::string version_str = std::string(""); 197 | std::string product_str = std::string(""); 198 | bool has_pulsemode = true; 199 | auto sensor_info_str = get_cmd("get_sensor_info", ""); 200 | auto tokens = str_tok(sensor_info_str, std::string(",: \"")); 201 | auto pos = std::find(tokens.begin(), tokens.end(), "build_rev"); 202 | if (pos != tokens.end()) { 203 | version_str = *(pos+1); 204 | } 205 | pos = std::find(tokens.begin(), tokens.end(), "prod_line"); 206 | if (pos != tokens.end()) { 207 | product_str = *(pos+1); 208 | } 209 | if (product_str == std::string("") || version_str == std::string("")) { 210 | std::cout << "Error: Failed to read product name and firmware version." << std::endl; 211 | return std::shared_ptr(); 212 | } 213 | std::cout << "Ouster model \"" << product_str << "\", firmware version \"" << version_str << "\"" << std::endl; 214 | //TODO: support OS-1-16 and OS-1-128 215 | if (product_str != std::string("OS-1-64")) { 216 | std::cout << "Error: this driver currently only supports Ouster model \"OS-1-64\"." << std::endl; 217 | return std::shared_ptr(); 218 | } 219 | 220 | auto ver_numbers = str_tok(version_str, std::string("v.")); 221 | //check the minor version 222 | auto ver_major = std::stoi(ver_numbers[0], nullptr); 223 | auto ver_minor = std::stoi(ver_numbers[1], nullptr); 224 | if (ver_major < 1 || ver_minor < 7) { 225 | std::cout << "Error: Firmware version \"" << version_str << "\" is not supported, please upgrade" << std::endl; 226 | return std::shared_ptr(); 227 | } 228 | if (std::stoi(ver_numbers[1], nullptr) >= 10) { 229 | std::cout << "On firmware version \"" << version_str << "\" the \"pulse_mode\" parameter is no longer available, will ignore it." << std::endl; 230 | has_pulsemode = false; 231 | } 232 | 233 | //read the current settings 234 | auto curr_operation_mode_str = get_cmd("get_config_param", "active lidar_mode"); 235 | auto curr_pulse_mode_str = std::string(""); 236 | if (has_pulsemode) { 237 | curr_pulse_mode_str = get_cmd("get_config_param", "active pulse_mode"); 238 | } 239 | auto curr_window_rejection_str = get_cmd("get_config_param", "active window_rejection_enable"); 240 | bool do_configure = false; 241 | success = true; 242 | 243 | //setting advanced mode parameters (if necessary) 244 | if (curr_operation_mode_str != _operation_mode_str) { 245 | success &= do_cmd_chk("set_config_param", "lidar_mode " + _operation_mode_str); 246 | do_configure = true; 247 | } 248 | if (has_pulsemode && (curr_pulse_mode_str != _pulse_mode_str)) { 249 | success &= do_cmd_chk("set_config_param", "pulse_mode " + _pulse_mode_str); 250 | do_configure = true; 251 | } 252 | if (curr_window_rejection_str != _window_rejection_str) { 253 | success &= do_cmd_chk("set_config_param", "window_rejection_enable " + _window_rejection_str); 254 | do_configure = true; 255 | } 256 | 257 | if (!success) return std::shared_ptr(); 258 | 259 | //reinitialize to take effect (if necessary) 260 | if (do_configure) { 261 | success &= do_cmd_chk("reinitialize", ""); 262 | if (!success) return std::shared_ptr(); 263 | std::cout << "Parameters configured, reinitializing sensor" << std::endl; 264 | } else { 265 | std::cout << "Parameters already configured, no need to reinitialize" << std::endl; 266 | } 267 | //---------------- 268 | 269 | close(sock_fd); 270 | 271 | int lidar_fd = udp_data_socket(lidar_port); 272 | int imu_fd = udp_data_socket(imu_port); 273 | auto cli = std::make_shared(); 274 | cli->lidar_fd = lidar_fd; 275 | cli->imu_fd = imu_fd; 276 | return cli; 277 | } 278 | 279 | client_state poll_client(const client& c) { 280 | fd_set rfds; 281 | FD_ZERO(&rfds); 282 | FD_SET(c.lidar_fd, &rfds); 283 | FD_SET(c.imu_fd, &rfds); 284 | 285 | int max_fd = std::max(c.lidar_fd, c.imu_fd); 286 | 287 | int retval = select(max_fd + 1, &rfds, NULL, NULL, NULL); 288 | 289 | client_state res = client_state(0); 290 | if (retval == -1) { 291 | std::cerr << "select: " << std::strerror(errno) << std::endl; 292 | res = client_state(res | ERROR); 293 | } else if (retval) { 294 | if (FD_ISSET(c.lidar_fd, &rfds)) res = client_state(res | LIDAR_DATA); 295 | if (FD_ISSET(c.imu_fd, &rfds)) res = client_state(res | IMU_DATA); 296 | } 297 | return res; 298 | } 299 | 300 | static bool recv_fixed(int fd, void* buf, size_t len) { 301 | ssize_t n = recvfrom(fd, buf, len + 1, 0, NULL, NULL); 302 | if (n == (ssize_t)len) 303 | return true; 304 | else if (n == -1) 305 | std::cerr << "recvfrom: " << std::strerror(errno) << std::endl; 306 | else 307 | std::cerr << "Unexpected udp packet length: " << n << std::endl; 308 | return false; 309 | } 310 | 311 | bool read_lidar_packet(const client& cli, uint8_t* buf) { 312 | return recv_fixed(cli.lidar_fd, buf, lidar_packet_bytes); 313 | } 314 | 315 | bool read_imu_packet(const client& cli, uint8_t* buf) { 316 | return recv_fixed(cli.imu_fd, buf, imu_packet_bytes); 317 | } 318 | 319 | /** 320 | * @note Added to support advanced mode parameters configuration for Autoware 321 | */ 322 | //---------------- 323 | void set_advanced_params(std::string operation_mode_str, std::string pulse_mode_str, bool window_rejection) 324 | { 325 | _operation_mode_str = operation_mode_str; 326 | _pulse_mode_str = pulse_mode_str; 327 | _window_rejection = window_rejection; 328 | 329 | _operation_mode = ouster::OS1::MODE_1024x10; 330 | if (_operation_mode_str == std::string("512x10")) { 331 | _operation_mode = ouster::OS1::MODE_512x10; 332 | } else if (_operation_mode_str == std::string("1024x10")) { 333 | _operation_mode = ouster::OS1::MODE_1024x10; 334 | } else if (_operation_mode_str == std::string("2048x10")) { 335 | _operation_mode = ouster::OS1::MODE_2048x10; 336 | } else if (_operation_mode_str == std::string("512x20")) { 337 | _operation_mode = ouster::OS1::MODE_512x20; 338 | } else if (_operation_mode_str == std::string("1024x20")) { 339 | _operation_mode = ouster::OS1::MODE_1024x20; 340 | } else { 341 | std::cout << "Selected operation mode " << _operation_mode_str << " is invalid, using default mode \"1024x10\"" << std::endl; 342 | } 343 | 344 | _pulse_mode = ouster::OS1::PULSE_STANDARD; 345 | if (_pulse_mode_str == std::string("STANDARD")) { 346 | _pulse_mode = ouster::OS1::PULSE_STANDARD; 347 | } else if (_pulse_mode_str == std::string("NARROW")) { 348 | _pulse_mode = ouster::OS1::PULSE_NARROW; 349 | } else { 350 | std::cout << "Selected pulse mode " << _pulse_mode_str << " is invalid, using default mode \"STANDARD\"" << std::endl; 351 | } 352 | 353 | _window_rejection_str = ((_window_rejection) ? std::string("1") : std::string("0")); 354 | } 355 | //---------------- 356 | 357 | } 358 | } 359 | -------------------------------------------------------------------------------- /src/os1_node.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Example node to publish OS1 output on ROS topics 3 | * 4 | * Additionally, this node can be used to record and replay raw sesnor output 5 | * by publishing and listening to PacketMsg topics 6 | * 7 | * ROS Parameters 8 | * scan_dur_ns: nanoseconds to batch lidar packets before publishing a cloud 9 | * os1_hostname: hostname or IP in dotted decimal form of the sensor 10 | * os1_udp_dest: hostname or IP where the sensor will send data packets 11 | * os1_lidar_port: port to which the sensor should send lidar data 12 | * os1_imu_port: port to which the sensor should send imu data 13 | * replay_mode: when true, the node will listen on ~/lidar_packets and 14 | * ~/imu_packets for data instead of attempting to connect to a sensor 15 | */ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include "ouster_driver/PacketMsg.h" 31 | #include "ouster_driver/os1_ros.h" 32 | 33 | using ns = std::chrono::nanoseconds; 34 | using PacketMsg = ouster_driver::PacketMsg; 35 | 36 | int main(int argc, char** argv) { 37 | ros::init(argc, argv, "os1_node"); 38 | ros::NodeHandle nh("~"); 39 | 40 | auto scan_dur = ns(nh.param("scan_dur_ns", 100000000)); 41 | auto os1_hostname = nh.param("os1_lidar_address", std::string("localhost")); 42 | auto os1_udp_dest = nh.param("pc_address", std::string("192.168.1.1")); 43 | auto os1_lidar_port = nh.param("os1_lidar_port", -1); 44 | auto os1_imu_port = nh.param("os1_imu_port", -1); 45 | auto replay_mode = nh.param("replay", true); 46 | auto points_topic_name = nh.param("points_topic_name", std::string("/points_raw")); 47 | auto imu_topic_name = nh.param("imu_topic_name", std::string("/imu_raw")); 48 | auto lidar_frame_name = nh.param("lidar_frame_name", std::string("/os1")); 49 | auto imu_frame_name = nh.param("imu_frame_name", std::string("/os1_imu")); 50 | auto pointcloud_mode = nh.param("pointcloud_mode", std::string("NATIVE")); 51 | auto operation_mode_str = nh.param("operation_mode", std::string("1024x10")); 52 | auto pulse_mode_str = nh.param("pulse_mode", std::string("STANDARD")); 53 | auto window_rejection = nh.param("window_rejection", true); 54 | 55 | /** 56 | * @note Added to support Velodyne compatible pointcloud format for Autoware 57 | */ 58 | //defines the pointcloud mode 59 | ouster_driver::OS1::set_point_mode(pointcloud_mode); 60 | //---------------- 61 | /** 62 | * @note Added to support advanced mode parameters configuration for Autoware 63 | */ 64 | //defines the advanced parameters 65 | ouster::OS1::set_advanced_params(operation_mode_str, pulse_mode_str, window_rejection); 66 | auto queue_size = 10; 67 | if (operation_mode_str == std::string("512x20") || operation_mode_str == std::string("1024x20")) { 68 | queue_size = 20; 69 | scan_dur = scan_dur / 2; //scan duration should be smaller at faster frame rates 70 | } 71 | //---------------- 72 | 73 | auto lidar_pub = nh.advertise(points_topic_name, queue_size); 74 | auto imu_pub = nh.advertise(imu_topic_name, queue_size); 75 | 76 | auto lidar_handler = ouster_driver::OS1::batch_packets( 77 | scan_dur, [&](ns scan_ts, const ouster_driver::OS1::CloudOS1& cloud) { 78 | lidar_pub.publish( 79 | ouster_driver::OS1::cloud_to_cloud_msg(cloud, scan_ts, lidar_frame_name)); 80 | }); 81 | 82 | auto imu_handler = [&](const PacketMsg& p) { 83 | imu_pub.publish(ouster_driver::OS1::packet_to_imu_msg(p, imu_frame_name)); 84 | }; 85 | 86 | if (replay_mode) { 87 | auto lidar_packet_sub = nh.subscribe( 88 | "lidar_packets", 500, lidar_handler); 89 | auto imu_packet_sub = nh.subscribe( 90 | "imu_packets", 500, imu_handler); 91 | ros::spin(); 92 | } else { 93 | auto lidar_packet_pub = nh.advertise("lidar_packets", 500); 94 | auto imu_packet_pub = nh.advertise("imu_packets", 500); 95 | 96 | auto cli = ouster::OS1::init_client(os1_hostname, os1_udp_dest, 97 | os1_lidar_port, os1_imu_port); 98 | if (!cli) { 99 | ROS_ERROR("Failed to initialize sensor at: %s", os1_hostname.c_str()); 100 | return 1; 101 | } 102 | 103 | ouster_driver::OS1::spin(*cli, 104 | [&](const PacketMsg& pm) { 105 | lidar_packet_pub.publish(pm); 106 | lidar_handler(pm); 107 | }, 108 | [&](const PacketMsg& pm) { 109 | imu_packet_pub.publish(pm); 110 | imu_handler(pm); 111 | }); 112 | } 113 | return 0; 114 | } 115 | -------------------------------------------------------------------------------- /src/os1_ros.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "ouster/os1.h" 4 | #include "ouster/os1_packet.h" 5 | #include "ouster_driver/os1_ros.h" 6 | 7 | namespace ouster_driver { 8 | namespace OS1 { 9 | 10 | using namespace ouster::OS1; 11 | 12 | static std::string _pointcloud_mode = "NATIVE"; 13 | 14 | ns timestamp_of_imu_packet(const PacketMsg& pm) { 15 | return ns(imu_gyro_ts(pm.buf.data())); 16 | } 17 | 18 | ns timestamp_of_lidar_packet(const PacketMsg& pm) { 19 | return ns(col_timestamp(nth_col(0, pm.buf.data()))); 20 | } 21 | 22 | bool read_imu_packet(const client& cli, PacketMsg& m) { 23 | m.buf.resize(imu_packet_bytes + 1); 24 | return read_imu_packet(cli, m.buf.data()); 25 | } 26 | 27 | bool read_lidar_packet(const client& cli, PacketMsg& m) { 28 | m.buf.resize(lidar_packet_bytes + 1); 29 | return read_lidar_packet(cli, m.buf.data()); 30 | } 31 | 32 | sensor_msgs::Imu packet_to_imu_msg(const PacketMsg& p, const std::string& frame) { 33 | const double standard_g = 9.80665; 34 | sensor_msgs::Imu m; 35 | const uint8_t* buf = p.buf.data(); 36 | 37 | m.header.stamp.fromNSec(imu_gyro_ts(buf)); 38 | /** 39 | * @note Modified to support custom message frame name 40 | */ 41 | //m.header.frame_id = "os1_imu"; // original code of OS1 driver 42 | m.header.frame_id = frame; //using defined message frame name 43 | 44 | m.orientation.x = 0; 45 | m.orientation.y = 0; 46 | m.orientation.z = 0; 47 | m.orientation.w = 0; 48 | 49 | m.linear_acceleration.x = imu_la_x(buf) * standard_g; 50 | m.linear_acceleration.y = imu_la_y(buf) * standard_g; 51 | m.linear_acceleration.z = imu_la_z(buf) * standard_g; 52 | 53 | m.angular_velocity.x = imu_av_x(buf) * M_PI / 180.0; 54 | m.angular_velocity.y = imu_av_y(buf) * M_PI / 180.0; 55 | m.angular_velocity.z = imu_av_z(buf) * M_PI / 180.0; 56 | 57 | for (int i = 0; i < 9; i++) { 58 | m.orientation_covariance[i] = -1; 59 | m.angular_velocity_covariance[i] = 0; 60 | m.linear_acceleration_covariance[i] = 0; 61 | } 62 | for (int i = 0; i < 9; i += 4) { 63 | m.linear_acceleration_covariance[i] = 0.01; 64 | m.angular_velocity_covariance[i] = 6e-4; 65 | } 66 | 67 | return m; 68 | } 69 | 70 | sensor_msgs::PointCloud2 cloud_to_cloud_msg(const CloudOS1& cloud, ns timestamp, 71 | const std::string& frame) { 72 | sensor_msgs::PointCloud2 msg; 73 | 74 | //pcl::toROSMsg(cloud, msg); //<-- original code of OS1 driver 75 | /** 76 | * @note Added to support Velodyne compatible pointcloud format for Autoware 77 | */ 78 | if (_pointcloud_mode == "XYZ") { 79 | CloudOS1XYZ cloud_aux; 80 | convert2XYZ(cloud, cloud_aux); 81 | pcl::toROSMsg(cloud_aux, msg); 82 | } else if (_pointcloud_mode == "XYZI") { 83 | CloudOS1XYZI cloud_aux; 84 | convert2XYZI(cloud, cloud_aux); 85 | pcl::toROSMsg(cloud_aux, msg); 86 | } else if (_pointcloud_mode == "XYZIR") { 87 | CloudOS1XYZIR cloud_aux; 88 | convert2XYZIR(cloud, cloud_aux); 89 | pcl::toROSMsg(cloud_aux, msg); 90 | } else if (_pointcloud_mode == "XYZIF") { 91 | CloudOS1XYZIF cloud_aux; 92 | convert2XYZIF(cloud, cloud_aux); 93 | pcl::toROSMsg(cloud_aux, msg); 94 | } else if (_pointcloud_mode == "XYZIRF") { 95 | CloudOS1XYZIRF cloud_aux; 96 | convert2XYZIRF(cloud, cloud_aux); 97 | pcl::toROSMsg(cloud_aux, msg); 98 | } else if (_pointcloud_mode == "XYZIFN") { 99 | CloudOS1XYZIFN cloud_aux; 100 | convert2XYZIFN(cloud, cloud_aux); 101 | pcl::toROSMsg(cloud_aux, msg); 102 | } else if (_pointcloud_mode == "XYZIRFN") { 103 | CloudOS1XYZIRFN cloud_aux; 104 | convert2XYZIRFN(cloud, cloud_aux); 105 | pcl::toROSMsg(cloud_aux, msg); 106 | } else { //"NATIVE" 107 | pcl::toROSMsg(cloud, msg); 108 | } 109 | 110 | msg.header.frame_id = frame; 111 | /** 112 | * @note Changed timestamp from LiDAR to ROS time for Autoware operation 113 | */ 114 | //msg.header.stamp.fromNSec(timestamp.count()); //<-- original code of OS1 driver 115 | msg.header.stamp = ros::Time::now(); //<-- prefered time mode in Autoware 116 | return msg; 117 | } 118 | 119 | static PointOS1 nth_point(int ind, const uint8_t* col_buf) { 120 | float h_angle_0 = col_h_angle(col_buf); 121 | auto tte = trig_table[ind]; 122 | const uint8_t* px_buf = nth_px(ind, col_buf); 123 | float r = px_range(px_buf) / 1000.0; 124 | float h_angle = tte.beam_azimuth_angles + h_angle_0; 125 | 126 | PointOS1 point; 127 | point.reflectivity = px_reflectivity(px_buf); 128 | point.intensity = px_signal_photons(px_buf); 129 | point.noise = px_noise_photons(px_buf); //added to extract ambient noise data 130 | point.x = -r * tte.cos_beam_altitude_angles * cosf(h_angle); 131 | point.y = r * tte.cos_beam_altitude_angles * sinf(h_angle); 132 | point.z = r * tte.sin_beam_altitude_angles; 133 | point.ring = ind; 134 | 135 | return point; 136 | } 137 | void add_packet_to_cloud(ns scan_start_ts, ns scan_duration, 138 | const PacketMsg& pm, CloudOS1& cloud) { 139 | const uint8_t* buf = pm.buf.data(); 140 | for (int icol = 0; icol < columns_per_buffer; icol++) { 141 | const uint8_t* col_buf = nth_col(icol, buf); 142 | float ts = (col_timestamp(col_buf) - scan_start_ts.count()) / 143 | (float)scan_duration.count(); 144 | 145 | for (int ipx = 0; ipx < pixels_per_column; ipx++) { 146 | auto p = nth_point(ipx, col_buf); 147 | p.t = ts; 148 | cloud.push_back(p); 149 | } 150 | } 151 | } 152 | 153 | void spin(const client& cli, 154 | const std::function& lidar_handler, 155 | const std::function& imu_handler) { 156 | PacketMsg lidar_packet, imu_packet; 157 | lidar_packet.buf.resize(lidar_packet_bytes + 1); 158 | imu_packet.buf.resize(imu_packet_bytes + 1); 159 | 160 | while (ros::ok()) { 161 | auto state = poll_client(cli); 162 | if (state & ERROR) { 163 | ROS_ERROR("spin: poll_client returned error"); 164 | return; 165 | } 166 | if (state & LIDAR_DATA) { 167 | if (read_lidar_packet(cli, lidar_packet.buf.data())) 168 | lidar_handler(lidar_packet); 169 | } 170 | if (state & IMU_DATA) { 171 | if (read_imu_packet(cli, imu_packet.buf.data())) 172 | imu_handler(imu_packet); 173 | } 174 | ros::spinOnce(); 175 | } 176 | } 177 | 178 | static ns nearest_scan_dur(ns scan_dur, ns ts) { 179 | return ns((ts.count() / scan_dur.count()) * scan_dur.count()); 180 | }; 181 | 182 | std::function batch_packets( 183 | ns scan_dur, const std::function& f) { 184 | auto cloud = std::make_shared(); 185 | auto scan_ts = ns(-1L); 186 | 187 | return [=](const PacketMsg& pm) mutable { 188 | ns packet_ts = OS1::timestamp_of_lidar_packet(pm); 189 | if (scan_ts.count() == -1L) 190 | scan_ts = nearest_scan_dur(scan_dur, packet_ts); 191 | 192 | OS1::add_packet_to_cloud(scan_ts, scan_dur, pm, *cloud); 193 | 194 | auto batch_dur = packet_ts - scan_ts; 195 | if (batch_dur >= scan_dur || batch_dur < ns(0)) { 196 | f(scan_ts, *cloud); 197 | 198 | cloud->clear(); 199 | scan_ts = ns(-1L); 200 | } 201 | }; 202 | } 203 | 204 | /** 205 | * @note Added to support Velodyne compatible pointcloud format for Autoware 206 | */ 207 | void set_point_mode(std::string mode_xyzir) 208 | { 209 | _pointcloud_mode = mode_xyzir; 210 | } 211 | 212 | void convert2XYZ(const CloudOS1& in, CloudOS1XYZ& out) 213 | { 214 | out.points.clear(); 215 | pcl::PointXYZ q; 216 | for (auto p : in.points) { 217 | q.x = p.x; 218 | q.y = p.y; 219 | q.z = p.z; 220 | out.points.push_back(q); 221 | } 222 | } 223 | 224 | void convert2XYZI(const CloudOS1& in, CloudOS1XYZI& out) 225 | { 226 | out.points.clear(); 227 | pcl::PointXYZI q; 228 | for (auto p : in.points) { 229 | q.x = p.x; 230 | q.y = p.y; 231 | q.z = p.z; 232 | q.intensity = p.intensity; 233 | out.points.push_back(q); 234 | } 235 | } 236 | 237 | /** 238 | * @note Added to support Velodyne compatible pointcloud format for Autoware 239 | */ 240 | void convert2XYZIR(const CloudOS1& in, CloudOS1XYZIR& out) 241 | { 242 | out.points.clear(); 243 | PointXYZIR q; 244 | for (auto p : in.points) { 245 | q.x = p.x; 246 | q.y = p.y; 247 | q.z = p.z; 248 | q.intensity = ((float)p.intensity/65535.0)*255.0; //velodyne uses values in [0..255] range 249 | q.ring = pixels_per_column - p.ring; //reverse the ring order to match Velodyne's (except NATIVE mode which respects Ouster original ring order) 250 | out.points.push_back(q); 251 | } 252 | } 253 | 254 | /** 255 | * @note Extract intensity and reflectivity data 256 | */ 257 | void convert2XYZIF(const CloudOS1& in, CloudOS1XYZIF& out) 258 | { 259 | out.points.clear(); 260 | PointXYZIF q; 261 | for (auto p : in.points) { 262 | q.x = p.x; 263 | q.y = p.y; 264 | q.z = p.z; 265 | q.intensity = p.intensity; 266 | q.reflectivity = p.reflectivity; 267 | out.points.push_back(q); 268 | } 269 | } 270 | 271 | /** 272 | * @note Extract intensity, ring and reflectivity data 273 | */ 274 | void convert2XYZIRF(const CloudOS1& in, CloudOS1XYZIRF& out) 275 | { 276 | out.points.clear(); 277 | PointXYZIRF q; 278 | for (auto p : in.points) { 279 | q.x = p.x; 280 | q.y = p.y; 281 | q.z = p.z; 282 | q.intensity = p.intensity; 283 | q.ring = pixels_per_column - p.ring; //reverse the ring order to match Velodyne's (except NATIVE mode which respects Ouster original ring order) 284 | q.reflectivity = p.reflectivity; 285 | out.points.push_back(q); 286 | } 287 | } 288 | 289 | /** 290 | * @note Extract intensity, reflectivity and ambient noise data 291 | */ 292 | void convert2XYZIFN(const CloudOS1& in, CloudOS1XYZIFN& out) 293 | { 294 | out.points.clear(); 295 | PointXYZIFN q; 296 | for (auto p : in.points) { 297 | q.x = p.x; 298 | q.y = p.y; 299 | q.z = p.z; 300 | q.intensity = p.intensity; 301 | q.reflectivity = p.reflectivity; 302 | q.noise = p.noise; 303 | out.points.push_back(q); 304 | } 305 | } 306 | 307 | /** 308 | * @note Extract intensity, ring, reflectivity and ambient noise data 309 | */ 310 | void convert2XYZIRFN(const CloudOS1& in, CloudOS1XYZIRFN& out) 311 | { 312 | out.points.clear(); 313 | PointXYZIRFN q; 314 | for (auto p : in.points) { 315 | q.x = p.x; 316 | q.y = p.y; 317 | q.z = p.z; 318 | q.intensity = p.intensity; 319 | q.ring = pixels_per_column - p.ring; //reverse the ring order to match Velodyne's (except NATIVE mode which respects Ouster original ring order) 320 | q.reflectivity = p.reflectivity; 321 | q.noise = p.noise; 322 | out.points.push_back(q); 323 | } 324 | } 325 | 326 | } 327 | } 328 | --------------------------------------------------------------------------------