diff --git a/edge_auto_jetson_launch/config/can_assign_matrix.yaml b/edge_auto_jetson_launch/config/can_assign_matrix.yaml new file mode 100644 index 0000000..fb73dc7 --- /dev/null +++ b/edge_auto_jetson_launch/config/can_assign_matrix.yaml @@ -0,0 +1,70 @@ +/**: + ros__parameters: + can_assign_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN <-roi_msg + [ + 1, + 0, + 0, + 0, + 0, + 0, + 0, + 0, # UNKNOWN <-detected_objects + 0, + 1, + 0, + 0, + 0, + 0, + 0, + 0, # CAR + 0, + 0, + 1, + 0, + 0, + 0, + 0, + 0, # TRUCK + 0, + 0, + 0, + 1, + 0, + 0, + 0, + 0, # BUS + 0, + 0, + 0, + 0, + 1, + 0, + 0, + 0, # TRAILER + 0, + 0, + 0, + 0, + 0, + 1, + 0, + 0, # MOTORBIKE + 0, + 0, + 0, + 0, + 0, + 0, + 1, + 0, # BICYCLE + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 1, + ] # PEDESTRIAN diff --git a/edge_auto_jetson_launch/config/centerpoint.param.yaml b/edge_auto_jetson_launch/config/centerpoint.param.yaml new file mode 100644 index 0000000..fea3f3b --- /dev/null +++ b/edge_auto_jetson_launch/config/centerpoint.param.yaml @@ -0,0 +1,15 @@ +/**: + ros__parameters: + class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"] + point_feature_size: 4 + max_voxel_size: 40000 + point_cloud_range: [-89.6, -89.6, -3.0, 89.6, 89.6, 5.0] + voxel_size: [0.32, 0.32, 8.0] + downsample_factor: 1 + encoder_in_feature_size: 9 + # post-process params + circle_nms_dist_threshold: 0.5 + iou_nms_target_class_names: ["CAR"] + iou_nms_search_distance_2d: 10.0 + iou_nms_threshold: 0.1 + yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0] diff --git a/edge_auto_jetson_launch/config/centerpoint_tiny.param.yaml b/edge_auto_jetson_launch/config/centerpoint_tiny.param.yaml new file mode 100644 index 0000000..92db4cd --- /dev/null +++ b/edge_auto_jetson_launch/config/centerpoint_tiny.param.yaml @@ -0,0 +1,15 @@ +/**: + ros__parameters: + class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"] + point_feature_size: 4 + max_voxel_size: 40000 + point_cloud_range: [-76.8, -76.8, -2.0, 76.8, 76.8, 4.0] + voxel_size: [0.32, 0.32, 6.0] + downsample_factor: 2 + encoder_in_feature_size: 9 + # post-process params + circle_nms_dist_threshold: 0.5 + iou_nms_target_class_names: ["CAR"] + iou_nms_search_distance_2d: 10.0 + iou_nms_threshold: 0.1 + yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0] diff --git a/edge_auto_jetson_launch/config/detection_class_remapper.param.yaml b/edge_auto_jetson_launch/config/detection_class_remapper.param.yaml new file mode 100644 index 0000000..ed378ff --- /dev/null +++ b/edge_auto_jetson_launch/config/detection_class_remapper.param.yaml @@ -0,0 +1,38 @@ +/**: + ros__parameters: + allow_remapping_by_area_matrix: + # NOTE(kl): We turn all vehicles into trailers if they go over 3x12 [m^2]. + # NOTE(kl): We turn cars into trucks if they have an area between 2.2 x 5.5 and 3.0 * 12.0 [m^2] + # row: original class. column: class to remap to + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN + [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + 0, 0, 1, 0, 1, 0, 0, 0, #CAR + 0, 0, 0, 0, 1, 0, 0, 0, #TRUCK + 0, 0, 0, 0, 1, 0, 0, 0, #BUS + 0, 0, 0, 0, 0, 0, 0, 0, #TRAILER + 0, 0, 0, 0, 0, 0, 0, 0, #MOTORBIKE + 0, 0, 0, 0, 0, 0, 0, 0, #BICYCLE + 0, 0, 0, 0, 0, 0, 0, 0] #PEDESTRIAN + + min_area_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [ 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #UNKNOWN + 0.000, 0.000, 12.100, 0.000, 36.000, 0.000, 0.000, 0.000, #CAR + 0.000, 0.000, 0.000, 0.000, 36.000, 0.000, 0.000, 0.000, #TRUCK + 0.000, 0.000, 0.000, 0.000, 36.000, 0.000, 0.000, 0.000, #BUS + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #TRAILER + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #MOTORBIKE + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #BICYCLE + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000] #PEDESTRIAN + + + max_area_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [ 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #UNKNOWN + 0.000, 0.000, 36.000, 0.000, inf, 0.000, 0.000, 0.000, #CAR + 0.000, 0.000, 0.000, 0.000, inf, 0.000, 0.000, 0.000, #TRUCK + 0.000, 0.000, 0.000, 0.000, inf, 0.000, 0.000, 0.000, #BUS + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #TRAILER + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #MOTORBIKE + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #BICYCLE + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000] #PEDESTRIAN diff --git a/edge_auto_jetson_launch/config/roi_sync.param.yaml b/edge_auto_jetson_launch/config/roi_sync.param.yaml new file mode 100644 index 0000000..364c9c5 --- /dev/null +++ b/edge_auto_jetson_launch/config/roi_sync.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + input_offset_ms: [50.0, 50.0] + timeout_ms: 70.0 + match_threshold_ms: 50.0 diff --git a/edge_auto_jetson_launch/launch/component/crop_box_filter.launch.xml b/edge_auto_jetson_launch/launch/component/crop_box_filter.launch.xml new file mode 100644 index 0000000..dcf4f05 --- /dev/null +++ b/edge_auto_jetson_launch/launch/component/crop_box_filter.launch.xml @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/edge_auto_jetson_launch/launch/component/hesai_at128.launch.xml b/edge_auto_jetson_launch/launch/component/hesai_at128.launch.xml new file mode 100755 index 0000000..b679985 --- /dev/null +++ b/edge_auto_jetson_launch/launch/component/hesai_at128.launch.xml @@ -0,0 +1,75 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/edge_auto_jetson_launch/launch/component/lidar_camera_tf_publisher.launch.py b/edge_auto_jetson_launch/launch/component/lidar_camera_tf_publisher.launch.py new file mode 100644 index 0000000..e5e3abf --- /dev/null +++ b/edge_auto_jetson_launch/launch/component/lidar_camera_tf_publisher.launch.py @@ -0,0 +1,54 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +import json + +def launch_setup(context, *args, **kwargs): + tf_file = LaunchConfiguration('tf_file_path').perform(context) + with open(tf_file, 'r') as f: + tf_data = json.load(f) + + return [ + Node( + name="lidar_camera_tf_publisher", + package='tf2_ros', + executable='static_transform_publisher', + arguments=[ + '--x', str(tf_data['transform']['translation']['x']), + '--y', str(tf_data['transform']['translation']['y']), + '--z', str(tf_data['transform']['translation']['z']), + '--qx', str(tf_data['transform']['rotation']['x']), + '--qy', str(tf_data['transform']['rotation']['y']), + '--qz', str(tf_data['transform']['rotation']['z']), + '--qw', str(tf_data['transform']['rotation']['w']), + '--frame-id', str(tf_data['header']['frame_id']), + '--child-frame-id', str(tf_data['child_frame_id']) + ]), + Node( + name="camera_optical_link_publisher", + package='tf2_ros', + executable='static_transform_publisher', + arguments=[ + '--x', '0.0', + '--y', '0.0', + '--z', '0.0', + '--qx', '0.5', + '--qy', '-0.5', + '--qz', '0.5', + '--qw', '-0.5', + '--frame-id', str(tf_data['child_frame_id']), + '--child-frame-id', str(tf_data['child_frame_id'].replace('camera_link', + 'camera_optical_link')) + ]) + ] + +def generate_launch_description(): + return LaunchDescription( + [ + DeclareLaunchArgument("tf_file_path"), + OpaqueFunction(function=launch_setup), + ] + ) + diff --git a/edge_auto_jetson_launch/launch/component/lidar_centerpoint.launch.xml b/edge_auto_jetson_launch/launch/component/lidar_centerpoint.launch.xml new file mode 100644 index 0000000..0003a49 --- /dev/null +++ b/edge_auto_jetson_launch/launch/component/lidar_centerpoint.launch.xml @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/edge_auto_jetson_launch/launch/component/roi_detected_objects_fusion.launch.xml b/edge_auto_jetson_launch/launch/component/roi_detected_objects_fusion.launch.xml new file mode 100644 index 0000000..7ef982a --- /dev/null +++ b/edge_auto_jetson_launch/launch/component/roi_detected_objects_fusion.launch.xml @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/edge_auto_jetson_launch/launch/component/voxel_downsample_filter.launch.xml b/edge_auto_jetson_launch/launch/component/voxel_downsample_filter.launch.xml new file mode 100644 index 0000000..a50b68d --- /dev/null +++ b/edge_auto_jetson_launch/launch/component/voxel_downsample_filter.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/edge_auto_jetson_launch/launch/perception_at128_sample.launch.xml b/edge_auto_jetson_launch/launch/perception_at128_sample.launch.xml new file mode 100644 index 0000000..6ca3ec7 --- /dev/null +++ b/edge_auto_jetson_launch/launch/perception_at128_sample.launch.xml @@ -0,0 +1,76 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/edge_auto_jetson_launch/package.xml b/edge_auto_jetson_launch/package.xml index 134b498..7f1bd08 100644 --- a/edge_auto_jetson_launch/package.xml +++ b/edge_auto_jetson_launch/package.xml @@ -19,6 +19,11 @@ bytetrack intrinsic_camera_calibrator image_transport_decompressor + image_projection_based_fusion + vehicle_info_util + lidar_centerpoint + nebula_sensor_driver + ament_lint_auto ament_lint_common