diff --git a/isaac_ros_examples/isaac_ros_examples/isaac_ros_launch_fragment_spec.py b/isaac_ros_examples/isaac_ros_examples/isaac_ros_launch_fragment_spec.py index a3f6d21..0073e8e 100644 --- a/isaac_ros_examples/isaac_ros_examples/isaac_ros_launch_fragment_spec.py +++ b/isaac_ros_examples/isaac_ros_examples/isaac_ros_launch_fragment_spec.py @@ -63,6 +63,20 @@ def __init__(self, package, class_name, filename=''): 'usb_cam': IsaacROSLaunchFragmentSpec( 'isaac_ros_usb_cam', 'IsaacROSUSBCameraLaunchFragment'), + # ZED Camera + 'zed_mono_rect': IsaacROSLaunchFragmentSpec( + 'isaac_ros_zed', 'IsaacROSZedMonoRectLaunchFragment', + 'isaac_ros_zed_mono_rect_core.launch.py'), + 'zed_stereo_rect': IsaacROSLaunchFragmentSpec( + 'isaac_ros_zed', 'IsaacROSZedStereoRectLaunchFragment', + 'isaac_ros_zed_stereo_rect_core.launch.py'), + 'zed_mono': IsaacROSLaunchFragmentSpec( + 'isaac_ros_zed', 'IsaacROSZedMonoLaunchFragment', + 'isaac_ros_zed_mono_core.launch.py'), + 'zed_mono_rect_depth': IsaacROSLaunchFragmentSpec( + 'isaac_ros_zed', 'IsaacROSZedMonoRectDepthLaunchFragment', + 'isaac_ros_zed_mono_rect_depth_core.launch.py'), + ########################### # Preprocessing Utilities # ########################### diff --git a/isaac_ros_examples/package.xml b/isaac_ros_examples/package.xml index 2d8e4be..fa0eada 100644 --- a/isaac_ros_examples/package.xml +++ b/isaac_ros_examples/package.xml @@ -2,7 +2,7 @@ isaac_ros_examples - 3.0.1 + 3.1.0 Utilities for launching example graphs of Isaac ROS packages Isaac ROS Maintainers diff --git a/isaac_ros_multicamera_vo/package.xml b/isaac_ros_multicamera_vo/package.xml index 2ecf6f6..ee12958 100644 --- a/isaac_ros_multicamera_vo/package.xml +++ b/isaac_ros_multicamera_vo/package.xml @@ -2,7 +2,7 @@ isaac_ros_multicamera_vo - 3.0.1 + 3.1.0 Isaac ROS Launch Fragment for cuVSLAM Visual Odomtry for multicamera cases Isaac ROS Maintainers diff --git a/isaac_ros_realsense/package.xml b/isaac_ros_realsense/package.xml index 0bcccbf..5baedc5 100644 --- a/isaac_ros_realsense/package.xml +++ b/isaac_ros_realsense/package.xml @@ -2,7 +2,7 @@ isaac_ros_realsense - 3.0.1 + 3.1.0 Isaac ROS Launch Fragment for Intel RealSense cameras Isaac ROS Maintainers diff --git a/isaac_ros_usb_cam/package.xml b/isaac_ros_usb_cam/package.xml index 4f878f9..7f367d4 100644 --- a/isaac_ros_usb_cam/package.xml +++ b/isaac_ros_usb_cam/package.xml @@ -2,7 +2,7 @@ isaac_ros_usb_cam - 3.0.1 + 3.1.0 Isaac ROS Launch Fragment for USB cameras Isaac ROS Maintainers diff --git a/isaac_ros_zed/config/zed.yaml b/isaac_ros_zed/config/zed.yaml new file mode 100644 index 0000000..d06a05f --- /dev/null +++ b/isaac_ros_zed/config/zed.yaml @@ -0,0 +1,163 @@ +# config/common_yaml +# Common parameters to Stereolabs ZED and ZED mini cameras +# +# Note: the parameter svo_file is passed as exe argumet +--- +/**: + ros__parameters: + + general: + svo_file: "" # usually overwritten by launch file + svo_loop: false # Enable loop mode when using an SVO as input source + svo_realtime: true # if true the SVO will be played trying to respect the original framerate eventually skipping frames, otherwise every frame will be processed respecting the `pub_frame_rate` setting + camera_timeout_sec: 5 + camera_max_reconnect: 5 + camera_flip: false + zed_id: 0 # IMPORTANT: to be used in simulation to distinguish individual cameras im multi-camera configurations - usually overwritten by launch file + serial_number: 0 # usually overwritten by launch file + pub_resolution: 'MEDIUM' # The resolution used for output. 'HD2K', 'HD1080', 'HD1200', 'HD720', 'MEDIUM', 'SVGA', 'VGA', 'LOW' + pub_frame_rate: 15.0 # [DYNAMIC] - frequency of publishing of visual images and depth images + gpu_id: -1 + region_of_interest: '[]' # A polygon defining the ROI where the ZED SDK perform the processing ignoring the rest. Coordinates must be normalized to '1.0' to be resolution independent. + #region_of_interest: '[[0.25,0.33],[0.75,0.33],[0.75,0.5],[0.5,0.75],[0.25,0.5]]' # A polygon defining the ROI where the ZED SDK perform the processing ignoring the rest. Coordinates must be normalized to '1.0' to be resolution independent. + #region_of_interest: '[[0.25,0.25],[0.75,0.25],[0.75,0.75],[0.25,0.75]]' # A polygon defining the ROI where the ZED SDK perform the processing ignoring the rest. Coordinates must be normalized to '1.0' to be resolution independent. + #region_of_interest: '[[0.5,0.25],[0.75,0.5],[0.5,0.75],[0.25,0.5]]' # A polygon defining the ROI where the ZED SDK perform the processing ignoring the rest. Coordinates must be normalized to '1.0' to be resolution independent. + sdk_verbose: 1 + + video: + brightness: 4 # [DYNAMIC] Not available for ZED X/ZED X Mini + contrast: 4 # [DYNAMIC] Not available for ZED X/ZED X Mini + hue: 0 # [DYNAMIC] Not available for ZED X/ZED X Mini + saturation: 4 # [DYNAMIC] + sharpness: 4 # [DYNAMIC] + gamma: 8 # [DYNAMIC] + auto_exposure_gain: true # [DYNAMIC] + exposure: 80 # [DYNAMIC] + gain: 80 # [DYNAMIC] + auto_whitebalance: true # [DYNAMIC] + whitebalance_temperature: 42 # [DYNAMIC] - [28,65] works only if `auto_whitebalance` is false + qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL + qos_depth: 1 # Queue size if using KEEP_LAST + qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT - + qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE + + depth: + depth_mode: 'NONE' # Matches the ZED SDK setting: 'NONE', 'PERFORMANCE', 'QUALITY', 'ULTRA', 'NEURAL' - Note: if 'NONE' all the modules that requires depth extraction are disabled by default (Pos. Tracking, Obj. Detection, Mapping, ...) + depth_stabilization: 1 # Forces positional tracking to start if major than 0 - Range: [0,100] + openni_depth_mode: false # 'false': 32bit float [meters], 'true': 16bit unsigned int [millimeters] + point_cloud_freq: 10.0 # [DYNAMIC] - frequency of the pointcloud publishing (equal or less to `grab_frame_rate` value) + depth_confidence: 50 # [DYNAMIC] + depth_texture_conf: 100 # [DYNAMIC] + remove_saturated_areas: true # [DYNAMIC] + qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL + qos_depth: 1 # Queue size if using KEEP_LAST + qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT - + qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE + + pos_tracking: + pos_tracking_enabled: false # True to enable positional tracking from start + imu_fusion: true # enable/disable IMU fusion. When set to false, only the optical odometry will be used. + publish_tf: true # [usually overwritten by launch file] publish `odom -> base_link` TF + publish_map_tf: true # [usually overwritten by launch file] publish `map -> odom` TF + publish_imu_tf: true # [usually overwritten by launch file] enable/disable the IMU TF broadcasting + base_frame: "base_link" # usually overwritten by launch file + map_frame: "map" + odometry_frame: "odom" + area_memory_db_path: "" + area_memory: true # Enable to detect loop closure + depth_min_range: 0.0 # Set this value for removing fixed zones of the robot in the FoV of the camerafrom the visual odometry evaluation + set_as_static: false # If 'true' the camera will be static and not move in the environment + set_gravity_as_origin: true # If 'true' align the positional tracking world to imu gravity measurement. Keep the yaw from the user initial pose. + floor_alignment: false # Enable to automatically calculate camera/floor offset + initial_base_pose: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Initial position of the `base_frame` in the map -> [X, Y, Z, R, P, Y] + init_odom_with_first_valid_pose: true # Enable to initialize the odometry with the first valid pose + path_pub_rate: 2.0 # [DYNAMIC] - Camera trajectory publishing frequency + path_max_count: -1 # use '-1' for unlimited path size + two_d_mode: false # Force navigation on a plane. If true the Z value will be fixed to "fixed_z_value", roll and pitch to zero + fixed_z_value: 0.00 # Value to be used for Z coordinate if `two_d_mode` is true + transform_time_offset: 0.0 # The value added to the timestamp of `map->odom` and `odom->base_link`` transform being generated + qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL + qos_depth: 1 # Queue size if using KEEP_LAST + qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT + qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE + + gnss_fusion: + gnss_fusion_enabled: false # fuse 'sensor_msg/NavSatFix' message information into pose data + gnss_fix_topic: "/gps/fix" # Name of the GNSS topic of type NavSatFix to subscribe [Default: "/gps/fix"] + gnss_init_distance: 5.0 # The minimum distance traveled by the robot required to initilize the GNSS fusion + gnss_zero_altitude: false # Set to `true` to ignore GNSS altitude information + gnss_frame: "gnss_link" # [usually overwritten by launch file] The TF frame of the GNSS sensor + publish_utm_tf: true # Publish `utm` -> `map` TF + broadcast_utm_transform_as_parent_frame: false # if 'true' publish `utm` -> `map` TF, otherwise `map` -> `utm` + + mapping: + mapping_enabled: false # True to enable mapping and fused point cloud pubblication + resolution: 0.05 # maps resolution in meters [min: 0.01f - max: 0.2f] + max_mapping_range: 5.0 # maximum depth range while mapping in meters (-1 for automatic calculation) [2.0, 20.0] + fused_pointcloud_freq: 1.0 # frequency of the publishing of the fused colored point cloud + clicked_point_topic: "/clicked_point" # Topic published by Rviz when a point of the cloud is clicked. Used for plane detection + qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL + qos_depth: 1 # Queue size if using KEEP_LAST + qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT - + qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE + + sensors: + sensors_image_sync: false # Synchronize Sensors messages with latest published video/depth message + sensors_pub_rate: 200. # frequency of publishing of sensors data. MAX: 400. - MIN: grab rate + qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL + qos_depth: 1 # Queue size if using KEEP_LAST + qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT - + qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE + + object_detection: + od_enabled: false # True to enable Object Detection + model: 'MULTI_CLASS_BOX_MEDIUM' # 'MULTI_CLASS_BOX_FAST', 'MULTI_CLASS_BOX_MEDIUM', 'MULTI_CLASS_BOX_ACCURATE', 'PERSON_HEAD_BOX_FAST', 'PERSON_HEAD_BOX_ACCURATE' + allow_reduced_precision_inference: true # Allow inference to run at a lower precision to improve runtime and memory usage + max_range: 20.0 # [m] Defines a upper depth range for detections + confidence_threshold: 50.0 # [DYNAMIC] - Minimum value of the detection confidence of an object [0,99] + prediction_timeout: 0.5 # During this time [sec], the object will have OK state even if it is not detected. Set this parameter to 0 to disable SDK predictions + filtering_mode: 1 # '0': NONE - '1': NMS3D - '2': NMS3D_PER_CLASS + mc_people: false # [DYNAMIC] - Enable/disable the detection of persons for 'MULTI_CLASS_X' models + mc_vehicle: true # [DYNAMIC] - Enable/disable the detection of vehicles for 'MULTI_CLASS_X' models + mc_bag: true # [DYNAMIC] - Enable/disable the detection of bags for 'MULTI_CLASS_X' models + mc_animal: true # [DYNAMIC] - Enable/disable the detection of animals for 'MULTI_CLASS_X' models + mc_electronics: true # [DYNAMIC] - Enable/disable the detection of electronic devices for 'MULTI_CLASS_X' models + mc_fruit_vegetable: true # [DYNAMIC] - Enable/disable the detection of fruits and vegetables for 'MULTI_CLASS_X' models + mc_sport: true # [DYNAMIC] - Enable/disable the detection of sport-related objects for 'MULTI_CLASS_X' models + qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL + qos_depth: 1 # Queue size if using KEEP_LAST + qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT + qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE + + body_tracking: + bt_enabled: false # True to enable Body Tracking + model: 'HUMAN_BODY_MEDIUM' # 'HUMAN_BODY_FAST', 'HUMAN_BODY_MEDIUM', 'HUMAN_BODY_ACCURATE' + body_format: 'BODY_38' # 'BODY_18','BODY_34','BODY_38','BODY_70' + allow_reduced_precision_inference: false # Allow inference to run at a lower precision to improve runtime and memory usage + max_range: 20.0 # [m] Defines a upper depth range for detections + body_kp_selection: 'FULL' # 'FULL', 'UPPER_BODY' + enable_body_fitting: false # Defines if the body fitting will be applied + enable_tracking: true # Defines if the object detection will track objects across images flow + prediction_timeout_s: 0.5 # During this time [sec], the skeleton will have OK state even if it is not detected. Set this parameter to 0 to disable SDK predictions + confidence_threshold: 50.0 # [DYNAMIC] - Minimum value of the detection confidence of skeleton key points [0,99] + minimum_keypoints_threshold: 5 # [DYNAMIC] - Minimum number of skeleton key points to be detected for a valid skeleton + qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL + qos_depth: 1 # Queue size if using KEEP_LAST + qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT + qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE + + use_sim_time: false # EXPERIMENTAL (only for development) - Set to true to enable SIMULATION mode # + sim_address: '192.168.1.90' # EXPERIMENTAL (only for development) - The local address of the machine running the simulator + + debug: + debug_common: false + debug_video_depth: false + debug_camera_controls: false + debug_point_cloud: false + debug_positional_tracking: false + debug_gnss: false + debug_sensors: false + debug_mapping : false + debug_terrain_mapping : false + debug_object_detection : false + debug_body_tracking : false \ No newline at end of file diff --git a/isaac_ros_zed/config/zed_mono.yaml b/isaac_ros_zed/config/zed_mono.yaml new file mode 100644 index 0000000..d06a05f --- /dev/null +++ b/isaac_ros_zed/config/zed_mono.yaml @@ -0,0 +1,163 @@ +# config/common_yaml +# Common parameters to Stereolabs ZED and ZED mini cameras +# +# Note: the parameter svo_file is passed as exe argumet +--- +/**: + ros__parameters: + + general: + svo_file: "" # usually overwritten by launch file + svo_loop: false # Enable loop mode when using an SVO as input source + svo_realtime: true # if true the SVO will be played trying to respect the original framerate eventually skipping frames, otherwise every frame will be processed respecting the `pub_frame_rate` setting + camera_timeout_sec: 5 + camera_max_reconnect: 5 + camera_flip: false + zed_id: 0 # IMPORTANT: to be used in simulation to distinguish individual cameras im multi-camera configurations - usually overwritten by launch file + serial_number: 0 # usually overwritten by launch file + pub_resolution: 'MEDIUM' # The resolution used for output. 'HD2K', 'HD1080', 'HD1200', 'HD720', 'MEDIUM', 'SVGA', 'VGA', 'LOW' + pub_frame_rate: 15.0 # [DYNAMIC] - frequency of publishing of visual images and depth images + gpu_id: -1 + region_of_interest: '[]' # A polygon defining the ROI where the ZED SDK perform the processing ignoring the rest. Coordinates must be normalized to '1.0' to be resolution independent. + #region_of_interest: '[[0.25,0.33],[0.75,0.33],[0.75,0.5],[0.5,0.75],[0.25,0.5]]' # A polygon defining the ROI where the ZED SDK perform the processing ignoring the rest. Coordinates must be normalized to '1.0' to be resolution independent. + #region_of_interest: '[[0.25,0.25],[0.75,0.25],[0.75,0.75],[0.25,0.75]]' # A polygon defining the ROI where the ZED SDK perform the processing ignoring the rest. Coordinates must be normalized to '1.0' to be resolution independent. + #region_of_interest: '[[0.5,0.25],[0.75,0.5],[0.5,0.75],[0.25,0.5]]' # A polygon defining the ROI where the ZED SDK perform the processing ignoring the rest. Coordinates must be normalized to '1.0' to be resolution independent. + sdk_verbose: 1 + + video: + brightness: 4 # [DYNAMIC] Not available for ZED X/ZED X Mini + contrast: 4 # [DYNAMIC] Not available for ZED X/ZED X Mini + hue: 0 # [DYNAMIC] Not available for ZED X/ZED X Mini + saturation: 4 # [DYNAMIC] + sharpness: 4 # [DYNAMIC] + gamma: 8 # [DYNAMIC] + auto_exposure_gain: true # [DYNAMIC] + exposure: 80 # [DYNAMIC] + gain: 80 # [DYNAMIC] + auto_whitebalance: true # [DYNAMIC] + whitebalance_temperature: 42 # [DYNAMIC] - [28,65] works only if `auto_whitebalance` is false + qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL + qos_depth: 1 # Queue size if using KEEP_LAST + qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT - + qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE + + depth: + depth_mode: 'NONE' # Matches the ZED SDK setting: 'NONE', 'PERFORMANCE', 'QUALITY', 'ULTRA', 'NEURAL' - Note: if 'NONE' all the modules that requires depth extraction are disabled by default (Pos. Tracking, Obj. Detection, Mapping, ...) + depth_stabilization: 1 # Forces positional tracking to start if major than 0 - Range: [0,100] + openni_depth_mode: false # 'false': 32bit float [meters], 'true': 16bit unsigned int [millimeters] + point_cloud_freq: 10.0 # [DYNAMIC] - frequency of the pointcloud publishing (equal or less to `grab_frame_rate` value) + depth_confidence: 50 # [DYNAMIC] + depth_texture_conf: 100 # [DYNAMIC] + remove_saturated_areas: true # [DYNAMIC] + qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL + qos_depth: 1 # Queue size if using KEEP_LAST + qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT - + qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE + + pos_tracking: + pos_tracking_enabled: false # True to enable positional tracking from start + imu_fusion: true # enable/disable IMU fusion. When set to false, only the optical odometry will be used. + publish_tf: true # [usually overwritten by launch file] publish `odom -> base_link` TF + publish_map_tf: true # [usually overwritten by launch file] publish `map -> odom` TF + publish_imu_tf: true # [usually overwritten by launch file] enable/disable the IMU TF broadcasting + base_frame: "base_link" # usually overwritten by launch file + map_frame: "map" + odometry_frame: "odom" + area_memory_db_path: "" + area_memory: true # Enable to detect loop closure + depth_min_range: 0.0 # Set this value for removing fixed zones of the robot in the FoV of the camerafrom the visual odometry evaluation + set_as_static: false # If 'true' the camera will be static and not move in the environment + set_gravity_as_origin: true # If 'true' align the positional tracking world to imu gravity measurement. Keep the yaw from the user initial pose. + floor_alignment: false # Enable to automatically calculate camera/floor offset + initial_base_pose: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Initial position of the `base_frame` in the map -> [X, Y, Z, R, P, Y] + init_odom_with_first_valid_pose: true # Enable to initialize the odometry with the first valid pose + path_pub_rate: 2.0 # [DYNAMIC] - Camera trajectory publishing frequency + path_max_count: -1 # use '-1' for unlimited path size + two_d_mode: false # Force navigation on a plane. If true the Z value will be fixed to "fixed_z_value", roll and pitch to zero + fixed_z_value: 0.00 # Value to be used for Z coordinate if `two_d_mode` is true + transform_time_offset: 0.0 # The value added to the timestamp of `map->odom` and `odom->base_link`` transform being generated + qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL + qos_depth: 1 # Queue size if using KEEP_LAST + qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT + qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE + + gnss_fusion: + gnss_fusion_enabled: false # fuse 'sensor_msg/NavSatFix' message information into pose data + gnss_fix_topic: "/gps/fix" # Name of the GNSS topic of type NavSatFix to subscribe [Default: "/gps/fix"] + gnss_init_distance: 5.0 # The minimum distance traveled by the robot required to initilize the GNSS fusion + gnss_zero_altitude: false # Set to `true` to ignore GNSS altitude information + gnss_frame: "gnss_link" # [usually overwritten by launch file] The TF frame of the GNSS sensor + publish_utm_tf: true # Publish `utm` -> `map` TF + broadcast_utm_transform_as_parent_frame: false # if 'true' publish `utm` -> `map` TF, otherwise `map` -> `utm` + + mapping: + mapping_enabled: false # True to enable mapping and fused point cloud pubblication + resolution: 0.05 # maps resolution in meters [min: 0.01f - max: 0.2f] + max_mapping_range: 5.0 # maximum depth range while mapping in meters (-1 for automatic calculation) [2.0, 20.0] + fused_pointcloud_freq: 1.0 # frequency of the publishing of the fused colored point cloud + clicked_point_topic: "/clicked_point" # Topic published by Rviz when a point of the cloud is clicked. Used for plane detection + qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL + qos_depth: 1 # Queue size if using KEEP_LAST + qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT - + qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE + + sensors: + sensors_image_sync: false # Synchronize Sensors messages with latest published video/depth message + sensors_pub_rate: 200. # frequency of publishing of sensors data. MAX: 400. - MIN: grab rate + qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL + qos_depth: 1 # Queue size if using KEEP_LAST + qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT - + qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE + + object_detection: + od_enabled: false # True to enable Object Detection + model: 'MULTI_CLASS_BOX_MEDIUM' # 'MULTI_CLASS_BOX_FAST', 'MULTI_CLASS_BOX_MEDIUM', 'MULTI_CLASS_BOX_ACCURATE', 'PERSON_HEAD_BOX_FAST', 'PERSON_HEAD_BOX_ACCURATE' + allow_reduced_precision_inference: true # Allow inference to run at a lower precision to improve runtime and memory usage + max_range: 20.0 # [m] Defines a upper depth range for detections + confidence_threshold: 50.0 # [DYNAMIC] - Minimum value of the detection confidence of an object [0,99] + prediction_timeout: 0.5 # During this time [sec], the object will have OK state even if it is not detected. Set this parameter to 0 to disable SDK predictions + filtering_mode: 1 # '0': NONE - '1': NMS3D - '2': NMS3D_PER_CLASS + mc_people: false # [DYNAMIC] - Enable/disable the detection of persons for 'MULTI_CLASS_X' models + mc_vehicle: true # [DYNAMIC] - Enable/disable the detection of vehicles for 'MULTI_CLASS_X' models + mc_bag: true # [DYNAMIC] - Enable/disable the detection of bags for 'MULTI_CLASS_X' models + mc_animal: true # [DYNAMIC] - Enable/disable the detection of animals for 'MULTI_CLASS_X' models + mc_electronics: true # [DYNAMIC] - Enable/disable the detection of electronic devices for 'MULTI_CLASS_X' models + mc_fruit_vegetable: true # [DYNAMIC] - Enable/disable the detection of fruits and vegetables for 'MULTI_CLASS_X' models + mc_sport: true # [DYNAMIC] - Enable/disable the detection of sport-related objects for 'MULTI_CLASS_X' models + qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL + qos_depth: 1 # Queue size if using KEEP_LAST + qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT + qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE + + body_tracking: + bt_enabled: false # True to enable Body Tracking + model: 'HUMAN_BODY_MEDIUM' # 'HUMAN_BODY_FAST', 'HUMAN_BODY_MEDIUM', 'HUMAN_BODY_ACCURATE' + body_format: 'BODY_38' # 'BODY_18','BODY_34','BODY_38','BODY_70' + allow_reduced_precision_inference: false # Allow inference to run at a lower precision to improve runtime and memory usage + max_range: 20.0 # [m] Defines a upper depth range for detections + body_kp_selection: 'FULL' # 'FULL', 'UPPER_BODY' + enable_body_fitting: false # Defines if the body fitting will be applied + enable_tracking: true # Defines if the object detection will track objects across images flow + prediction_timeout_s: 0.5 # During this time [sec], the skeleton will have OK state even if it is not detected. Set this parameter to 0 to disable SDK predictions + confidence_threshold: 50.0 # [DYNAMIC] - Minimum value of the detection confidence of skeleton key points [0,99] + minimum_keypoints_threshold: 5 # [DYNAMIC] - Minimum number of skeleton key points to be detected for a valid skeleton + qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL + qos_depth: 1 # Queue size if using KEEP_LAST + qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT + qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE + + use_sim_time: false # EXPERIMENTAL (only for development) - Set to true to enable SIMULATION mode # + sim_address: '192.168.1.90' # EXPERIMENTAL (only for development) - The local address of the machine running the simulator + + debug: + debug_common: false + debug_video_depth: false + debug_camera_controls: false + debug_point_cloud: false + debug_positional_tracking: false + debug_gnss: false + debug_sensors: false + debug_mapping : false + debug_terrain_mapping : false + debug_object_detection : false + debug_body_tracking : false \ No newline at end of file diff --git a/isaac_ros_zed/config/zed_mono_depth.yaml b/isaac_ros_zed/config/zed_mono_depth.yaml new file mode 100644 index 0000000..b429cf7 --- /dev/null +++ b/isaac_ros_zed/config/zed_mono_depth.yaml @@ -0,0 +1,185 @@ +# NOTE(remos): This file was copied from stereolabs/zed-ros2-wrapper/zed_wrapper/config/common.yaml +# and modified according to our needs. + +# config/common_yaml +# Common parameters to Stereolabs ZED and ZED mini cameras +# +# Note: the parameter svo_file is passed as exe argumet +--- +/**: + ros__parameters: + + general: + svo_file: "live" # usually overwritten by launch file + svo_loop: false # Enable loop mode when using an SVO as input source + svo_realtime: true # if true the SVO will be played trying to respect the original framerate eventually skipping frames, otherwise every frame will be processed respecting the `pub_frame_rate` setting + camera_timeout_sec: 5 + camera_max_reconnect: 5 + camera_flip: false + zed_id: 0 # IMPORTANT: to be used in simulation to distinguish individual cameras im multi-camera configurations - usually overwritten by launch file + serial_number: 0 # usually overwritten by launch file + pub_resolution: 'CUSTOM' # The resolution used for output. 'NATIVE' to use the same `general.grab_resolution` - `CUSTOM` to apply the `general.pub_downscale_factor` downscale factory to reduce bandwidth in transmission + pub_downscale_factor: 2.0 # rescale factor used to rescale image before publishing when 'pub_resolution' is 'CUSTOM' + pub_frame_rate: 30.0 # frequency of publishing of visual images and depth images + gpu_id: -1 + region_of_interest: '[]' # A polygon defining the ROI where the ZED SDK perform the processing ignoring the rest. Coordinates must be normalized to '1.0' to be resolution independent. + #region_of_interest: '[[0.25,0.33],[0.75,0.33],[0.75,0.5],[0.5,0.75],[0.25,0.5]]' # A polygon defining the ROI where the ZED SDK perform the processing ignoring the rest. Coordinates must be normalized to '1.0' to be resolution independent. + #region_of_interest: '[[0.25,0.25],[0.75,0.25],[0.75,0.75],[0.25,0.75]]' # A polygon defining the ROI where the ZED SDK perform the processing ignoring the rest. Coordinates must be normalized to '1.0' to be resolution independent. + #region_of_interest: '[[0.5,0.25],[0.75,0.5],[0.5,0.75],[0.25,0.5]]' # A polygon defining the ROI where the ZED SDK perform the processing ignoring the rest. Coordinates must be normalized to '1.0' to be resolution independent. + sdk_verbose: 1 + + video: + brightness: 4 # [DYNAMIC] Not available for ZED X/ZED X Mini + contrast: 4 # [DYNAMIC] Not available for ZED X/ZED X Mini + hue: 0 # [DYNAMIC] Not available for ZED X/ZED X Mini + saturation: 4 # [DYNAMIC] + sharpness: 4 # [DYNAMIC] + gamma: 8 # [DYNAMIC] + auto_exposure_gain: true # [DYNAMIC] + exposure: 80 # [DYNAMIC] + gain: 80 # [DYNAMIC] + auto_whitebalance: true # [DYNAMIC] + whitebalance_temperature: 42 # [DYNAMIC] - [28,65] works only if `auto_whitebalance` is false + qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL + qos_depth: 1 # Queue size if using KEEP_LAST + qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT - + qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE + + depth: + depth_mode: 'NEURAL' # Matches the ZED SDK setting: 'NONE', 'PERFORMANCE', 'QUALITY', 'ULTRA', 'NEURAL' - Note: if 'NONE' all the modules that requires depth extraction are disabled by default (Pos. Tracking, Obj. Detection, Mapping, ...) + depth_stabilization: 1 # Forces positional tracking to start if major than 0 - Range: [0,100] + openni_depth_mode: false # 'false': 32bit float [meters], 'true': 16bit unsigned int [millimeters] + point_cloud_freq: 10.0 # [DYNAMIC] - frequency of the pointcloud publishing (equal or less to `grab_frame_rate` value) + depth_confidence: 10 # [DYNAMIC] + depth_texture_conf: 100 # [DYNAMIC] + remove_saturated_areas: true # [DYNAMIC] + qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL + qos_depth: 1 # Queue size if using KEEP_LAST + qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT - + qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE + + pos_tracking: + pos_tracking_enabled: true # True to enable positional tracking from start + pos_tracking_mode: 'STANDARD' # Matches the ZED SDK setting: 'QUALITY', 'STANDARD' + imu_fusion: true # enable/disable IMU fusion. When set to false, only the optical odometry will be used. + publish_tf: true # [usually overwritten by launch file] publish `odom -> base_link` TF + publish_map_tf: true # [usually overwritten by launch file] publish `map -> odom` TF + base_frame: "base_link" # usually overwritten by launch file + map_frame: "map" + odometry_frame: "odom" + area_memory_db_path: "" + area_memory: true # Enable to detect loop closure + depth_min_range: 0.0 # Set this value for removing fixed zones of the robot in the FoV of the camerafrom the visual odometry evaluation + set_as_static: false # If 'true' the camera will be static and not move in the environment + set_gravity_as_origin: true # If 'true' align the positional tracking world to imu gravity measurement. Keep the yaw from the user initial pose. + floor_alignment: false # Enable to automatically calculate camera/floor offset + initial_base_pose: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Initial position of the `base_frame` in the map -> [X, Y, Z, R, P, Y] + init_odom_with_first_valid_pose: true # Enable to initialize the odometry with the first valid pose + path_pub_rate: 2.0 # [DYNAMIC] - Camera trajectory publishing frequency + path_max_count: -1 # use '-1' for unlimited path size + two_d_mode: false # Force navigation on a plane. If true the Z value will be fixed to "fixed_z_value", roll and pitch to zero + fixed_z_value: 0.00 # Value to be used for Z coordinate if `two_d_mode` is true + transform_time_offset: 0.0 # The value added to the timestamp of `map->odom` and `odom->base_link`` transform being generated + qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL + qos_depth: 1 # Queue size if using KEEP_LAST + qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT + qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE + + gnss_fusion: + gnss_fusion_enabled: false # fuse 'sensor_msg/NavSatFix' message information into pose data + gnss_fix_topic: "/gps/fix" # Name of the GNSS topic of type NavSatFix to subscribe [Default: "/gps/fix"] + gnss_init_distance: 5.0 # The minimum distance traveled by the robot required to initilize the GNSS fusion [SDK <= v4.0.5] + gnss_zero_altitude: false # Set to `true` to ignore GNSS altitude information + gnss_frame: "gnss_link" # [usually overwritten by launch file] The TF frame of the GNSS sensor + publish_utm_tf: true # Publish `utm` -> `map` TF + broadcast_utm_transform_as_parent_frame: false # if 'true' publish `utm` -> `map` TF, otherwise `map` -> `utm` + enable_reinitialization: true # determines whether reinitialization should be performed between GNSS and VIO fusion when a significant disparity is detected between GNSS data and the current fusion data. It becomes particularly crucial during prolonged GNSS signal loss scenarios. + enable_rolling_calibration: true # If this parameter is set to true, the fusion algorithm will used a rough VIO / GNSS calibration at first and then refine it. This allow you to quickly get a fused position. + enable_translation_uncertainty_target: false # When this parameter is enabled (set to true), the calibration process between GNSS and VIO accounts for the uncertainty in the determined translation, thereby facilitating the calibration termination. The maximum allowable uncertainty is controlled by the 'target_translation_uncertainty' parameter. + gnss_vio_reinit_threshold: 5 # determines the threshold for GNSS/VIO reinitialization. If the fused position deviates beyond out of the region defined by the product of the GNSS covariance and the gnss_vio_reinit_threshold, a reinitialization will be triggered. + target_translation_uncertainty: 10e-2 # defines the target translation uncertainty at which the calibration process between GNSS and VIO concludes. By default, the threshold is set at 10 centimeters. + target_yaw_uncertainty: 0.1 # defines the target yaw uncertainty at which the calibration process between GNSS and VIO concludes. The unit of this parameter is in radian. By default, the threshold is set at 0.1 radians. + + mapping: + mapping_enabled: false # True to enable mapping and fused point cloud pubblication + resolution: 0.05 # maps resolution in meters [min: 0.01f - max: 0.2f] + max_mapping_range: 5.0 # maximum depth range while mapping in meters (-1 for automatic calculation) [2.0, 20.0] + fused_pointcloud_freq: 1.0 # frequency of the publishing of the fused colored point cloud + clicked_point_topic: "/clicked_point" # Topic published by Rviz when a point of the cloud is clicked. Used for plane detection + pd_max_distance_threshold: 0.15 # Plane detection: controls the spread of plane by checking the position difference. + pd_normal_similarity_threshold: 15.0 # Plane detection: controls the spread of plane by checking the angle difference. + qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL + qos_depth: 1 # Queue size if using KEEP_LAST + qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT - + qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE + + sensors: + publish_imu_tf: true # [usually overwritten by launch file] enable/disable the IMU TF broadcasting + sensors_image_sync: false # Synchronize Sensors messages with latest published video/depth message + sensors_pub_rate: 200. # frequency of publishing of sensors data. MAX: 400. - MIN: grab rate + qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL + qos_depth: 1 # Queue size if using KEEP_LAST + qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT - + qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE + + object_detection: + od_enabled: false # True to enable Object Detection + model: 'MULTI_CLASS_BOX_MEDIUM' # 'MULTI_CLASS_BOX_FAST', 'MULTI_CLASS_BOX_MEDIUM', 'MULTI_CLASS_BOX_ACCURATE', 'PERSON_HEAD_BOX_FAST', 'PERSON_HEAD_BOX_ACCURATE' + allow_reduced_precision_inference: true # Allow inference to run at a lower precision to improve runtime and memory usage + max_range: 20.0 # [m] Defines a upper depth range for detections + confidence_threshold: 50.0 # [DYNAMIC] - Minimum value of the detection confidence of an object [0,99] + prediction_timeout: 0.5 # During this time [sec], the object will have OK state even if it is not detected. Set this parameter to 0 to disable SDK predictions + filtering_mode: 1 # '0': NONE - '1': NMS3D - '2': NMS3D_PER_CLASS + mc_people: true # [DYNAMIC] - Enable/disable the detection of persons for 'MULTI_CLASS_X' models + mc_vehicle: true # [DYNAMIC] - Enable/disable the detection of vehicles for 'MULTI_CLASS_X' models + mc_bag: true # [DYNAMIC] - Enable/disable the detection of bags for 'MULTI_CLASS_X' models + mc_animal: true # [DYNAMIC] - Enable/disable the detection of animals for 'MULTI_CLASS_X' models + mc_electronics: true # [DYNAMIC] - Enable/disable the detection of electronic devices for 'MULTI_CLASS_X' models + mc_fruit_vegetable: true # [DYNAMIC] - Enable/disable the detection of fruits and vegetables for 'MULTI_CLASS_X' models + mc_sport: true # [DYNAMIC] - Enable/disable the detection of sport-related objects for 'MULTI_CLASS_X' models + qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL + qos_depth: 1 # Queue size if using KEEP_LAST + qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT + qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE + + body_tracking: + bt_enabled: false # True to enable Body Tracking + model: 'HUMAN_BODY_MEDIUM' # 'HUMAN_BODY_FAST', 'HUMAN_BODY_MEDIUM', 'HUMAN_BODY_ACCURATE' + body_format: 'BODY_38' # 'BODY_18','BODY_34','BODY_38','BODY_70' + allow_reduced_precision_inference: false # Allow inference to run at a lower precision to improve runtime and memory usage + max_range: 20.0 # [m] Defines a upper depth range for detections + body_kp_selection: 'FULL' # 'FULL', 'UPPER_BODY' + enable_body_fitting: false # Defines if the body fitting will be applied + enable_tracking: true # Defines if the object detection will track objects across images flow + prediction_timeout_s: 0.5 # During this time [sec], the skeleton will have OK state even if it is not detected. Set this parameter to 0 to disable SDK predictions + confidence_threshold: 50.0 # [DYNAMIC] - Minimum value of the detection confidence of skeleton key points [0,99] + minimum_keypoints_threshold: 5 # [DYNAMIC] - Minimum number of skeleton key points to be detected for a valid skeleton + qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL + qos_depth: 1 # Queue size if using KEEP_LAST + qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT + qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE + + use_sim_time: false # EXPERIMENTAL (only for development) - Set to true to enable SIMULATION mode # + sim_address: '192.168.1.90' # EXPERIMENTAL (only for development) - The local address of the machine running the simulator + + advanced: # WARNING: do not modify unless you are confident of what you are doing + # Reference documentation: https://man7.org/linux/man-pages/man7/sched.7.html + thread_sched_policy: 'SCHED_BATCH' # 'SCHED_OTHER', 'SCHED_BATCH', 'SCHED_FIFO', 'SCHED_RR' - NOTE: 'SCHED_FIFO' and 'SCHED_RR' require 'sudo' + thread_grab_priority: 50 # ONLY with 'SCHED_FIFO' and 'SCHED_RR' - [1 (LOW) z-> 99 (HIGH)] - NOTE: 'sudo' required + thread_sensor_priority: 70 # ONLY with 'SCHED_FIFO' and 'SCHED_RR' - [1 (LOW) z-> 99 (HIGH)] - NOTE: 'sudo' required + thread_pointcloud_priority: 60 # ONLY with 'SCHED_FIFO' and 'SCHED_RR' - [1 (LOW) z-> 99 (HIGH)] - NOTE: 'sudo' required + + + debug: + debug_common: false + debug_video_depth: false + debug_camera_controls: false + debug_point_cloud: false + debug_positional_tracking: false + debug_gnss: false + debug_sensors: false + debug_mapping : false + debug_terrain_mapping : false + debug_object_detection : false + debug_body_tracking : false + debug_advanced: true diff --git a/isaac_ros_zed/isaac_ros_zed/__init__.py b/isaac_ros_zed/isaac_ros_zed/__init__.py new file mode 100644 index 0000000..34d9bfe --- /dev/null +++ b/isaac_ros_zed/isaac_ros_zed/__init__.py @@ -0,0 +1,16 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 diff --git a/isaac_ros_zed/launch/isaac_ros_zed_mono_core.launch.py b/isaac_ros_zed/launch/isaac_ros_zed_mono_core.launch.py new file mode 100644 index 0000000..ed2531e --- /dev/null +++ b/isaac_ros_zed/launch/isaac_ros_zed_mono_core.launch.py @@ -0,0 +1,98 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +import os +from typing import Any, Dict + +from ament_index_python import get_package_share_directory +from isaac_ros_examples import IsaacROSLaunchFragment +from launch.substitutions import Command +from launch_ros.actions import Node + + +class IsaacROSZedMonoLaunchFragment(IsaacROSLaunchFragment): + + @staticmethod + def get_interface_specs() -> Dict[str, Any]: + return { + 'camera_resolution': {'width': 1200, 'height': 720}, + 'camera_frame': 'zed2_base_link', + 'camera_model': 'zed2', + 'focal_length': { + # Approximation - most zed cameras should be close to this value + 'f_x': 380.0, + # Approximation - most zed cameras should be close to this value + 'f_y': 380.0 + } + } + + @staticmethod + def get_launch_actions(interface_specs: Dict[str, Any]) -> Dict[str, Any]: + # The zed camera mode name. zed, zed2, zed2i, zedm, zedx or zedxm + camera_model = interface_specs['camera_model'] + + # URDF/xacro file to be loaded by the Robot State Publisher node + xacro_path = os.path.join( + get_package_share_directory('zed_wrapper'), + 'urdf', 'zed_descr.urdf.xacro' + ) + + # ZED Configurations to be loaded by ZED Node + config_common = os.path.join( + get_package_share_directory('isaac_ros_zed'), + 'config', + 'zed_mono.yaml' + ) + + config_camera = os.path.join( + get_package_share_directory('zed_wrapper'), + 'config', + camera_model + '.yaml' + ) + + return { + # Robot State Publisher node + 'rsp_node': Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='zed_state_publisher', + output='screen', + parameters=[{ + 'robot_description': Command( + [ + 'xacro', ' ', xacro_path, ' ', + 'camera_name:=', camera_model, ' ', + 'camera_model:=', camera_model + ]) + }] + ), + # ZED node using manual composition + 'zed_node': Node( + package='zed_wrapper', + executable='zed_wrapper', + output='screen', + parameters=[ + config_common, # Common parameters + config_camera, # Camera related parameters + ], + arguments=[ + '--ros-args', + '--remap', '/zed_node/rgb_raw/image_raw_color:=/image_raw', + '--remap', '/zed_node/rgb_raw/camera_info:=/camera_info' + ] + ) + } diff --git a/isaac_ros_zed/launch/isaac_ros_zed_mono_rect_core.launch.py b/isaac_ros_zed/launch/isaac_ros_zed_mono_rect_core.launch.py new file mode 100644 index 0000000..51d0424 --- /dev/null +++ b/isaac_ros_zed/launch/isaac_ros_zed_mono_rect_core.launch.py @@ -0,0 +1,133 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +import os +from typing import Any, Dict + +from ament_index_python import get_package_share_directory +from isaac_ros_examples import IsaacROSLaunchFragment +import launch +from launch.substitutions import Command +from launch_ros.actions import ComposableNodeContainer, Node +from launch_ros.descriptions import ComposableNode + + +class IsaacROSZedMonoRectLaunchFragment(IsaacROSLaunchFragment): + + @staticmethod + def get_interface_specs() -> Dict[str, Any]: + return { + 'camera_resolution': {'width': 1200, 'height': 720}, + 'camera_frame': 'zed2_base_link', + 'camera_model': 'zed2', + 'focal_length': { + # Approximation - most zed cameras should be close to this value + 'f_x': 380.0, + # Approximation - most zed cameras should be close to this value + 'f_y': 380.0 + } + } + + @staticmethod + def get_composable_nodes(interface_specs: Dict[str, Any]) -> Dict[str, ComposableNode]: + return { + 'image_format_converter_node_left': ComposableNode( + package='isaac_ros_image_proc', + plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode', + name='image_format_node_left', + parameters=[{ + 'encoding_desired': 'rgb8', + 'image_width': interface_specs['camera_resolution']['width'], + 'image_height': interface_specs['camera_resolution']['height'], + }], + remappings=[ + ('image_raw', 'zed_node/left/image_rect_color'), + ('image', 'image_rect')] + ) + } + + @staticmethod + def get_launch_actions(interface_specs: Dict[str, Any]) -> Dict[str, Any]: + # The zed camera mode name. zed, zed2, zed2i, zedm, zedx or zedxm + camera_model = interface_specs['camera_model'] + + # URDF/xacro file to be loaded by the Robot State Publisher node + xacro_path = os.path.join( + get_package_share_directory('zed_wrapper'), + 'urdf', 'zed_descr.urdf.xacro' + ) + + # ZED Configurations to be loaded by ZED Node + config_common = os.path.join( + get_package_share_directory('isaac_ros_zed'), + 'config', + 'zed_mono.yaml' + ) + + config_camera = os.path.join( + get_package_share_directory('zed_wrapper'), + 'config', + camera_model + '.yaml' + ) + + return { + # Robot State Publisher node + 'rsp_node': Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='zed_state_publisher', + output='screen', + parameters=[{ + 'robot_description': Command( + [ + 'xacro', ' ', xacro_path, ' ', + 'camera_name:=', camera_model, ' ', + 'camera_model:=', camera_model + ]) + }] + ), + # ZED node using manual composition + 'zed_node': Node( + package='zed_wrapper', + executable='zed_wrapper', + output='screen', + parameters=[ + config_common, # Common parameters + config_camera, # Camera related parameters + ], + arguments=[ + '--ros-args', + '--remap', 'zed_node/left/camera_info:=/camera_info_rect' + ] + ) + } + + +def generate_launch_description(): + zed_container = ComposableNodeContainer( + package='rclcpp_components', + name='zed_container', + namespace='', + executable='component_container_mt', + composable_node_descriptions=( + IsaacROSZedMonoRectLaunchFragment.get_composable_nodes() + ), + output='screen' + ) + + return launch.LaunchDescription( + [zed_container] + IsaacROSZedMonoRectLaunchFragment.get_launch_actions()) diff --git a/isaac_ros_zed/launch/isaac_ros_zed_mono_rect_depth_core.launch.py b/isaac_ros_zed/launch/isaac_ros_zed_mono_rect_depth_core.launch.py new file mode 100644 index 0000000..f288b37 --- /dev/null +++ b/isaac_ros_zed/launch/isaac_ros_zed_mono_rect_depth_core.launch.py @@ -0,0 +1,135 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +import os +from typing import Any, Dict + +from ament_index_python import get_package_share_directory +from isaac_ros_examples import IsaacROSLaunchFragment +import launch +from launch.substitutions import Command +from launch_ros.actions import ComposableNodeContainer, Node +from launch_ros.descriptions import ComposableNode + + +class IsaacROSZedMonoRectDepthLaunchFragment(IsaacROSLaunchFragment): + + @staticmethod + def get_interface_specs() -> Dict[str, Any]: + return { + 'camera_resolution': {'width': 1200, 'height': 720}, + 'camera_frame': 'zed2_base_link', + 'camera_model': 'zed2', + 'focal_length': { + # Approximation - most zed cameras should be close to this value + 'f_x': 380.0, + # Approximation - most zed cameras should be close to this value + 'f_y': 380.0 + } + } + + @staticmethod + def get_composable_nodes(interface_specs: Dict[str, Any]) -> Dict[str, ComposableNode]: + return { + 'image_format_converter_node_left': ComposableNode( + package='isaac_ros_image_proc', + plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode', + name='image_format_node_left', + parameters=[{ + 'encoding_desired': 'rgb8', + 'image_width': interface_specs['camera_resolution']['width'], + 'image_height': interface_specs['camera_resolution']['height'], + }], + remappings=[ + ('image_raw', 'zed_node/left/image_rect_color'), + ('image', 'image_rect')] + ) + } + + @staticmethod + def get_launch_actions(interface_specs: Dict[str, Any]) -> Dict[str, Any]: + # The zed camera mode name. zed, zed2, zed2i, zedm, zedx or zedxm + camera_model = interface_specs['camera_model'] + + # URDF/xacro file to be loaded by the Robot State Publisher node + xacro_path = os.path.join( + get_package_share_directory('zed_wrapper'), + 'urdf', 'zed_descr.urdf.xacro' + ) + + # ZED Configurations to be loaded by ZED Node + config_common = os.path.join( + get_package_share_directory('isaac_ros_zed'), + 'config', + 'zed_mono_depth.yaml' + ) + + config_camera = os.path.join( + get_package_share_directory('zed_wrapper'), + 'config', + camera_model + '.yaml' + ) + + return { + # Robot State Publisher node + 'rsp_node': Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='zed_state_publisher', + output='screen', + parameters=[{ + 'robot_description': Command( + [ + 'xacro', ' ', xacro_path, ' ', + 'camera_name:=', camera_model, ' ', + 'camera_model:=', camera_model + ]) + }] + ), + # ZED node using manual composition + 'zed_node': Node( + package='zed_wrapper', + executable='zed_wrapper', + output='screen', + parameters=[ + config_common, # Common parameters + config_camera, # Camera related parameters + ], + arguments=[ + '--ros-args', + '--remap', '/zed_node/left/camera_info:=/camera_info_rect', + '--remap', '/zed_node/depth/camera_info:=/camera_info_depth', + '--remap', '/zed_node/depth/depth_registered:=/depth' + ] + ) + } + + +def generate_launch_description(): + zed_container = ComposableNodeContainer( + package='rclcpp_components', + name='zed_container', + namespace='', + executable='component_container_mt', + composable_node_descriptions=( + IsaacROSZedMonoRectDepthLaunchFragment.get_composable_nodes() + ), + output='screen' + ) + + return launch.LaunchDescription( + [zed_container] + IsaacROSZedMonoRectDepthLaunchFragment.get_launch_actions()) diff --git a/isaac_ros_zed/launch/isaac_ros_zed_stereo_rect_core.launch.py b/isaac_ros_zed/launch/isaac_ros_zed_stereo_rect_core.launch.py new file mode 100644 index 0000000..3053437 --- /dev/null +++ b/isaac_ros_zed/launch/isaac_ros_zed_stereo_rect_core.launch.py @@ -0,0 +1,164 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +import os +from typing import Any, Dict + +from ament_index_python import get_package_share_directory +from isaac_ros_examples import IsaacROSLaunchFragment +import launch +from launch.substitutions import Command +from launch_ros.actions import ComposableNodeContainer, Node +from launch_ros.descriptions import ComposableNode + + +class IsaacROSZedStereoRectLaunchFragment(IsaacROSLaunchFragment): + + @staticmethod + def get_interface_specs() -> Dict[str, Any]: + return { + 'camera_resolution': {'width': 1200, 'height': 720}, + 'camera_frame': 'zed2_base_link', + 'camera_model': 'zed2', + 'focal_length': { + # Approximation - most zed cameras should be close to this value + 'f_x': 527.886474609375, + # Approximation - most zed cameras should be close to this value + 'f_y': 527.886474609375 + } + } + + @staticmethod + def get_composable_nodes(interface_specs: Dict[str, Any]) -> Dict[str, ComposableNode]: + # The zed camera mode name. zed, zed2, zed2i, zedm, zedx or zedxm + camera_model = interface_specs['camera_model'] + return { + 'image_format_converter_node_left': ComposableNode( + package='isaac_ros_image_proc', + plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode', + name='image_format_node_left', + parameters=[{ + 'encoding_desired': 'rgb8', + 'image_width': interface_specs['camera_resolution']['width'], + 'image_height': interface_specs['camera_resolution']['height'], + }], + remappings=[ + ('image_raw', 'zed_node/left/image_rect_color'), + ('image', 'left/image_rect')] + ), + 'image_format_converter_node_right': ComposableNode( + package='isaac_ros_image_proc', + plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode', + name='image_format_node_right', + parameters=[{ + 'encoding_desired': 'rgb8', + 'image_width': interface_specs['camera_resolution']['width'], + 'image_height': interface_specs['camera_resolution']['height'], + }], + remappings=[ + ('image_raw', 'zed_node/right/image_rect_color'), + ('image', 'right/image_rect')] + ), + 'tf_publisher': ComposableNode( + name='static_transform_publisher', + package='tf2_ros', + plugin='tf2_ros::StaticTransformBroadcasterNode', + parameters=[{ + 'frame_id': 'base_link', + 'child_frame_id': camera_model+'_camera_link', + 'translation.x': 0.0, + 'translation.y': 0.0, + 'translation.z': 0.1, + 'rotation.x': -0.5, + 'rotation.y': 0.5, + 'rotation.z': -0.5, + 'rotation.w': 0.5 + }]) + } + + @staticmethod + def get_launch_actions(interface_specs: Dict[str, Any]) -> Dict[str, Any]: + # The zed camera mode name. zed, zed2, zed2i, zedm, zedx or zedxm + camera_model = interface_specs['camera_model'] + + # URDF/xacro file to be loaded by the Robot State Publisher node + xacro_path = os.path.join( + get_package_share_directory('zed_wrapper'), + 'urdf', 'zed_descr.urdf.xacro' + ) + + # ZED Configurations to be loaded by ZED Node + config_common = os.path.join( + get_package_share_directory('isaac_ros_zed'), + 'config', + 'zed.yaml' + ) + + config_camera = os.path.join( + get_package_share_directory('zed_wrapper'), + 'config', + camera_model + '.yaml' + ) + + return { + # Robot State Publisher node + 'rsp_node': Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='zed_state_publisher', + output='screen', + parameters=[{ + 'robot_description': Command( + [ + 'xacro', ' ', xacro_path, ' ', + 'camera_name:=', camera_model, ' ', + 'camera_model:=', camera_model + ]) + }] + ), + # ZED node using manual composition + 'zed_node': Node( + package='zed_wrapper', + executable='zed_wrapper', + output='screen', + parameters=[ + config_common, # Common parameters + config_camera, # Camera related parameters + ], + arguments=[ + '--ros-args', + '--remap', 'zed_node/left/camera_info:=/left/camera_info_rect', + '--remap', 'zed_node/right/camera_info:=/right/camera_info_rect', + ] + ) + } + + +def generate_launch_description(): + zed_container = ComposableNodeContainer( + package='rclcpp_components', + name='zed_container', + namespace='', + executable='component_container_mt', + composable_node_descriptions=( + IsaacROSZedStereoRectLaunchFragment.get_composable_nodes() + ), + output='screen' + ) + + return launch.LaunchDescription( + [zed_container] + IsaacROSZedStereoRectLaunchFragment.get_launch_actions()) diff --git a/isaac_ros_zed/package.xml b/isaac_ros_zed/package.xml new file mode 100644 index 0000000..e7187ef --- /dev/null +++ b/isaac_ros_zed/package.xml @@ -0,0 +1,21 @@ + + + + isaac_ros_zed + 3.1.0 + Isaac ROS Launch Fragment for Intel zed cameras + + Isaac ROS Maintainers + Apache-2.0 + https://developer.nvidia.com/isaac-ros/ + Sameer Tangade + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/isaac_ros_zed/resource/isaac_ros_zed b/isaac_ros_zed/resource/isaac_ros_zed new file mode 100644 index 0000000..e69de29 diff --git a/isaac_ros_zed/setup.cfg b/isaac_ros_zed/setup.cfg new file mode 100644 index 0000000..01678b9 --- /dev/null +++ b/isaac_ros_zed/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/isaac_ros_zed +[install] +install_scripts=$base/lib/isaac_ros_zed diff --git a/isaac_ros_zed/setup.py b/isaac_ros_zed/setup.py new file mode 100644 index 0000000..1691738 --- /dev/null +++ b/isaac_ros_zed/setup.py @@ -0,0 +1,48 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 +from glob import glob +import os + +from setuptools import find_packages, setup + +package_name = 'isaac_ros_zed' + +setup( + name=package_name, + version='3.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), + glob(os.path.join('launch', '*launch.[pxy][yma]*'))), + (os.path.join('share', package_name, 'config'), + glob(os.path.join('config', '*'))), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Isaac ROS Maintainers', + maintainer_email='isaac-ros-maintainers@nvidia.com', + description='Isaac ROS Launch Fragment for zed cameras', + license='NVIDIA Isaac ROS Software License', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + ], + }, +) diff --git a/isaac_ros_zed/test/test_copyright.py b/isaac_ros_zed/test/test_copyright.py new file mode 100644 index 0000000..cc8ff03 --- /dev/null +++ b/isaac_ros_zed/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/isaac_ros_zed/test/test_flake8.py b/isaac_ros_zed/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/isaac_ros_zed/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/isaac_ros_zed/test/test_pep257.py b/isaac_ros_zed/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/isaac_ros_zed/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings'