diff --git a/edge_auto_launch/config/roi_cluster_fusion.param.yaml b/edge_auto_launch/config/roi_cluster_fusion.param.yaml
new file mode 100644
index 0000000..e8c9f85
--- /dev/null
+++ b/edge_auto_launch/config/roi_cluster_fusion.param.yaml
@@ -0,0 +1,16 @@
+/**:
+ ros__parameters:
+ fusion_distance: 100.0
+ trust_object_distance: 100.0
+ trust_object_iou_mode: "iou"
+ non_trust_object_iou_mode: "iou"
+ use_cluster_semantic_type: false
+ # only_allow_inside_cluster: true
+ only_allow_inside_cluster: false
+ roi_scale_factor: 1.1
+ iou_threshold: 0.3
+ unknown_iou_threshold: 0.1
+ remove_unknown: true
+ debug_mode: true
+ image_buffer_size: 1000
+
diff --git a/edge_auto_launch/config/scan_ground_filter.param.yaml b/edge_auto_launch/config/scan_ground_filter.param.yaml
index cd18c66..35d494a 100644
--- a/edge_auto_launch/config/scan_ground_filter.param.yaml
+++ b/edge_auto_launch/config/scan_ground_filter.param.yaml
@@ -1,6 +1,18 @@
/**:
ros__parameters:
global_slope_max_angle_deg: 10.0
- local_slope_max_angle_deg: 30.0
+ local_slope_max_angle_deg: 13.0 # recommended 30.0 for non elevation_grid_mode
split_points_distance_tolerance: 0.2
+ use_virtual_ground_point: True
split_height_distance: 0.2
+ non_ground_height_threshold: 0.20
+ grid_size_m: 0.1
+ grid_mode_switch_radius: 20.0
+ gnd_grid_buffer_size: 4
+ detection_range_z_max: 2.5
+ elevation_grid_mode: true
+ low_priority_region_x: -20.0
+ center_pcl_shift: 0.0
+ radial_divider_angle_deg: 1.0
+ use_recheck_ground_cluster: true
+ use_lowest_point: true
diff --git a/edge_auto_launch/launch/calibration_extrinsic_at128_sample.launch.xml b/edge_auto_launch/launch/calibration_extrinsic_at128_sample.launch.xml
index 270755e..e93c70a 100644
--- a/edge_auto_launch/launch/calibration_extrinsic_at128_sample.launch.xml
+++ b/edge_auto_launch/launch/calibration_extrinsic_at128_sample.launch.xml
@@ -6,17 +6,12 @@
-
-
-
-
-
@@ -29,42 +24,28 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
diff --git a/edge_auto_launch/launch/calibration_extrinsic_xt32_sample.launch.xml b/edge_auto_launch/launch/calibration_extrinsic_xt32_sample.launch.xml
index 1a9db0b..58da251 100755
--- a/edge_auto_launch/launch/calibration_extrinsic_xt32_sample.launch.xml
+++ b/edge_auto_launch/launch/calibration_extrinsic_xt32_sample.launch.xml
@@ -5,17 +5,12 @@
-
-
-
-
-
@@ -27,42 +22,28 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
diff --git a/edge_auto_launch/launch/component/extrinsic_manual_calibrator.launch.xml b/edge_auto_launch/launch/component/extrinsic_manual_calibrator.launch.xml
deleted file mode 100644
index 6f95ae0..0000000
--- a/edge_auto_launch/launch/component/extrinsic_manual_calibrator.launch.xml
+++ /dev/null
@@ -1,31 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/edge_auto_launch/launch/component/hesai_pandar_xt32.launch.xml b/edge_auto_launch/launch/component/hesai_pandar_xt32.launch.xml
index f63b2c5..ce1fe72 100755
--- a/edge_auto_launch/launch/component/hesai_pandar_xt32.launch.xml
+++ b/edge_auto_launch/launch/component/hesai_pandar_xt32.launch.xml
@@ -23,6 +23,7 @@
+
@@ -50,6 +51,9 @@
+
+
+
diff --git a/edge_auto_launch/launch/component/lidar_camera_tf_publisher.launch.py b/edge_auto_launch/launch/component/lidar_camera_tf_publisher.launch.py
index a0e659a..477f748 100644
--- a/edge_auto_launch/launch/component/lidar_camera_tf_publisher.launch.py
+++ b/edge_auto_launch/launch/component/lidar_camera_tf_publisher.launch.py
@@ -19,17 +19,59 @@
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
import json
+import yaml
+import re
+from pathlib import Path
+import transforms3d
+
+def parse_yaml(contents: dict):
+ get_first_key = lambda d: list(d.keys())[0]
+ frame_id = get_first_key(contents)
+ child_frame_id = get_first_key(contents[frame_id])
+
+ x = contents[frame_id][child_frame_id]['x']
+ y = contents[frame_id][child_frame_id]['y']
+ z = contents[frame_id][child_frame_id]['z']
+ roll = contents[frame_id][child_frame_id]['roll']
+ pitch = contents[frame_id][child_frame_id]['pitch']
+ yaw = contents[frame_id][child_frame_id]['yaw']
+
+ q_wxyz = transforms3d.euler.euler2quat(roll, pitch, yaw) # output format is wxyz
+
+ # pack contents into a similar format as json one
+ return {
+ 'header': {'frame_id': frame_id},
+ 'child_frame_id': child_frame_id,
+ 'transform': {
+ 'translation': {'x': x, 'y': y, 'z': z},
+ 'rotation': {'w': q_wxyz[0], 'x': q_wxyz[1], 'y': q_wxyz[2], 'z': q_wxyz[3]}
+ }
+ }
def launch_setup(context, *args, **kwargs):
"""Return Launch Configuration for the TF between the lidar and the camera."""
tf_file = LaunchConfiguration('tf_file_path').perform(context)
- with open(tf_file, 'r') as f:
- tf_data = json.load(f)
+ file_format = Path(tf_file).suffix.lstrip('.')
+
+ '''
+ Here assumes json/yaml format generated by CalibrationTools
+ '''
+ if file_format == 'json':
+ with open(tf_file, 'r') as f:
+ tf_data = json.load(f)
+ elif file_format == 'yaml' or file_format == 'yml':
+ with open(tf_file, 'r') as f:
+ yaml_contents = yaml.safe_load(f)
+ tf_data = parse_yaml(yaml_contents)
+ else:
+ print(f'Invalid TF file ({tf_file}) was specified.')
+
+ camera_id = re.findall(r'\d+', str(tf_data['child_frame_id']))[0]
return [
Node(
- name="lidar_camera_tf_publisher",
+ name=f'lidar_camera_tf_publisher{camera_id}',
package='tf2_ros',
executable='static_transform_publisher',
arguments=[
@@ -44,7 +86,7 @@ def launch_setup(context, *args, **kwargs):
'--child-frame-id', str(tf_data['child_frame_id'])
]),
Node(
- name="camera_optical_link_publisher",
+ name=f'camera_optical_link_publisher{camera_id}',
package='tf2_ros',
executable='static_transform_publisher',
arguments=[
@@ -61,7 +103,6 @@ def launch_setup(context, *args, **kwargs):
])
]
-
def generate_launch_description():
return LaunchDescription(
[
diff --git a/edge_auto_launch/launch/component/roi_cluster_fusion.launch.xml b/edge_auto_launch/launch/component/roi_cluster_fusion.launch.xml
index 1f4a916..22d1c08 100644
--- a/edge_auto_launch/launch/component/roi_cluster_fusion.launch.xml
+++ b/edge_auto_launch/launch/component/roi_cluster_fusion.launch.xml
@@ -8,8 +8,7 @@
-
-
+
@@ -20,23 +19,12 @@
-
+
+
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/edge_auto_launch/launch/component/voxel_grid_based_euclidean_cluster.launch.xml b/edge_auto_launch/launch/component/voxel_grid_based_euclidean_cluster.launch.xml
index 0755265..740b2f4 100644
--- a/edge_auto_launch/launch/component/voxel_grid_based_euclidean_cluster.launch.xml
+++ b/edge_auto_launch/launch/component/voxel_grid_based_euclidean_cluster.launch.xml
@@ -5,7 +5,7 @@
-
+
diff --git a/edge_auto_launch/launch/perception_xt32_sample.launch.xml b/edge_auto_launch/launch/perception_xt32_sample.launch.xml
index d0b4320..f651be1 100755
--- a/edge_auto_launch/launch/perception_xt32_sample.launch.xml
+++ b/edge_auto_launch/launch/perception_xt32_sample.launch.xml
@@ -7,7 +7,7 @@
-
+
@@ -60,7 +60,7 @@
-
+
@@ -78,12 +78,12 @@
-
+
-
+
diff --git a/edge_auto_launch/package.xml b/edge_auto_launch/package.xml
index 5bdc229..a07c61b 100644
--- a/edge_auto_launch/package.xml
+++ b/edge_auto_launch/package.xml
@@ -12,10 +12,9 @@
autoware_perception_rviz_plugin
autoware_pointcloud_preprocessor
- detected_object_feature_remover
- euclidean_cluster
- extrinsic_calibration_manager
- extrinsic_manual_calibrator
+ autoware_detected_object_feature_remover
+ autoware_euclidean_cluster
+ sensor_calibration_manager
tag_based_pnp_calibrator
autoware_ground_segmentation
intrinsic_camera_calibrator
@@ -27,7 +26,9 @@
autoware_image_transport_decompressor
individual_params
autoware_image_projection_based_fusion
- sensor_calibration_tools
+
+ image_transport_plugins
+ topic_tools
ament_lint_auto
ament_lint_common