diff --git a/common_sensor_launch/config/ring_outlier_filter_node.param.yaml b/common_sensor_launch/config/ring_outlier_filter_node.param.yaml
new file mode 100644
index 00000000..76bf6895
--- /dev/null
+++ b/common_sensor_launch/config/ring_outlier_filter_node.param.yaml
@@ -0,0 +1,14 @@
+/**:
+ ros__parameters:
+ distance_ratio: 1.03
+ object_length_threshold: 0.1
+ num_points_threshold: 4
+ max_rings_num: 128
+ max_points_num_per_ring: 4000
+ publish_outlier_pointcloud: false
+ min_azimuth_deg: 0.0
+ max_azimuth_deg: 360.0
+ max_distance: 12.0
+ vertical_bins: 128
+ horizontal_bins: 36
+ noise_threshold: 2
diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py
index 49e0b958..d1f978aa 100644
--- a/common_sensor_launch/launch/nebula_node_container.launch.py
+++ b/common_sensor_launch/launch/nebula_node_container.launch.py
@@ -34,6 +34,8 @@ def get_lidar_make(sensor_name):
return "Hesai", ".csv"
elif sensor_name[:3].lower() in ["hdl", "vlp", "vls"]:
return "Velodyne", ".yaml"
+ elif sensor_name.lower() in ["helios", "bpearl"]:
+ return "Robosense", None
return "unrecognized_sensor_model"
@@ -75,21 +77,28 @@ def create_parameter_dict(*args):
nebula_decoders_share_dir = get_package_share_directory("nebula_decoders")
# Calibration file
- sensor_calib_fp = os.path.join(
- nebula_decoders_share_dir,
- "calibration",
- sensor_make.lower(),
- sensor_model + sensor_extension,
- )
- assert os.path.exists(
- sensor_calib_fp
- ), "Sensor calib file under calibration/ was not found: {}".format(sensor_calib_fp)
+ if sensor_extension is not None: # Velodyne and Hesai
+ sensor_calib_fp = os.path.join(
+ nebula_decoders_share_dir,
+ "calibration",
+ sensor_make.lower(),
+ sensor_model + sensor_extension,
+ )
+ assert os.path.exists(
+ sensor_calib_fp
+ ), "Sensor calib file under calibration/ was not found: {}".format(sensor_calib_fp)
+ else: # Robosense
+ sensor_calib_fp = ""
# Pointcloud preprocessor parameters
distortion_corrector_node_param = ParameterFile(
param_file=LaunchConfiguration("distortion_correction_node_param_path").perform(context),
allow_substs=True,
)
+ ring_outlier_filter_node_param = ParameterFile(
+ param_file=LaunchConfiguration("ring_outlier_filter_node_param_path").perform(context),
+ allow_substs=True,
+ )
nodes = []
@@ -129,6 +138,8 @@ def create_parameter_dict(*args):
# cSpell:ignore knzo25
# TODO(knzo25): fix the remapping once nebula gets updated
("velodyne_points", "pointcloud_raw_ex"),
+ # ("robosense_points", "pointcloud_raw_ex"), #for robosense
+ # ("pandar_points", "pointcloud_raw_ex"), # for hesai
],
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
)
@@ -199,11 +210,9 @@ def create_parameter_dict(*args):
# Ring Outlier Filter is the last component in the pipeline, so control the output frame here
if LaunchConfiguration("output_as_sensor_frame").perform(context).lower() == "true":
- ring_outlier_filter_parameters = {"output_frame": LaunchConfiguration("frame_id")}
+ ring_outlier_output_frame = {"output_frame": LaunchConfiguration("frame_id")}
else:
- ring_outlier_filter_parameters = {
- "output_frame": ""
- } # keep the output frame as the input frame
+ ring_outlier_output_frame = {"output_frame": ""} # keep the output frame as the input frame
nodes.append(
ComposableNode(
package="autoware_pointcloud_preprocessor",
@@ -213,7 +222,7 @@ def create_parameter_dict(*args):
("input", "rectified/pointcloud_ex"),
("output", "pointcloud_before_sync"),
],
- parameters=[ring_outlier_filter_parameters],
+ parameters=[ring_outlier_filter_node_param, ring_outlier_output_frame],
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
)
)
@@ -312,6 +321,15 @@ def add_launch_arg(name: str, default_value=None, description=None):
),
description="path to parameter file of distortion correction node",
)
+ add_launch_arg(
+ "ring_outlier_filter_node_param_path",
+ os.path.join(
+ common_sensor_share_dir,
+ "config",
+ "ring_outlier_filter_node.param.yaml",
+ ),
+ description="path to parameter file of ring outlier filter node",
+ )
set_container_executable = SetLaunchConfiguration(
"container_executable",
diff --git a/common_sensor_launch/launch/robosense_Bpearl.launch.xml b/common_sensor_launch/launch/robosense_Bpearl.launch.xml
new file mode 100644
index 00000000..8fa49ef1
--- /dev/null
+++ b/common_sensor_launch/launch/robosense_Bpearl.launch.xml
@@ -0,0 +1,37 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/common_sensor_launch/launch/robosense_Helios.launch.xml b/common_sensor_launch/launch/robosense_Helios.launch.xml
new file mode 100644
index 00000000..09addc20
--- /dev/null
+++ b/common_sensor_launch/launch/robosense_Helios.launch.xml
@@ -0,0 +1,37 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sample_sensor_kit_launch/launch/gnss.launch.xml b/sample_sensor_kit_launch/launch/gnss.launch.xml
index 0d04cd39..d4fbd403 100644
--- a/sample_sensor_kit_launch/launch/gnss.launch.xml
+++ b/sample_sensor_kit_launch/launch/gnss.launch.xml
@@ -24,7 +24,7 @@
-
+
diff --git a/sample_sensor_kit_launch/launch/imu.launch.xml b/sample_sensor_kit_launch/launch/imu.launch.xml
index ff01fa85..fc2d006d 100644
--- a/sample_sensor_kit_launch/launch/imu.launch.xml
+++ b/sample_sensor_kit_launch/launch/imu.launch.xml
@@ -15,13 +15,13 @@
-
+
-
+
diff --git a/sample_sensor_kit_launch/package.xml b/sample_sensor_kit_launch/package.xml
index 210c5a88..5e4472eb 100644
--- a/sample_sensor_kit_launch/package.xml
+++ b/sample_sensor_kit_launch/package.xml
@@ -10,9 +10,9 @@
ament_cmake_auto
+ autoware_gnss_poser
autoware_pointcloud_preprocessor
common_sensor_launch
- gnss_poser
tamagawa_imu_driver
topic_tools
ublox_gps