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 <juan.gonzalez@unibw.de> * Add VLS128 launch and calibration file (#382) Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * vls128: add line only once all four banks are processed (#413) Signed-off-by: Sebastian Scherer (CR/AAS3) <sebastian.scherer2@de.bosch.com> Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix: apply uncrustify Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * 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 <border_goldenmarket@yahoo.co.jp> Signed-off-by: Sebastian Scherer (CR/AAS3) <sebastian.scherer2@de.bosch.com> Co-authored-by: Joshua Whitley <josh@electrifiedautonomy.com> Co-authored-by: Institute for Autonomous Systems Technology <tas@unibw.de> Co-authored-by: jugo <juan.gonzalez@unibw.de> Co-authored-by: Sebastian Scherer <sebastian.scherer2@de.bosch.com> Co-authored-by: wep21 <border_goldenmarket@yahoo.co.jp> Co-authored-by: Joshua Whitley <josh.whitley@autoware.org> Co-authored-by: Mtestor <mucevval@gmail.com> * Fix double include --------- Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> Signed-off-by: Sebastian Scherer (CR/AAS3) <sebastian.scherer2@de.bosch.com> Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Institute for Autonomous Systems Technology <tas@unibw.de> Co-authored-by: jugo <juan.gonzalez@unibw.de> Co-authored-by: Sebastian Scherer <sebastian.scherer2@de.bosch.com> Co-authored-by: wep21 <border_goldenmarket@yahoo.co.jp> Co-authored-by: Joshua Whitley <josh.whitley@autoware.org> Co-authored-by: Mtestor <mucevval@gmail.com>
ros-drivers · May 31, 2023 · d8cf623 · d8cf623
1 parent 8e65c00
commit d8cf623
Show file tree

Hide file tree

Showing 13 changed files with 1,103 additions and 11 deletions.
diff --git a/velodyne/launch/velodyne-all-nodes-VLS128-composed-launch.py 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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<std::vector<float>> 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