diff --git a/aip_x1_launch/config/gyro_bias_estimator.param.yaml b/aip_x1_launch/config/gyro_bias_estimator.param.yaml
new file mode 100644
index 00000000..d552569f
--- /dev/null
+++ b/aip_x1_launch/config/gyro_bias_estimator.param.yaml
@@ -0,0 +1,6 @@
+/**:
+ ros__parameters:
+ gyro_bias_threshold: 0.008 # [rad/s]
+ timer_callback_interval_sec: 0.5 # [sec]
+ diagnostics_updater_interval_sec: 0.5 # [sec]
+ straight_motion_ang_vel_upper_limit: 0.015 # [rad/s]
diff --git a/aip_x1_launch/launch/imu.launch.xml b/aip_x1_launch/launch/imu.launch.xml
index c192c5a1..bf00b7ca 100644
--- a/aip_x1_launch/launch/imu.launch.xml
+++ b/aip_x1_launch/launch/imu.launch.xml
@@ -14,10 +14,12 @@
+
+
diff --git a/aip_x1_launch/launch/lidar.launch.xml b/aip_x1_launch/launch/lidar.launch.xml
index a6ceb956..56066df5 100644
--- a/aip_x1_launch/launch/lidar.launch.xml
+++ b/aip_x1_launch/launch/lidar.launch.xml
@@ -4,7 +4,6 @@
-
@@ -57,7 +56,6 @@
-
diff --git a/aip_x1_launch/launch/new_livox_horizon.launch.py b/aip_x1_launch/launch/new_livox_horizon.launch.py
index de3f8b9a..922d23ba 100644
--- a/aip_x1_launch/launch/new_livox_horizon.launch.py
+++ b/aip_x1_launch/launch/new_livox_horizon.launch.py
@@ -52,12 +52,12 @@ def get_crop_box_min_range_component(context, livox_frame_id):
name="crop_box_filter_min_range",
remappings=[
("input", "livox/tag_filtered/lidar" if use_tag_filter else "livox/lidar"),
- ("output", "min_range_cropped/pointcloud"),
+ ("output", "min_range_cropped/pointcloud_before_sync"),
],
parameters=[
{
"input_frame": livox_frame_id,
- "output_frame": LaunchConfiguration("base_frame"),
+ "output_frame": LaunchConfiguration("frame_id"),
"min_x": 0.0,
"max_x": LaunchConfiguration("min_range"),
"min_y": -2.0,
diff --git a/aip_x1_launch/launch/pointcloud_preprocessor.launch.py b/aip_x1_launch/launch/pointcloud_preprocessor.launch.py
index 8d9892b0..8d799261 100644
--- a/aip_x1_launch/launch/pointcloud_preprocessor.launch.py
+++ b/aip_x1_launch/launch/pointcloud_preprocessor.launch.py
@@ -19,7 +19,6 @@
from launch.conditions import IfCondition
from launch.conditions import UnlessCondition
from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import ComposableNodeContainer
from launch_ros.actions import LoadComposableNodes
from launch_ros.descriptions import ComposableNode
@@ -37,43 +36,27 @@ def launch_setup(context, *args, **kwargs):
parameters=[
{
"input_topics": [
- "/sensing/lidar/top/pointcloud",
- "/sensing/lidar/front_left/min_range_cropped/pointcloud",
- "/sensing/lidar/front_right/min_range_cropped/pointcloud",
- "/sensing/lidar/front_center/min_range_cropped/pointcloud",
+ "/sensing/lidar/top/pointcloud_before_sync",
+ "/sensing/lidar/front_left/min_range_cropped/pointcloud_before_sync",
+ "/sensing/lidar/front_right/min_range_cropped/pointcloud_before_sync",
+ "/sensing/lidar/front_center/min_range_cropped/pointcloud_before_sync",
],
"output_frame": LaunchConfiguration("base_frame"),
"timeout_sec": 1.0,
"input_twist_topic_type": "twist",
+ "publish_synchronized_pointcloud": True,
}
],
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
)
- # set container to run all required components in the same process
- container = ComposableNodeContainer(
- name=LaunchConfiguration("individual_container_name"),
- namespace="",
- package="rclcpp_components",
- executable=LaunchConfiguration("container_executable"),
- composable_node_descriptions=[],
- condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")),
- output="screen",
- )
-
- target_container = (
- container
- if UnlessCondition(LaunchConfiguration("use_pointcloud_container")).evaluate(context)
- else LaunchConfiguration("pointcloud_container_name")
- )
-
# load concat or passthrough filter
concat_loader = LoadComposableNodes(
composable_node_descriptions=[concat_component],
- target_container=target_container,
+ target_container=LaunchConfiguration("pointcloud_container_name"),
)
- return [container, concat_loader]
+ return [concat_loader]
def generate_launch_description():
@@ -85,9 +68,7 @@ def add_launch_arg(name: str, default_value=None):
add_launch_arg("base_frame", "base_link")
add_launch_arg("use_multithread", "False")
add_launch_arg("use_intra_process", "False")
- add_launch_arg("use_pointcloud_container", "False")
add_launch_arg("pointcloud_container_name", "pointcloud_container")
- add_launch_arg("individual_container_name", "concatenate_container")
set_container_executable = SetLaunchConfiguration(
"container_executable",
diff --git a/aip_x1_launch/launch/sensing.launch.xml b/aip_x1_launch/launch/sensing.launch.xml
index a0fbd4b2..1c294fdf 100644
--- a/aip_x1_launch/launch/sensing.launch.xml
+++ b/aip_x1_launch/launch/sensing.launch.xml
@@ -2,7 +2,6 @@
-
@@ -12,7 +11,6 @@
-
diff --git a/aip_x1_launch/launch/topic_state_monitor.launch.py b/aip_x1_launch/launch/topic_state_monitor.launch.py
index b45b0f4e..1c17606c 100644
--- a/aip_x1_launch/launch/topic_state_monitor.launch.py
+++ b/aip_x1_launch/launch/topic_state_monitor.launch.py
@@ -155,7 +155,7 @@ def generate_launch_description():
name="topic_state_monitor_rough_no_ground",
parameters=[
{
- "topic": "/perception/obstacle_segmentation/single_frame/pointcloud_raw",
+ "topic": "/perception/obstacle_segmentation/single_frame/pointcloud",
"topic_type": "sensor_msgs/msg/PointCloud2",
"best_effort": True,
"diag_name": "sensing_topic_status",
diff --git a/aip_x1_launch/launch/topic_state_monitor.launch.xml b/aip_x1_launch/launch/topic_state_monitor.launch.xml
index 8912fb67..aa89fccd 100644
--- a/aip_x1_launch/launch/topic_state_monitor.launch.xml
+++ b/aip_x1_launch/launch/topic_state_monitor.launch.xml
@@ -76,7 +76,7 @@
-
+
diff --git a/aip_x1_launch/launch/velodyne_node_container.launch.py b/aip_x1_launch/launch/velodyne_node_container.launch.py
index 56b35116..2ce274b1 100644
--- a/aip_x1_launch/launch/velodyne_node_container.launch.py
+++ b/aip_x1_launch/launch/velodyne_node_container.launch.py
@@ -121,6 +121,13 @@ 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):
+ ring_outlier_filter_parameters = {"output_frame": LaunchConfiguration("frame_id")}
+ else:
+ ring_outlier_filter_parameters = {
+ "output_frame": ""
+ } # keep the output frame as the input frame
nodes.append(
ComposableNode(
package="pointcloud_preprocessor",
@@ -128,8 +135,9 @@ def create_parameter_dict(*args):
name="ring_outlier_filter",
remappings=[
("input", "rectified/pointcloud_ex"),
- ("output", "pointcloud"),
+ ("output", "pointcloud_before_sync"),
],
+ parameters=[ring_outlier_filter_parameters],
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
)
)
@@ -218,6 +226,7 @@ def add_launch_arg(name: str, default_value=None, description=None):
)
add_launch_arg("use_multithread", "False", "use multithread")
add_launch_arg("use_intra_process", "False", "use ROS2 component container communication")
+ add_launch_arg("output_as_sensor_frame", "True", "output final pointcloud in sensor frame")
set_container_executable = SetLaunchConfiguration(
"container_executable",
diff --git a/aip_x2_launch/config/blockage_diagnostics_param_file.yaml b/aip_x2_launch/config/blockage_diagnostics_param_file.yaml
index b8b62b50..e43b2039 100644
--- a/aip_x2_launch/config/blockage_diagnostics_param_file.yaml
+++ b/aip_x2_launch/config/blockage_diagnostics_param_file.yaml
@@ -4,8 +4,11 @@
blockage_count_threshold: 50
blockage_buffering_frames: 2
blockage_buffering_interval: 1
+ enable_dust_diag: false
+ publish_debug_image: false
dust_ratio_threshold: 0.2
dust_count_threshold: 10
dust_kernel_size: 2
dust_buffering_frames: 10
dust_buffering_interval: 1
+ blockage_kernel: 10
diff --git a/aip_x2_launch/config/gyro_bias_estimator.param.yaml b/aip_x2_launch/config/gyro_bias_estimator.param.yaml
new file mode 100644
index 00000000..d552569f
--- /dev/null
+++ b/aip_x2_launch/config/gyro_bias_estimator.param.yaml
@@ -0,0 +1,6 @@
+/**:
+ ros__parameters:
+ gyro_bias_threshold: 0.008 # [rad/s]
+ timer_callback_interval_sec: 0.5 # [sec]
+ diagnostics_updater_interval_sec: 0.5 # [sec]
+ straight_motion_ang_vel_upper_limit: 0.015 # [rad/s]
diff --git a/aip_x2_launch/launch/imu.launch.xml b/aip_x2_launch/launch/imu.launch.xml
index 432316cb..e3f1a056 100644
--- a/aip_x2_launch/launch/imu.launch.xml
+++ b/aip_x2_launch/launch/imu.launch.xml
@@ -29,10 +29,12 @@
+
+
diff --git a/aip_x2_launch/launch/lidar.launch.xml b/aip_x2_launch/launch/lidar.launch.xml
index c7e21aa1..9ccf52af 100644
--- a/aip_x2_launch/launch/lidar.launch.xml
+++ b/aip_x2_launch/launch/lidar.launch.xml
@@ -4,9 +4,9 @@
-
+
@@ -26,6 +26,9 @@
+
+
+
@@ -46,6 +49,9 @@
+
+
+
@@ -68,6 +74,9 @@
+
+
+
@@ -89,6 +98,9 @@
+
+
+
@@ -112,6 +124,9 @@
+
+
+
@@ -133,6 +148,9 @@
+
+
+
@@ -156,6 +174,9 @@
+
+
+
@@ -177,6 +198,9 @@
+
+
+
@@ -189,7 +213,6 @@
-
diff --git a/aip_x2_launch/launch/pandar_node_container.launch.py b/aip_x2_launch/launch/pandar_node_container.launch.py
index cfb39cc4..b950761e 100644
--- a/aip_x2_launch/launch/pandar_node_container.launch.py
+++ b/aip_x2_launch/launch/pandar_node_container.launch.py
@@ -43,6 +43,10 @@ def get_pandar_monitor_info():
return p
+def str2vector(string):
+ return [float(x) for x in string.strip("[]").split(",")]
+
+
def get_vehicle_info(context):
# TODO(TIER IV): Use Parameter Substitution after we drop Galactic support
# https://github.com/ros2/launch_ros/blob/master/launch_ros/launch_ros/substitutions/parameter.py
@@ -202,14 +206,22 @@ def create_parameter_dict(*args):
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
)
+ # Ring Outlier Filter is the last component in the pipeline, so control the output frame here
+ if LaunchConfiguration("output_as_sensor_frame").perform(context):
+ ring_outlier_filter_parameters = {"output_frame": LaunchConfiguration("frame_id")}
+ else:
+ ring_outlier_filter_parameters = {
+ "output_frame": ""
+ } # keep the output frame as the input frame
ring_outlier_filter_component = ComposableNode(
package="pointcloud_preprocessor",
plugin="pointcloud_preprocessor::RingOutlierFilterComponent",
name="ring_outlier_filter",
remappings=[
("input", "rectified/pointcloud_ex"),
- ("output", "pointcloud"),
+ ("output", "pointcloud_before_sync"),
],
+ parameters=[ring_outlier_filter_parameters],
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
)
@@ -219,7 +231,7 @@ def create_parameter_dict(*args):
name="dual_return_filter",
remappings=[
("input", "rectified/pointcloud_ex"),
- ("output", "pointcloud"),
+ ("output", "pointcloud_before_sync"),
],
parameters=[
{
@@ -232,6 +244,7 @@ def create_parameter_dict(*args):
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
)
+ distance_range = str2vector(context.perform_substitution(LaunchConfiguration("distance_range")))
blockage_diag_component = ComposableNode(
package="pointcloud_preprocessor",
plugin="pointcloud_preprocessor::BlockageDiagComponent",
@@ -245,7 +258,9 @@ def create_parameter_dict(*args):
"angle_range": LaunchConfiguration("angle_range"),
"horizontal_ring_id": LaunchConfiguration("horizontal_ring_id"),
"vertical_bins": LaunchConfiguration("vertical_bins"),
- "model": LaunchConfiguration("model"),
+ "is_channel_order_top2down": LaunchConfiguration("is_channel_order_top2down"),
+ "max_distance_range": distance_range[1],
+ "horizontal_resolution": LaunchConfiguration("horizontal_resolution"),
}
]
+ [load_composable_node_param("blockage_diagnostics_param_file")],
@@ -323,6 +338,7 @@ def add_launch_arg(name: str, default_value=None):
add_launch_arg("input_frame", LaunchConfiguration("base_frame"))
add_launch_arg("output_frame", LaunchConfiguration("base_frame"))
add_launch_arg("dual_return_filter_param_file")
+ add_launch_arg("horizontal_resolution", "0.4")
add_launch_arg(
"blockage_diagnostics_param_file",
[FindPackageShare("aip_x2_launch"), "/config/blockage_diagnostics_param_file.yaml"],
@@ -336,6 +352,7 @@ def add_launch_arg(name: str, default_value=None):
add_launch_arg("min_azimuth_deg", "135.0")
add_launch_arg("max_azimuth_deg", "225.0")
add_launch_arg("enable_blockage_diag", "true")
+ add_launch_arg("output_as_sensor_frame", "True")
set_container_executable = SetLaunchConfiguration(
"container_executable",
"component_container",
diff --git a/aip_x2_launch/launch/pointcloud_preprocessor.launch.py b/aip_x2_launch/launch/pointcloud_preprocessor.launch.py
index 6aa031b0..3c0a09d8 100644
--- a/aip_x2_launch/launch/pointcloud_preprocessor.launch.py
+++ b/aip_x2_launch/launch/pointcloud_preprocessor.launch.py
@@ -20,7 +20,6 @@
from launch.conditions import IfCondition
from launch.conditions import UnlessCondition
from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import ComposableNodeContainer
from launch_ros.actions import LoadComposableNodes
from launch_ros.descriptions import ComposableNode
@@ -38,49 +37,33 @@ def launch_setup(context, *args, **kwargs):
parameters=[
{
"input_topics": [
- "/sensing/lidar/front_upper/pointcloud",
- "/sensing/lidar/front_lower/pointcloud",
- "/sensing/lidar/left_upper/pointcloud",
- "/sensing/lidar/left_lower/pointcloud",
- "/sensing/lidar/right_upper/pointcloud",
- "/sensing/lidar/right_lower/pointcloud",
- "/sensing/lidar/rear_upper/pointcloud",
- "/sensing/lidar/rear_lower/pointcloud",
+ "/sensing/lidar/front_upper/pointcloud_before_sync",
+ "/sensing/lidar/front_lower/pointcloud_before_sync",
+ "/sensing/lidar/left_upper/pointcloud_before_sync",
+ "/sensing/lidar/left_lower/pointcloud_before_sync",
+ "/sensing/lidar/right_upper/pointcloud_before_sync",
+ "/sensing/lidar/right_lower/pointcloud_before_sync",
+ "/sensing/lidar/rear_upper/pointcloud_before_sync",
+ "/sensing/lidar/rear_lower/pointcloud_before_sync",
],
"input_offset": [0.025, 0.025, 0.01, 0.0, 0.05, 0.05, 0.05, 0.05],
"timeout_sec": 0.075,
"output_frame": LaunchConfiguration("base_frame"),
"input_twist_topic_type": "twist",
+ "publish_synchronized_pointcloud": True,
}
],
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
)
- # set container to run all required components in the same process
- container = ComposableNodeContainer(
- name=LaunchConfiguration("individual_container_name"),
- namespace="",
- package="rclcpp_components",
- executable=LaunchConfiguration("container_executable"),
- composable_node_descriptions=[],
- condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")),
- output="screen",
- )
-
- target_container = (
- container
- if UnlessCondition(LaunchConfiguration("use_pointcloud_container")).evaluate(context)
- else LaunchConfiguration("pointcloud_container_name")
- )
-
# load concat or passthrough filter
concat_loader = LoadComposableNodes(
composable_node_descriptions=[concat_component],
- target_container=target_container,
+ target_container=LaunchConfiguration("pointcloud_container_name"),
condition=IfCondition(LaunchConfiguration("use_concat_filter")),
)
- return [container, concat_loader]
+ return [concat_loader]
def generate_launch_description():
@@ -92,9 +75,7 @@ def add_launch_arg(name: str, default_value=None):
add_launch_arg("base_frame", "base_link")
add_launch_arg("use_multithread", "True")
add_launch_arg("use_intra_process", "True")
- add_launch_arg("use_pointcloud_container", "False")
add_launch_arg("pointcloud_container_name", "pointcloud_container")
- add_launch_arg("individual_container_name", "concatenate_container")
set_container_executable = SetLaunchConfiguration(
"container_executable",
diff --git a/aip_x2_launch/launch/sensing.launch.xml b/aip_x2_launch/launch/sensing.launch.xml
index 51614263..274f6334 100644
--- a/aip_x2_launch/launch/sensing.launch.xml
+++ b/aip_x2_launch/launch/sensing.launch.xml
@@ -2,7 +2,6 @@
-
@@ -12,7 +11,6 @@
-
diff --git a/aip_xx1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml b/aip_xx1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml
index ab32e0fe..56fc9dbd 100644
--- a/aip_xx1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml
+++ b/aip_xx1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml
@@ -18,6 +18,15 @@
type: diagnostic_aggregator/AnalyzerGroup
path: lidar
analyzers:
+ performance_monitoring:
+ type: diagnostic_aggregator/AnalyzerGroup
+ path: performance_monitoring
+ analyzers:
+ blockage:
+ type: diagnostic_aggregator/GenericAnalyzer
+ path: blockage
+ contains: [": blockage_validation"]
+ timeout: 1.0
velodyne:
type: diagnostic_aggregator/AnalyzerGroup
path: velodyne
diff --git a/aip_xx1_launch/config/gyro_bias_estimator.param.yaml b/aip_xx1_launch/config/gyro_bias_estimator.param.yaml
new file mode 100644
index 00000000..d552569f
--- /dev/null
+++ b/aip_xx1_launch/config/gyro_bias_estimator.param.yaml
@@ -0,0 +1,6 @@
+/**:
+ ros__parameters:
+ gyro_bias_threshold: 0.008 # [rad/s]
+ timer_callback_interval_sec: 0.5 # [sec]
+ diagnostics_updater_interval_sec: 0.5 # [sec]
+ straight_motion_ang_vel_upper_limit: 0.015 # [rad/s]
diff --git a/aip_xx1_launch/launch/imu.launch.xml b/aip_xx1_launch/launch/imu.launch.xml
index e88e8d8e..29795ae1 100644
--- a/aip_xx1_launch/launch/imu.launch.xml
+++ b/aip_xx1_launch/launch/imu.launch.xml
@@ -44,10 +44,12 @@
+
+
diff --git a/aip_xx1_launch/launch/lidar.launch.xml b/aip_xx1_launch/launch/lidar.launch.xml
index 7f77ad2d..b524b788 100644
--- a/aip_xx1_launch/launch/lidar.launch.xml
+++ b/aip_xx1_launch/launch/lidar.launch.xml
@@ -5,8 +5,8 @@
-
+
@@ -23,6 +23,11 @@
+
+
+
+
+
@@ -40,6 +45,11 @@
+
+
+
+
+
@@ -57,6 +67,11 @@
+
+
+
+
+
@@ -74,6 +89,11 @@
+
+
+
+
+
@@ -98,7 +118,6 @@
-
diff --git a/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py b/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py
index c7a30175..433b7f58 100644
--- a/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py
+++ b/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py
@@ -20,7 +20,6 @@
from launch.conditions import IfCondition
from launch.conditions import UnlessCondition
from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import ComposableNodeContainer
from launch_ros.actions import LoadComposableNodes
from launch_ros.descriptions import ComposableNode
@@ -52,30 +51,14 @@ def launch_setup(context, *args, **kwargs):
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
)
- # set container to run all required components in the same process
- container = ComposableNodeContainer(
- name=LaunchConfiguration("individual_container_name"),
- namespace="",
- package="rclcpp_components",
- executable=LaunchConfiguration("container_executable"),
- condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")),
- output="screen",
- )
-
- target_container = (
- container
- if UnlessCondition(LaunchConfiguration("use_pointcloud_container")).evaluate(context)
- else LaunchConfiguration("pointcloud_container_name")
- )
-
# load concat or passthrough filter
concat_loader = LoadComposableNodes(
composable_node_descriptions=[concat_component],
- target_container=target_container,
+ target_container=LaunchConfiguration("pointcloud_container_name"),
condition=IfCondition(LaunchConfiguration("use_concat_filter")),
)
- return [container, concat_loader]
+ return [concat_loader]
def generate_launch_description():
@@ -87,7 +70,6 @@ def add_launch_arg(name: str, default_value=None):
add_launch_arg("base_frame", "base_link")
add_launch_arg("use_multithread", "False")
add_launch_arg("use_intra_process", "False")
- add_launch_arg("use_pointcloud_container", "False")
add_launch_arg("pointcloud_container_name", "pointcloud_container")
add_launch_arg("individual_container_name", "concatenate_container")
add_launch_arg(
diff --git a/aip_xx1_launch/launch/sensing.launch.xml b/aip_xx1_launch/launch/sensing.launch.xml
index 44477cae..8213cba8 100644
--- a/aip_xx1_launch/launch/sensing.launch.xml
+++ b/aip_xx1_launch/launch/sensing.launch.xml
@@ -3,7 +3,6 @@
-
@@ -13,7 +12,6 @@
-
diff --git a/common_sensor_launch/CMakeLists.txt b/common_sensor_launch/CMakeLists.txt
index 60a50fc2..5e719ce7 100644
--- a/common_sensor_launch/CMakeLists.txt
+++ b/common_sensor_launch/CMakeLists.txt
@@ -12,4 +12,5 @@ endif()
ament_auto_package(INSTALL_TO_SHARE
config
launch
+ config
)
diff --git a/common_sensor_launch/config/blockage_diagnostics_param_file.yaml b/common_sensor_launch/config/blockage_diagnostics_param_file.yaml
new file mode 100644
index 00000000..e43b2039
--- /dev/null
+++ b/common_sensor_launch/config/blockage_diagnostics_param_file.yaml
@@ -0,0 +1,14 @@
+/**:
+ ros__parameters:
+ blockage_ratio_threshold: 0.1
+ blockage_count_threshold: 50
+ blockage_buffering_frames: 2
+ blockage_buffering_interval: 1
+ enable_dust_diag: false
+ publish_debug_image: false
+ dust_ratio_threshold: 0.2
+ dust_count_threshold: 10
+ dust_kernel_size: 2
+ dust_buffering_frames: 10
+ dust_buffering_interval: 1
+ blockage_kernel: 10
diff --git a/common_sensor_launch/config/radar_tracks_msgs_converter.param.yaml b/common_sensor_launch/config/radar_tracks_msgs_converter.param.yaml
index 5a30a6bb..7b344aaa 100644
--- a/common_sensor_launch/config/radar_tracks_msgs_converter.param.yaml
+++ b/common_sensor_launch/config/radar_tracks_msgs_converter.param.yaml
@@ -1,6 +1,7 @@
/**:
ros__parameters:
update_rate_hz: 20.0
+ new_frame_id: "base_link"
use_twist_compensation: true
use_twist_yaw_compensation: false
static_object_speed_threshold: 1.0
diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py
index 7b6ee0c2..1990f695 100644
--- a/common_sensor_launch/launch/nebula_node_container.launch.py
+++ b/common_sensor_launch/launch/nebula_node_container.launch.py
@@ -25,6 +25,7 @@
from launch_ros.actions import ComposableNodeContainer
from launch_ros.actions import LoadComposableNodes
from launch_ros.descriptions import ComposableNode
+from launch_ros.substitutions import FindPackageShare
import yaml
@@ -54,14 +55,11 @@ def get_vehicle_info(context):
return p
-def get_vehicle_mirror_info(context):
- path = LaunchConfiguration("vehicle_mirror_param_file").perform(context)
- with open(path, "r") as f:
- p = yaml.safe_load(f)["/**"]["ros__parameters"]
- return p
-
-
def launch_setup(context, *args, **kwargs):
+ def load_composable_node_param(param_path):
+ with open(LaunchConfiguration(param_path).perform(context), "r") as f:
+ return yaml.safe_load(f)["/**"]["ros__parameters"]
+
def create_parameter_dict(*args):
result = {}
for x in args:
@@ -183,7 +181,7 @@ def create_parameter_dict(*args):
)
)
- mirror_info = get_vehicle_mirror_info(context)
+ mirror_info = load_composable_node_param("vehicle_mirror_param_file")
cropbox_parameters["min_x"] = mirror_info["min_longitudinal_offset"]
cropbox_parameters["max_x"] = mirror_info["max_longitudinal_offset"]
cropbox_parameters["min_y"] = mirror_info["min_lateral_offset"]
@@ -284,13 +282,43 @@ def create_parameter_dict(*args):
],
)
+ blockage_diag_component = ComposableNode(
+ package="pointcloud_preprocessor",
+ plugin="pointcloud_preprocessor::BlockageDiagComponent",
+ name="blockage_diag",
+ remappings=[
+ ("input", "pointcloud_raw_ex"),
+ ("output", "blockage_diag/pointcloud"),
+ ],
+ parameters=[
+ {
+ "angle_range": [
+ float(context.perform_substitution(LaunchConfiguration("cloud_min_angle"))),
+ float(context.perform_substitution(LaunchConfiguration("cloud_max_angle"))),
+ ],
+ "horizontal_ring_id": LaunchConfiguration("horizontal_ring_id"),
+ "vertical_bins": LaunchConfiguration("vertical_bins"),
+ "is_channel_order_top2down": LaunchConfiguration("is_channel_order_top2down"),
+ "max_distance_range": LaunchConfiguration("max_range"),
+ "horizontal_resolution": LaunchConfiguration("horizontal_resolution"),
+ }
+ ]
+ + [load_composable_node_param("blockage_diagnostics_param_file")],
+ extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+ )
+
driver_component_loader = LoadComposableNodes(
composable_node_descriptions=[driver_component],
target_container=container,
condition=IfCondition(LaunchConfiguration("launch_driver")),
)
+ blockage_diag_loader = LoadComposableNodes(
+ composable_node_descriptions=[blockage_diag_component],
+ target_container=container,
+ condition=IfCondition(LaunchConfiguration("enable_blockage_diag")),
+ )
- return [container, driver_component_loader]
+ return [container, driver_component_loader, blockage_diag_loader]
def generate_launch_description():
@@ -336,6 +364,17 @@ def add_launch_arg(name: str, default_value=None, description=None):
add_launch_arg("output_as_sensor_frame", "True", "output final pointcloud in sensor frame")
add_launch_arg("diag_span", "1000", "")
add_launch_arg("delay_monitor_ms", "2000", "")
+ add_launch_arg("output_as_sensor_frame", "True", "output final pointcloud in sensor frame")
+
+ add_launch_arg("enable_blockage_diag", "true")
+ add_launch_arg("horizontal_ring_id", "64")
+ add_launch_arg("vertical_bins", "128")
+ add_launch_arg("is_channel_order_top2down", "true")
+ add_launch_arg("horizontal_resolution", "0.4")
+ add_launch_arg(
+ "blockage_diagnostics_param_file",
+ [FindPackageShare("common_sensor_launch"), "/config/blockage_diagnostics_param_file.yaml"],
+ )
set_container_executable = SetLaunchConfiguration(
"container_executable",
diff --git a/common_sensor_launch/launch/velodyne_VLP16.launch.xml b/common_sensor_launch/launch/velodyne_VLP16.launch.xml
index faa32305..a105b0a2 100644
--- a/common_sensor_launch/launch/velodyne_VLP16.launch.xml
+++ b/common_sensor_launch/launch/velodyne_VLP16.launch.xml
@@ -16,6 +16,11 @@
+
+
+
+
+
@@ -34,6 +39,11 @@
+
+
+
+
+
diff --git a/common_sensor_launch/launch/velodyne_VLP32C.launch.xml b/common_sensor_launch/launch/velodyne_VLP32C.launch.xml
index c3db6428..3544d16e 100644
--- a/common_sensor_launch/launch/velodyne_VLP32C.launch.xml
+++ b/common_sensor_launch/launch/velodyne_VLP32C.launch.xml
@@ -16,6 +16,11 @@
+
+
+
+
+
@@ -34,6 +39,11 @@
+
+
+
+
+
diff --git a/common_sensor_launch/launch/velodyne_VLS128.launch.xml b/common_sensor_launch/launch/velodyne_VLS128.launch.xml
index f1d32469..01c8affa 100644
--- a/common_sensor_launch/launch/velodyne_VLS128.launch.xml
+++ b/common_sensor_launch/launch/velodyne_VLS128.launch.xml
@@ -16,6 +16,11 @@
+
+
+
+
+
@@ -34,6 +39,11 @@
+
+
+
+
+