From d8cf623a922b1f12995e8c71295924c2905bd9a3 Mon Sep 17 00:00:00 2001 From: Joshua Whitley Date: Wed, 31 May 2023 14:44:30 +0800 Subject: [PATCH] feat: support vls128 for ros2 (#493) (#514) * feat: support vls128 for ros2 (#493) * Update rolling ci (#512) Fix Rolling CI and associated linter errors. * Add support for the velodyne Alpha Prime (#370) * Add support for the velodyne Alpha Prime * Change packet rate for the VLS128 according to the times specified in the manual * Organize setup functions to avoid code duplication. Add a constant for the model ID of the VLS128. * Use the defined constants to calculate the time offset of the points for the VLS128 Co-authored-by: jugo * Add VLS128 launch and calibration file (#382) Signed-off-by: wep21 * vls128: add line only once all four banks are processed (#413) Signed-off-by: Sebastian Scherer (CR/AAS3) Signed-off-by: wep21 * fix: apply uncrustify Signed-off-by: wep21 * Fixing incorrect type in velodyne_pointcloud. * Fixed non working vls128 fork * Added organized cloud compliance and remove some useless code * Corrected the azimuth offset calculation and put the declation of variable where there are used * Fix linter errors. --------- Signed-off-by: wep21 Signed-off-by: Sebastian Scherer (CR/AAS3) Co-authored-by: Joshua Whitley Co-authored-by: Institute for Autonomous Systems Technology Co-authored-by: jugo Co-authored-by: Sebastian Scherer Co-authored-by: wep21 Co-authored-by: Joshua Whitley Co-authored-by: Mtestor * Fix double include --------- Signed-off-by: wep21 Signed-off-by: Sebastian Scherer (CR/AAS3) Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Institute for Autonomous Systems Technology Co-authored-by: jugo Co-authored-by: Sebastian Scherer Co-authored-by: wep21 Co-authored-by: Joshua Whitley Co-authored-by: Mtestor --- ...lodyne-all-nodes-VLS128-composed-launch.py | 85 ++++ .../velodyne-all-nodes-VLS128-launch.py | 78 ++++ .../VLS128-velodyne_driver_node-params.yaml | 14 + ...dyne_driver_node-VLS128-composed-launch.py | 68 +++ .../velodyne_driver_node-VLS128-launch.py | 60 +++ velodyne_driver/src/driver/driver.cpp | 11 +- ...VLS128-velodyne_transform_node-params.yaml | 10 + .../include/velodyne_pointcloud/rawdata.hpp | 32 ++ ...e_transform_node-VLS128-composed-launch.py | 67 +++ .../velodyne_transform_node-VLS128-launch.py | 63 +++ velodyne_pointcloud/params/VLS128.yaml | 387 ++++++++++++++++++ .../src/conversions/transform.cpp | 2 +- velodyne_pointcloud/src/lib/rawdata.cpp | 237 ++++++++++- 13 files changed, 1103 insertions(+), 11 deletions(-) create mode 100644 velodyne/launch/velodyne-all-nodes-VLS128-composed-launch.py create mode 100644 velodyne/launch/velodyne-all-nodes-VLS128-launch.py create mode 100644 velodyne_driver/config/VLS128-velodyne_driver_node-params.yaml create mode 100644 velodyne_driver/launch/velodyne_driver_node-VLS128-composed-launch.py create mode 100644 velodyne_driver/launch/velodyne_driver_node-VLS128-launch.py create mode 100644 velodyne_pointcloud/config/VLS128-velodyne_transform_node-params.yaml create mode 100644 velodyne_pointcloud/launch/velodyne_transform_node-VLS128-composed-launch.py create mode 100644 velodyne_pointcloud/launch/velodyne_transform_node-VLS128-launch.py create mode 100644 velodyne_pointcloud/params/VLS128.yaml diff --git a/velodyne/launch/velodyne-all-nodes-VLS128-composed-launch.py b/velodyne/launch/velodyne-all-nodes-VLS128-composed-launch.py new file mode 100644 index 000000000..4884e85ad --- /dev/null +++ b/velodyne/launch/velodyne-all-nodes-VLS128-composed-launch.py @@ -0,0 +1,85 @@ +# Copyright 2019 Open Source Robotics Foundation, Inc. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# +# 3. Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +"""Launch the velodyne driver, pointcloud, and laserscan nodes in a composable container with default configuration.""" + +import os +import yaml + +import ament_index_python.packages +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + driver_share_dir = ament_index_python.packages.get_package_share_directory('velodyne_driver') + driver_params_file = os.path.join(driver_share_dir, 'config', 'VLS128-velodyne_driver_node-params.yaml') + with open(driver_params_file, 'r') as f: + driver_params = yaml.safe_load(f)['velodyne_driver_node']['ros__parameters'] + + convert_share_dir = ament_index_python.packages.get_package_share_directory('velodyne_pointcloud') + convert_params_file = os.path.join(convert_share_dir, 'config', 'VLS128-velodyne_transform_node-params.yaml') + with open(convert_params_file, 'r') as f: + convert_params = yaml.safe_load(f)['velodyne_transform_node']['ros__parameters'] + convert_params['calibration'] = os.path.join(convert_share_dir, 'params', 'VLS128.yaml') + + laserscan_share_dir = ament_index_python.packages.get_package_share_directory('velodyne_laserscan') + laserscan_params_file = os.path.join(laserscan_share_dir, 'config', 'default-velodyne_laserscan_node-params.yaml') + with open(laserscan_params_file, 'r') as f: + laserscan_params = yaml.safe_load(f)['velodyne_laserscan_node']['ros__parameters'] + + container = ComposableNodeContainer( + name='velodyne_container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + ComposableNode( + package='velodyne_driver', + plugin='velodyne_driver::VelodyneDriver', + name='velodyne_driver_node', + parameters=[driver_params]), + ComposableNode( + package='velodyne_pointcloud', + plugin='velodyne_pointcloud::Transform', + name='velodyne_transform_node', + parameters=[convert_params]), + ComposableNode( + package='velodyne_laserscan', + plugin='velodyne_laserscan::VelodyneLaserScan', + name='velodyne_laserscan_node', + parameters=[laserscan_params]), + ], + output='both', + ) + + return LaunchDescription([container]) diff --git a/velodyne/launch/velodyne-all-nodes-VLS128-launch.py b/velodyne/launch/velodyne-all-nodes-VLS128-launch.py new file mode 100644 index 000000000..1fd8d652a --- /dev/null +++ b/velodyne/launch/velodyne-all-nodes-VLS128-launch.py @@ -0,0 +1,78 @@ +# Copyright 2019 Open Source Robotics Foundation, Inc. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# +# 3. Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +"""Launch the velodyne driver, pointcloud, and laserscan nodes with default configuration.""" + +import os +import yaml + +import ament_index_python.packages +import launch +import launch_ros.actions + + +def generate_launch_description(): + driver_share_dir = ament_index_python.packages.get_package_share_directory('velodyne_driver') + driver_params_file = os.path.join(driver_share_dir, 'config', 'VLS128-velodyne_driver_node-params.yaml') + velodyne_driver_node = launch_ros.actions.Node(package='velodyne_driver', + executable='velodyne_driver_node', + output='both', + parameters=[driver_params_file]) + + convert_share_dir = ament_index_python.packages.get_package_share_directory('velodyne_pointcloud') + convert_params_file = os.path.join(convert_share_dir, 'config', 'VLS128-velodyne_transform_node-params.yaml') + with open(convert_params_file, 'r') as f: + convert_params = yaml.safe_load(f)['velodyne_transform_node']['ros__parameters'] + convert_params['calibration'] = os.path.join(convert_share_dir, 'params', 'VLS128.yaml') + velodyne_transform_node = launch_ros.actions.Node(package='velodyne_pointcloud', + executable='velodyne_transform_node', + output='both', + parameters=[convert_params]) + + laserscan_share_dir = ament_index_python.packages.get_package_share_directory('velodyne_laserscan') + laserscan_params_file = os.path.join(laserscan_share_dir, 'config', 'default-velodyne_laserscan_node-params.yaml') + velodyne_laserscan_node = launch_ros.actions.Node(package='velodyne_laserscan', + executable='velodyne_laserscan_node', + output='both', + parameters=[laserscan_params_file]) + + + return launch.LaunchDescription([velodyne_driver_node, + velodyne_transform_node, + velodyne_laserscan_node, + + launch.actions.RegisterEventHandler( + event_handler=launch.event_handlers.OnProcessExit( + target_action=velodyne_driver_node, + on_exit=[launch.actions.EmitEvent( + event=launch.events.Shutdown())], + )), + ]) diff --git a/velodyne_driver/config/VLS128-velodyne_driver_node-params.yaml b/velodyne_driver/config/VLS128-velodyne_driver_node-params.yaml new file mode 100644 index 000000000..18c3f543f --- /dev/null +++ b/velodyne_driver/config/VLS128-velodyne_driver_node-params.yaml @@ -0,0 +1,14 @@ +velodyne_driver_node: + ros__parameters: + device_ip: 192.168.1.201 + gps_time: false + time_offset: 0.0 + enabled: true + read_once: false + read_fast: false + repeat_delay: 0.0 + frame_id: velodyne + model: VLS128 + rpm: 600.0 + port: 2368 + timestamp_first_packet: false diff --git a/velodyne_driver/launch/velodyne_driver_node-VLS128-composed-launch.py b/velodyne_driver/launch/velodyne_driver_node-VLS128-composed-launch.py new file mode 100644 index 000000000..59860f00a --- /dev/null +++ b/velodyne_driver/launch/velodyne_driver_node-VLS128-composed-launch.py @@ -0,0 +1,68 @@ +# Copyright 2019 Open Source Robotics Foundation, Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +"""Launch the velodyne driver node in a composable container with default configuration.""" + +import os + +import ament_index_python.packages + +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +import yaml + + +def generate_launch_description(): + config_directory = os.path.join( + ament_index_python.packages.get_package_share_directory('velodyne_driver'), + 'config') + param_config = os.path.join(config_directory, 'VLS128-velodyne_driver_node-params.yaml') + with open(param_config, 'r') as f: + params = yaml.safe_load(f)['velodyne_driver_node']['ros__parameters'] + container = ComposableNodeContainer( + name='velodyne_driver_container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + ComposableNode( + package='velodyne_driver', + plugin='velodyne_driver::VelodyneDriver', + name='velodyne_driver_node', + parameters=[params]), + ], + output='both', + ) + + return LaunchDescription([container]) diff --git a/velodyne_driver/launch/velodyne_driver_node-VLS128-launch.py b/velodyne_driver/launch/velodyne_driver_node-VLS128-launch.py new file mode 100644 index 000000000..3bde2dc90 --- /dev/null +++ b/velodyne_driver/launch/velodyne_driver_node-VLS128-launch.py @@ -0,0 +1,60 @@ +# Copyright 2019 Open Source Robotics Foundation, Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +"""Launch the velodyne driver node with default configuration.""" + +import os + +import ament_index_python.packages +import launch +import launch_ros.actions + + +def generate_launch_description(): + config_directory = os.path.join( + ament_index_python.packages.get_package_share_directory('velodyne_driver'), + 'config') + params = os.path.join(config_directory, 'VLS128-velodyne_driver_node-params.yaml') + velodyne_driver_node = launch_ros.actions.Node(package='velodyne_driver', + executable='velodyne_driver_node', + output='both', + parameters=[params]) + + return launch.LaunchDescription([velodyne_driver_node, + + launch.actions.RegisterEventHandler( + event_handler=launch.event_handlers.OnProcessExit( + target_action=velodyne_driver_node, + on_exit=[launch.actions.EmitEvent( + event=launch.events.Shutdown())], + )), + ]) diff --git a/velodyne_driver/src/driver/driver.cpp b/velodyne_driver/src/driver/driver.cpp index 4064aa95f..dd1402987 100644 --- a/velodyne_driver/src/driver/driver.cpp +++ b/velodyne_driver/src/driver/driver.cpp @@ -88,9 +88,14 @@ VelodyneDriver::VelodyneDriver(const rclcpp::NodeOptions & options) // get model name, validate string, determine packet rate double packet_rate; // packet frequency (Hz) std::string model_full_name; - if ((config_.model == "64E_S2") || - (config_.model == "64E_S2.1")) - { // generates 1333312 points per second + if (config_.model == "VLS128") { + packet_rate = 6253.9; // 3 firing cycles in a data packet. + // 3 x 53.3 μs = 0.1599 ms is the accumulation delay per packet. + // 1 packet/0.1599 ms = 6253.9 packets/second + + model_full_name = config_.model; + } else if ((config_.model == "64E_S2") || (config_.model == "64E_S2.1")) { + // generates 1333312 points per second packet_rate = 3472.17; // 1 packet holds 384 points - 1333312 / 384 model_full_name = std::string("HDL-") + config_.model; } else if (config_.model == "64E") { diff --git a/velodyne_pointcloud/config/VLS128-velodyne_transform_node-params.yaml b/velodyne_pointcloud/config/VLS128-velodyne_transform_node-params.yaml new file mode 100644 index 000000000..5187044ad --- /dev/null +++ b/velodyne_pointcloud/config/VLS128-velodyne_transform_node-params.yaml @@ -0,0 +1,10 @@ +velodyne_transform_node: + ros__parameters: + calibration: VLS128.yaml + model: VLS128 + min_range: 0.4 + max_range: 300.0 + view_direction: 0.0 + fixed_frame: "" + target_frame: "" + organize_cloud: true diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.hpp b/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.hpp index 89748b4cc..07ba5aa98 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.hpp +++ b/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.hpp @@ -98,6 +98,20 @@ static const int BLOCKS_PER_PACKET = 12; static const int PACKET_STATUS_SIZE = 4; static const int SCANS_PER_PACKET = (SCANS_PER_BLOCK * BLOCKS_PER_PACKET); +/** Special Definitions for VLS128 support **/ +// These are used to detect which bank of 32 lasers is in this block +static const uint16_t VLS128_BANK_1 = 0xeeff; +static const uint16_t VLS128_BANK_2 = 0xddff; +static const uint16_t VLS128_BANK_3 = 0xccff; +static const uint16_t VLS128_BANK_4 = 0xbbff; + +static const float VLS128_CHANNEL_TDURATION = 2.665f; // [µs] Channels corresponds to one laser firing // NOLINT +static const float VLS128_SEQ_TDURATION = 53.3f; // [µs] Sequence is a set of laser firings including recharging // NOLINT +static const float VLS128_TOH_ADJUSTMENT = 8.7f; // [µs] μs. Top Of the Hour is aligned with the fourth firing group in a firing sequence. // NOLINT +static const float VLS128_DISTANCE_RESOLUTION = 0.004f; // [m] +static const float VLS128_MODEL_ID = 161; + + /** \brief Raw Velodyne packet. * * revolution is described in the device manual as incrementing @@ -123,6 +137,10 @@ class RawData final public: RawData(const std::string & calibration_file, const std::string & model); + void setupSinCosCache(); + void setupAzimuthCache(); + bool loadCalibration(); + void unpack( const velodyne_msgs::msg::VelodynePacket & pkt, DataContainerBase & data, const rclcpp::Time & scan_start_time); @@ -152,6 +170,9 @@ class RawData final float sin_rot_table_[ROTATION_MAX_UNITS]{}; float cos_rot_table_[ROTATION_MAX_UNITS]{}; + // Caches the azimuth percent offset for the VLS-128 laser firings + float vls_128_laser_azimuth_cache_[16]; + // timing offset lookup table std::vector> timing_offsets_; @@ -167,6 +188,17 @@ class RawData final void unpack_vlp16( const velodyne_msgs::msg::VelodynePacket & pkt, DataContainerBase & data, const rclcpp::Time & scan_start_time); + + void unpack_vls128( + const velodyne_msgs::msg::VelodynePacket & pkt, DataContainerBase & data, + const rclcpp::Time & scan_start_time); + + /** in-line test whether a point is in range */ + inline bool pointInRange(const float range) + { + return range >= config_.min_range && + range <= config_.max_range; + } }; } // namespace velodyne_rawdata diff --git a/velodyne_pointcloud/launch/velodyne_transform_node-VLS128-composed-launch.py b/velodyne_pointcloud/launch/velodyne_transform_node-VLS128-composed-launch.py new file mode 100644 index 000000000..0bfa45240 --- /dev/null +++ b/velodyne_pointcloud/launch/velodyne_transform_node-VLS128-composed-launch.py @@ -0,0 +1,67 @@ +# Copyright 2019 Open Source Robotics Foundation, Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +"""Launch the velodyne pointcloud convert with default configuration.""" + +import os + +import ament_index_python.packages + +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +import yaml + + +def generate_launch_description(): + share_dir = ament_index_python.packages.get_package_share_directory('velodyne_pointcloud') + params_file = os.path.join(share_dir, 'config', 'VLS128-velodyne_transform_node-params.yaml') + with open(params_file, 'r') as f: + params = yaml.safe_load(f)['velodyne_transform_node']['ros__parameters'] + params['calibration'] = os.path.join(share_dir, 'params', 'VLS128.yaml') + container = ComposableNodeContainer( + name='velodyne_pointcloud_transform_container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + ComposableNode( + package='velodyne_pointcloud', + plugin='velodyne_pointcloud::Transform', + name='velodyne_transform_node', + parameters=[params]), + ], + output='both', + ) + + return LaunchDescription([container]) diff --git a/velodyne_pointcloud/launch/velodyne_transform_node-VLS128-launch.py b/velodyne_pointcloud/launch/velodyne_transform_node-VLS128-launch.py new file mode 100644 index 000000000..9bfd089fe --- /dev/null +++ b/velodyne_pointcloud/launch/velodyne_transform_node-VLS128-launch.py @@ -0,0 +1,63 @@ +# Copyright 2019 Open Source Robotics Foundation, Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +"""Launch the velodyne pointcloud transform node with default configuration.""" + +import os + +import ament_index_python.packages +import launch +import launch_ros.actions + +import yaml + + +def generate_launch_description(): + share_dir = ament_index_python.packages.get_package_share_directory('velodyne_pointcloud') + params_file = os.path.join(share_dir, 'config', 'VLS128-velodyne_transform_node-params.yaml') + with open(params_file, 'r') as f: + params = yaml.safe_load(f)['velodyne_transform_node']['ros__parameters'] + params['calibration'] = os.path.join(share_dir, 'params', 'VLS128.yaml') + velodyne_transform_node = launch_ros.actions.Node(package='velodyne_pointcloud', + executable='velodyne_transform_node', + output='both', + parameters=[params]) + + return launch.LaunchDescription([velodyne_transform_node, + + launch.actions.RegisterEventHandler( + event_handler=launch.event_handlers.OnProcessExit( + target_action=velodyne_transform_node, + on_exit=[launch.actions.EmitEvent( + event=launch.events.Shutdown())], + )), + ]) diff --git a/velodyne_pointcloud/params/VLS128.yaml b/velodyne_pointcloud/params/VLS128.yaml new file mode 100644 index 000000000..a014d1778 --- /dev/null +++ b/velodyne_pointcloud/params/VLS128.yaml @@ -0,0 +1,387 @@ +lasers: +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 0, rot_correction: -0.1108982206717197, vert_correction: -0.2049365607691742, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 1, rot_correction: -0.07937757438070212, vert_correction: -0.034732052114687155, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 2, rot_correction: -0.04768239516448509, vert_correction: 0.059341194567807204, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 3, rot_correction: -0.015899949485668342, vert_correction: -0.09232791743050003, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 4, rot_correction: 0.015899949485668342, vert_correction: -0.013613568165555772, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 5, rot_correction: 0.04768239516448509, vert_correction: 0.0804596785169386, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 6, rot_correction: 0.07937757438070212, vert_correction: -0.07120943348136864, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 7, rot_correction: 0.1108982206717197, vert_correction: 0.022863813201125717, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 8, rot_correction: -0.1108982206717197, vert_correction: -0.11344640137963143, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 9, rot_correction: -0.07937757438070212, vert_correction: -0.01937315469713706, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 10, rot_correction: -0.04768239516448509, vert_correction: 0.0747000919853573, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 11, rot_correction: -0.015899949485668342, vert_correction: -0.07696902001294993, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 12, rot_correction: 0.015899949485668342, vert_correction: 0.0017453292519943296, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 13, rot_correction: 0.04768239516448509, vert_correction: 0.11309733552923257, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 14, rot_correction: 0.07937757438070212, vert_correction: -0.05585053606381855, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 15, rot_correction: 0.1108982206717197, vert_correction: 0.03822271061867582, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 16, rot_correction: -0.1108982206717197, vert_correction: -0.06736970912698112, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 17, rot_correction: -0.07937757438070212, vert_correction: 0.026703537555513242, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 18, rot_correction: -0.04768239516448509, vert_correction: -0.16133823605435582, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 19, rot_correction: -0.015899949485668342, vert_correction: -0.030892327760299633, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 20, rot_correction: 0.015899949485668342, vert_correction: 0.04782202150464463, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 21, rot_correction: 0.04768239516448509, vert_correction: -0.10384709049366261, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 22, rot_correction: 0.07937757438070212, vert_correction: -0.009773843811168246, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 23, rot_correction: 0.1108982206717197, vert_correction: 0.08429940287132612, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 24, rot_correction: -0.1108982206717197, vert_correction: -0.05201081170943102, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 25, rot_correction: -0.07937757438070212, vert_correction: 0.04206243497306335, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 26, rot_correction: -0.04768239516448509, vert_correction: -0.1096066770252439, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 27, rot_correction: -0.015899949485668342, vert_correction: -0.015533430342749533, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 28, rot_correction: 0.015899949485668342, vert_correction: 0.06318091892219473, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 29, rot_correction: 0.04768239516448509, vert_correction: -0.08848819307611251, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 30, rot_correction: 0.07937757438070212, vert_correction: 0.005585053606381855, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 31, rot_correction: 0.1108982206717197, vert_correction: 0.1322959573011702, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 32, rot_correction: -0.1108982206717197, vert_correction: -0.005934119456780721, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 33, rot_correction: -0.07937757438070212, vert_correction: 0.09040805525330627, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 34, rot_correction: -0.04768239516448509, vert_correction: -0.0635299847725936, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 35, rot_correction: -0.015899949485668342, vert_correction: 0.030543261909900768, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 36, rot_correction: 0.015899949485668342, vert_correction: -0.4363323129985824, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 37, rot_correction: 0.04768239516448509, vert_correction: -0.04241150082346221, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 38, rot_correction: 0.07937757438070212, vert_correction: 0.05166174585903215, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 39, rot_correction: 0.1108982206717197, vert_correction: -0.10000736613927509, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 40, rot_correction: -0.1108982206717197, vert_correction: 0.00942477796076938, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 41, rot_correction: -0.07937757438070212, vert_correction: 0.16929693744344995, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 42, rot_correction: -0.04768239516448509, vert_correction: -0.04817108735504349, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 43, rot_correction: -0.015899949485668342, vert_correction: 0.04590215932745086, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 44, rot_correction: 0.015899949485668342, vert_correction: -0.13351768777756623, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 45, rot_correction: 0.04768239516448509, vert_correction: -0.027052603405912107, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 46, rot_correction: 0.07937757438070212, vert_correction: 0.06702064327658225, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 47, rot_correction: 0.1108982206717197, vert_correction: -0.08464846872172498, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 48, rot_correction: -0.1108982206717197, vert_correction: 0.05550147021341968, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 49, rot_correction: -0.07937757438070212, vert_correction: -0.09616764178488756, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 50, rot_correction: -0.04768239516448509, vert_correction: -0.0020943951023931952, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 51, rot_correction: -0.015899949485668342, vert_correction: 0.10000736613927509, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 52, rot_correction: 0.015899949485668342, vert_correction: -0.07504915783575616, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 53, rot_correction: 0.04768239516448509, vert_correction: 0.019024088846738195, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 54, rot_correction: 0.07937757438070212, vert_correction: -0.2799857186049304, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 55, rot_correction: 0.1108982206717197, vert_correction: -0.038571776469074684, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 56, rot_correction: -0.1108982206717197, vert_correction: 0.07086036763096977, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 57, rot_correction: -0.07937757438070212, vert_correction: -0.08080874436733745, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 58, rot_correction: -0.04768239516448509, vert_correction: 0.013264502315156905, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 59, rot_correction: -0.015899949485668342, vert_correction: 0.2617993877991494, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 60, rot_correction: 0.015899949485668342, vert_correction: -0.05969026041820607, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 61, rot_correction: 0.04768239516448509, vert_correction: 0.03438298626428829, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 62, rot_correction: 0.07937757438070212, vert_correction: -0.11955505376161157, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 63, rot_correction: 0.1108982206717197, vert_correction: -0.023212879051524585, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 64, rot_correction: -0.1108982206717197, vert_correction: -0.09808750396208132, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 65, rot_correction: -0.07937757438070212, vert_correction: -0.004014257279586958, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 66, rot_correction: -0.04768239516448509, vert_correction: 0.0947713783832921, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 67, rot_correction: -0.015899949485668342, vert_correction: -0.06161012259539983, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 68, rot_correction: 0.015899949485668342, vert_correction: 0.01710422666954443, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 69, rot_correction: 0.04768239516448509, vert_correction: -0.34177037412552963, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 70, rot_correction: 0.07937757438070212, vert_correction: -0.040491638646268445, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 71, rot_correction: 0.1108982206717197, vert_correction: 0.053581608036225914, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 72, rot_correction: -0.1108982206717197, vert_correction: -0.08272860654453122, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 73, rot_correction: -0.07937757438070212, vert_correction: 0.011344640137963142, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 74, rot_correction: -0.04768239516448509, vert_correction: 0.20507618710933373, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 75, rot_correction: -0.015899949485668342, vert_correction: -0.046251225177849735, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 76, rot_correction: 0.015899949485668342, vert_correction: 0.03246312408709453, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 77, rot_correction: 0.04768239516448509, vert_correction: -0.12479104151759457, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 78, rot_correction: 0.07937757438070212, vert_correction: -0.025132741228718343, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 79, rot_correction: 0.1108982206717197, vert_correction: 0.06894050545377602, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 80, rot_correction: -0.1108982206717197, vert_correction: -0.03665191429188092, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 81, rot_correction: -0.07937757438070212, vert_correction: 0.05742133239061344, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 82, rot_correction: -0.04768239516448509, vert_correction: -0.0942477796076938, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 83, rot_correction: -0.015899949485668342, vert_correction: -0.00017453292519943296, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 84, rot_correction: 0.015899949485668342, vert_correction: 0.07853981633974483, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 85, rot_correction: 0.04768239516448509, vert_correction: -0.07312929565856241, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 86, rot_correction: 0.07937757438070212, vert_correction: 0.020943951023931952, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 87, rot_correction: 0.1108982206717197, vert_correction: -0.23675391303303078, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 88, rot_correction: -0.1108982206717197, vert_correction: -0.02129301687433082, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 89, rot_correction: -0.07937757438070212, vert_correction: 0.07278022980816354, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 90, rot_correction: -0.04768239516448509, vert_correction: -0.07888888219014369, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 91, rot_correction: -0.015899949485668342, vert_correction: 0.015184364492350668, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 92, rot_correction: 0.015899949485668342, vert_correction: 0.10611601852125524, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 93, rot_correction: 0.04768239516448509, vert_correction: -0.05777039824101231, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 94, rot_correction: 0.07937757438070212, vert_correction: 0.03630284844148206, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 95, rot_correction: 0.1108982206717197, vert_correction: -0.11606439525762292, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 96, rot_correction: -0.1108982206717197, vert_correction: 0.024783675378319478, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 97, rot_correction: -0.07937757438070212, vert_correction: -0.18057176441133332, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 98, rot_correction: -0.04768239516448509, vert_correction: -0.032812189937493394, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 99, rot_correction: -0.015899949485668342, vert_correction: 0.061261056745000965, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 100, rot_correction: 0.015899949485668342, vert_correction: -0.10576695267085637, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 101, rot_correction: 0.04768239516448509, vert_correction: -0.011693705988362009, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 102, rot_correction: 0.07937757438070212, vert_correction: 0.08237954069413235, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 103, rot_correction: 0.1108982206717197, vert_correction: -0.06928957130417489, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 104, rot_correction: -0.1108982206717197, vert_correction: 0.04014257279586958, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 105, rot_correction: -0.07937757438070212, vert_correction: -0.11152653920243766, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 106, rot_correction: -0.04768239516448509, vert_correction: -0.017453292519943295, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 107, rot_correction: -0.015899949485668342, vert_correction: 0.07661995416255106, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 108, rot_correction: 0.015899949485668342, vert_correction: -0.09040805525330627, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 109, rot_correction: 0.04768239516448509, vert_correction: 0.003665191429188092, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 110, rot_correction: 0.07937757438070212, vert_correction: 0.12182398178920421, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 111, rot_correction: 0.1108982206717197, vert_correction: -0.05393067388662478, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 112, rot_correction: -0.1108982206717197, vert_correction: 0.08691739674931762, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 113, rot_correction: -0.07937757438070212, vert_correction: -0.06544984694978735, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 114, rot_correction: -0.04768239516448509, vert_correction: 0.028623399732707003, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 115, rot_correction: -0.015899949485668342, vert_correction: -0.1457698991265664, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 116, rot_correction: 0.015899949485668342, vert_correction: -0.044331363000655974, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 117, rot_correction: 0.04768239516448509, vert_correction: 0.04974188368183839, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 118, rot_correction: 0.07937757438070212, vert_correction: -0.10192722831646885, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 119, rot_correction: 0.1108982206717197, vert_correction: -0.007853981633974483, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 120, rot_correction: -0.1108982206717197, vert_correction: 0.14713125594312199, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 121, rot_correction: -0.07937757438070212, vert_correction: -0.05009094953223726, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 122, rot_correction: -0.04768239516448509, vert_correction: 0.0439822971502571, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 123, rot_correction: -0.015899949485668342, vert_correction: -0.10768681484805014, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 124, rot_correction: 0.015899949485668342, vert_correction: -0.02897246558310587, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 125, rot_correction: 0.04768239516448509, vert_correction: 0.0651007810993885, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 126, rot_correction: 0.07937757438070212, vert_correction: -0.08656833089891874, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, laser_id: 127, rot_correction: 0.1108982206717197, vert_correction: 0.007504915783575617, + vert_offset_correction: 0.0} +num_lasers: 128 +distance_resolution: 0.004 diff --git a/velodyne_pointcloud/src/conversions/transform.cpp b/velodyne_pointcloud/src/conversions/transform.cpp index a2d438d4e..10a7db210 100644 --- a/velodyne_pointcloud/src/conversions/transform.cpp +++ b/velodyne_pointcloud/src/conversions/transform.cpp @@ -75,7 +75,7 @@ Transform::Transform(const rclcpp::NodeOptions & options) max_range_desc.description = "maximum range to publish"; rcl_interfaces::msg::FloatingPointRange max_range_range; max_range_range.from_value = 0.1; - max_range_range.to_value = 200.0; + max_range_range.to_value = 300.0; max_range_desc.floating_point_range.push_back(max_range_range); double max_range = this->declare_parameter("max_range", 130.0, max_range_desc); diff --git a/velodyne_pointcloud/src/lib/rawdata.cpp b/velodyne_pointcloud/src/lib/rawdata.cpp index 76ff6be74..d95962a48 100644 --- a/velodyne_pointcloud/src/lib/rawdata.cpp +++ b/velodyne_pointcloud/src/lib/rawdata.cpp @@ -35,6 +35,7 @@ #include #include #include +#include #include @@ -63,12 +64,8 @@ RawData::RawData(const std::string & calibration_file, const std::string & model // RCLCPP_INFO(this->get_logger(), "Number of lasers: %d.", calibration_->num_lasers); - // Set up cached values for sin and cos of all the possible headings - for (uint16_t rot_index = 0; rot_index < ROTATION_MAX_UNITS; ++rot_index) { - float rotation = angles::from_degrees(ROTATION_RESOLUTION * rot_index); - cos_rot_table_[rot_index] = ::cosf(rotation); - sin_rot_table_[rot_index] = ::sinf(rotation); - } + setupSinCosCache(); + setupAzimuthCache(); } /** Update parameters: conversions and update */ @@ -188,6 +185,29 @@ bool RawData::buildTimings() (single_firing * dataPointIndex); } } + } else if (config_.model == "VLS128") { + timing_offsets_.resize(3); + for (size_t i = 0; i < timing_offsets_.size(); ++i) { + timing_offsets_[i].resize(17); // 17 (+1 for the maintenance time after firing group 8) + } + + double full_firing_cycle = VLS128_SEQ_TDURATION * 1e-6; // seconds + double single_firing = VLS128_CHANNEL_TDURATION * 1e-6; // seconds + double offset_paket_time = VLS128_TOH_ADJUSTMENT * 1e-6; // seconds + double sequenceIndex, firingGroupIndex; + // Compute timing offsets + for (size_t x = 0; x < timing_offsets_.size(); ++x) { + for (size_t y = 0; y < timing_offsets_[x].size(); ++y) { + sequenceIndex = x; + firingGroupIndex = y; + timing_offsets_[x][y] = + (full_firing_cycle * sequenceIndex) + (single_firing * firingGroupIndex) - + offset_paket_time; + RCLCPP_DEBUG( + rclcpp::get_logger("velodyne_pointcloud"), + "firing_seque %lu firing_group %lu offset %f", x, y, timing_offsets_[x][y]); + } + } } else { timing_offsets_.clear(); RCLCPP_WARN( @@ -219,6 +239,30 @@ int RawData::numLasers() const return calibration_->num_lasers; } +void RawData::setupSinCosCache() +{ + // Set up cached values for sin and cos of all the possible headings + for (uint16_t rot_index = 0; rot_index < ROTATION_MAX_UNITS; ++rot_index) { + float rotation = angles::from_degrees(ROTATION_RESOLUTION * rot_index); + cos_rot_table_[rot_index] = ::cosf(rotation); + sin_rot_table_[rot_index] = ::sinf(rotation); + } +} + +void RawData::setupAzimuthCache() +{ + if (config_.model == "VLS128") { + for (uint8_t i = 0; i < 16; i++) { + vls_128_laser_azimuth_cache_[i] = + (VLS128_CHANNEL_TDURATION / VLS128_SEQ_TDURATION) * (i + i / 8); + } + } else { + RCLCPP_WARN( + rclcpp::get_logger("velodyne_pointcloud"), + "No Azimuth Cache configured for model %s", config_.model.c_str()); + } +} + /** @brief convert raw packet to point cloud * * @param pkt raw packet to unpack @@ -230,6 +274,12 @@ void RawData::unpack( { // RCLCPP_DEBUG(this->get_logger(), "Received packet, time: %s", pkt.stamp); + /** special parsing for the VLS128 **/ + if (pkt.data[1205] == VLS128_MODEL_ID) { // VLS 128 + unpack_vls128(pkt, data, scan_start_time); + return; + } + /** special parsing for the VLP16 **/ if (calibration_->num_lasers == 16) { unpack_vlp16(pkt, data, scan_start_time); @@ -276,6 +326,7 @@ void RawData::unpack( (block.rotation <= config_.max_angle || block.rotation >= config_.min_angle))) { + float time = 0; if (timing_offsets_.size()) { time = timing_offsets_[i][j] + time_diff_start_to_this_packet; } @@ -403,6 +454,179 @@ void RawData::unpack( } } +/** @brief convert raw VLS128 packet to point cloud + * + * @param pkt raw packet to unpack + * @param pc shared pointer to point cloud (points are appended) + */ +void RawData::unpack_vls128( + const velodyne_msgs::msg::VelodynePacket & pkt, DataContainerBase & data, + const rclcpp::Time & scan_start_time) +{ + const raw_packet * raw = reinterpret_cast(&pkt.data[0]); + + bool dual_return = (pkt.data[1204] == 57); + for (int block = 0; block < BLOCKS_PER_PACKET - (4 * dual_return); block++) { + // cache block for use + const raw_block & current_block = raw->blocks[block]; + + int bank_origin = 0; + // Used to detect which bank of 32 lasers is in this block + switch (current_block.header) { + case VLS128_BANK_1: + bank_origin = 0; + break; + case VLS128_BANK_2: + bank_origin = 32; + break; + case VLS128_BANK_3: + bank_origin = 64; + break; + case VLS128_BANK_4: + bank_origin = 96; + break; + default: + // ignore packets with mangled or otherwise different contents + // Do not flood the log with messages, only issue at most one + // of these warnings per minute. + auto clock = rclcpp::Clock{}; + RCLCPP_WARN_STREAM_THROTTLE( + rclcpp::get_logger("velodyne_pointcloud"), clock, 60000, + "skipping invalid VLS-128 packet: block " << + block << " header value is " << + raw->blocks[block].header); + return; // bad packet: skip the rest + } + + uint16_t azimuth{}; // Uniform initializion. equivalent to azimuth = 0; + uint16_t azimuth_next{}; + float azimuth_diff{}; + // Calculate difference between current and next block's azimuth angle. + if (!dual_return) { + switch (block) { + // 1 firing sequence span across 4 data block, causing them to have the same azimuth + case 0: + case 1: + case 2: + case 3: + azimuth = raw->blocks[0].rotation; + azimuth_next = raw->blocks[4].rotation; + break; + case 4: + case 5: + case 6: + case 7: + // We assume that the difference between 3rd firing sequence and the next packet firing + // sequence is the same as difference between 2nd and 3rd firing sequence. + case 8: + case 9: + case 10: + case 11: + azimuth = raw->blocks[4].rotation; + azimuth_next = raw->blocks[8].rotation; + break; + default: + assert(false && "block should be between 0 and BLOCK_PER_PACKET(12)"); + break; + } + azimuth_diff = static_cast((36000 + azimuth_next - azimuth) % 36000); + } else { + // We can't calculate for dual return, since it have 1 azimuth per data packet and + // we don't have access to the next packet. + azimuth = raw->blocks[0].rotation; + } + + for (int j = 0, k = 0; j < SCANS_PER_BLOCK; j++, k += RAW_SCAN_SIZE) { + // Offset the laser in this block by which block it's in + uint8_t laser_number = j + bank_origin; + // VLS-128 fires 8 lasers at a time + uint8_t firing_order = laser_number / 8; + + float time_diff_start_to_this_packet = (rclcpp::Time(pkt.stamp) - scan_start_time).seconds(); + float time {}; + if (timing_offsets_.size()) { + time = timing_offsets_[block / 4][firing_order + laser_number / 64] + + time_diff_start_to_this_packet; + } + + velodyne_pointcloud::LaserCorrection & corrections = + calibration_->laser_corrections[laser_number]; + + // distance extraction + union two_bytes tmp; + tmp.bytes[0] = current_block.data[k]; + tmp.bytes[1] = current_block.data[k + 1]; + float distance = tmp.uint * VLS128_DISTANCE_RESOLUTION; + + // correct for the laser rotation as a function of timing during the firings + float azimuth_corrected_f = azimuth + + (azimuth_diff * vls_128_laser_azimuth_cache_[firing_order]); + uint16_t azimuth_corrected = ((uint16_t) round(azimuth_corrected_f)) % 36000; + + // condition added to avoid calculating points + // which are not in the interesting defined area (min_angle < area < max_angle) + // ((Confusing ! Are min_angle and max_angle included or not ?)) + if (((config_.min_angle < config_.max_angle && + azimuth_corrected >= config_.min_angle && + azimuth_corrected <= config_.max_angle) || + (config_.min_angle > config_.max_angle)) && + pointInRange(distance)) + { + // convert polar coordinates to Euclidean XYZ + // Each laser has an vertical offset and + // an horizontal offset in addition to azimut + float cos_vert_angle = corrections.cos_vert_correction; + float sin_vert_angle = corrections.sin_vert_correction; + float cos_rot_correction = corrections.cos_rot_correction; + float sin_rot_correction = corrections.sin_rot_correction; + + // cos(a-b) = cos(a)*cos(b) + sin(a)*sin(b) + // sin(a-b) = sin(a)*cos(b) - cos(a)*sin(b) + float cos_rot_angle = + cos_rot_table_[azimuth_corrected] * cos_rot_correction + + sin_rot_table_[azimuth_corrected] * sin_rot_correction; + float sin_rot_angle = + sin_rot_table_[azimuth_corrected] * cos_rot_correction - + cos_rot_table_[azimuth_corrected] * sin_rot_correction; + + // (TODO) Improve calculation. + // We can see different calculation in the implementation of other sensors. + // This implementation is in agreement (if no error was made) with VLS128 manuel, + // But it might not be the case with physics. + + // The velodyne VLS-128 manual user show this calcultations. + float x = distance * cos_vert_angle * sin_rot_angle; + float y = distance * cos_vert_angle * cos_rot_angle; + float z = distance * sin_vert_angle; + + /** Use standard ROS coordinate system (right-hand rule) */ + float x_coord = y; + float y_coord = -x; + float z_coord = z; + + data.addPoint( + x_coord, + y_coord, + z_coord, + corrections.laser_ring, + // azimuth_corrected, + distance, + current_block.data[k + 2], // intensity + time); + } else { + // call to addPoint is still required since output could be organized + data.addPoint( + nanf(""), nanf(""), nanf(""), corrections.laser_ring, nanf(""), nanf(""), time); + } + } + + if (current_block.header == VLS128_BANK_4) { + // add a new line only after the last bank (VLS128_BANK_4) + data.newLine(); + } + } +} + /** @brief convert raw VLP16 packet to point cloud * * @param pkt raw packet to unpack @@ -600,5 +824,4 @@ void RawData::unpack_vlp16( } } } - } // namespace velodyne_rawdata