diff --git a/aip_x1_launch/config/dummy_diag_publisher/sensor_kit.param.yaml b/aip_x1_launch/config/dummy_diag_publisher/sensor_kit.param.yaml
index 75712024..be704546 100644
--- a/aip_x1_launch/config/dummy_diag_publisher/sensor_kit.param.yaml
+++ b/aip_x1_launch/config/dummy_diag_publisher/sensor_kit.param.yaml
@@ -12,6 +12,19 @@
/**:
ros__parameters:
required_diags:
+ # sensing
+ sensing_topic_status: default
+
+ # velodyne
+ velodyne_connection: default
+ velodyne_temperature: default
+ velodyne_rpm: default
+
+ # pandar
+ pandar_connection: default
+ pandar_temperature: default
+ pandar_ptp: default
+
# gnss
gnss_connection: default
gnss_data: default
@@ -21,17 +34,5 @@
gnss_jamming: default
fix topic status: default
- # pandar
- pandar_connection: default
- pandar_temperature: default
- pandar_ptp: default
-
- # velodyne
- velodyne_connection: default
- velodyne_temperature: default
- velodyne_rpm: default
-
- sensing_topic_status: default
-
# imu
gyro_bias_estimator: default
diff --git a/aip_x1_launch/launch/lidar.launch.xml b/aip_x1_launch/launch/lidar.launch.xml
index f1796100..4c31e1d0 100644
--- a/aip_x1_launch/launch/lidar.launch.xml
+++ b/aip_x1_launch/launch/lidar.launch.xml
@@ -3,7 +3,6 @@
-
diff --git a/aip_x2_gen2_launch/config/ARS548.param.yaml b/aip_x2_gen2_launch/config/ARS548.param.yaml
index 1fe5e7e2..03943890 100644
--- a/aip_x2_gen2_launch/config/ARS548.param.yaml
+++ b/aip_x2_gen2_launch/config/ARS548.param.yaml
@@ -10,8 +10,8 @@
sensor_model: ARS548
configuration_host_port: 42401
configuration_sensor_port: 42101
- use_sensor_time: false
- configuration_vehicle_length: 7.2369
- configuration_vehicle_width: 2.2916
+ use_sensor_time: true
+ configuration_vehicle_length: 7.2369 # wheel_base + front_overhang + rear_overhang
+ configuration_vehicle_width: 2.2916 # wheel_tread + left_overhang + right_overhang
configuration_vehicle_height: 3.08
configuration_vehicle_wheelbase: 4.76012
diff --git a/aip_x2_gen2_launch/config/concatenate_and_time_sync_node.param.yaml b/aip_x2_gen2_launch/config/concatenate_and_time_sync_node.param.yaml
new file mode 100644
index 00000000..c0a9a892
--- /dev/null
+++ b/aip_x2_gen2_launch/config/concatenate_and_time_sync_node.param.yaml
@@ -0,0 +1,28 @@
+/**:
+ ros__parameters:
+ debug_mode: false
+ has_static_tf_only: false
+ rosbag_length: 10.0
+ maximum_queue_size: 5
+ timeout_sec: 0.2
+ is_motion_compensated: true
+ publish_synchronized_pointcloud: true
+ keep_input_frame_in_synchronized_pointcloud: true
+ publish_previous_but_late_pointcloud: false
+ synchronized_pointcloud_postfix: pointcloud
+ input_twist_topic_type: twist
+ input_topics: [
+ "/sensing/lidar/rear_upper/pointcloud_before_sync", # 0.047
+ "/sensing/lidar/rear_lower/pointcloud_before_sync", # 0.048
+ "/sensing/lidar/left_upper/pointcloud_before_sync", # 0.048
+ "/sensing/lidar/left_lower/pointcloud_before_sync", # 0.047
+ "/sensing/lidar/front_upper/pointcloud_before_sync", # 0.072
+ "/sensing/lidar/front_lower/pointcloud_before_sync", # 0.072
+ "/sensing/lidar/right_upper/pointcloud_before_sync", # 0.073
+ "/sensing/lidar/right_lower/pointcloud_before_sync" # 0.097
+ ]
+ output_frame: base_link
+ matching_strategy:
+ type: advanced
+ lidar_timestamp_offsets: [0.0, 0.001, 0.001, 0.0, 0.025, 0.025, 0.026, 0.050]
+ lidar_timestamp_noise_window: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01]
diff --git a/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml b/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml
index 4adc9311..8bbaddca 100644
--- a/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml
+++ b/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml
@@ -21,7 +21,6 @@
-
@@ -60,7 +59,6 @@
-
diff --git a/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml b/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml
index 443294ba..67b9f0b8 100644
--- a/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml
+++ b/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml
@@ -22,7 +22,6 @@
-
@@ -59,7 +58,6 @@
-
diff --git a/aip_x2_gen2_launch/launch/nebula_node_container.launch.py b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py
index 6f17007b..e8a4a55a 100644
--- a/aip_x2_gen2_launch/launch/nebula_node_container.launch.py
+++ b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py
@@ -26,6 +26,7 @@
from launch_ros.actions import ComposableNodeContainer
from launch_ros.actions import LoadComposableNodes
from launch_ros.descriptions import ComposableNode
+from launch_ros.parameter_descriptions import ParameterFile
from launch_ros.substitutions import FindPackageShare
import yaml
@@ -76,7 +77,7 @@ def str2vector(string):
glog_component = ComposableNode(
package="autoware_glog_component",
- plugin="GlogComponent",
+ plugin="autoware::glog_component::GlogComponent",
name="glog_component",
)
@@ -161,14 +162,27 @@ def str2vector(string):
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
)
+ ring_outlier_filter_node_param = ParameterFile(
+ param_file=LaunchConfiguration("ring_outlier_filter_node_param_file").perform(context),
+ allow_substs=True,
+ )
+
+ # 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_output_frame = {"output_frame": LaunchConfiguration("frame_id")}
+ else:
+ # keep the output frame as the input frame
+ ring_outlier_output_frame = {"output_frame": ""}
+
ring_outlier_filter_component = ComposableNode(
package="autoware_pointcloud_preprocessor",
plugin="autoware::pointcloud_preprocessor::RingOutlierFilterComponent",
name="ring_outlier_filter",
remappings=[
("input", "rectified/pointcloud_ex"),
- ("output", "pointcloud"),
+ ("output", "pointcloud_before_sync"),
],
+ parameters=[ring_outlier_filter_node_param, ring_outlier_output_frame],
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
)
@@ -178,7 +192,7 @@ def str2vector(string):
name="dual_return_filter",
remappings=[
("input", "rectified/pointcloud_ex"),
- ("output", "pointcloud"),
+ ("output", "pointcloud_before_sync"),
],
parameters=[
{
@@ -191,7 +205,6 @@ def str2vector(string):
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
)
- distance_range = str2vector(context.perform_substitution(LaunchConfiguration("distance_range")))
blockage_diag_component = ComposableNode(
package="autoware_pointcloud_preprocessor",
plugin="autoware::pointcloud_preprocessor::BlockageDiagComponent",
@@ -206,7 +219,7 @@ def str2vector(string):
"horizontal_ring_id": LaunchConfiguration("horizontal_ring_id"),
"vertical_bins": LaunchConfiguration("vertical_bins"),
"is_channel_order_top2down": LaunchConfiguration("is_channel_order_top2down"),
- "max_distance_range": distance_range[1],
+ "max_distance_range": LaunchConfiguration("max_range"),
"horizontal_resolution": LaunchConfiguration("horizontal_resolution"),
}
]
@@ -299,6 +312,10 @@ def add_launch_arg(name: str, default_value=None, description=None):
"blockage_diagnostics_param_file",
[FindPackageShare("common_sensor_launch"), "/config/blockage_diagnostics.param.yaml"],
)
+ add_launch_arg(
+ "ring_outlier_filter_node_param_file",
+ [FindPackageShare("common_sensor_launch"), "/config/ring_outlier_filter_node.param.yaml"],
+ )
add_launch_arg(
"distortion_corrector_node_param_file",
[FindPackageShare("common_sensor_launch"), "/config/distortion_corrector_node.param.yaml"],
@@ -313,6 +330,7 @@ def add_launch_arg(name: str, default_value=None, description=None):
add_launch_arg("point_filters_param_file")
add_launch_arg("calibration_file", "")
+ 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_gen2_launch/launch/pointcloud_preprocessor.launch.py b/aip_x2_gen2_launch/launch/pointcloud_preprocessor.launch.py
index 0775e3a2..dbac5d8e 100644
--- a/aip_x2_gen2_launch/launch/pointcloud_preprocessor.launch.py
+++ b/aip_x2_gen2_launch/launch/pointcloud_preprocessor.launch.py
@@ -12,7 +12,9 @@
# See the License for the specific language governing permissions and
# limitations under the License.
+import os
+from ament_index_python.packages import get_package_share_directory
import launch
from launch.actions import DeclareLaunchArgument
from launch.actions import OpaqueFunction
@@ -22,9 +24,18 @@
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import LoadComposableNodes
from launch_ros.descriptions import ComposableNode
+from launch_ros.parameter_descriptions import ParameterFile
def launch_setup(context, *args, **kwargs):
+ # concatenate node parameters
+ concatenate_and_time_sync_node_param = ParameterFile(
+ param_file=LaunchConfiguration("concatenate_and_time_sync_node_param_path").perform(
+ context
+ ),
+ allow_substs=True,
+ )
+
# set concat filter as a component
concat_component = ComposableNode(
package="autoware_pointcloud_preprocessor",
@@ -34,30 +45,15 @@ def launch_setup(context, *args, **kwargs):
("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"),
("output", "concatenated/pointcloud"),
],
- 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",
- ],
- "input_offset": [0.005, 0.025, 0.050, 0.005, 0.050, 0.005, 0.005, 0.025],
- "timeout_sec": 0.075,
- "output_frame": LaunchConfiguration("base_frame"),
- "input_twist_topic_type": "twist",
- }
- ],
+ parameters=[concatenate_and_time_sync_node_param],
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
)
+ # load concat or passthrough filter
concat_loader = LoadComposableNodes(
composable_node_descriptions=[concat_component],
target_container=LaunchConfiguration("pointcloud_container_name"),
+ condition=IfCondition(LaunchConfiguration("use_concat_filter")),
)
return [concat_loader]
@@ -69,10 +65,19 @@ def generate_launch_description():
def add_launch_arg(name: str, default_value=None):
launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value))
- add_launch_arg("base_frame", "base_link")
+ aip_x2_gen2_launch_share_dir = get_package_share_directory("aip_x2_gen2_launch")
+
add_launch_arg("use_multithread", "True")
add_launch_arg("use_intra_process", "True")
add_launch_arg("pointcloud_container_name", "pointcloud_container")
+ add_launch_arg(
+ "concatenate_and_time_sync_node_param_path",
+ os.path.join(
+ aip_x2_gen2_launch_share_dir,
+ "config",
+ "concatenate_and_time_sync_node.param.yaml",
+ ),
+ )
set_container_executable = SetLaunchConfiguration(
"container_executable",
diff --git a/aip_x2_gen2_launch/launch/radar.launch.xml b/aip_x2_gen2_launch/launch/radar.launch.xml
index d793d8de..599076a9 100644
--- a/aip_x2_gen2_launch/launch/radar.launch.xml
+++ b/aip_x2_gen2_launch/launch/radar.launch.xml
@@ -7,6 +7,14 @@
+
+
+
+
+
+
+
+
@@ -14,9 +22,9 @@
-
-
-
+
+
+
@@ -45,9 +53,9 @@
-
-
-
+
+
+
@@ -75,9 +83,9 @@
-
-
-
+
+
+
@@ -105,9 +113,9 @@
-
-
-
+
+
+
@@ -135,9 +143,9 @@
-
-
-
+
+
+
@@ -165,9 +173,9 @@
-
-
-
+
+
+
diff --git a/common_sensor_launch/launch/hesai_OT128.launch.xml b/common_sensor_launch/launch/hesai_OT128.launch.xml
index 08ecff61..9fed2384 100644
--- a/common_sensor_launch/launch/hesai_OT128.launch.xml
+++ b/common_sensor_launch/launch/hesai_OT128.launch.xml
@@ -2,9 +2,6 @@
-
-
@@ -22,10 +19,9 @@
-
-
+
@@ -34,9 +30,9 @@
+
-
diff --git a/common_sensor_launch/launch/hesai_XT32.launch.xml b/common_sensor_launch/launch/hesai_XT32.launch.xml
index 9f74a71a..762b47ff 100644
--- a/common_sensor_launch/launch/hesai_XT32.launch.xml
+++ b/common_sensor_launch/launch/hesai_XT32.launch.xml
@@ -2,9 +2,6 @@
-
-
@@ -15,6 +12,7 @@
+
@@ -22,10 +20,9 @@
-
-
+
@@ -35,6 +32,7 @@
+
@@ -43,6 +41,5 @@
-