From 1dae867e675789cfb71b2c76471e33f74576e081 Mon Sep 17 00:00:00 2001 From: Nathan Sprague Date: Fri, 24 Jun 2022 14:53:16 -0400 Subject: [PATCH] Attempt to move two-zed configuation into the cart_endpoints package. (untested) --- .../launch/include/zed_camera_mod.launch | 95 ++++++++++++ cart_endpoints/launch/jacart_multi_cam.launch | 135 ++++++++++++++++++ cart_endpoints/zed_params/common_front.yaml | 69 +++++++++ .../zed_params/common_passenger.yaml | 69 +++++++++ cart_endpoints/zed_params/zed2i_front.yaml | 34 +++++ .../zed_params/zed2i_passenger.yaml | 34 +++++ run.sh | 2 +- 7 files changed, 437 insertions(+), 1 deletion(-) create mode 100644 cart_endpoints/launch/include/zed_camera_mod.launch create mode 100644 cart_endpoints/launch/jacart_multi_cam.launch create mode 100644 cart_endpoints/zed_params/common_front.yaml create mode 100644 cart_endpoints/zed_params/common_passenger.yaml create mode 100644 cart_endpoints/zed_params/zed2i_front.yaml create mode 100644 cart_endpoints/zed_params/zed2i_passenger.yaml diff --git a/cart_endpoints/launch/include/zed_camera_mod.launch b/cart_endpoints/launch/include/zed_camera_mod.launch new file mode 100644 index 0000000..b3c3ca7 --- /dev/null +++ b/cart_endpoints/launch/include/zed_camera_mod.launch @@ -0,0 +1,95 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/cart_endpoints/launch/jacart_multi_cam.launch b/cart_endpoints/launch/jacart_multi_cam.launch new file mode 100644 index 0000000..a9c99fa --- /dev/null +++ b/cart_endpoints/launch/jacart_multi_cam.launch @@ -0,0 +1,135 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + target_frame: velodyne + + min_height: -0.6 + max_height: 1.0 + range_min: 0.01 + concurrency_level: 1 + angle_increment: 0.02616 + + + + + + + + + + filter_field_name: x + filter_limit_min: 0.01 + filter_limit_max: 8 + filter_limit_negative: False + leaf_size: 0.1 + + + + diff --git a/cart_endpoints/zed_params/common_front.yaml b/cart_endpoints/zed_params/common_front.yaml new file mode 100644 index 0000000..9a87fa4 --- /dev/null +++ b/cart_endpoints/zed_params/common_front.yaml @@ -0,0 +1,69 @@ +# params/common.yaml +# Common parameters to Stereolabs ZED and ZED mini cameras +--- + +# Dynamic parameters cannot have a namespace +brightness: 4 # Dynamic +contrast: 4 # Dynamic +hue: 0 # Dynamic +saturation: 4 # Dynamic +sharpness: 4 # Dynamic +gamma: 8 # Dynamic - Requires SDK >=v3.1 +auto_exposure_gain: true # Dynamic +gain: 100 # Dynamic - works only if `auto_exposure_gain` is false +exposure: 100 # Dynamic - works only if `auto_exposure_gain` is false +auto_whitebalance: true # Dynamic +whitebalance_temperature: 42 # Dynamic - works only if `auto_whitebalance` is false +depth_confidence: 50 # Dynamic +depth_texture_conf: 100 # Dynamic +pub_frame_rate: 30.0 # Dynamic - frequency of publishing of video and depth data +point_cloud_freq: 10.0 # Dynamic - frequency of the pointcloud publishing (equal or less to `grab_frame_rate` value) + +general: + camera_name: zed # A name for the camera (can be different from camera model and node name and can be overwritten by the launch file) + zed_id: 0 + serial_number: 0 + resolution: 2 # '0': HD2K, '1': HD1080, '2': HD720, '3': VGA + grab_frame_rate: 30 # Frequency of frame grabbing for internal SDK operations + gpu_id: -1 + base_frame: 'base_link' # must be equal to the frame_id used in the URDF file + verbose: false # Enable info message by the ZED SDK + svo_compression: 2 # `0`: LOSSLESS, `1`: AVCHD, `2`: HEVC + self_calib: true # enable/disable self calibration at starting + camera_flip: false + +video: + img_downsample_factor: 1.0 # Resample factor for images [0.01,1.0] The SDK works with native image sizes, but publishes rescaled image. + extrinsic_in_camera_frame: true # if `false` extrinsic parameter in `camera_info` will use ROS native frame (X FORWARD, Z UP) instead of the camera frame (Z FORWARD, Y DOWN) [`true` use old behavior as for version < v3.1] + +depth: + quality: 3 # '0': NONE, '1': PERFORMANCE, '2': QUALITY, '3': ULTRA + sensing_mode: 0 # '0': STANDARD, '1': FILL (not use FILL for robotic applications) + depth_stabilization: 1 # `0`: disabled, `1`: enabled + openni_depth_mode: false # 'false': 32bit float meters, 'true': 16bit uchar millimeters + depth_downsample_factor: 1.0 # Resample factor for depth data matrices [0.01,1.0] The SDK works with native data sizes, but publishes rescaled matrices (depth map, point cloud, ...) + +pos_tracking: + pos_tracking_enabled: true # True to enable positional tracking from start + publish_tf: false # publish `odom -> base_link` TF + publish_map_tf: false # publish `map -> odom` TF + map_frame: 'map' # main frame + odometry_frame: 'odom' # odometry frame + area_memory_db_path: 'zed_area_memory.area' # file loaded when the node starts to restore the "known visual features" map. + save_area_memory_db_on_exit: false # save the "known visual features" map when the node is correctly closed to the path indicated by `area_memory_db_path` + area_memory: true # Enable to detect loop closure + 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` -> [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 # 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 + +mapping: + mapping_enabled: false # True to enable mapping and fused point cloud pubblication + resolution: 0.05 # maps resolution in meters [0.01f, 0.2f] + max_mapping_range: -1 # 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 + + diff --git a/cart_endpoints/zed_params/common_passenger.yaml b/cart_endpoints/zed_params/common_passenger.yaml new file mode 100644 index 0000000..916e1f2 --- /dev/null +++ b/cart_endpoints/zed_params/common_passenger.yaml @@ -0,0 +1,69 @@ +# params/common.yaml +# Common parameters to Stereolabs ZED and ZED mini cameras +--- + +# Dynamic parameters cannot have a namespace +brightness: 4 # Dynamic +contrast: 4 # Dynamic +hue: 0 # Dynamic +saturation: 4 # Dynamic +sharpness: 4 # Dynamic +gamma: 8 # Dynamic - Requires SDK >=v3.1 +auto_exposure_gain: true # Dynamic +gain: 100 # Dynamic - works only if `auto_exposure_gain` is false +exposure: 100 # Dynamic - works only if `auto_exposure_gain` is false +auto_whitebalance: true # Dynamic +whitebalance_temperature: 42 # Dynamic - works only if `auto_whitebalance` is false +depth_confidence: 50 # Dynamic +depth_texture_conf: 100 # Dynamic +pub_frame_rate: 30.0 # Dynamic - frequency of publishing of video and depth data +point_cloud_freq: 1.0 # Dynamic - frequency of the pointcloud publishing (equal or less to `grab_frame_rate` value) + +general: + camera_name: zed # A name for the camera (can be different from camera model and node name and can be overwritten by the launch file) + zed_id: 0 + serial_number: 0 + resolution: 2 # '0': HD2K, '1': HD1080, '2': HD720, '3': VGA + grab_frame_rate: 30 # Frequency of frame grabbing for internal SDK operations + gpu_id: -1 + base_frame: 'base_link' # must be equal to the frame_id used in the URDF file + verbose: false # Enable info message by the ZED SDK + svo_compression: 2 # `0`: LOSSLESS, `1`: AVCHD, `2`: HEVC + self_calib: true # enable/disable self calibration at starting + camera_flip: false + +video: + img_downsample_factor: 1.0 # Resample factor for images [0.01,1.0] The SDK works with native image sizes, but publishes rescaled image. + extrinsic_in_camera_frame: true # if `false` extrinsic parameter in `camera_info` will use ROS native frame (X FORWARD, Z UP) instead of the camera frame (Z FORWARD, Y DOWN) [`true` use old behavior as for version < v3.1] + +depth: + quality: 3 # '0': NONE, '1': PERFORMANCE, '2': QUALITY, '3': ULTRA + sensing_mode: 0 # '0': STANDARD, '1': FILL (not use FILL for robotic applications) + depth_stabilization: 1 # `0`: disabled, `1`: enabled + openni_depth_mode: false # 'false': 32bit float meters, 'true': 16bit uchar millimeters + depth_downsample_factor: 1.0 # Resample factor for depth data matrices [0.01,1.0] The SDK works with native data sizes, but publishes rescaled matrices (depth map, point cloud, ...) + +pos_tracking: + pos_tracking_enabled: true # True to enable positional tracking from start + publish_tf: false # publish `odom -> base_link` TF + publish_map_tf: false # publish `map -> odom` TF + map_frame: 'map' # main frame + odometry_frame: 'odom' # odometry frame + area_memory_db_path: 'zed_area_memory.area' # file loaded when the node starts to restore the "known visual features" map. + save_area_memory_db_on_exit: false # save the "known visual features" map when the node is correctly closed to the path indicated by `area_memory_db_path` + area_memory: true # Enable to detect loop closure + 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` -> [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 # 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 + +mapping: + mapping_enabled: false # True to enable mapping and fused point cloud pubblication + resolution: 0.05 # maps resolution in meters [0.01f, 0.2f] + max_mapping_range: -1 # 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 + + diff --git a/cart_endpoints/zed_params/zed2i_front.yaml b/cart_endpoints/zed_params/zed2i_front.yaml new file mode 100644 index 0000000..9465a88 --- /dev/null +++ b/cart_endpoints/zed_params/zed2i_front.yaml @@ -0,0 +1,34 @@ +# params/zed2i.yaml +# Parameters for Stereolabs ZED2 camera +--- + +general: + camera_model: 'zed2i' + resolution: 2 # '0': HD2K, '1': HD1080, '2': HD720, '3': VGA + grab_frame_rate: 15 # Frequency of frame grabbing for internal SDK operations + +depth: + min_depth: 0.7 # Min: 0.2, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory + max_depth: 40.0 # Max: 40.0 + +pos_tracking: + imu_fusion: false # enable/disable IMU fusion. When set to false, only the optical odometry will be used. + pos_tracking_enabled: false # we are adding this feild as a test to see if pose tracking can be published from here + +sensors: + sensors_timestamp_sync: false # Synchronize Sensors messages timestamp with latest received frame + publish_imu_tf: false # publish `IMU -> _left_camera_frame` TF + +object_detection: + od_enabled: true # True to enable Object Detection [only ZED 2] + model: 0 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE + confidence_threshold: 40 # Minimum value of the detection confidence of an object [0,100] initially set to 50 + max_range: 10. # Maximum detection range default range: 15 + object_tracking_enabled: true # Enable/disable the tracking of the detected objects + body_fitting: true # Enable/disable body fitting for 'HUMAN_BODY_FAST' and 'HUMAN_BODY_ACCURATE' models + mc_people: true # Enable/disable the detection of persons for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models + mc_vehicle: true # Enable/disable the detection of vehicles for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models + mc_bag: true # Enable/disable the detection of bags for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models + mc_animal: true # Enable/disable the detection of animals for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models + mc_electronics: true # Enable/disable the detection of electronic devices for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models + mc_fruit_vegetable: true # Enable/disable the detection of fruits and vegetables for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models diff --git a/cart_endpoints/zed_params/zed2i_passenger.yaml b/cart_endpoints/zed_params/zed2i_passenger.yaml new file mode 100644 index 0000000..185f05e --- /dev/null +++ b/cart_endpoints/zed_params/zed2i_passenger.yaml @@ -0,0 +1,34 @@ +# params/zed2i.yaml +# Parameters for Stereolabs ZED2 camera +--- + +general: + camera_model: 'zed2i' + resolution: 2 # '0': HD2K, '1': HD1080, '2': HD720, '3': VGA + grab_frame_rate: 15 # Frequency of frame grabbing for internal SDK operations + +depth: + min_depth: 0..3 # Min: 0.2, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory + max_depth: 40.0 # Max: 40.0 + +pos_tracking: + imu_fusion: false # enable/disable IMU fusion. When set to false, only the optical odometry will be used. + pos_tracking_enabled: false # we are adding this feild as a test to see if pose tracking can be published from here + +sensors: + sensors_timestamp_sync: false # Synchronize Sensors messages timestamp with latest received frame + publish_imu_tf: false # publish `IMU -> _left_camera_frame` TF + +object_detection: + od_enabled: false # True to enable Object Detection [only ZED 2] + model: 0 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE + confidence_threshold: 40 # Minimum value of the detection confidence of an object [0,100] initially set to 50 + max_range: 10. # Maximum detection range default range: 15 + object_tracking_enabled: true # Enable/disable the tracking of the detected objects + body_fitting: true # Enable/disable body fitting for 'HUMAN_BODY_FAST' and 'HUMAN_BODY_ACCURATE' models + mc_people: true # Enable/disable the detection of persons for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models + mc_vehicle: true # Enable/disable the detection of vehicles for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models + mc_bag: true # Enable/disable the detection of bags for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models + mc_animal: true # Enable/disable the detection of animals for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models + mc_electronics: true # Enable/disable the detection of electronic devices for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models + mc_fruit_vegetable: true # Enable/disable the detection of fruits and vegetables for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models diff --git a/run.sh b/run.sh index 088256a..f679adb 100755 --- a/run.sh +++ b/run.sh @@ -36,7 +36,7 @@ wait # Zed camera launch all new echo "Launching Zed camera nodes" #gnome-terminal --tab -e 'sh -c "cd ~; roslaunch zed_wrapper jacart_multi_cam.launch node_name_2:=passenger camera_name_2:=passenger_cam node_name_1:=front camera_name_1:=front_cam; exec bash"' -gnome-terminal --tab -e 'sh -c "cd ~; roslaunch zed_wrapper jacart_multi_cam.launch; exec bash"' +gnome-terminal --tab -e 'sh -c "cd ~; roslaunch cart_endpoints jacart_multi_cam.launch; exec bash"' sleep 4 # end new Zed launch