Skip to content

Commit aafb524

Browse files
SakodaShintaroSakodaShintaro
authored
feat: fix to be able to use nebula (#19)
* Merge https://github.com/knzo25/awsim_sensor_kit_launch/tree/test/awsim_nebula_integration Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed some code Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed order Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * FIxed order Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> --------- Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>
1 parent df41e52 commit aafb524

File tree

5 files changed

+246
-24
lines changed

5 files changed

+246
-24
lines changed

‎awsim_sensor_kit_launch/launch/lidar.launch.xml

+30-6
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,6 @@
11
<launch>
2+
<arg name="launch_driver" default="true"/>
3+
<arg name="host_ip" default="255.255.255.255"/>
24
<arg name="use_concat_filter" default="true"/>
35
<arg name="vehicle_mirror_param_file"/>
46
<arg name="pointcloud_container_name" default="pointcloud_container"/>
@@ -9,27 +11,49 @@
911

1012
<group>
1113
<push-ros-namespace namespace="top"/>
12-
<include file="$(find-pkg-share common_awsim_sensor_launch)/launch/velodyne_lidar.launch.xml">
14+
<include file="$(find-pkg-share common_awsim_sensor_launch)/launch/velodyne_VLP16.launch.xml">
15+
<arg name="max_range" value="250.0"/>
16+
<arg name="sensor_frame" value="velodyne_top"/>
17+
<arg name="sensor_ip" value="127.0.0.1"/>
18+
<arg name="host_ip" value="$(var host_ip)"/>
19+
<arg name="data_port" value="2368"/>
20+
<arg name="scan_phase" value="300.0"/>
21+
<arg name="launch_driver" value="$(var launch_driver)"/>
1322
<arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
14-
<arg name="use_distortion_corrector" value="$(var use_distortion_corrector)"/>
1523
<arg name="container_name" value="pointcloud_container"/>
1624
</include>
1725
</group>
1826

1927
<group>
2028
<push-ros-namespace namespace="left"/>
21-
<include file="$(find-pkg-share common_awsim_sensor_launch)/launch/velodyne_lidar.launch.xml">
29+
<include file="$(find-pkg-share common_awsim_sensor_launch)/launch/velodyne_VLP16.launch.xml">
30+
<arg name="max_range" value="5.0"/>
31+
<arg name="sensor_frame" value="velodyne_left"/>
32+
<arg name="sensor_ip" value="127.0.0.1"/>
33+
<arg name="host_ip" value="$(var host_ip)"/>
34+
<arg name="data_port" value="2369"/>
35+
<arg name="scan_phase" value="180.0"/>
36+
<arg name="cloud_min_angle" value="300"/>
37+
<arg name="cloud_max_angle" value="60"/>
38+
<arg name="launch_driver" value="$(var launch_driver)"/>
2239
<arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
23-
<arg name="use_distortion_corrector" value="$(var use_distortion_corrector)"/>
2440
<arg name="container_name" value="pointcloud_container"/>
2541
</include>
2642
</group>
2743

2844
<group>
2945
<push-ros-namespace namespace="right"/>
30-
<include file="$(find-pkg-share common_awsim_sensor_launch)/launch/velodyne_lidar.launch.xml">
46+
<include file="$(find-pkg-share common_awsim_sensor_launch)/launch/velodyne_VLP16.launch.xml">
47+
<arg name="max_range" value="5.0"/>
48+
<arg name="sensor_frame" value="velodyne_right"/>
49+
<arg name="sensor_ip" value="127.0.0.1"/>
50+
<arg name="host_ip" value="$(var host_ip)"/>
51+
<arg name="data_port" value="2370"/>
52+
<arg name="scan_phase" value="180.0"/>
53+
<arg name="cloud_min_angle" value="300"/>
54+
<arg name="cloud_max_angle" value="60"/>
55+
<arg name="launch_driver" value="$(var launch_driver)"/>
3156
<arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
32-
<arg name="use_distortion_corrector" value="$(var use_distortion_corrector)"/>
3357
<arg name="container_name" value="pointcloud_container"/>
3458
</include>
3559
</group>

‎common_awsim_sensor_launch/config/distortion_corrector_node.param.yaml

+5
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
/**:
2+
ros__parameters:
3+
base_frame: base_link
4+
use_imu: true
5+
use_3d_distortion_correction: false

‎common_awsim_sensor_launch/launch/velodyne_node_container.launch.py ‎common_awsim_sensor_launch/launch/nebula_node_container.launch.py

+172-4
Original file line numberDiff line numberDiff line change
@@ -23,10 +23,17 @@
2323
from launch_ros.actions import LoadComposableNodes
2424
from launch_ros.descriptions import ComposableNode
2525
from launch_ros.parameter_descriptions import ParameterFile
26+
from launch_ros.substitutions import FindPackageShare
2627
import yaml
2728
from ament_index_python.packages import get_package_share_directory
2829
from pathlib import Path
2930

31+
def get_lidar_make(sensor_name):
32+
if sensor_name[:6].lower() == "pandar":
33+
return "Hesai", ".csv"
34+
elif sensor_name[:3].lower() in ["hdl", "vlp", "vls"]:
35+
return "Velodyne", ".yaml"
36+
return "unrecognized_sensor_model"
3037

3138
def get_vehicle_info(context):
3239
# TODO(TIER IV): Use Parameter Substitution after we drop Galactic support
@@ -62,6 +69,99 @@ def create_parameter_dict(*args):
6269

6370
nodes = []
6471

72+
# Model and make
73+
sensor_model = LaunchConfiguration("sensor_model").perform(context)
74+
sensor_make, sensor_extension = get_lidar_make(sensor_model)
75+
nebula_decoders_share_dir = Path(get_package_share_directory("nebula_decoders"))
76+
77+
# Calibration file
78+
sensor_calib_fp = (
79+
nebula_decoders_share_dir
80+
/ "calibration"
81+
/ sensor_make.lower()
82+
/ (sensor_model + sensor_extension)
83+
)
84+
assert (
85+
sensor_calib_fp.exists()
86+
), f"Sensor calib file under calibration/ was not found: {sensor_calib_fp}"
87+
sensor_calib_fp = str(sensor_calib_fp)
88+
89+
nodes = []
90+
91+
nodes.append(
92+
ComposableNode(
93+
package="glog_component",
94+
plugin="GlogComponent",
95+
name="glog_component",
96+
)
97+
)
98+
99+
nodes.append(
100+
ComposableNode(
101+
package="nebula_ros",
102+
plugin=sensor_make + "DriverRosWrapper",
103+
name=sensor_make.lower() + "_driver_ros_wrapper_node",
104+
parameters=[
105+
{
106+
"calibration_file": sensor_calib_fp,
107+
"sensor_model": sensor_model,
108+
**create_parameter_dict(
109+
"host_ip",
110+
"sensor_ip",
111+
"data_port",
112+
"return_mode",
113+
"min_range",
114+
"max_range",
115+
"frame_id",
116+
"scan_phase",
117+
"cloud_min_angle",
118+
"cloud_max_angle",
119+
"dual_return_distance_threshold",
120+
"setup_sensor",
121+
"retry_hw",
122+
),
123+
},
124+
],
125+
remappings=[
126+
# cSpell:ignore knzo25
127+
# TODO(knzo25): fix the remapping once nebula gets updated
128+
("pandar_points", "pointcloud_raw_ex"),
129+
("velodyne_points", "pointcloud_raw_ex"),
130+
],
131+
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
132+
)
133+
)
134+
135+
nodes.append(
136+
ComposableNode(
137+
package="nebula_ros",
138+
plugin=sensor_make + "HwMonitorRosWrapper",
139+
name=sensor_make.lower() + "_hw_monitor_ros_wrapper_node",
140+
parameters=[
141+
{
142+
"sensor_model": sensor_model,
143+
**create_parameter_dict(
144+
"return_mode",
145+
"frame_id",
146+
"scan_phase",
147+
"sensor_ip",
148+
"host_ip",
149+
"data_port",
150+
"gnss_port",
151+
"packet_mtu_size",
152+
"rotation_speed",
153+
"cloud_min_angle",
154+
"cloud_max_angle",
155+
"diag_span",
156+
"dual_return_distance_threshold",
157+
"delay_monitor_ms",
158+
),
159+
},
160+
],
161+
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
162+
)
163+
)
164+
65165
cropbox_parameters = create_parameter_dict("input_frame", "output_frame")
66166
cropbox_parameters["negative"] = True
67167

@@ -147,6 +247,45 @@ def create_parameter_dict(*args):
147247
composable_node_descriptions=nodes,
148248
)
149249

250+
driver_component = ComposableNode(
251+
package="nebula_ros",
252+
plugin=sensor_make + "HwInterfaceRosWrapper",
253+
# node is created in a global context, need to avoid name clash
254+
name=sensor_make.lower() + "_hw_interface_ros_wrapper_node",
255+
parameters=[
256+
{
257+
"sensor_model": sensor_model,
258+
"calibration_file": sensor_calib_fp,
259+
**create_parameter_dict(
260+
"sensor_ip",
261+
"host_ip",
262+
"scan_phase",
263+
"return_mode",
264+
"frame_id",
265+
"rotation_speed",
266+
"data_port",
267+
"gnss_port",
268+
"cloud_min_angle",
269+
"cloud_max_angle",
270+
"packet_mtu_size",
271+
"dual_return_distance_threshold",
272+
"setup_sensor",
273+
"ptp_profile",
274+
"ptp_transport_type",
275+
"ptp_switch_type",
276+
"ptp_domain",
277+
"retry_hw",
278+
),
279+
}
280+
],
281+
)
282+
283+
driver_component_loader = LoadComposableNodes(
284+
composable_node_descriptions=[driver_component],
285+
target_container=container,
286+
condition=IfCondition(LaunchConfiguration("launch_driver")),
287+
)
288+
150289
distortion_component = ComposableNode(
151290
package="autoware_pointcloud_preprocessor",
152291
plugin="autoware::pointcloud_preprocessor::DistortionCorrectorComponent",
@@ -185,7 +324,7 @@ def create_parameter_dict(*args):
185324
condition=launch.conditions.UnlessCondition(LaunchConfiguration("use_distortion_corrector")),
186325
)
187326

188-
return [container, distortion_loader, distortion_relay_loader]
327+
return [container, driver_component_loader, distortion_loader, distortion_relay_loader]
189328

190329

191330
def generate_launch_description():
@@ -201,21 +340,50 @@ def add_launch_arg(name: str, default_value=None, description=None):
201340
get_package_share_directory("common_awsim_sensor_launch")
202341
)
203342

343+
add_launch_arg("sensor_model", description="sensor model name")
344+
add_launch_arg("config_file", "", description="sensor configuration file")
345+
add_launch_arg("launch_driver", "True", "do launch driver")
346+
add_launch_arg("setup_sensor", "True", "configure sensor")
347+
add_launch_arg("retry_hw", "false", "retry hw")
348+
add_launch_arg("sensor_ip", "192.168.1.201", "device ip address")
349+
add_launch_arg("host_ip", "255.255.255.255", "host ip address")
350+
add_launch_arg("scan_phase", "0.0")
204351
add_launch_arg("base_frame", "base_link", "base frame id")
205-
add_launch_arg("container_name", "velodyne_composable_node_container", "container name")
352+
add_launch_arg("min_range", "0.3", "minimum view range for Velodyne sensors")
353+
add_launch_arg("max_range", "300.0", "maximum view range for Velodyne sensors")
354+
add_launch_arg("cloud_min_angle", "0", "minimum view angle setting on device")
355+
add_launch_arg("cloud_max_angle", "360", "maximum view angle setting on device")
356+
add_launch_arg("data_port", "2368", "device data port number")
357+
add_launch_arg("gnss_port", "2380", "device gnss port number")
358+
add_launch_arg("packet_mtu_size", "1500", "packet mtu size")
359+
add_launch_arg("rotation_speed", "600", "rotational frequency")
360+
add_launch_arg("dual_return_distance_threshold", "0.1", "dual return distance threshold")
361+
add_launch_arg("frame_id", "lidar", "frame id")
206362
add_launch_arg("input_frame", LaunchConfiguration("base_frame"), "use for cropbox")
207363
add_launch_arg("output_frame", LaunchConfiguration("base_frame"), "use for cropbox")
208364
add_launch_arg(
209365
"vehicle_mirror_param_file", description="path to the file of vehicle mirror position yaml"
210366
)
211367
add_launch_arg("use_multithread", "False", "use multithread")
212-
add_launch_arg("use_intra_process", "False", "use ROS2 component container communication")
368+
add_launch_arg("use_intra_process", "False", "use ROS 2 component container communication")
213369
add_launch_arg("output_as_sensor_frame", "False", "output final pointcloud in sensor frame")
214370
add_launch_arg("frame_id", "base_link", "frame id")
215371
add_launch_arg(
216372
"ring_outlier_filter_node_param_path",
217373
str(common_sensor_share_dir / "config" / "ring_outlier_filter_node.param.yaml"),
218-
description="path to parameter file of ring outlier filter node",
374+
description="path to parameter file of ring outlier filter node")
375+
add_launch_arg("container_name", "nebula_node_container")
376+
add_launch_arg("ptp_profile", "1588v2")
377+
add_launch_arg("ptp_transport_type", "L2")
378+
add_launch_arg("ptp_switch_type", "TSN")
379+
add_launch_arg("ptp_domain", "0")
380+
add_launch_arg("output_as_sensor_frame", "True", "output final pointcloud in sensor frame")
381+
add_launch_arg("diag_span", "1000", "")
382+
add_launch_arg("delay_monitor_ms", "2000", "")
383+
384+
add_launch_arg(
385+
"distortion_corrector_node_param_file",
386+
[FindPackageShare("common_sensor_launch"), "/config/distortion_corrector_node.param.yaml"],
219387
)
220388

221389
set_container_executable = SetLaunchConfiguration(

‎common_awsim_sensor_launch/launch/velodyne_VLP16.launch.xml

+39
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,39 @@
1+
<launch>
2+
3+
<!-- Params -->
4+
<arg name="launch_driver" default="true"/>
5+
6+
<arg name="model" default="VLP16"/>
7+
<arg name="max_range" default="130.0"/>
8+
<arg name="min_range" default="0.4"/>
9+
<arg name="sensor_frame" default="velodyne"/>
10+
<arg name="return_mode" default="SingleStrongest"/>
11+
<arg name="sensor_ip" default="192.168.1.201"/>
12+
<arg name="host_ip" default="255.255.255.255"/>
13+
<arg name="data_port" default="2368"/>
14+
<arg name="scan_phase" default="0.0"/>
15+
<arg name="cloud_min_angle" default="0"/>
16+
<arg name="cloud_max_angle" default="360"/>
17+
<arg name="vehicle_mirror_param_file"/>
18+
<arg name="container_name" default="velodyne_node_container"/>
19+
20+
<include file="$(find-pkg-share common_awsim_sensor_launch)/launch/nebula_node_container.launch.py">
21+
<arg name="launch_driver" value="$(var launch_driver)"/>
22+
<arg name="sensor_model" value="$(var model)"/>
23+
<arg name="return_mode" value="$(var return_mode)"/>
24+
<arg name="max_range" value="$(var max_range)"/>
25+
<arg name="min_range" value="$(var min_range)"/>
26+
<arg name="frame_id" value="$(var sensor_frame)"/>
27+
<arg name="sensor_ip" value="$(var sensor_ip)"/>
28+
<arg name="host_ip" value="$(var host_ip)"/>
29+
<arg name="data_port" value="$(var data_port)"/>
30+
<arg name="scan_phase" value="$(var scan_phase)"/>
31+
<arg name="cloud_min_angle" value="$(var cloud_min_angle)"/>
32+
<arg name="cloud_max_angle" value="$(var cloud_max_angle)"/>
33+
<arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
34+
<arg name="use_intra_process" value="true"/>
35+
<arg name="use_multithread" value="true"/>
36+
<arg name="container_name" value="$(var container_name)"/>
37+
</include>
38+
39+
</launch>

‎common_awsim_sensor_launch/launch/velodyne_lidar.launch.xml

-14
This file was deleted.

0 commit comments

Comments
 (0)