From 1db6ee03daaa0a5adbaa62210ce49ecdae69c8d6 Mon Sep 17 00:00:00 2001 From: Name Date: Fri, 3 May 2024 15:54:38 -0400 Subject: [PATCH 01/18] initial working commit --- src/rove_zed/config/zed_body_trck.yaml | 20 ++++++++++++++ src/rove_zed/launch/zed_body_trck.launch.py | 24 +++++++++++++++++ src/rove_zed/launch/zed_mapping.launch.py | 0 src/rove_zed/package.xml | 22 ++++++++++++++++ src/rove_zed/resource/rove_zed | 0 src/rove_zed/setup.cfg | 4 +++ src/rove_zed/setup.py | 29 +++++++++++++++++++++ 7 files changed, 99 insertions(+) create mode 100644 src/rove_zed/config/zed_body_trck.yaml create mode 100644 src/rove_zed/launch/zed_body_trck.launch.py create mode 100644 src/rove_zed/launch/zed_mapping.launch.py create mode 100644 src/rove_zed/package.xml create mode 100644 src/rove_zed/resource/rove_zed create mode 100644 src/rove_zed/setup.cfg create mode 100644 src/rove_zed/setup.py diff --git a/src/rove_zed/config/zed_body_trck.yaml b/src/rove_zed/config/zed_body_trck.yaml new file mode 100644 index 0000000..4c5190c --- /dev/null +++ b/src/rove_zed/config/zed_body_trck.yaml @@ -0,0 +1,20 @@ +# Parameters for Stereolabs ZED2i camera +camera_name: zed2i +camera_model: zed2i + + # depth: + # min_depth: 0.2 # Min: 0.2, Max: 3.0 + # max_depth: 10.0 # Max: 40.0 + + # body_tracking: + # bt_enabled: true # True to enable Body Tracking + # model: "HUMAN_BODY_MEDIUM" # 'HUMAN_BODY_FAST', 'HUMAN_BODY_MEDIUM', 'HUMAN_BODY_ACCURATE' + # body_format: "BODY_18" # '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: "UPPER_BODY" # '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 diff --git a/src/rove_zed/launch/zed_body_trck.launch.py b/src/rove_zed/launch/zed_body_trck.launch.py new file mode 100644 index 0000000..0d1d7dc --- /dev/null +++ b/src/rove_zed/launch/zed_body_trck.launch.py @@ -0,0 +1,24 @@ +from pathlib import Path +import yaml +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + + pkg_zed_wrapper = get_package_share_directory('zed_wrapper') + launch_file_path = os.path.join(pkg_zed_wrapper, 'launch', 'zed_camera.launch.py') + + # Parse yml config file to iterable array + pkg_rove_zed = get_package_share_directory('rove_zed') + config_file_path = os.path.join(pkg_rove_zed, 'config', 'zed_body_trck.yaml') + args = yaml.safe_load(Path(config_file_path).read_text()).items() + + return LaunchDescription([ + IncludeLaunchDescription( + launch_description_source=launch_file_path, + launch_arguments=args + ) + ]) diff --git a/src/rove_zed/launch/zed_mapping.launch.py b/src/rove_zed/launch/zed_mapping.launch.py new file mode 100644 index 0000000..e69de29 diff --git a/src/rove_zed/package.xml b/src/rove_zed/package.xml new file mode 100644 index 0000000..d47a137 --- /dev/null +++ b/src/rove_zed/package.xml @@ -0,0 +1,22 @@ + + + + rove_zed + 0.0.0 + Zed package for rove + rove + MIT + + + zed_wrapper + + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/rove_zed/resource/rove_zed b/src/rove_zed/resource/rove_zed new file mode 100644 index 0000000..e69de29 diff --git a/src/rove_zed/setup.cfg b/src/rove_zed/setup.cfg new file mode 100644 index 0000000..36013e4 --- /dev/null +++ b/src/rove_zed/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/rove_zed +[install] +install_scripts=$base/lib/rove_zed diff --git a/src/rove_zed/setup.py b/src/rove_zed/setup.py new file mode 100644 index 0000000..9f213ae --- /dev/null +++ b/src/rove_zed/setup.py @@ -0,0 +1,29 @@ +from setuptools import find_packages, setup +import os +from glob import glob + +package_name = 'rove_zed' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name + '/launch', glob('launch/*.launch.py')), + ('share/' + package_name + '/config', glob('config/*')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='rove', + maintainer_email='capra@etsmtl.ca', + description='Zed package for rove', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + ], + }, +) From cba75ce6bd333574cb45f5420c4697baea5b8683 Mon Sep 17 00:00:00 2001 From: Name Date: Fri, 3 May 2024 16:50:21 -0400 Subject: [PATCH 02/18] overriding zed_ros launch parameters --- src/rove_zed/config/zed_body_trck.yaml | 35 +++++++++------------ src/rove_zed/launch/zed_body_trck.launch.py | 10 ++++-- 2 files changed, 22 insertions(+), 23 deletions(-) diff --git a/src/rove_zed/config/zed_body_trck.yaml b/src/rove_zed/config/zed_body_trck.yaml index 4c5190c..c0169b9 100644 --- a/src/rove_zed/config/zed_body_trck.yaml +++ b/src/rove_zed/config/zed_body_trck.yaml @@ -1,20 +1,15 @@ -# Parameters for Stereolabs ZED2i camera -camera_name: zed2i -camera_model: zed2i - - # depth: - # min_depth: 0.2 # Min: 0.2, Max: 3.0 - # max_depth: 10.0 # Max: 40.0 - - # body_tracking: - # bt_enabled: true # True to enable Body Tracking - # model: "HUMAN_BODY_MEDIUM" # 'HUMAN_BODY_FAST', 'HUMAN_BODY_MEDIUM', 'HUMAN_BODY_ACCURATE' - # body_format: "BODY_18" # '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: "UPPER_BODY" # '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 +# Parameters to override zed_wrapper parameters +/**: + ros__parameters: + body_tracking: + bt_enabled: true # True to enable Body Tracking + model: "HUMAN_BODY_MEDIUM" # 'HUMAN_BODY_FAST', 'HUMAN_BODY_MEDIUM', 'HUMAN_BODY_ACCURATE' + body_format: "BODY_18" # '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: "UPPER_BODY" # '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 diff --git a/src/rove_zed/launch/zed_body_trck.launch.py b/src/rove_zed/launch/zed_body_trck.launch.py index 0d1d7dc..c5f8b7d 100644 --- a/src/rove_zed/launch/zed_body_trck.launch.py +++ b/src/rove_zed/launch/zed_body_trck.launch.py @@ -8,17 +8,21 @@ def generate_launch_description(): + camera_model = 'zed2i' + pkg_zed_wrapper = get_package_share_directory('zed_wrapper') launch_file_path = os.path.join(pkg_zed_wrapper, 'launch', 'zed_camera.launch.py') # Parse yml config file to iterable array pkg_rove_zed = get_package_share_directory('rove_zed') - config_file_path = os.path.join(pkg_rove_zed, 'config', 'zed_body_trck.yaml') - args = yaml.safe_load(Path(config_file_path).read_text()).items() + config_override_path = os.path.join(pkg_rove_zed, 'config', 'zed_body_trck.yaml') return LaunchDescription([ IncludeLaunchDescription( launch_description_source=launch_file_path, - launch_arguments=args + launch_arguments={ # `ros2 launch rove_zed zed_body_trck.launch.py --show-arguments` + 'camera_model': camera_model, + 'ros_params_override_path': config_override_path, + }.items() ) ]) From 9bfe4f2b231c5c8bddfd9de405ca54460b4ef575 Mon Sep 17 00:00:00 2001 From: Name Date: Fri, 3 May 2024 17:13:24 -0400 Subject: [PATCH 03/18] removed edit() --- src/rove_zed/launch/zed_body_trck.launch.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/rove_zed/launch/zed_body_trck.launch.py b/src/rove_zed/launch/zed_body_trck.launch.py index c5f8b7d..f3c050b 100644 --- a/src/rove_zed/launch/zed_body_trck.launch.py +++ b/src/rove_zed/launch/zed_body_trck.launch.py @@ -20,9 +20,10 @@ def generate_launch_description(): return LaunchDescription([ IncludeLaunchDescription( launch_description_source=launch_file_path, - launch_arguments={ # `ros2 launch rove_zed zed_body_trck.launch.py --show-arguments` - 'camera_model': camera_model, - 'ros_params_override_path': config_override_path, - }.items() + launch_arguments=[ # `ros2 launch rove_zed zed_body_trck.launch.py --show-arguments` + ['camera_model', camera_model], + ['camera_name', camera_model], + ['ros_params_override_path', config_override_path] + ] ) ]) From f8755e21cf5ad09e310a5f20ef8209c72ba37650 Mon Sep 17 00:00:00 2001 From: Name Date: Mon, 6 May 2024 18:06:59 -0400 Subject: [PATCH 04/18] Added mapping launch file --- src/rove_zed/config/zed_mapping.yaml | 11 +++++++++ src/rove_zed/launch/zed_mapping.launch.py | 29 +++++++++++++++++++++++ 2 files changed, 40 insertions(+) create mode 100644 src/rove_zed/config/zed_mapping.yaml diff --git a/src/rove_zed/config/zed_mapping.yaml b/src/rove_zed/config/zed_mapping.yaml new file mode 100644 index 0000000..c217994 --- /dev/null +++ b/src/rove_zed/config/zed_mapping.yaml @@ -0,0 +1,11 @@ +# Parameters to override zed_wrapper parameters +/**: + ros__parameters: + mapping: + mapping_enabled: true # 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 \ No newline at end of file diff --git a/src/rove_zed/launch/zed_mapping.launch.py b/src/rove_zed/launch/zed_mapping.launch.py index e69de29..a733013 100644 --- a/src/rove_zed/launch/zed_mapping.launch.py +++ b/src/rove_zed/launch/zed_mapping.launch.py @@ -0,0 +1,29 @@ +from pathlib import Path +import yaml +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + + camera_model = 'zed2i' + + pkg_zed_wrapper = get_package_share_directory('zed_wrapper') + launch_file_path = os.path.join(pkg_zed_wrapper, 'launch', 'zed_camera.launch.py') + + # Parse yml config file to iterable array + pkg_rove_zed = get_package_share_directory('rove_zed') + config_override_path = os.path.join(pkg_rove_zed, 'config', 'zed_mapping.yaml') + + return LaunchDescription([ + IncludeLaunchDescription( + launch_description_source=launch_file_path, + launch_arguments=[ # `ros2 launch rove_zed zed_body_trck.launch.py --show-arguments` + ['camera_model', camera_model], + ['camera_name', camera_model], + ['ros_params_override_path', config_override_path] + ] + ) + ]) \ No newline at end of file From d484d00cd52f2b15397747f8d22fb628d55ac35f Mon Sep 17 00:00:00 2001 From: samb271 Date: Thu, 30 May 2024 15:49:12 -0400 Subject: [PATCH 05/18] config changes --- rove.repos | 9 +++++++++ src/rove_zed/config/zed_body_trck.yaml | 2 +- src/rove_zed/config/zed_mapping.yaml | 19 ++++++++++++++++++- src/rove_zed/launch/zed_body_trck.launch.py | 2 +- src/rove_zed/launch/zed_mapping.launch.py | 2 +- 5 files changed, 30 insertions(+), 4 deletions(-) diff --git a/rove.repos b/rove.repos index 628e2b7..4bf0e7d 100644 --- a/rove.repos +++ b/rove.repos @@ -11,3 +11,12 @@ repositories: type: git url: https://github.com/dawonn/vectornav.git version: ros2 + zed_wrapper: + type: git + url: https://github.com/stereolabs/zed-ros2-wrapper.git + version: master + zed_wrapper/zed-ros2-interfaces: + type: git + url: https://github.com/stereolabs/zed-ros2-interfaces.git + version: main + diff --git a/src/rove_zed/config/zed_body_trck.yaml b/src/rove_zed/config/zed_body_trck.yaml index c0169b9..e421a25 100644 --- a/src/rove_zed/config/zed_body_trck.yaml +++ b/src/rove_zed/config/zed_body_trck.yaml @@ -10,6 +10,6 @@ body_kp_selection: "UPPER_BODY" # '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 + prediction_timeout_s: 3.0 # 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 diff --git a/src/rove_zed/config/zed_mapping.yaml b/src/rove_zed/config/zed_mapping.yaml index c217994..bc0175e 100644 --- a/src/rove_zed/config/zed_mapping.yaml +++ b/src/rove_zed/config/zed_mapping.yaml @@ -8,4 +8,21 @@ 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 \ No newline at end of file + pd_normal_similarity_threshold: 15.0 # Plane detection: controls the spread of plane by checking the angle difference + + + + +docker run -it --rm \ + --privileged \ + -e DISPLAY=$DISPLAY \ + -e QT_X11_NO_MITSHM=1 \ + -e NVIDIA_VISIBLE_DEVICES=all \ + -e NVIDIA_DRIVER_CAPABILITIES=all \ + -e XAUTHORITY=$XAUTH \ + --runtime=nvidia \ + --network host \ + -v $XAUTH:$XAUTH \ + -v /tmp/.X11-unix:/tmp/.X11-unix \ + introlab3it/rtabmap_ros:humble-latest \ + /bin/bash -c "export ROS_NAMESPACE=rtabmap && ros2 launch rtabmap_viz rtabmap_viz" \ No newline at end of file diff --git a/src/rove_zed/launch/zed_body_trck.launch.py b/src/rove_zed/launch/zed_body_trck.launch.py index f3c050b..657b8a9 100644 --- a/src/rove_zed/launch/zed_body_trck.launch.py +++ b/src/rove_zed/launch/zed_body_trck.launch.py @@ -8,7 +8,7 @@ def generate_launch_description(): - camera_model = 'zed2i' + camera_model = 'zedm' pkg_zed_wrapper = get_package_share_directory('zed_wrapper') launch_file_path = os.path.join(pkg_zed_wrapper, 'launch', 'zed_camera.launch.py') diff --git a/src/rove_zed/launch/zed_mapping.launch.py b/src/rove_zed/launch/zed_mapping.launch.py index a733013..27fd6e9 100644 --- a/src/rove_zed/launch/zed_mapping.launch.py +++ b/src/rove_zed/launch/zed_mapping.launch.py @@ -8,7 +8,7 @@ def generate_launch_description(): - camera_model = 'zed2i' + camera_model = 'zedm' pkg_zed_wrapper = get_package_share_directory('zed_wrapper') launch_file_path = os.path.join(pkg_zed_wrapper, 'launch', 'zed_camera.launch.py') From 4191fd1bb68ceca5a878c96a0c80ba2eea869c6c Mon Sep 17 00:00:00 2001 From: patates-cipsi418 Date: Sun, 2 Jun 2024 16:18:34 -0400 Subject: [PATCH 06/18] add velodyne launch file --- .../config/VLP16_velodyne_params.yaml | 30 +++++++++++ src/rove_bringup/config/VLP16db.yaml | 53 +++++++++++++++++++ src/rove_bringup/launch/velodyne.launch.py | 50 +++++++++++++++++ 3 files changed, 133 insertions(+) create mode 100644 src/rove_bringup/config/VLP16_velodyne_params.yaml create mode 100644 src/rove_bringup/config/VLP16db.yaml create mode 100644 src/rove_bringup/launch/velodyne.launch.py diff --git a/src/rove_bringup/config/VLP16_velodyne_params.yaml b/src/rove_bringup/config/VLP16_velodyne_params.yaml new file mode 100644 index 0000000..b18fc47 --- /dev/null +++ b/src/rove_bringup/config/VLP16_velodyne_params.yaml @@ -0,0 +1,30 @@ +# from https://github.com/ros-drivers/velodyne/tree/ros2 +velodyne_driver_node: + ros__parameters: + device_ip: 192.168.84.201 # be sure that your ip adress is 192.168.84.150/24 or look destination in the velodyne settings + gps_time: false + time_offset: 0.0 + enabled: true + read_once: false + read_fast: false + repeat_delay: 0.0 + frame_id: velodyne + model: VLP16 + rpm: 600.0 + port: 2368 + timestamp_first_packet: false + +velodyne_laserscan_node: + ros__parameters: + ring: -1 + resolution: 0.007 + +velodyne_transform_node: + ros__parameters: + model: VLP16 + min_range: 0.9 + max_range: 130.0 + view_direction: 0.0 + fixed_frame: "" + target_frame: "" + organize_cloud: true \ No newline at end of file diff --git a/src/rove_bringup/config/VLP16db.yaml b/src/rove_bringup/config/VLP16db.yaml new file mode 100644 index 0000000..9680061 --- /dev/null +++ b/src/rove_bringup/config/VLP16db.yaml @@ -0,0 +1,53 @@ +# callibration of the Velodyne lidar +# from https://github.com/ros-drivers/velodyne/tree/ros2 +lasers: +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 0, rot_correction: 0.0, + vert_correction: -0.2617993877991494, vert_offset_correction: 0.0112} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 1, rot_correction: 0.0, + vert_correction: 0.017453292519943295, vert_offset_correction: -0.0007} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 2, rot_correction: 0.0, + vert_correction: -0.22689280275926285, vert_offset_correction: 0.0097} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 3, rot_correction: 0.0, + vert_correction: 0.05235987755982989, vert_offset_correction: -0.0022} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 4, rot_correction: 0.0, + vert_correction: -0.19198621771937624, vert_offset_correction: 0.0081} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 5, rot_correction: 0.0, + vert_correction: 0.08726646259971647, vert_offset_correction: -0.0037} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 6, rot_correction: 0.0, + vert_correction: -0.15707963267948966, vert_offset_correction: 0.0066} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 7, rot_correction: 0.0, + vert_correction: 0.12217304763960307, vert_offset_correction: -0.0051} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 8, rot_correction: 0.0, + vert_correction: -0.12217304763960307, vert_offset_correction: 0.0051} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 9, rot_correction: 0.0, + vert_correction: 0.15707963267948966, vert_offset_correction: -0.0066} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 10, rot_correction: 0.0, + vert_correction: -0.08726646259971647, vert_offset_correction: 0.0037} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 11, rot_correction: 0.0, + vert_correction: 0.19198621771937624, vert_offset_correction: -0.0081} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 12, rot_correction: 0.0, + vert_correction: -0.05235987755982989, vert_offset_correction: 0.0022} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 13, rot_correction: 0.0, + vert_correction: 0.22689280275926285, vert_offset_correction: -0.0097} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 14, rot_correction: 0.0, + vert_correction: -0.017453292519943295, vert_offset_correction: 0.0007} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 15, rot_correction: 0.0, + vert_correction: 0.2617993877991494, vert_offset_correction: -0.0112} +num_lasers: 16 +distance_resolution: 0.002 diff --git a/src/rove_bringup/launch/velodyne.launch.py b/src/rove_bringup/launch/velodyne.launch.py new file mode 100644 index 0000000..3b5c85f --- /dev/null +++ b/src/rove_bringup/launch/velodyne.launch.py @@ -0,0 +1,50 @@ +import os +import launch +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + +import yaml + +# from https://github.com/ros-drivers/velodyne/tree/ros2 +def generate_launch_description(): + package_name = 'rove_bringup' + dir = get_package_share_directory(package_name) + # Get params files + params = os.path.join(dir, 'config', 'VLP16_velodyne_params.yaml') + with open(params, 'r') as f: + transform_params = yaml.safe_load(f)['velodyne_transform_node']['ros__parameters'] + transform_params['calibration'] = os.path.join(dir, 'config', 'VLP16db.yaml') + + driver_node = Node(package='velodyne_driver', + executable='velodyne_driver_node', + output='both', + parameters=[params]) + + driver_exit_event = launch.actions.RegisterEventHandler( + event_handler=launch.event_handlers.OnProcessExit( + target_action=driver_node, + on_exit=[launch.actions.EmitEvent( + event=launch.events.Shutdown())], + )) + + # laserscan + laserscan_node = Node(package='velodyne_laserscan', + executable='velodyne_laserscan_node', + name='scan', + output='both', + parameters=[params]) + + # pointcloud + transform_node = Node(package='velodyne_pointcloud', + executable='velodyne_transform_node', + name='velodyne_points', + output='both', + parameters=[transform_params]) + + return LaunchDescription([ + driver_node, + driver_exit_event, + laserscan_node, + transform_node, + ]) \ No newline at end of file From da2459278157b4183901f7f515d8c9edab992b7c Mon Sep 17 00:00:00 2001 From: patates-cipsi418 Date: Sun, 2 Jun 2024 16:49:05 -0400 Subject: [PATCH 07/18] add velodyne launch to real.launch.py --- src/rove_bringup/launch/real.launch.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/rove_bringup/launch/real.launch.py b/src/rove_bringup/launch/real.launch.py index ef14e0b..dfb7592 100644 --- a/src/rove_bringup/launch/real.launch.py +++ b/src/rove_bringup/launch/real.launch.py @@ -28,7 +28,14 @@ def generate_launch_description(): ), ) + velodyne = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_rove_bringup, "launch", "velodyne.launch.py"), + ), + ) + return LaunchDescription([ common, vectornav, + velodyne, ]) From 6292a7348f05824b02273a2551c66ad42a8a521e Mon Sep 17 00:00:00 2001 From: patates-cipsi418 Date: Sun, 2 Jun 2024 16:55:08 -0400 Subject: [PATCH 08/18] add use_sim_time parameter to common launch file --- src/rove_bringup/launch/common.launch.py | 13 ++++++++----- src/rove_bringup/launch/real.launch.py | 3 +++ src/rove_bringup/launch/sim.launch.py | 5 +++++ 3 files changed, 16 insertions(+), 5 deletions(-) diff --git a/src/rove_bringup/launch/common.launch.py b/src/rove_bringup/launch/common.launch.py index 458e0fa..00a1b5d 100644 --- a/src/rove_bringup/launch/common.launch.py +++ b/src/rove_bringup/launch/common.launch.py @@ -8,9 +8,12 @@ from launch.substitutions import Command from launch_ros.actions import Node from launch_ros.parameter_descriptions import ParameterValue +from launch.substitutions import LaunchConfiguration def generate_launch_description(): + use_sim_time = LaunchConfiguration('use_sim_time') + # Get the launch directory pkg_rove_bringup = get_package_share_directory('rove_bringup') pkg_rove_description = get_package_share_directory('rove_description') @@ -30,7 +33,7 @@ def generate_launch_description(): output='both', parameters=[ {'robot_description': robot_desc}, - {"use_sim_time": True, } + {"use_sim_time": use_sim_time, } ] ) @@ -50,7 +53,7 @@ def generate_launch_description(): name='ekf_filter_node_local', output='screen', parameters=[os.path.join(pkg_rove_slam, 'config/ekf.yaml'), - {'use_sim_time': True}], + {'use_sim_time': use_sim_time}], remappings=[('odometry/filtered', 'odometry/local')]) @@ -60,7 +63,7 @@ def generate_launch_description(): name='ekf_filter_node_global', output='screen', parameters=[os.path.join(pkg_rove_slam, 'config/ekf.yaml'), - {'use_sim_time': True}], + {'use_sim_time': use_sim_time}], remappings=[('odometry/filtered', 'odometry/global')]) @@ -70,7 +73,7 @@ def generate_launch_description(): name="navsat_transform", output="screen", parameters=[os.path.join(pkg_rove_description, 'config/navsat_transform.yaml'), - {'use_sim_time': True}], + {'use_sim_time': use_sim_time}], remappings=[ # Subscribed Topics ("imu/data", "imu"), @@ -90,7 +93,7 @@ def generate_launch_description(): os.path.join(pkg_rove_bringup, "launch", "autonomy.launch.py"), ), launch_arguments={ - "use_sim_time": "true", + 'use_sim_time': use_sim_time, "deskewing": "true", "use_slam3d": "false", }.items(), diff --git a/src/rove_bringup/launch/real.launch.py b/src/rove_bringup/launch/real.launch.py index dfb7592..a9d6bf0 100644 --- a/src/rove_bringup/launch/real.launch.py +++ b/src/rove_bringup/launch/real.launch.py @@ -18,6 +18,9 @@ def generate_launch_description(): PythonLaunchDescriptionSource( os.path.join(pkg_rove_bringup, "launch", "common.launch.py"), ), + launch_arguments={ + "use_sim_time": "false", + }.items(), ) diff --git a/src/rove_bringup/launch/sim.launch.py b/src/rove_bringup/launch/sim.launch.py index 8b5e118..b13a321 100644 --- a/src/rove_bringup/launch/sim.launch.py +++ b/src/rove_bringup/launch/sim.launch.py @@ -19,6 +19,8 @@ def generate_launch_description(): # Get the URDF file urdf_path = os.path.join(pkg_rove_description, 'urdf', 'rove.urdf.xacro') robot_desc = ParameterValue(Command(['xacro ', urdf_path]), value_type=str) + + # Get simulation file world_file_name = 'worlds/base_world.world' world = os.path.join(pkg_rove_description, world_file_name) @@ -72,6 +74,9 @@ def generate_launch_description(): PythonLaunchDescriptionSource( os.path.join(pkg_rove_bringup, "launch", "common.launch.py"), ), + launch_arguments={ + "use_sim_time": "true", + }.items(), ) return LaunchDescription([ From e49b899f2e3b6abad3268dc1542d8e3f19679ef7 Mon Sep 17 00:00:00 2001 From: samb271 Date: Tue, 4 Jun 2024 00:32:28 -0400 Subject: [PATCH 09/18] added rtabmap launchfile --- src/rove_rtabmap/launch/rtabmap.py | 76 ++++++++++++++++++++++++++++ src/rove_zed/config/zed_mapping.yaml | 25 +++------ src/rove_zed/package.xml | 11 ++++ 3 files changed, 95 insertions(+), 17 deletions(-) create mode 100644 src/rove_rtabmap/launch/rtabmap.py diff --git a/src/rove_rtabmap/launch/rtabmap.py b/src/rove_rtabmap/launch/rtabmap.py new file mode 100644 index 0000000..044b274 --- /dev/null +++ b/src/rove_rtabmap/launch/rtabmap.py @@ -0,0 +1,76 @@ +# Requirements: +# A realsense D435i +# Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera) +# Example: +# $ ros2 launch realsense2_camera rs_launch.py enable_gyro:=true enable_accel:=true unite_imu_method:=1 enable_infra1:=true enable_infra2:=true enable_sync:=true +# $ ros2 param set /camera/camera depth_module.emitter_enabled 0 +# +# $ ros2 launch rtabmap_examples realsense_d435i_stereo.launch.py + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def generate_launch_description(): + camera_model = 'zedm' + + parameters=[{ + 'frame_id':'camera_link', + 'subscribe_stereo':True, + 'subscribe_odom_info':True, + 'wait_imu_to_init':True}] + +# ros2 launch zed_wrapper zed_camera.launch.py camera_model:=zedm +# ros2 launch rtabmap_launch rtabmap.launch.py \ +# rtabmap_args:="--delete_db_on_start" \ +# rgb_topic:=/zed/zed_node/rgb/image_rect_color \ +# depth_topic:=/zed/zed_node/depth/depth_registered \ +# camera_info_topic:=/zed/zed_node/depth/camera_info \ +# odom_topic:=/zed/zed_node/odom \ +# imu_topic:=/zed/zed_node/imu/data \ +# visual_odometry:=false \ +# frame_id:=base_link \ +# approx_sync:=false \ +# rgbd_sync:=true \ +# approx_rgbd_sync:=false + + remappings=[ + ('rgb/image', '/%s/zed_node/rgb/image_rect_color'), + ('depth/image', "/%s/zed/zed_node/depth/depth_registered" % camera_model), + ('left/camera_info', '/%s/zed_node/left/camera_info' % camera_model), + ('right/image_rect', '/%s/zed_node/right/image_rect_color' % camera_model), + ('right/camera_info', '/%s/zed_node/right/camera_info' % camera_model)] + + return LaunchDescription([ + + # Nodes to launch + Node( + package='rtabmap_odom', executable='stereo_odometry', output='screen', + parameters=parameters, + remappings=remappings), + + Node( + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=parameters, + remappings=remappings, + arguments=['-d']), + + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + parameters=parameters, + remappings=remappings), + + # Compute quaternion of the IMU + Node( + package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen', + parameters=[{'use_mag': False, + 'world_frame':'enu', + 'publish_tf':False}], + remappings=[('imu/data_raw', '/camera/imu')]), + + # The IMU frame is missing in TF tree, add it: + Node( + package='tf2_ros', executable='static_transform_publisher', output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'camera_gyro_optical_frame', 'camera_imu_optical_frame']), + ]) \ No newline at end of file diff --git a/src/rove_zed/config/zed_mapping.yaml b/src/rove_zed/config/zed_mapping.yaml index bc0175e..7693ec3 100644 --- a/src/rove_zed/config/zed_mapping.yaml +++ b/src/rove_zed/config/zed_mapping.yaml @@ -9,20 +9,11 @@ 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 - - - - -docker run -it --rm \ - --privileged \ - -e DISPLAY=$DISPLAY \ - -e QT_X11_NO_MITSHM=1 \ - -e NVIDIA_VISIBLE_DEVICES=all \ - -e NVIDIA_DRIVER_CAPABILITIES=all \ - -e XAUTHORITY=$XAUTH \ - --runtime=nvidia \ - --network host \ - -v $XAUTH:$XAUTH \ - -v /tmp/.X11-unix:/tmp/.X11-unix \ - introlab3it/rtabmap_ros:humble-latest \ - /bin/bash -c "export ROS_NAMESPACE=rtabmap && ros2 launch rtabmap_viz rtabmap_viz" \ No newline at end of file + depth: + depth_mode: "ULTRA" # Matches the ZED SDK setting: 'NONE', 'PERFORMANCE', 'QUALITY', 'ULTRA', 'NEURAL', 'NEURAL_PLUS' - 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: 100 # [DYNAMIC] + depth_texture_conf: 100 # [DYNAMIC] + remove_saturated_areas: true # [DYNAMIC] \ No newline at end of file diff --git a/src/rove_zed/package.xml b/src/rove_zed/package.xml index d47a137..0cc0d4c 100644 --- a/src/rove_zed/package.xml +++ b/src/rove_zed/package.xml @@ -10,6 +10,17 @@ zed_wrapper + rtabmap_odom + rtabmap_slam + rtabmap_util + rtabmap_rviz_plugins + rtabmap_slam + rtabmap_util + rtabmap_viz + imu_filter_madgwick + tf2_ros + realsense2_camera + velodyne ament_copyright ament_flake8 From 0da584e450ad48b498a200d78a949002bc652237 Mon Sep 17 00:00:00 2001 From: samb271 Date: Fri, 7 Jun 2024 21:37:15 -0400 Subject: [PATCH 10/18] added working config --- src/rove_rtabmap/launch/rtabmap.py | 3 ++- src/rove_zed/config/zed_mapping.yaml | 10 ++++++++-- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/src/rove_rtabmap/launch/rtabmap.py b/src/rove_rtabmap/launch/rtabmap.py index 044b274..bcdb9a9 100644 --- a/src/rove_rtabmap/launch/rtabmap.py +++ b/src/rove_rtabmap/launch/rtabmap.py @@ -24,13 +24,14 @@ def generate_launch_description(): # ros2 launch zed_wrapper zed_camera.launch.py camera_model:=zedm # ros2 launch rtabmap_launch rtabmap.launch.py \ # rtabmap_args:="--delete_db_on_start" \ +# database_path:="/media/SSD/stable/rove/src/rove_rtabmap/maps/map.db" \ # rgb_topic:=/zed/zed_node/rgb/image_rect_color \ # depth_topic:=/zed/zed_node/depth/depth_registered \ # camera_info_topic:=/zed/zed_node/depth/camera_info \ # odom_topic:=/zed/zed_node/odom \ # imu_topic:=/zed/zed_node/imu/data \ # visual_odometry:=false \ -# frame_id:=base_link \ +# frame_id:=zed_camera_link \ # approx_sync:=false \ # rgbd_sync:=true \ # approx_rgbd_sync:=false diff --git a/src/rove_zed/config/zed_mapping.yaml b/src/rove_zed/config/zed_mapping.yaml index 7693ec3..10ef04d 100644 --- a/src/rove_zed/config/zed_mapping.yaml +++ b/src/rove_zed/config/zed_mapping.yaml @@ -1,6 +1,12 @@ # Parameters to override zed_wrapper parameters /**: ros__parameters: + general: + camera_model: 'zedm' + camera_name: 'zedm' # usually overwritten by launch file + grab_resolution: 'VGA' # The native camera grab resolution. 'HD2K', 'HD1080', 'HD720', 'VGA', 'AUTO' + pub_frame_rate: 30.0 # frequency of publishing of visual images and depth images + grab_frame_rate: 30 # ZED SDK internal grabbing rate mapping: mapping_enabled: true # True to enable mapping and fused point cloud pubblication resolution: 0.05 # maps resolution in meters [min: 0.01f - max: 0.2f] @@ -10,10 +16,10 @@ 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 depth: - depth_mode: "ULTRA" # Matches the ZED SDK setting: 'NONE', 'PERFORMANCE', 'QUALITY', 'ULTRA', 'NEURAL', 'NEURAL_PLUS' - Note: if 'NONE' all the modules that requires depth extraction are disabled by default (Pos. Tracking, Obj. Detection, Mapping, ...) + depth_mode: "PERFORMANCE" # Matches the ZED SDK setting: 'NONE', 'PERFORMANCE', 'QUALITY', 'ULTRA', 'NEURAL', 'NEURAL_PLUS' - 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) + point_cloud_freq: 30.0 # [DYNAMIC] - frequency of the pointcloud publishing (equal or less to `grab_frame_rate` value) depth_confidence: 100 # [DYNAMIC] depth_texture_conf: 100 # [DYNAMIC] remove_saturated_areas: true # [DYNAMIC] \ No newline at end of file From 4cc70bca0f3c88e44c8e16dd61efd4beb212663b Mon Sep 17 00:00:00 2001 From: patates-cipsi418 Date: Fri, 7 Jun 2024 22:06:01 -0400 Subject: [PATCH 11/18] add temp test.launch.py --- src/rove_bringup/launch/test.launch.py | 108 +++++++++++++++++++++++++ 1 file changed, 108 insertions(+) create mode 100644 src/rove_bringup/launch/test.launch.py diff --git a/src/rove_bringup/launch/test.launch.py b/src/rove_bringup/launch/test.launch.py new file mode 100644 index 0000000..736754f --- /dev/null +++ b/src/rove_bringup/launch/test.launch.py @@ -0,0 +1,108 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription + +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue + + +def generate_launch_description(): + # Get the launch directory + pkg_rove_bringup = get_package_share_directory('rove_bringup') + pkg_rove_description = get_package_share_directory('rove_description') + pkg_rove_slam = get_package_share_directory('rove_slam') + bringup_pkg_path = get_package_share_directory('rove_bringup') + + # Get the URDF file + urdf_path = os.path.join(pkg_rove_description, 'urdf', 'rove.urdf.xacro') + robot_desc = ParameterValue(Command(['xacro ', urdf_path]), value_type=str) + + # Takes the description and joint angles as inputs and publishes + # the 3D poses of the robot links + robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='both', + parameters=[ + {'robot_description': robot_desc}, + # {"use_sim_time": True, } + ] + ) + + # Visualize in RViz + rviz = Node( + package='rviz2', + executable='rviz2', + arguments=[ + '-f', 'velodyne', + # '-d', os.path.join(pkg_rove_description, 'config','basic.rviz') + ], + ) + + # used tutorial: https://navigation.ros.org/tutorials/docs/navigation2_with_gps.html + + robot_localization_node_local = Node( + package='robot_localization', + executable='ekf_node', + name='ekf_filter_node_local', + output='screen', + parameters=[os.path.join(pkg_rove_slam, 'config/ekf.yaml'), + # {'use_sim_time': True} + ], + remappings=[('odometry/filtered', 'odometry/local')]) + + + robot_localization_node_global = Node( + package='robot_localization', + executable='ekf_node', + name='ekf_filter_node_global', + output='screen', + parameters=[os.path.join(pkg_rove_slam, 'config/ekf.yaml'), + # {'use_sim_time': True} + ], + remappings=[('odometry/filtered', 'odometry/global')]) + + + navsat_transform = Node( + package="robot_localization", + executable="navsat_transform_node", + name="navsat_transform", + output="screen", + parameters=[os.path.join(pkg_rove_description, 'config/navsat_transform.yaml'), + # {'use_sim_time': True} + ], + remappings=[ + # Subscribed Topics + ("imu/data", "imu"), + ("gps/fix", "gps"), + ('odometry/filtered', 'odometry/global'), + ], + ) + + ###### Sensor ###### + vectornav = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_rove_bringup, "launch", "vectornav.launch.py"), + ), + ) + + velodyne = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_rove_bringup, "launch", "velodyne.launch.py"), + ), + ) + + return LaunchDescription([ + # robot_state_publisher, + robot_localization_node_local, + robot_localization_node_global, + navsat_transform, + vectornav, + velodyne, + rviz, + ]) From f09d0b4056cfa9e75917b8c659be7ded5df416fd Mon Sep 17 00:00:00 2001 From: samb271 Date: Sat, 8 Jun 2024 00:12:18 -0400 Subject: [PATCH 12/18] added zed readme --- src/rove_zed/readme.md | 35 +++++++++++++++++++++++++++++++++++ 1 file changed, 35 insertions(+) create mode 100644 src/rove_zed/readme.md diff --git a/src/rove_zed/readme.md b/src/rove_zed/readme.md new file mode 100644 index 0000000..d4333b7 --- /dev/null +++ b/src/rove_zed/readme.md @@ -0,0 +1,35 @@ +# Setup rove_zed + +Être sûr: +1. D'avoir la zed de branchée (port usb d'en dessous, celui du dessus ne fonctionne pas) +2. D'avoir le lidar de branché, 12V + ethernet (FAIRE ATTENTIONQUE LE CABLE FONCTIONNE ET EST BIEN BRANCHÉ) +3. D'avoir CUDA 12 d'installé et que zed Diagnostics fonctionne (`/usr/local/zed/tools/ZED_Diagnostic`) +4. D'avoir le network profile mis à `LIDAR profile` (IP adress `192.168.84.150`, aller dans settings > network) + +## Pour démarrer le mapping rtabmap avec la zed: +```bash +ros2 launch zed_wrapper zed_camera.launch.py camera_model:=zedm +ros2 launch rtabmap_launch rtabmap.launch.py \ + rtabmap_args:="--delete_db_on_start" \ + database_path:="/media/SSD/stable/rove/src/rove_rtabmap/maps/map.db" \ + rgb_topic:=/zed/zed_node/rgb/image_rect_color \ + depth_topic:=/zed/zed_node/depth/depth_registered \ + camera_info_topic:=/zed/zed_node/depth/camera_info \ + odom_topic:=/zed/zed_node/odom \ + imu_topic:=/zed/zed_node/imu/data \ + visual_odometry:=false \ + frame_id:=zed_camera_link \ + approx_sync:=false \ + rgbd_sync:=true \ + approx_rgbd_sync:=false +``` + +## Pour démarrer le body tracking: +```bash +ros2 launch rove_zed body_trck.launch.py +``` + +## Pour démarrer le lidar: +```bash +ros2 launch rove_bringup test.launch.py +``` \ No newline at end of file From be016a8c7b28cb69ee5f1ca2f457590408499b9c Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Sat, 8 Jun 2024 16:29:37 -0400 Subject: [PATCH 13/18] add vcs to docker and fix build --- .devcontainer/Dockerfile | 4 ++++ .github/workflows/build.yml | 2 +- src/rove_slam/setup.py | 2 +- src/rove_zed/setup.py | 2 +- 4 files changed, 7 insertions(+), 3 deletions(-) diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 71bdd98..bd2c1df 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -67,6 +67,10 @@ RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> ~/.bashrc COPY --chown=$USERNAME ./ /workspace/$USERNAME +# Add needed packages +RUN vcs import src < rove.repos + + # Rosdep update RUN rosdep update diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index a3080a7..f969f2a 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -17,5 +17,5 @@ jobs: docker compose build --build-arg USER_UID=$(id -u) --build-arg USER_GID=$(id -g) - name: Build rove run: | - docker compose run devcontainer bash -c "colcon build" + docker compose run devcontainer bash -c "colcon build --packages-ignore zed_components zed_wrapper" diff --git a/src/rove_slam/setup.py b/src/rove_slam/setup.py index 5a5ec71..2a3276f 100644 --- a/src/rove_slam/setup.py +++ b/src/rove_slam/setup.py @@ -18,7 +18,7 @@ install_requires=['setuptools'], zip_safe=True, maintainer='rove', - maintainer_email='capra@etsmtl.ca', + maintainer_email='capra@ens.etsmtl.ca', description='TODO: Package description', license='TODO: License declaration', tests_require=['pytest'], diff --git a/src/rove_zed/setup.py b/src/rove_zed/setup.py index 9f213ae..871e9e8 100644 --- a/src/rove_zed/setup.py +++ b/src/rove_zed/setup.py @@ -18,7 +18,7 @@ install_requires=['setuptools'], zip_safe=True, maintainer='rove', - maintainer_email='capra@etsmtl.ca', + maintainer_email='capra@ens.etsmtl.ca', description='Zed package for rove', license='MIT', tests_require=['pytest'], From eefba5cc75a9233b4edc40c51765d502fb718d94 Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Sat, 8 Jun 2024 17:06:24 -0400 Subject: [PATCH 14/18] remove cache cleaning --- .devcontainer/Dockerfile | 11 +---------- 1 file changed, 1 insertion(+), 10 deletions(-) diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index bd2c1df..1867f76 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -41,17 +41,10 @@ RUN apt-get update && apt-get install -y \ RUN wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg RUN echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null -RUN apt update -y +RUN apt update -y RUN apt install -y ros-humble-ros-gzharmonic -# Adding an APT update and clean step -RUN apt-get update && \ - apt-get install -y --no-install-recommends apt-utils && \ - rm -rf /var/lib/apt/lists/* && \ - apt-get clean && \ - apt-get update - # Change user RUN chown $USER_UID:$USER_GID /workspace/$USERNAME @@ -60,8 +53,6 @@ USER $USERNAME # Copy to preload the ros packages COPY ./ /workspace/$USERNAME/ -# Install Tesseract - # Source the ROS setup file RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> ~/.bashrc From 13022d0361aa63f11dac4b3ad6cc7370109656d1 Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Sat, 8 Jun 2024 17:13:43 -0400 Subject: [PATCH 15/18] test fix missing --- .devcontainer/Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 1867f76..6f6d51b 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -41,7 +41,7 @@ RUN apt-get update && apt-get install -y \ RUN wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg RUN echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null -RUN apt update -y +RUN apt update -y --fix-missing RUN apt install -y ros-humble-ros-gzharmonic From 0de6ce1630c596272c3ad590a19178d68964abda Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Sat, 8 Jun 2024 17:23:07 -0400 Subject: [PATCH 16/18] remove capra gazebo --- rove.repos | 4 ---- 1 file changed, 4 deletions(-) diff --git a/rove.repos b/rove.repos index 4bf0e7d..900658a 100644 --- a/rove.repos +++ b/rove.repos @@ -1,8 +1,4 @@ repositories: - capra_gazebo: - type: git - url: https://github.com/clubcapra/capra_gazebo.git - version: main ovis_description: type: git url: https://github.com/clubcapra/ovis_ros2.git From b53421d2c03b8b0fd898dcc36ebf46c51958e716 Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Sat, 8 Jun 2024 18:48:15 -0400 Subject: [PATCH 17/18] fix gz conflict docker --- .devcontainer/Dockerfile | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 6f6d51b..2768eb8 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -41,9 +41,7 @@ RUN apt-get update && apt-get install -y \ RUN wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg RUN echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null -RUN apt update -y --fix-missing - -RUN apt install -y ros-humble-ros-gzharmonic +RUN apt update -y # Change user RUN chown $USER_UID:$USER_GID /workspace/$USERNAME @@ -67,3 +65,6 @@ RUN rosdep update # Install ROS dependencies RUN rosdep install --from-paths src --ignore-src --rosdistro humble -y + +# Install gzharmonic after rosdep install +RUN sudo apt install -y ros-humble-ros-gzharmonic \ No newline at end of file From ac4c6cc3ee3eaabca88eaaaf04147c8ab6f6ffc1 Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Sat, 8 Jun 2024 18:49:51 -0400 Subject: [PATCH 18/18] add fix missing --- .devcontainer/Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 2768eb8..00ab61a 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -41,7 +41,7 @@ RUN apt-get update && apt-get install -y \ RUN wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg RUN echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null -RUN apt update -y +RUN apt update -y --fix-missing # Change user RUN chown $USER_UID:$USER_GID /workspace/$USERNAME