diff --git a/object_detection/launch/debayer.launch.py b/object_detection/launch/debayer.launch.py new file mode 100644 index 0000000..7ad16cd --- /dev/null +++ b/object_detection/launch/debayer.launch.py @@ -0,0 +1,151 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import ( + LaunchConfiguration, + EnvironmentVariable, + PathJoinSubstitution, + TextSubstitution, +) +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from ament_index_python.packages import get_package_share_directory +import os + + +def generate_launch_description(): + # Declare all launch arguments + declared_arguments = [ + DeclareLaunchArgument( + "smb_name", + default_value=EnvironmentVariable("SMB_NAME", default_value="smb261"), + description="Name of the SMB in the format smb26x (relevant for calibrations)", + ), + DeclareLaunchArgument( + "gpu", + default_value="local", + description="Run on GPU? Options: 'local', 'remote' (default), 'off'", + choices=["local", "remote", "off"], + ), + DeclareLaunchArgument( + "GPU_user", + default_value=EnvironmentVariable("USER"), + description="Username to use on the jetson xavier GPU", + ), + DeclareLaunchArgument( + "input_camera_name", + default_value="/rgb_camera", + description="Name of the camera, i.e. topic prefix for camera stream and camera info", + ), + DeclareLaunchArgument( + "debayer_image", + default_value="true", + description="Debayer the images (supplied in $input_camera_name/image_raw)", + ), + DeclareLaunchArgument( + "lidar_topic", + default_value="/rslidar/points", + description="Topic containing the point cloud from the lidar", + ), + DeclareLaunchArgument( + "object_detection_classes", + default_value="[0,24,25,28,32,39,41,45,46,47,56]", + description="List of the ids of classes for detection (COCO dataset)", + ), + DeclareLaunchArgument( + "model_dir_path", + default_value=PathJoinSubstitution( + [ + FindPackageShare("object_detection"), + "models", + ] + ), + description="path to the yolo model directory", + ), + DeclareLaunchArgument( + "model", default_value="yolov5l6", description="yolo model name" + ), + ] + + # Debayer the image (conditionally included) + debayer_image_group = GroupAction( + [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("object_detection"), + "launch", + "debayer.launch.py", + ] + ) + ), + condition=IfCondition(LaunchConfiguration("debayer_image")), + launch_arguments={ + "input_camera_name": LaunchConfiguration("input_camera_name") + }, + ) + ] + ) + + # Object detection node + + object_detection_group = GroupAction( + [ + Node( + package="object_detection", + executable="object_detection_node.py", + name="object_detector", + output="screen", + parameters=[ + # Input related + { + "camera_topic": PathJoinSubstitution( + [LaunchConfiguration("input_camera_name"), "image_raw"] + ) + }, + { + "camera_info_topic": PathJoinSubstitution( + [LaunchConfiguration("input_camera_name"), "camera_info"] + ) + }, + {"lidar_topic": LaunchConfiguration("lidar_topic")}, + # Output related - load from file + {"project_object_points_to_image": True}, + {"project_all_points_to_image": False}, + {"object_detection_pos_topic": "object_positions"}, + {"object_detection_output_image_topic": "detections_in_image"}, + {"object_detection_point_clouds_topic": "detection_point_clouds"}, + {"object_detection_info_topic": "detection_info"}, + # Camera Lidar synchronization related + {"camera_lidar_sync_queue_size": 10}, + {"camera_lidar_sync_slop": 0.1}, + # Point Projector related + { + "project_config": [ + TextSubstitution(text="projector_config_"), + LaunchConfiguration("smb_name"), + TextSubstitution(text=".yaml"), + ] + }, + # Object detection related + {"model": LaunchConfiguration("model")}, + {"model_dir_path": LaunchConfiguration("model_dir_path")}, + {"device": "0" if LaunchConfiguration("gpu") != "off" else "cpu"}, + {"confident": 0.0}, + {"iou": 0.1}, + {"classes": LaunchConfiguration("object_detection_classes")}, + {"multiple_instance": False}, + # Object localization related + {"model_method": "hdbscan"}, + {"ground_percentage": 25}, + {"bb_contract_percentage": 10}, + ], + ) + ] + ) + + return LaunchDescription( + declared_arguments + [debayer_image_group, object_detection_group] + ) \ No newline at end of file diff --git a/object_detection/launch/object_detection_real.launch.py b/object_detection/launch/object_detection_real.launch.py new file mode 100755 index 0000000..37d8e55 --- /dev/null +++ b/object_detection/launch/object_detection_real.launch.py @@ -0,0 +1,151 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import ( + LaunchConfiguration, + EnvironmentVariable, + PathJoinSubstitution, + TextSubstitution, +) +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from ament_index_python.packages import get_package_share_directory +import os + + +def generate_launch_description(): + # Declare all launch arguments + declared_arguments = [ + DeclareLaunchArgument( + "smb_name", + default_value=EnvironmentVariable("SMB_NAME", default_value="smb261"), + description="Name of the SMB in the format smb26x (relevant for calibrations)", + ), + DeclareLaunchArgument( + "gpu", + default_value="local", + description="Run on GPU? Options: 'local', 'remote' (default), 'off'", + choices=["local", "remote", "off"], + ), + DeclareLaunchArgument( + "GPU_user", + default_value=EnvironmentVariable("USER"), + description="Username to use on the jetson xavier GPU", + ), + DeclareLaunchArgument( + "input_camera_name", + default_value="/rgb_camera", + description="Name of the camera, i.e. topic prefix for camera stream and camera info", + ), + DeclareLaunchArgument( + "debayer_image", + default_value="false", + description="Debayer the images (supplied in $input_camera_name/image_raw)", + ), + DeclareLaunchArgument( + "lidar_topic", + default_value="/rslidar/points", + description="Topic containing the point cloud from the lidar", + ), + DeclareLaunchArgument( + "object_detection_classes", + default_value="[0,24,25,28,32,39,41,45,46,47,56]", + description="List of the ids of classes for detection (COCO dataset)", + ), + DeclareLaunchArgument( + "model_dir_path", + default_value=PathJoinSubstitution( + [ + FindPackageShare("object_detection"), + "models", + ] + ), + description="path to the yolo model directory", + ), + DeclareLaunchArgument( + "model", default_value="yolov5l6", description="yolo model name" + ), + ] + + # Debayer the image (conditionally included) + debayer_image_group = GroupAction( + [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("object_detection"), + "launch", + "debayer.launch.py", + ] + ) + ), + condition=IfCondition(LaunchConfiguration("debayer_image")), + launch_arguments=[ + ("input_camera_name", LaunchConfiguration("input_camera_name")) + ], + ) + ] +) + + # Object detection node + + object_detection_group = GroupAction( + [ + Node( + package="object_detection", + executable="object_detection_node.py", + name="object_detector", + output="screen", + parameters=[ + # Input related + { + "camera_topic": PathJoinSubstitution( + [LaunchConfiguration("input_camera_name"), "image_debayered"] + ) + }, + { + "camera_info_topic": PathJoinSubstitution( + [LaunchConfiguration("input_camera_name"), "camera_info"] + ) + }, + {"lidar_topic": LaunchConfiguration("lidar_topic")}, + # Output related - load from file + {"project_object_points_to_image": True}, + {"project_all_points_to_image": False}, + {"object_detection_pos_topic": "object_positions"}, + {"object_detection_output_image_topic": "detections_in_image"}, + {"object_detection_point_clouds_topic": "detection_point_clouds"}, + {"object_detection_info_topic": "detection_info"}, + # Camera Lidar synchronization related + {"camera_lidar_sync_queue_size": 10}, + {"camera_lidar_sync_slop": 0.1}, + # Point Projector related + { + "project_config": [ + TextSubstitution(text="projector_config_"), + LaunchConfiguration("smb_name"), + TextSubstitution(text=".yaml"), + ] + }, + # Object detection related + {"model": LaunchConfiguration("model")}, + {"model_dir_path": LaunchConfiguration("model_dir_path")}, + {"device": "0" if LaunchConfiguration("gpu") != "off" else "cpu"}, + {"confident": 0.0}, + {"iou": 0.1}, + {"classes": LaunchConfiguration("object_detection_classes")}, + {"multiple_instance": False}, + # Object localization related + {"model_method": "hdbscan"}, + {"ground_percentage": 25}, + {"bb_contract_percentage": 10}, + ], + ) + ] + ) + + return LaunchDescription( + declared_arguments + [debayer_image_group, object_detection_group] + ) \ No newline at end of file diff --git a/object_detection/models/.gitkeep b/object_detection/models/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/requirements.txt b/requirements.txt index dc8d61e..7b235f1 100644 --- a/requirements.txt +++ b/requirements.txt @@ -4,28 +4,24 @@ # pip install -r requirements.txt # Base ---------------------------------------- -matplotlib>=3.8.0 -numpy==1.26.0 -opencv-python>=4.8.0 -Pillow>=10.0.0 -PyYAML>=6.0.1 -requests>=2.31.0 -scipy>=1.11.0 -torch>=2.1.0 -torchvision>=0.16.0 -tqdm>=4.65.0 +matplotlib==3.7.5 +numpy==1.23.5 +opencv-python==4.10.0.84 +pillow==10.4.0 +PyYAML==5.3.1 +requests==2.32.4 +scipy==1.10.1 +tqdm==4.67.1 # detection -yolov5 +yolov5==7.0.14 # Logging ------------------------------------- -tensorboard>=2.15.0 +tensorboard==2.14.0 # Plotting ------------------------------------ -pandas>=2.1.0 -seaborn>=0.13.0 +pandas==2.0.3 +seaborn==0.13.2 -hdbscan>=0.8.33 - -thop # FLOPs computation +hdbscan==0.8.40