Skip to content

Commit d8cf623

Browse files
JWhitleyWorkwep21Institute for Autonomous Systems Technologyjudav25bst
authored
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>
1 parent 8e65c00 commit d8cf623

13 files changed

+1103
-11
lines changed

‎velodyne/launch/velodyne-all-nodes-VLS128-composed-launch.py

+85
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,85 @@
1+
# Copyright 2019 Open Source Robotics Foundation, Inc.
2+
#
3+
# Redistribution and use in source and binary forms, with or without
4+
# modification, are permitted provided that the following conditions
5+
# are met:
6+
#
7+
# 1. Redistributions of source code must retain the above copyright
8+
# notice, this list of conditions and the following disclaimer.
9+
#
10+
# 2. Redistributions in binary form must reproduce the above
11+
# copyright notice, this list of conditions and the following
12+
# disclaimer in the documentation and/or other materials provided
13+
# with the distribution.
14+
#
15+
# 3. Neither the name of the copyright holder nor the names of its
16+
# contributors may be used to endorse or promote products derived
17+
# from this software without specific prior written permission.
18+
#
19+
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20+
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21+
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22+
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23+
# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24+
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25+
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
26+
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
27+
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28+
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29+
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30+
# POSSIBILITY OF SUCH DAMAGE.
31+
32+
"""Launch the velodyne driver, pointcloud, and laserscan nodes in a composable container with default configuration."""
33+
34+
import os
35+
import yaml
36+
37+
import ament_index_python.packages
38+
from launch import LaunchDescription
39+
from launch_ros.actions import ComposableNodeContainer
40+
from launch_ros.descriptions import ComposableNode
41+
42+
43+
def generate_launch_description():
44+
driver_share_dir = ament_index_python.packages.get_package_share_directory('velodyne_driver')
45+
driver_params_file = os.path.join(driver_share_dir, 'config', 'VLS128-velodyne_driver_node-params.yaml')
46+
with open(driver_params_file, 'r') as f:
47+
driver_params = yaml.safe_load(f)['velodyne_driver_node']['ros__parameters']
48+
49+
convert_share_dir = ament_index_python.packages.get_package_share_directory('velodyne_pointcloud')
50+
convert_params_file = os.path.join(convert_share_dir, 'config', 'VLS128-velodyne_transform_node-params.yaml')
51+
with open(convert_params_file, 'r') as f:
52+
convert_params = yaml.safe_load(f)['velodyne_transform_node']['ros__parameters']
53+
convert_params['calibration'] = os.path.join(convert_share_dir, 'params', 'VLS128.yaml')
54+
55+
laserscan_share_dir = ament_index_python.packages.get_package_share_directory('velodyne_laserscan')
56+
laserscan_params_file = os.path.join(laserscan_share_dir, 'config', 'default-velodyne_laserscan_node-params.yaml')
57+
with open(laserscan_params_file, 'r') as f:
58+
laserscan_params = yaml.safe_load(f)['velodyne_laserscan_node']['ros__parameters']
59+
60+
container = ComposableNodeContainer(
61+
name='velodyne_container',
62+
namespace='',
63+
package='rclcpp_components',
64+
executable='component_container',
65+
composable_node_descriptions=[
66+
ComposableNode(
67+
package='velodyne_driver',
68+
plugin='velodyne_driver::VelodyneDriver',
69+
name='velodyne_driver_node',
70+
parameters=[driver_params]),
71+
ComposableNode(
72+
package='velodyne_pointcloud',
73+
plugin='velodyne_pointcloud::Transform',
74+
name='velodyne_transform_node',
75+
parameters=[convert_params]),
76+
ComposableNode(
77+
package='velodyne_laserscan',
78+
plugin='velodyne_laserscan::VelodyneLaserScan',
79+
name='velodyne_laserscan_node',
80+
parameters=[laserscan_params]),
81+
],
82+
output='both',
83+
)
84+
85+
return LaunchDescription([container])

‎velodyne/launch/velodyne-all-nodes-VLS128-launch.py

+78
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,78 @@
1+
# Copyright 2019 Open Source Robotics Foundation, Inc.
2+
#
3+
# Redistribution and use in source and binary forms, with or without
4+
# modification, are permitted provided that the following conditions
5+
# are met:
6+
#
7+
# 1. Redistributions of source code must retain the above copyright
8+
# notice, this list of conditions and the following disclaimer.
9+
#
10+
# 2. Redistributions in binary form must reproduce the above
11+
# copyright notice, this list of conditions and the following
12+
# disclaimer in the documentation and/or other materials provided
13+
# with the distribution.
14+
#
15+
# 3. Neither the name of the copyright holder nor the names of its
16+
# contributors may be used to endorse or promote products derived
17+
# from this software without specific prior written permission.
18+
#
19+
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20+
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21+
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22+
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23+
# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24+
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25+
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
26+
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
27+
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28+
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29+
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30+
# POSSIBILITY OF SUCH DAMAGE.
31+
32+
"""Launch the velodyne driver, pointcloud, and laserscan nodes with default configuration."""
33+
34+
import os
35+
import yaml
36+
37+
import ament_index_python.packages
38+
import launch
39+
import launch_ros.actions
40+
41+
42+
def generate_launch_description():
43+
driver_share_dir = ament_index_python.packages.get_package_share_directory('velodyne_driver')
44+
driver_params_file = os.path.join(driver_share_dir, 'config', 'VLS128-velodyne_driver_node-params.yaml')
45+
velodyne_driver_node = launch_ros.actions.Node(package='velodyne_driver',
46+
executable='velodyne_driver_node',
47+
output='both',
48+
parameters=[driver_params_file])
49+
50+
convert_share_dir = ament_index_python.packages.get_package_share_directory('velodyne_pointcloud')
51+
convert_params_file = os.path.join(convert_share_dir, 'config', 'VLS128-velodyne_transform_node-params.yaml')
52+
with open(convert_params_file, 'r') as f:
53+
convert_params = yaml.safe_load(f)['velodyne_transform_node']['ros__parameters']
54+
convert_params['calibration'] = os.path.join(convert_share_dir, 'params', 'VLS128.yaml')
55+
velodyne_transform_node = launch_ros.actions.Node(package='velodyne_pointcloud',
56+
executable='velodyne_transform_node',
57+
output='both',
58+
parameters=[convert_params])
59+
60+
laserscan_share_dir = ament_index_python.packages.get_package_share_directory('velodyne_laserscan')
61+
laserscan_params_file = os.path.join(laserscan_share_dir, 'config', 'default-velodyne_laserscan_node-params.yaml')
62+
velodyne_laserscan_node = launch_ros.actions.Node(package='velodyne_laserscan',
63+
executable='velodyne_laserscan_node',
64+
output='both',
65+
parameters=[laserscan_params_file])
66+
67+
68+
return launch.LaunchDescription([velodyne_driver_node,
69+
velodyne_transform_node,
70+
velodyne_laserscan_node,
71+
72+
launch.actions.RegisterEventHandler(
73+
event_handler=launch.event_handlers.OnProcessExit(
74+
target_action=velodyne_driver_node,
75+
on_exit=[launch.actions.EmitEvent(
76+
event=launch.events.Shutdown())],
77+
)),
78+
])

‎velodyne_driver/config/VLS128-velodyne_driver_node-params.yaml

+14
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,14 @@
1+
velodyne_driver_node:
2+
ros__parameters:
3+
device_ip: 192.168.1.201
4+
gps_time: false
5+
time_offset: 0.0
6+
enabled: true
7+
read_once: false
8+
read_fast: false
9+
repeat_delay: 0.0
10+
frame_id: velodyne
11+
model: VLS128
12+
rpm: 600.0
13+
port: 2368
14+
timestamp_first_packet: false

‎velodyne_driver/launch/velodyne_driver_node-VLS128-composed-launch.py

+68
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,68 @@
1+
# Copyright 2019 Open Source Robotics Foundation, Inc.
2+
# All rights reserved.
3+
#
4+
# Software License Agreement (BSD License 2.0)
5+
#
6+
# Redistribution and use in source and binary forms, with or without
7+
# modification, are permitted provided that the following conditions
8+
# are met:
9+
#
10+
# * Redistributions of source code must retain the above copyright
11+
# notice, this list of conditions and the following disclaimer.
12+
# * Redistributions in binary form must reproduce the above
13+
# copyright notice, this list of conditions and the following
14+
# disclaimer in the documentation and/or other materials provided
15+
# with the distribution.
16+
# * Neither the name of {copyright_holder} nor the names of its
17+
# contributors may be used to endorse or promote products derived
18+
# from this software without specific prior written permission.
19+
#
20+
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21+
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22+
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23+
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24+
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25+
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26+
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27+
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28+
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29+
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30+
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31+
# POSSIBILITY OF SUCH DAMAGE.
32+
33+
"""Launch the velodyne driver node in a composable container with default configuration."""
34+
35+
import os
36+
37+
import ament_index_python.packages
38+
39+
from launch import LaunchDescription
40+
from launch_ros.actions import ComposableNodeContainer
41+
from launch_ros.descriptions import ComposableNode
42+
43+
import yaml
44+
45+
46+
def generate_launch_description():
47+
config_directory = os.path.join(
48+
ament_index_python.packages.get_package_share_directory('velodyne_driver'),
49+
'config')
50+
param_config = os.path.join(config_directory, 'VLS128-velodyne_driver_node-params.yaml')
51+
with open(param_config, 'r') as f:
52+
params = yaml.safe_load(f)['velodyne_driver_node']['ros__parameters']
53+
container = ComposableNodeContainer(
54+
name='velodyne_driver_container',
55+
namespace='',
56+
package='rclcpp_components',
57+
executable='component_container',
58+
composable_node_descriptions=[
59+
ComposableNode(
60+
package='velodyne_driver',
61+
plugin='velodyne_driver::VelodyneDriver',
62+
name='velodyne_driver_node',
63+
parameters=[params]),
64+
],
65+
output='both',
66+
)
67+
68+
return LaunchDescription([container])

‎velodyne_driver/launch/velodyne_driver_node-VLS128-launch.py

+60
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,60 @@
1+
# Copyright 2019 Open Source Robotics Foundation, Inc.
2+
# All rights reserved.
3+
#
4+
# Software License Agreement (BSD License 2.0)
5+
#
6+
# Redistribution and use in source and binary forms, with or without
7+
# modification, are permitted provided that the following conditions
8+
# are met:
9+
#
10+
# * Redistributions of source code must retain the above copyright
11+
# notice, this list of conditions and the following disclaimer.
12+
# * Redistributions in binary form must reproduce the above
13+
# copyright notice, this list of conditions and the following
14+
# disclaimer in the documentation and/or other materials provided
15+
# with the distribution.
16+
# * Neither the name of {copyright_holder} nor the names of its
17+
# contributors may be used to endorse or promote products derived
18+
# from this software without specific prior written permission.
19+
#
20+
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21+
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22+
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23+
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24+
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25+
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26+
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27+
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28+
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29+
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30+
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31+
# POSSIBILITY OF SUCH DAMAGE.
32+
33+
"""Launch the velodyne driver node with default configuration."""
34+
35+
import os
36+
37+
import ament_index_python.packages
38+
import launch
39+
import launch_ros.actions
40+
41+
42+
def generate_launch_description():
43+
config_directory = os.path.join(
44+
ament_index_python.packages.get_package_share_directory('velodyne_driver'),
45+
'config')
46+
params = os.path.join(config_directory, 'VLS128-velodyne_driver_node-params.yaml')
47+
velodyne_driver_node = launch_ros.actions.Node(package='velodyne_driver',
48+
executable='velodyne_driver_node',
49+
output='both',
50+
parameters=[params])
51+
52+
return launch.LaunchDescription([velodyne_driver_node,
53+
54+
launch.actions.RegisterEventHandler(
55+
event_handler=launch.event_handlers.OnProcessExit(
56+
target_action=velodyne_driver_node,
57+
on_exit=[launch.actions.EmitEvent(
58+
event=launch.events.Shutdown())],
59+
)),
60+
])

‎velodyne_driver/src/driver/driver.cpp

+8-3
Original file line numberDiff line numberDiff line change
@@ -88,9 +88,14 @@ VelodyneDriver::VelodyneDriver(const rclcpp::NodeOptions & options)
8888
// get model name, validate string, determine packet rate
8989
double packet_rate; // packet frequency (Hz)
9090
std::string model_full_name;
91-
if ((config_.model == "64E_S2") ||
92-
(config_.model == "64E_S2.1"))
93-
{ // generates 1333312 points per second
91+
if (config_.model == "VLS128") {
92+
packet_rate = 6253.9; // 3 firing cycles in a data packet.
93+
// 3 x 53.3 μs = 0.1599 ms is the accumulation delay per packet.
94+
// 1 packet/0.1599 ms = 6253.9 packets/second
95+
96+
model_full_name = config_.model;
97+
} else if ((config_.model == "64E_S2") || (config_.model == "64E_S2.1")) {
98+
// generates 1333312 points per second
9499
packet_rate = 3472.17; // 1 packet holds 384 points - 1333312 / 384
95100
model_full_name = std::string("HDL-") + config_.model;
96101
} else if (config_.model == "64E") {

‎velodyne_pointcloud/config/VLS128-velodyne_transform_node-params.yaml

+10
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,10 @@
1+
velodyne_transform_node:
2+
ros__parameters:
3+
calibration: VLS128.yaml
4+
model: VLS128
5+
min_range: 0.4
6+
max_range: 300.0
7+
view_direction: 0.0
8+
fixed_frame: ""
9+
target_frame: ""
10+
organize_cloud: true

‎velodyne_pointcloud/include/velodyne_pointcloud/rawdata.hpp

+32
Original file line numberDiff line numberDiff line change
@@ -98,6 +98,20 @@ static const int BLOCKS_PER_PACKET = 12;
9898
static const int PACKET_STATUS_SIZE = 4;
9999
static const int SCANS_PER_PACKET = (SCANS_PER_BLOCK * BLOCKS_PER_PACKET);
100100

101+
/** Special Definitions for VLS128 support **/
102+
// These are used to detect which bank of 32 lasers is in this block
103+
static const uint16_t VLS128_BANK_1 = 0xeeff;
104+
static const uint16_t VLS128_BANK_2 = 0xddff;
105+
static const uint16_t VLS128_BANK_3 = 0xccff;
106+
static const uint16_t VLS128_BANK_4 = 0xbbff;
107+
108+
static const float VLS128_CHANNEL_TDURATION = 2.665f; // [µs] Channels corresponds to one laser firing // NOLINT
109+
static const float VLS128_SEQ_TDURATION = 53.3f; // [µs] Sequence is a set of laser firings including recharging // NOLINT
110+
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
111+
static const float VLS128_DISTANCE_RESOLUTION = 0.004f; // [m]
112+
static const float VLS128_MODEL_ID = 161;
113+
114+
101115
/** \brief Raw Velodyne packet.
102116
*
103117
* revolution is described in the device manual as incrementing
@@ -123,6 +137,10 @@ class RawData final
123137
public:
124138
RawData(const std::string & calibration_file, const std::string & model);
125139

140+
void setupSinCosCache();
141+
void setupAzimuthCache();
142+
bool loadCalibration();
143+
126144
void unpack(
127145
const velodyne_msgs::msg::VelodynePacket & pkt, DataContainerBase & data,
128146
const rclcpp::Time & scan_start_time);
@@ -152,6 +170,9 @@ class RawData final
152170
float sin_rot_table_[ROTATION_MAX_UNITS]{};
153171
float cos_rot_table_[ROTATION_MAX_UNITS]{};
154172

173+
// Caches the azimuth percent offset for the VLS-128 laser firings
174+
float vls_128_laser_azimuth_cache_[16];
175+
155176
// timing offset lookup table
156177
std::vector<std::vector<float>> timing_offsets_;
157178

@@ -167,6 +188,17 @@ class RawData final
167188
void unpack_vlp16(
168189
const velodyne_msgs::msg::VelodynePacket & pkt, DataContainerBase & data,
169190
const rclcpp::Time & scan_start_time);
191+
192+
void unpack_vls128(
193+
const velodyne_msgs::msg::VelodynePacket & pkt, DataContainerBase & data,
194+
const rclcpp::Time & scan_start_time);
195+
196+
/** in-line test whether a point is in range */
197+
inline bool pointInRange(const float range)
198+
{
199+
return range >= config_.min_range &&
200+
range <= config_.max_range;
201+
}
170202
};
171203

172204
} // namespace velodyne_rawdata

0 commit comments

Comments
 (0)