From 02cbba115c6475cfcaffc4c872363cc4f2280262 Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Sun, 9 Jun 2024 19:25:38 -0400 Subject: [PATCH 01/13] add 3d map --- src/rove_bringup/launch/autonomy.launch.py | 13 ++- src/rove_bringup/launch/common.launch.py | 2 +- src/rove_description/config/basic.rviz | 100 ++++++++++++++++---- src/rove_description/urdf/gazebo.urdf.xacro | 2 + src/rove_slam/config/ekf.yaml | 4 +- src/rove_slam/launch/slam3d.launch.py | 3 +- 6 files changed, 95 insertions(+), 29 deletions(-) diff --git a/src/rove_bringup/launch/autonomy.launch.py b/src/rove_bringup/launch/autonomy.launch.py index c1d2ba2..bf0132b 100644 --- a/src/rove_bringup/launch/autonomy.launch.py +++ b/src/rove_bringup/launch/autonomy.launch.py @@ -16,6 +16,8 @@ def generate_launch_description(): slam_pkg_path = get_package_share_directory("slam_toolbox") use_slam3d = LaunchConfiguration('use_slam3d') + use_sim_time = LaunchConfiguration('use_sim_time') + slam = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -27,6 +29,7 @@ def generate_launch_description(): pkg_rove_slam, "config", "slam_config.yaml" ) }.items(), + condition=IfCondition(use_slam3d) ) slam3d = IncludeLaunchDescription( @@ -34,7 +37,7 @@ def generate_launch_description(): os.path.join(pkg_rove_slam, "launch", "slam3d.launch.py"), ), launch_arguments={ - "use_sim_time": "true", + "use_sim_time": use_sim_time, "deskewing": "true", }.items(), condition=IfCondition(use_slam3d) @@ -43,12 +46,14 @@ def generate_launch_description(): nav = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(pkg_rove_nav, "navigation.launch.py"), - ) + ), + launch_arguments={ + "use_sim_time": use_sim_time, + }.items() ) - return LaunchDescription([ - slam, + #slam, slam3d, nav, ]) diff --git a/src/rove_bringup/launch/common.launch.py b/src/rove_bringup/launch/common.launch.py index 87781ff..ec0935d 100644 --- a/src/rove_bringup/launch/common.launch.py +++ b/src/rove_bringup/launch/common.launch.py @@ -94,7 +94,7 @@ def generate_launch_description(): launch_arguments={ 'use_sim_time': use_sim_time, "deskewing": "true", - "use_slam3d": "false", + "use_slam3d": "true", }.items(), ) diff --git a/src/rove_description/config/basic.rviz b/src/rove_description/config/basic.rviz index d662f35..0f8f1c7 100644 --- a/src/rove_description/config/basic.rviz +++ b/src/rove_description/config/basic.rviz @@ -3,7 +3,8 @@ Panels: Help Height: 78 Name: Displays Property Tree Widget: - Expanded: ~ + Expanded: + - /Global Options1 Splitter Ratio: 0.5 Tree Height: 517 - Class: rviz_common/Selection @@ -152,9 +153,9 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 125 + Max Intensity: 0 Min Color: 0; 0; 0 - Min Intensity: 1 + Min Intensity: 0 Name: LaserScan Position Transformer: XYZ Selectable: true @@ -178,6 +179,18 @@ Visualization Manager: All Enabled: false base_link: Value: true + flipper_fl: + Value: true + flipper_fr: + Value: true + flipper_rl: + Value: true + flipper_rr: + Value: true + map: + Value: true + odom: + Value: true sensor_link: Value: true track_fl: @@ -202,20 +215,31 @@ Visualization Manager: Show Axes: true Show Names: false Tree: - base_link: - sensor_link: - {} - track_l: - track_fl: - {} - track_rl: - {} - track_r: - track_fr: - {} - track_rr: - {} - velodyne_link: + map: + odom: + base_link: + flipper_fl: + {} + flipper_fr: + {} + flipper_rl: + {} + flipper_rr: + {} + sensor_link: + {} + track_l: + track_fl: + {} + track_rl: + {} + track_r: + track_fr: + {} + track_rr: + {} + velodyne_link: + {} velodyne_laser: {} Update Interval: 0 @@ -336,10 +360,44 @@ Visualization Manager: Value: /imu Value: true fixed_frame_orientation: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: 3d map + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /assembled_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: base_link + Fixed Frame: map Frame Rate: 30 Name: root Tools: @@ -382,7 +440,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 7.311348915100098 + Distance: 19.558191299438477 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -397,10 +455,10 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.0553982257843018 + Pitch: 0.7503987550735474 Target Frame: Value: Orbit (rviz) - Yaw: 4.913582801818848 + Yaw: 4.633583068847656 Saved: ~ Window Geometry: Displays: diff --git a/src/rove_description/urdf/gazebo.urdf.xacro b/src/rove_description/urdf/gazebo.urdf.xacro index 290568c..73121f8 100644 --- a/src/rove_description/urdf/gazebo.urdf.xacro +++ b/src/rove_description/urdf/gazebo.urdf.xacro @@ -29,6 +29,8 @@ cmd_vel odom + odom + base_link Date: Sun, 9 Jun 2024 19:27:19 -0400 Subject: [PATCH 02/13] add rviz 3d map --- src/rove_description/config/basic.rviz | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/rove_description/config/basic.rviz b/src/rove_description/config/basic.rviz index 0f8f1c7..df8d7c0 100644 --- a/src/rove_description/config/basic.rviz +++ b/src/rove_description/config/basic.rviz @@ -5,6 +5,7 @@ Panels: Property Tree Widget: Expanded: - /Global Options1 + - /3d map1 Splitter Ratio: 0.5 Tree Height: 517 - Class: rviz_common/Selection @@ -390,7 +391,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /assembled_cloud + Value: /cloud_map Use Fixed Frame: true Use rainbow: true Value: true @@ -440,25 +441,25 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 19.558191299438477 + Distance: 10.68095874786377 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -0.7862069010734558 - Y: 0.829606294631958 - Z: -1.3278541564941406 + X: -1.4718998670578003 + Y: 2.8054065704345703 + Z: 2.2840819358825684 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.7503987550735474 + Pitch: 0.6003987789154053 Target Frame: Value: Orbit (rviz) - Yaw: 4.633583068847656 + Yaw: 3.7735798358917236 Saved: ~ Window Geometry: Displays: From d00598d72c8d9beba0911f03a0e95eccdb7b0d23 Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Sun, 9 Jun 2024 22:57:29 -0400 Subject: [PATCH 03/13] slam 3d basic real hardware --- src/rove_bringup/launch/autonomy.launch.py | 6 +++--- src/rove_slam/launch/slam.launch.py | 2 +- src/rove_slam/launch/slam3d.launch.py | 12 ++---------- 3 files changed, 6 insertions(+), 14 deletions(-) diff --git a/src/rove_bringup/launch/autonomy.launch.py b/src/rove_bringup/launch/autonomy.launch.py index bf0132b..64c1db7 100644 --- a/src/rove_bringup/launch/autonomy.launch.py +++ b/src/rove_bringup/launch/autonomy.launch.py @@ -6,7 +6,7 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration -from launch.conditions import IfCondition +from launch.conditions import IfCondition, UnlessCondition def generate_launch_description(): @@ -29,7 +29,7 @@ def generate_launch_description(): pkg_rove_slam, "config", "slam_config.yaml" ) }.items(), - condition=IfCondition(use_slam3d) + condition=UnlessCondition(use_slam3d) ) slam3d = IncludeLaunchDescription( @@ -53,7 +53,7 @@ def generate_launch_description(): ) return LaunchDescription([ - #slam, + slam, slam3d, nav, ]) diff --git a/src/rove_slam/launch/slam.launch.py b/src/rove_slam/launch/slam.launch.py index 569ad47..0f0b729 100644 --- a/src/rove_slam/launch/slam.launch.py +++ b/src/rove_slam/launch/slam.launch.py @@ -9,7 +9,7 @@ def generate_launch_description(): pkg_rove_slam = get_package_share_directory('rove_slam') config_file = os.path.join(pkg_rove_slam, 'config', - 'mapper_params_online_async.yaml') + 'slam_config.yaml') return LaunchDescription([ Node( diff --git a/src/rove_slam/launch/slam3d.launch.py b/src/rove_slam/launch/slam3d.launch.py index 8b22f22..00710e6 100644 --- a/src/rove_slam/launch/slam3d.launch.py +++ b/src/rove_slam/launch/slam3d.launch.py @@ -8,20 +8,15 @@ def generate_launch_description(): camera_model = 'zed2i' lidar_frame_id = LaunchConfiguration('lidar_frame_id') - use_sim_time = LaunchConfiguration('use_sim_time') + deskewing = False return LaunchDescription([ - - DeclareLaunchArgument( - 'lidar_frame_id', default_value='velodyne_laser', - description='Lidar frame id'), - # Launch arguments DeclareLaunchArgument( - 'use_sim_time', default_value='true', + 'use_sim_time', default_value='false', description='Use simulation (Gazebo) clock if true'), DeclareLaunchArgument( @@ -32,7 +27,6 @@ def generate_launch_description(): Node( package='rtabmap_odom', executable='icp_odometry', output='screen', parameters=[{ - 'frame_id': lidar_frame_id, # 'livox_frame' 'odom_frame_id': 'odom', 'wait_for_transform': 0.2, 'expected_update_rate': 50.0, @@ -73,7 +67,6 @@ def generate_launch_description(): Node( package='rtabmap_slam', executable='rtabmap', output='screen', parameters=[{ - 'frame_id': lidar_frame_id, # 'livox_frame' 'subscribe_depth': False, 'subscribe_rgb': False, 'subscribe_scan_cloud': True, @@ -118,7 +111,6 @@ def generate_launch_description(): Node( package='rtabmap_viz', executable='rtabmap_viz', output='screen', parameters=[{ - 'frame_id': lidar_frame_id, 'odom_frame_id': 'odom', 'subscribe_odom_info': True, 'subscribe_scan_cloud': True, From f0eb72626fc5533328735d4d954e58f932de87e6 Mon Sep 17 00:00:00 2001 From: samb271 Date: Tue, 11 Jun 2024 22:47:52 -0400 Subject: [PATCH 04/13] added rtabmap launchfile (doesnt work) --- .gitignore | 3 ++ src/rove_bringup/launch/test.launch.py | 4 +- .../launch/{rtabmap.py => rtabmap.launch.py} | 48 +++++++------------ src/rove_rtabmap/package.xml | 33 +++++++++++++ src/rove_rtabmap/resource/rove_rtabmap | 0 src/rove_rtabmap/setup.cfg | 4 ++ src/rove_rtabmap/setup.py | 29 +++++++++++ src/rove_zed/config/zed_mapping.yaml | 11 +++-- src/rove_zed/launch/zed_mapping.launch.py | 3 +- src/rove_zed/readme.md | 9 +++- 10 files changed, 102 insertions(+), 42 deletions(-) rename src/rove_rtabmap/launch/{rtabmap.py => rtabmap.launch.py} (55%) create mode 100644 src/rove_rtabmap/package.xml create mode 100644 src/rove_rtabmap/resource/rove_rtabmap create mode 100644 src/rove_rtabmap/setup.cfg create mode 100644 src/rove_rtabmap/setup.py diff --git a/.gitignore b/.gitignore index bd36e05..0aa2664 100644 --- a/.gitignore +++ b/.gitignore @@ -17,3 +17,6 @@ __pycache__/ # Ignore files related to your dev environment .vscode/ + +# generated map database by rtabmap +*.db \ No newline at end of file diff --git a/src/rove_bringup/launch/test.launch.py b/src/rove_bringup/launch/test.launch.py index 736754f..7c05d0e 100644 --- a/src/rove_bringup/launch/test.launch.py +++ b/src/rove_bringup/launch/test.launch.py @@ -101,8 +101,8 @@ def generate_launch_description(): # robot_state_publisher, robot_localization_node_local, robot_localization_node_global, - navsat_transform, + # navsat_transform, vectornav, - velodyne, + #velodyne, rviz, ]) diff --git a/src/rove_rtabmap/launch/rtabmap.py b/src/rove_rtabmap/launch/rtabmap.launch.py similarity index 55% rename from src/rove_rtabmap/launch/rtabmap.py rename to src/rove_rtabmap/launch/rtabmap.launch.py index bcdb9a9..dfbda4d 100644 --- a/src/rove_rtabmap/launch/rtabmap.py +++ b/src/rove_rtabmap/launch/rtabmap.launch.py @@ -7,60 +7,44 @@ # # $ ros2 launch rtabmap_examples realsense_d435i_stereo.launch.py +from sympy import false 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" \ -# 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 - - 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)] + "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, + "delete_db_on_start" : True, + }] return LaunchDescription([ # Nodes to launch Node( package='rtabmap_odom', executable='stereo_odometry', output='screen', - parameters=parameters, - remappings=remappings), + parameters=parameters), 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), + parameters=parameters), # Compute quaternion of the IMU Node( diff --git a/src/rove_rtabmap/package.xml b/src/rove_rtabmap/package.xml new file mode 100644 index 0000000..671fbef --- /dev/null +++ b/src/rove_rtabmap/package.xml @@ -0,0 +1,33 @@ + + + + rove_rtabmap + 0.0.0 + Rtabmap package for rove + rove + MIT + + + rtabmap_ros + + 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 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/rove_rtabmap/resource/rove_rtabmap b/src/rove_rtabmap/resource/rove_rtabmap new file mode 100644 index 0000000..e69de29 diff --git a/src/rove_rtabmap/setup.cfg b/src/rove_rtabmap/setup.cfg new file mode 100644 index 0000000..c9ff06a --- /dev/null +++ b/src/rove_rtabmap/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/rove_rtabmap +[install] +install_scripts=$base/lib/rove_rtabmap diff --git a/src/rove_rtabmap/setup.py b/src/rove_rtabmap/setup.py new file mode 100644 index 0000000..37c0e51 --- /dev/null +++ b/src/rove_rtabmap/setup.py @@ -0,0 +1,29 @@ +from setuptools import find_packages, setup +import os +from glob import glob + +package_name = 'rove_rtabmap' + +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@ens.etsmtl.ca', + description='Rtabmap package for rove', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + ], + }, +) diff --git a/src/rove_zed/config/zed_mapping.yaml b/src/rove_zed/config/zed_mapping.yaml index 10ef04d..d197d29 100644 --- a/src/rove_zed/config/zed_mapping.yaml +++ b/src/rove_zed/config/zed_mapping.yaml @@ -2,9 +2,7 @@ /**: 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' + grab_resolution: 'HD720' # 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: @@ -16,10 +14,13 @@ 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: "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_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: 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 + remove_saturated_areas: true # [DYNAMIC] + + min_depth: 0.2 # Min: 0.2, Max: 3.0 + max_depth: 10.0 # Max: 40.0 \ 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 27fd6e9..557c4ff 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 = 'zedm' + 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') @@ -22,7 +22,6 @@ def generate_launch_description(): 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] ] ) diff --git a/src/rove_zed/readme.md b/src/rove_zed/readme.md index d4333b7..9baa516 100644 --- a/src/rove_zed/readme.md +++ b/src/rove_zed/readme.md @@ -8,7 +8,7 @@ ## Pour démarrer le mapping rtabmap avec la zed: ```bash -ros2 launch zed_wrapper zed_camera.launch.py camera_model:=zedm +ros2 launch rove_zed zed_mapping.launch.py 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" \ @@ -32,4 +32,11 @@ ros2 launch rove_zed body_trck.launch.py ## Pour démarrer le lidar: ```bash ros2 launch rove_bringup test.launch.py +``` + +## Si le launch file de la zed ne fonctionne pas: + +Appeler zed_wrapper directement avec: +```bash +ros2 launch zed_wrapper zed_camera.launch.py camera_model:=zed2i ``` \ No newline at end of file From c4ab9d982f363ce861beeba28ecec2cd0080b635 Mon Sep 17 00:00:00 2001 From: samb271 Date: Thu, 13 Jun 2024 03:04:09 -0400 Subject: [PATCH 05/13] rtabmap launch file --- src/rove_rtabmap/launch/rtabmap.launch.py | 81 ++++++++--------------- src/rove_zed/launch/zed_mapping.launch.py | 1 - 2 files changed, 28 insertions(+), 54 deletions(-) diff --git a/src/rove_rtabmap/launch/rtabmap.launch.py b/src/rove_rtabmap/launch/rtabmap.launch.py index dfbda4d..6618925 100644 --- a/src/rove_rtabmap/launch/rtabmap.launch.py +++ b/src/rove_rtabmap/launch/rtabmap.launch.py @@ -1,61 +1,36 @@ -# 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 - +import os from sympy import false +from ament_index_python import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable -from launch.substitutions import LaunchConfiguration +from launch.actions import IncludeLaunchDescription from launch_ros.actions import Node def generate_launch_description(): - parameters=[{ - "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, - "delete_db_on_start" : True, - }] - - return LaunchDescription([ + parameters=[ + ['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' ], + ['odom_frame_id', '/zed/zed_node/odom'], + ['imu_topic', '/zed/zed_node/imu/data' ], + ['visual_odometry', 'false' ], + ['frame_id', 'zed_camera_link' ], + ['approx_sync', 'true' ], + ['rgbd_sync', 'true' ], + ['approx_rgbd_sync', 'false'], + ['delete_db_on_start', 'true'], + ['subscribe_scan', 'true'], + ['scan_topic', '/scan'], + ] - # Nodes to launch - Node( - package='rtabmap_odom', executable='stereo_odometry', output='screen', - parameters=parameters), + pkg_rtabmap = get_package_share_directory('rtabmap_launch') + launch_file_path = os.path.join(pkg_rtabmap, 'launch', 'rtabmap.launch.py') - Node( - package='rtabmap_slam', executable='rtabmap', output='screen', - parameters=parameters, - arguments=['-d']), - - Node( - package='rtabmap_viz', executable='rtabmap_viz', output='screen', - parameters=parameters), - - # 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 + return LaunchDescription([ + IncludeLaunchDescription( + launch_description_source=launch_file_path, + launch_arguments=parameters, + ) + ]) diff --git a/src/rove_zed/launch/zed_mapping.launch.py b/src/rove_zed/launch/zed_mapping.launch.py index 557c4ff..cb78318 100644 --- a/src/rove_zed/launch/zed_mapping.launch.py +++ b/src/rove_zed/launch/zed_mapping.launch.py @@ -13,7 +13,6 @@ 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_override_path = os.path.join(pkg_rove_zed, 'config', 'zed_mapping.yaml') From 28c16b605f22e132fb93984508d15b031afec731 Mon Sep 17 00:00:00 2001 From: samb271 Date: Thu, 13 Jun 2024 03:56:42 -0400 Subject: [PATCH 06/13] fix launch options --- src/rove_rtabmap/launch/rtabmap.launch.py | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/src/rove_rtabmap/launch/rtabmap.launch.py b/src/rove_rtabmap/launch/rtabmap.launch.py index 6618925..54b934a 100644 --- a/src/rove_rtabmap/launch/rtabmap.launch.py +++ b/src/rove_rtabmap/launch/rtabmap.launch.py @@ -13,16 +13,27 @@ def generate_launch_description(): ['depth_topic', '/zed/zed_node/depth/depth_registered' ], ['camera_info_topic', '/zed/zed_node/depth/camera_info'], ['odom_topic', '/zed/zed_node/odom' ], - ['odom_frame_id', '/zed/zed_node/odom'], + # ['odom_frame_id', '/zed/zed_node/odom'], ['imu_topic', '/zed/zed_node/imu/data' ], ['visual_odometry', 'false' ], ['frame_id', 'zed_camera_link' ], - ['approx_sync', 'true' ], + ['approx_sync', 'false' ], ['rgbd_sync', 'true' ], ['approx_rgbd_sync', 'false'], ['delete_db_on_start', 'true'], - ['subscribe_scan', 'true'], - ['scan_topic', '/scan'], + # ['subscribe_scan_cloud', 'true'], + # ['scan_cloud_topic', '/velodyne_points'], + # ['RGBD/NeighborLinkRefining', 'true'], + # ['RGBD/ProximityBySpace', 'true'], + # ['RGBD/AngularUpdate', '0.01'], + # ['RGBD/LinearUpdate', '0.01'], + # ['RGBD/OptimizeFromGraphEnd', 'false'], + # ['Grid/FromDepth', 'false'], + # ['Reg/Force3DoF', 'true'], + # ['Reg/Strategy', '1'], + # ['Icp/VoxelSize', '0.05'], + # ['Icp/MaxCorrespondenceDistance', '0.1'], + # ['Optimizer/Slam2D', 'true'], ] pkg_rtabmap = get_package_share_directory('rtabmap_launch') From d57ad0edbc2a77d941f50920d66639d36ed4be2e Mon Sep 17 00:00:00 2001 From: samb271 Date: Thu, 13 Jun 2024 04:04:57 -0400 Subject: [PATCH 07/13] added doc --- src/rove_rtabmap/launch/rtabmap.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/rove_rtabmap/launch/rtabmap.launch.py b/src/rove_rtabmap/launch/rtabmap.launch.py index 54b934a..2d82ede 100644 --- a/src/rove_rtabmap/launch/rtabmap.launch.py +++ b/src/rove_rtabmap/launch/rtabmap.launch.py @@ -21,7 +21,7 @@ def generate_launch_description(): ['rgbd_sync', 'true' ], ['approx_rgbd_sync', 'false'], ['delete_db_on_start', 'true'], - # ['subscribe_scan_cloud', 'true'], + # ['subscribe_scan_cloud', 'true'], https://wiki.ros.org/rtabmap_ros/Tutorials/SetupOnYourRobot#Kinect_.2B-_Odometry_.2B-_2D_laser # ['scan_cloud_topic', '/velodyne_points'], # ['RGBD/NeighborLinkRefining', 'true'], # ['RGBD/ProximityBySpace', 'true'], From db1f82ef3432af2a5349651ae6411893ccbb239a Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Fri, 14 Jun 2024 20:07:08 -0400 Subject: [PATCH 08/13] rtabmap good 3d map --- src/rove_description/config/basic.rviz | 66 ++--- src/rove_rtabmap/package.xml | 33 --- src/rove_rtabmap/resource/rove_rtabmap | 0 src/rove_rtabmap/setup.cfg | 4 - src/rove_rtabmap/setup.py | 29 --- src/rove_slam/config/ekf.yaml | 9 + .../launch/rtabmap.launch.py | 0 src/rove_slam/launch/slam3d.launch.py | 235 +++++++++--------- 8 files changed, 155 insertions(+), 221 deletions(-) delete mode 100644 src/rove_rtabmap/package.xml delete mode 100644 src/rove_rtabmap/resource/rove_rtabmap delete mode 100644 src/rove_rtabmap/setup.cfg delete mode 100644 src/rove_rtabmap/setup.py rename src/{rove_rtabmap => rove_slam}/launch/rtabmap.launch.py (100%) diff --git a/src/rove_description/config/basic.rviz b/src/rove_description/config/basic.rviz index df8d7c0..fa2d445 100644 --- a/src/rove_description/config/basic.rviz +++ b/src/rove_description/config/basic.rviz @@ -154,9 +154,9 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 0 + Max Intensity: 100 Min Color: 0; 0; 0 - Min Intensity: 0 + Min Intensity: 1 Name: LaserScan Position Transformer: XYZ Selectable: true @@ -180,16 +180,6 @@ Visualization Manager: All Enabled: false base_link: Value: true - flipper_fl: - Value: true - flipper_fr: - Value: true - flipper_rl: - Value: true - flipper_rr: - Value: true - map: - Value: true odom: Value: true sensor_link: @@ -216,33 +206,23 @@ Visualization Manager: Show Axes: true Show Names: false Tree: - map: - odom: - base_link: - flipper_fl: - {} - flipper_fr: + odom: + base_link: + sensor_link: + {} + track_l: + track_fl: {} - flipper_rl: + track_rl: {} - flipper_rr: + track_r: + track_fr: {} - sensor_link: + track_rr: {} - track_l: - track_fl: - {} - track_rl: - {} - track_r: - track_fr: - {} - track_rr: - {} - velodyne_link: + velodyne_link: + velodyne_laser: {} - velodyne_laser: - {} Update Interval: 0 Value: true - Alpha: 0.699999988079071 @@ -331,7 +311,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /odometry/filtered + Value: /odometry/local Value: true - Acceleration properties: Acc. vector alpha: 1 @@ -376,7 +356,7 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 0 + Max Intensity: 255 Min Color: 0; 0; 0 Min Intensity: 0 Name: 3d map @@ -391,7 +371,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /cloud_map + Value: /assembled_cloud Use Fixed Frame: true Use rainbow: true Value: true @@ -441,25 +421,25 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 10.68095874786377 + Distance: 9.305854797363281 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -1.4718998670578003 - Y: 2.8054065704345703 - Z: 2.2840819358825684 + X: -0.6736313104629517 + Y: -1.0152995586395264 + Z: 2.0505826473236084 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.6003987789154053 + Pitch: 0.600398600101471 Target Frame: Value: Orbit (rviz) - Yaw: 3.7735798358917236 + Yaw: 1.893579363822937 Saved: ~ Window Geometry: Displays: diff --git a/src/rove_rtabmap/package.xml b/src/rove_rtabmap/package.xml deleted file mode 100644 index 671fbef..0000000 --- a/src/rove_rtabmap/package.xml +++ /dev/null @@ -1,33 +0,0 @@ - - - - rove_rtabmap - 0.0.0 - Rtabmap package for rove - rove - MIT - - - rtabmap_ros - - 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 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/src/rove_rtabmap/resource/rove_rtabmap b/src/rove_rtabmap/resource/rove_rtabmap deleted file mode 100644 index e69de29..0000000 diff --git a/src/rove_rtabmap/setup.cfg b/src/rove_rtabmap/setup.cfg deleted file mode 100644 index c9ff06a..0000000 --- a/src/rove_rtabmap/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/rove_rtabmap -[install] -install_scripts=$base/lib/rove_rtabmap diff --git a/src/rove_rtabmap/setup.py b/src/rove_rtabmap/setup.py deleted file mode 100644 index 37c0e51..0000000 --- a/src/rove_rtabmap/setup.py +++ /dev/null @@ -1,29 +0,0 @@ -from setuptools import find_packages, setup -import os -from glob import glob - -package_name = 'rove_rtabmap' - -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@ens.etsmtl.ca', - description='Rtabmap package for rove', - license='MIT', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - ], - }, -) diff --git a/src/rove_slam/config/ekf.yaml b/src/rove_slam/config/ekf.yaml index 93e59ca..56c3cc8 100644 --- a/src/rove_slam/config/ekf.yaml +++ b/src/rove_slam/config/ekf.yaml @@ -24,6 +24,15 @@ ekf_filter_node_local: false, false, false] # ax ay az imu0_queue_size: 20 + # Add ICP odometry as a new sensor input + odom1: /icp_odom + odom1_config: [true, true, false, + false, false, true, + true, true, false, + false, false, true, + false, false, false] + + ekf_filter_node_global: ros__parameters: frequency: 30.0 diff --git a/src/rove_rtabmap/launch/rtabmap.launch.py b/src/rove_slam/launch/rtabmap.launch.py similarity index 100% rename from src/rove_rtabmap/launch/rtabmap.launch.py rename to src/rove_slam/launch/rtabmap.launch.py diff --git a/src/rove_slam/launch/slam3d.launch.py b/src/rove_slam/launch/slam3d.launch.py index 00710e6..b175edd 100644 --- a/src/rove_slam/launch/slam3d.launch.py +++ b/src/rove_slam/launch/slam3d.launch.py @@ -2,123 +2,134 @@ from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node +import os def generate_launch_description(): - camera_model = 'zed2i' - - lidar_frame_id = LaunchConfiguration('lidar_frame_id') + # Launch configuration use_sim_time = LaunchConfiguration('use_sim_time') - deskewing = False - - return LaunchDescription([ - # Launch arguments - DeclareLaunchArgument( - 'use_sim_time', default_value='false', - description='Use simulation (Gazebo) clock if true'), - - DeclareLaunchArgument( - 'deskewing', default_value='true', - description='Enable lidar deskewing'), - - # Nodes to launch - Node( - package='rtabmap_odom', executable='icp_odometry', output='screen', - parameters=[{ - 'odom_frame_id': 'odom', - 'wait_for_transform': 0.2, - 'expected_update_rate': 50.0, - 'deskewing': deskewing, - 'use_sim_time': use_sim_time, - }], - remappings=[ - ('scan_cloud', '/velodyne_points') - ], - arguments=[ - 'Icp/PointToPlane', 'true', - 'Icp/Iterations', '10', - 'Icp/VoxelSize', '0.1', - 'Icp/Epsilon', '0.001', - 'Icp/PointToPlaneK', '20', - 'Icp/PointToPlaneRadius', '0', - 'Icp/MaxTranslation', '2', - 'Icp/MaxCorrespondenceDistance', '1', - 'Icp/Strategy', '1', - 'Icp/OutlierRatio', '0.7', - 'Icp/CorrespondenceRatio', '0.01', - 'Odom/ScanKeyFrameThr', '0.4', - 'OdomF2M/ScanSubtractRadius', '0.1', - 'OdomF2M/ScanMaxSize', '15000', - 'OdomF2M/BundleAdjustment', 'false', - ]), - - Node( - package='rtabmap_util', executable='point_cloud_assembler', output='screen', - parameters=[{ - 'max_clouds': 10, - 'use_sim_time': use_sim_time, - }], - remappings=[ - ('cloud', '/velodyne_points') - ]), - - Node( - package='rtabmap_slam', executable='rtabmap', output='screen', - parameters=[{ - 'subscribe_depth': False, - 'subscribe_rgb': False, - 'subscribe_scan_cloud': True, - 'approx_sync': False, - 'wait_for_transform': 0.2, - 'use_sim_time': use_sim_time, - 'wait_imu_to_int': True, - 'imu_topic': 'imu', - }], - remappings=[ - ('scan_cloud', 'assembled_cloud') - ], - arguments=[ - '-d', # This will delete the previous database (~/.ros/rtabmap.db) - 'RGBD/ProximityMaxGraphDepth', '0', - 'RGBD/ProximityPathMaxNeighbors', '1', - 'RGBD/AngularUpdate', '0.05', - 'RGBD/LinearUpdate', '0.05', - 'RGBD/CreateOccupancyGrid', 'true', - 'Mem/NotLinkedNodesKept', 'false', - 'Mem/STMSize', '30', - 'Mem/LaserScanNormalK', '20', - 'Reg/Strategy', '1', - 'Icp/VoxelSize', '0.5', - 'Icp/PointToPlaneK', '20', - 'Icp/PointToPlaneRadius', '0', - 'Icp/PointToPlane', 'true', - 'Icp/Iterations', '10', - 'Icp/Epsilon', '0.001', - 'Icp/MaxTranslation', '3', - 'Icp/MaxCorrespondenceDistance', '1', - 'Icp/Strategy', '1', - 'Icp/OutlierRatio', '0.7', - 'Icp/CorrespondenceRatio', '0.2', - 'Grid/MaxGroundHeight', '0.05', - 'Grid/MaxObstacleHeight', '1.5', - 'Reg/Force3DoF', 'true', - 'Optimizer/Slam2D', 'true', + # Nodes + icp_odometry_node = Node( + package='rtabmap_odom', + executable='icp_odometry', + name='icp_odometry', + output='screen', + parameters=[{ + 'odom_frame_id': '/odom', + 'wait_for_transform': 0.2, + 'publish_tf': False, + 'expected_update_rate': 50.0, + 'deskewing': deskewing, + 'use_sim_time': use_sim_time, + }], + remappings=[ + ('scan_cloud', '/velodyne_points'), + ('odom', '/icp_odom') + ], + arguments=[ + 'Icp/PointToPlane', 'true', + 'Icp/Iterations', '10', + 'Icp/VoxelSize', '0.1', + 'Icp/Epsilon', '0.001', + 'Icp/PointToPlaneK', '20', + 'Icp/PointToPlaneRadius', '0', + 'Icp/MaxTranslation', '2', + 'Icp/MaxCorrespondenceDistance', '1', + 'Icp/Strategy', '1', + 'Icp/OutlierRatio', '0.7', + 'Icp/CorrespondenceRatio', '0.01', + 'Odom/ScanKeyFrameThr', '0.4', + 'OdomF2M/ScanSubtractRadius', '0.1', + 'OdomF2M/ScanMaxSize', '15000', + 'OdomF2M/BundleAdjustment', 'false', + ] + ) + + point_cloud_assembler_node = Node( + package='rtabmap_util', + executable='point_cloud_assembler', + name='point_cloud_assembler', + output='screen', + parameters=[{ + 'max_clouds': 10, + 'use_sim_time': use_sim_time, + }], + remappings=[ + ('cloud', '/velodyne_points'), + ('odom', '/odometry/local') + ] + ) + + rtabmap_node = Node( + package='rtabmap_slam', + executable='rtabmap', + name='rtabmap', + output='screen', + parameters=[{ + 'subscribe_depth': False, + 'subscribe_rgb': False, + 'subscribe_scan_cloud': True, + 'approx_sync': True, + 'wait_for_transform': 0.2, + 'use_sim_time': use_sim_time, + }], + remappings=[ + ('scan_cloud', '/assembled_cloud'), + ('odom', '/odometry/local') - ]), - - Node( - package='rtabmap_viz', executable='rtabmap_viz', output='screen', - parameters=[{ - 'odom_frame_id': 'odom', - 'subscribe_odom_info': True, - 'subscribe_scan_cloud': True, - 'approx_sync': False, - 'use_sim_time': use_sim_time, - }], - remappings=[ - ('scan_cloud', 'odom_filtered_input_scan') - ]), + ], + arguments=[ + '-d', # This will delete the previous database (~/.ros/rtabmap.db) + 'RGBD/ProximityMaxGraphDepth', '0', + 'RGBD/ProximityPathMaxNeighbors', '1', + 'RGBD/AngularUpdate', '0.05', + 'RGBD/LinearUpdate', '0.05', + 'RGBD/CreateOccupancyGrid', 'true', + 'Mem/NotLinkedNodesKept', 'false', + 'Mem/STMSize', '30', + 'Mem/LaserScanNormalK', '20', + 'Reg/Strategy', '1', + 'Icp/VoxelSize', '0.5', + 'Icp/PointToPlaneK', '20', + 'Icp/PointToPlaneRadius', '0', + 'Icp/PointToPlane', 'true', + 'Icp/Iterations', '10', + 'Icp/Epsilon', '0.001', + 'Icp/MaxTranslation', '3', + 'Icp/MaxCorrespondenceDistance', '1', + 'Icp/Strategy', '1', + 'Icp/OutlierRatio', '0.7', + 'Icp/CorrespondenceRatio', '0.2', + 'Grid/MaxGroundHeight', '0.05', + 'Grid/MaxObstacleHeight', '1.5', + 'Reg/Force3DoF', 'true', + 'Optimizer/Slam2D', 'true', + ] + ) + + rtabmap_viz_node = Node( + package='rtabmap_viz', + executable='rtabmap_viz', + name='rtabmap_viz', + output='screen', + parameters=[{ + 'odom_frame_id': 'odom', + 'subscribe_odom_info': True, + 'subscribe_scan_cloud': True, + 'approx_sync': True, + 'use_sim_time': use_sim_time, + }], + remappings=[ + ('scan_cloud', '/assembled_cloud'), + ('odom', '/odometry/local') + ] + ) + + return LaunchDescription([ + icp_odometry_node, + point_cloud_assembler_node, + rtabmap_node, + #rtabmap_viz_node ]) - \ No newline at end of file From 62006f6d6d5047fd30006179bd29f98e75fe01e5 Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Sat, 15 Jun 2024 12:06:00 -0400 Subject: [PATCH 09/13] faster map computation + better grid --- src/rove_slam/launch/slam3d.launch.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/rove_slam/launch/slam3d.launch.py b/src/rove_slam/launch/slam3d.launch.py index b175edd..618de2f 100644 --- a/src/rove_slam/launch/slam3d.launch.py +++ b/src/rove_slam/launch/slam3d.launch.py @@ -102,8 +102,10 @@ def generate_launch_description(): 'Icp/Strategy', '1', 'Icp/OutlierRatio', '0.7', 'Icp/CorrespondenceRatio', '0.2', - 'Grid/MaxGroundHeight', '0.05', + 'Grid/MaxGroundHeight', '0.1', 'Grid/MaxObstacleHeight', '1.5', + 'Grid/RayTracing', 'true', + 'Grid/3D', 'false', 'Reg/Force3DoF', 'true', 'Optimizer/Slam2D', 'true', ] From a601ed1e64338b84f4a93737d2f5c37749c4ae10 Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Sat, 15 Jun 2024 20:06:49 -0400 Subject: [PATCH 10/13] add more config example --- src/rove_description/config/basic.rviz | 21 +-- src/rove_slam/launch/lidar_zed.launch.py | 164 +++++++++++++++++++++++ src/rove_slam/launch/slam3d.launch.py | 19 +-- src/rove_slam/launch/zed.launch.py | 69 ++++++++++ 4 files changed, 236 insertions(+), 37 deletions(-) create mode 100644 src/rove_slam/launch/lidar_zed.launch.py create mode 100644 src/rove_slam/launch/zed.launch.py diff --git a/src/rove_description/config/basic.rviz b/src/rove_description/config/basic.rviz index fa2d445..0fac324 100644 --- a/src/rove_description/config/basic.rviz +++ b/src/rove_description/config/basic.rviz @@ -5,7 +5,6 @@ Panels: Property Tree Widget: Expanded: - /Global Options1 - - /3d map1 Splitter Ratio: 0.5 Tree Height: 517 - Class: rviz_common/Selection @@ -25,7 +24,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: LaserScan + SyncSource: "" Visualization Manager: Class: "" Displays: @@ -206,23 +205,7 @@ Visualization Manager: Show Axes: true Show Names: false Tree: - odom: - base_link: - sensor_link: - {} - track_l: - track_fl: - {} - track_rl: - {} - track_r: - track_fr: - {} - track_rr: - {} - velodyne_link: - velodyne_laser: - {} + {} Update Interval: 0 Value: true - Alpha: 0.699999988079071 diff --git a/src/rove_slam/launch/lidar_zed.launch.py b/src/rove_slam/launch/lidar_zed.launch.py new file mode 100644 index 0000000..86a5579 --- /dev/null +++ b/src/rove_slam/launch/lidar_zed.launch.py @@ -0,0 +1,164 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + + pkg_rove_zed = get_package_share_directory('rove_zed') + + # Launch configuration + use_sim_time = LaunchConfiguration('use_sim_time') + deskewing = False + + zed = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_rove_zed, 'launch', 'zed_mapping.launch.py'), + ) + ) + + # Nodes + icp_odometry_node = Node( + package='rtabmap_odom', + executable='icp_odometry', + name='icp_odometry', + output='screen', + parameters=[{ + 'odom_frame_id': '/odom', + 'wait_for_transform': 0.2, + 'publish_tf': False, + 'expected_update_rate': 50.0, + 'deskewing': deskewing, + 'use_sim_time': use_sim_time, + }], + remappings=[ + ('scan_cloud', '/velodyne_points'), + ('odom', '/icp_odom') + ], + arguments=[ + 'Icp/PointToPlane', 'true', + 'Icp/Iterations', '10', + 'Icp/VoxelSize', '0.1', + 'Icp/Epsilon', '0.001', + 'Icp/PointToPlaneK', '20', + 'Icp/PointToPlaneRadius', '0', + 'Icp/MaxTranslation', '2', + 'Icp/MaxCorrespondenceDistance', '1', + 'Icp/Strategy', '1', + 'Icp/OutlierRatio', '0.7', + 'Icp/CorrespondenceRatio', '0.01', + 'Odom/ScanKeyFrameThr', '0.4', + 'OdomF2M/ScanSubtractRadius', '0.1', + 'OdomF2M/ScanMaxSize', '15000', + 'OdomF2M/BundleAdjustment', 'false', + ] + ) + + point_cloud_assembler_node = Node( + package='rtabmap_util', + executable='point_cloud_assembler', + name='point_cloud_assembler', + output='screen', + parameters=[{ + 'max_clouds': 10, + 'use_sim_time': use_sim_time, + }], + remappings=[ + ('cloud', '/velodyne_points'), + ('odom', '/odometry/local') + ] + ) + + camera_sync = Node( + package='rtabmap_sync', executable='rgbd_sync', output='screen', + parameters=[{ + 'approx_sync':True + }], + remappings=[ + ('/depth/image', '/zed/zed_node/depth/depth_registered'), + ('/rgb/camera_info' , '/zed/zed_node/depth/camera_info'), + ('/rgb/image', '/zed/zed_node/rgb/image_rect_color') + ]) + + + rtabmap_node = Node( + package='rtabmap_slam', + executable='rtabmap', + name='rtabmap', + output='screen', + parameters=[{ + 'subscribe_depth': False, + 'subscribe_rgb': False, + 'subscribe_scan_cloud': True, + 'approx_sync': True, + 'wait_for_transform': 0.2, + 'use_sim_time': use_sim_time, + }], + remappings=[ + ('scan_cloud', '/assembled_cloud'), + ('odom', '/odometry/local') + + ], + arguments=[ + '-d', # This will delete the previous database (~/.ros/rtabmap.db) + 'RGBD/ProximityMaxGraphDepth', '0', + 'RGBD/ProximityPathMaxNeighbors', '1', + 'RGBD/AngularUpdate', '0.05', + 'RGBD/LinearUpdate', '0.05', + 'RGBD/CreateOccupancyGrid', 'true', + 'Mem/NotLinkedNodesKept', 'false', + 'Mem/STMSize', '30', + 'Mem/LaserScanNormalK', '20', + 'Reg/Strategy', '1', + 'Icp/VoxelSize', '0.5', + 'Icp/PointToPlaneK', '20', + 'Icp/PointToPlaneRadius', '0', + 'Icp/PointToPlane', 'true', + 'Icp/Iterations', '10', + 'Icp/Epsilon', '0.001', + 'Icp/MaxTranslation', '3', + 'Icp/MaxCorrespondenceDistance', '1', + 'Icp/Strategy', '1', + 'Icp/OutlierRatio', '0.7', + 'Icp/CorrespondenceRatio', '0.2', + 'Grid/MaxGroundHeight', '0.1', + 'Grid/MaxObstacleHeight', '1.5', + 'Grid/RayTracing', 'true', + 'Grid/3D', 'false', + 'Reg/Force3DoF', 'true', + 'Optimizer/Slam2D', 'true', + ] + ) + + rtabmap_viz_node = Node( + package='rtabmap_viz', + executable='rtabmap_viz', + name='rtabmap_viz', + output='screen', + parameters=[{ + 'odom_frame_id': 'odom', + 'subscribe_rgbd':True, + 'subscribe_odom_info': True, + 'subscribe_scan_cloud': True, + 'approx_sync': True, + 'use_sim_time': use_sim_time, + }], + remappings=[ + ('scan_cloud', '/assembled_cloud'), + ('odom', '/odometry/local') + ] + ) + + return LaunchDescription([ + icp_odometry_node, + point_cloud_assembler_node, + rtabmap_node, + rtabmap_viz_node, + zed, + camera_sync, + ]) diff --git a/src/rove_slam/launch/slam3d.launch.py b/src/rove_slam/launch/slam3d.launch.py index 618de2f..fad8ea8 100644 --- a/src/rove_slam/launch/slam3d.launch.py +++ b/src/rove_slam/launch/slam3d.launch.py @@ -28,23 +28,6 @@ def generate_launch_description(): ('scan_cloud', '/velodyne_points'), ('odom', '/icp_odom') ], - arguments=[ - 'Icp/PointToPlane', 'true', - 'Icp/Iterations', '10', - 'Icp/VoxelSize', '0.1', - 'Icp/Epsilon', '0.001', - 'Icp/PointToPlaneK', '20', - 'Icp/PointToPlaneRadius', '0', - 'Icp/MaxTranslation', '2', - 'Icp/MaxCorrespondenceDistance', '1', - 'Icp/Strategy', '1', - 'Icp/OutlierRatio', '0.7', - 'Icp/CorrespondenceRatio', '0.01', - 'Odom/ScanKeyFrameThr', '0.4', - 'OdomF2M/ScanSubtractRadius', '0.1', - 'OdomF2M/ScanMaxSize', '15000', - 'OdomF2M/BundleAdjustment', 'false', - ] ) point_cloud_assembler_node = Node( @@ -133,5 +116,5 @@ def generate_launch_description(): icp_odometry_node, point_cloud_assembler_node, rtabmap_node, - #rtabmap_viz_node + rtabmap_viz_node ]) diff --git a/src/rove_slam/launch/zed.launch.py b/src/rove_slam/launch/zed.launch.py new file mode 100644 index 0000000..3336dd2 --- /dev/null +++ b/src/rove_slam/launch/zed.launch.py @@ -0,0 +1,69 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + + pkg_rove_zed = get_package_share_directory('rove_zed') + + parameters=[{ + 'odom_frame_id': 'odom', + 'publish_tf': False, + 'frame_id':'zed_camera_link', + 'subscribe_depth': True, + 'subscribe_odom_info': True, + 'approx_sync': True, + 'wait_imu_to_init':True}] + + remappings=[ + ('imu', '/zed/zed_node/imu/data'), + ('rgb/image', '/zed/zed_node/rgb/image_rect_color'), + ('rgb/camera_info', '/zed/zed_node/depth/camera_info'), + ('depth/image', '/zed/zed_node/depth/depth_registered'), + ('odom', '/zed/zed_node/odom') + + ] + + + + zed = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_rove_zed, 'launch', 'zed_mapping.launch.py'), + ) + ) + + rtabmap_slam = Node( + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=parameters, + remappings=remappings, + arguments=['-d']) + + rtabmap_viz_node = Node( + package='rtabmap_viz', + executable='rtabmap_viz', + name='rtabmap_viz', + output='screen', + parameters=parameters, + remappings=remappings + ) + + + + odom = Node( + package='rtabmap_odom', executable='rgbd_odometry', output='screen', + parameters=parameters, + remappings=remappings) + + + + return LaunchDescription([ + odom, + zed, + rtabmap_slam, + rtabmap_viz_node, + ]) From ba7ed0e3fd8be9e0759d5e219438c41a8a5dbd23 Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Sun, 16 Jun 2024 00:24:22 -0400 Subject: [PATCH 11/13] add zed simulation --- src/rove_bringup/launch/real.launch.py | 8 + .../config/default_bridge.yaml | 21 ++ src/rove_description/urdf/sensor.urdf.xacro | 63 +++-- src/rove_description/worlds/base_world.world | 235 ++++++++++++------ src/rove_slam/launch/zed.launch.py | 23 +- src/rove_zed/config/zed_mapping.yaml | 6 +- 6 files changed, 242 insertions(+), 114 deletions(-) diff --git a/src/rove_bringup/launch/real.launch.py b/src/rove_bringup/launch/real.launch.py index 6b091e2..5b5eff6 100644 --- a/src/rove_bringup/launch/real.launch.py +++ b/src/rove_bringup/launch/real.launch.py @@ -14,6 +14,7 @@ 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_zed = get_package_share_directory('rove_zed') # Get the URDF file urdf_path = os.path.join(pkg_rove_description, 'urdf', 'rove.urdf.xacro') @@ -120,6 +121,12 @@ def create_controller_node(node_name:str): ), ) + zed = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_rove_zed, 'launch', 'zed_mapping.launch.py'), + ) + ) + return LaunchDescription([ control_node, common, @@ -127,4 +134,5 @@ def create_controller_node(node_name:str): *delayed_controller_nodes, vectornav, velodyne, + zed, ]) diff --git a/src/rove_description/config/default_bridge.yaml b/src/rove_description/config/default_bridge.yaml index 822cf55..29566e2 100644 --- a/src/rove_description/config/default_bridge.yaml +++ b/src/rove_description/config/default_bridge.yaml @@ -53,3 +53,24 @@ ros_type_name: "sensor_msgs/msg/NavSatFix" gz_type_name: "ignition.msgs.NavSat" direction: GZ_TO_ROS + +# gz topic published by camera (rgb) +- ros_topic_name: "/zed/zed_node/rgb/image_rect_color" + gz_topic_name: "/camera" + ros_type_name: "sensor_msgs/msg/Image" + gz_type_name: "gz.msgs.Image" + direction: GZ_TO_ROS + +# gz topic published by camera (depth) +- ros_topic_name: "/zed/zed_node/depth/depth_registered" + gz_topic_name: "/depth_camera" + ros_type_name: "sensor_msgs/msg/Image" + gz_type_name: "gz.msgs.Image" + direction: GZ_TO_ROS + +# gz topic published by camera (info) +- ros_topic_name: "/zed/zed_node/depth/camera_info" + gz_topic_name: "/camera_info" + ros_type_name: "sensor_msgs/msg/CameraInfo" + gz_type_name: "gz.msgs.CameraInfo" + direction: GZ_TO_ROS \ No newline at end of file diff --git a/src/rove_description/urdf/sensor.urdf.xacro b/src/rove_description/urdf/sensor.urdf.xacro index 4d17df4..3b8612b 100644 --- a/src/rove_description/urdf/sensor.urdf.xacro +++ b/src/rove_description/urdf/sensor.urdf.xacro @@ -2,52 +2,81 @@ - - - - + + + - - - - + + + - + - + - + - + - - + Gazebo/Black - sensor_link true 50 imu - sensor_link true 1 navsat + + sensor_link + 10 + depth_camera + + 1.88 + + 256 + 256 + R_FLOAT32 + + + 0.5 + 20.0 + + + + + sensor_link + 10 + camera + + 1.88 + + 256 + 256 + R_FLOAT32 + + + 0.5 + 20.0 + + + - + \ No newline at end of file diff --git a/src/rove_description/worlds/base_world.world b/src/rove_description/worlds/base_world.world index ae1681a..ac235e0 100644 --- a/src/rove_description/worlds/base_world.world +++ b/src/rove_description/worlds/base_world.world @@ -1,4 +1,4 @@ - + - - 2 18 15 0 0.75 -1.7 - + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + World control + false + false + 72 + 1 + + floating + + + + + + + + + + docked + + depth_camera + + + + docked + + camera + - + + - sensor_link + camera_link 10 camera diff --git a/src/rove_description/worlds/base_world.world b/src/rove_description/worlds/base_world.world index ac235e0..af8a24a 100644 --- a/src/rove_description/worlds/base_world.world +++ b/src/rove_description/worlds/base_world.world @@ -114,6 +114,13 @@ + + + 3.5 0 0 0 0 0 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Pine Tree + + + true 0 0 10 0 0 0 @@ -175,8 +182,9 @@ - - 1 1 1 1 + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 0 @@ -201,8 +209,9 @@ - - 1 1 1 1 + 1 1 0 1 + 1 1 0 1 + 1 1 0 1 0 @@ -253,8 +262,9 @@ - - 1 1 1 1 + 1 0 1 1 + 1 0 1 1 + 1 0 1 1 0 @@ -305,8 +315,9 @@ - - 1 1 1 1 + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 0 From d3c0aa681c0ac00d2cdbd1710939ae0d2fb7a7fd Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Mon, 17 Jun 2024 12:37:09 -0400 Subject: [PATCH 13/13] merge 3d lidar + 3d cam --- src/rove_bringup/launch/autonomy.launch.py | 2 +- src/rove_description/config/basic.rviz | 129 ++++++++++++-- src/rove_description/urdf/flipper.urdf.xacro | 81 ++++----- src/rove_description/urdf/rove.urdf.xacro | 2 +- src/rove_description/urdf/sensor.urdf.xacro | 12 +- src/rove_description/urdf/track.urdf.xacro | 2 +- src/rove_slam/README.md | 10 ++ src/rove_slam/launch/icp_zed.launch.py | 95 ++++++++++ src/rove_slam/launch/lidar_zed.launch.py | 164 ------------------ src/rove_slam/launch/rtabmap.launch.py | 47 ----- .../{slam.launch.py => slam2d.launch.py} | 0 src/rove_slam/launch/slam3d_full.launch.py | 125 +++++++++++++ ...slam3d.launch.py => velodyne_3d.launch.py} | 2 + src/rove_slam/launch/zed.launch.py | 62 ------- 14 files changed, 393 insertions(+), 340 deletions(-) create mode 100644 src/rove_slam/README.md create mode 100644 src/rove_slam/launch/icp_zed.launch.py delete mode 100644 src/rove_slam/launch/lidar_zed.launch.py delete mode 100644 src/rove_slam/launch/rtabmap.launch.py rename src/rove_slam/launch/{slam.launch.py => slam2d.launch.py} (100%) create mode 100644 src/rove_slam/launch/slam3d_full.launch.py rename src/rove_slam/launch/{slam3d.launch.py => velodyne_3d.launch.py} (99%) delete mode 100644 src/rove_slam/launch/zed.launch.py diff --git a/src/rove_bringup/launch/autonomy.launch.py b/src/rove_bringup/launch/autonomy.launch.py index 64c1db7..076b9d1 100644 --- a/src/rove_bringup/launch/autonomy.launch.py +++ b/src/rove_bringup/launch/autonomy.launch.py @@ -34,7 +34,7 @@ def generate_launch_description(): slam3d = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(pkg_rove_slam, "launch", "slam3d.launch.py"), + os.path.join(pkg_rove_slam, "launch", "slam3d_full.launch.py"), ), launch_arguments={ "use_sim_time": use_sim_time, diff --git a/src/rove_description/config/basic.rviz b/src/rove_description/config/basic.rviz index 0fac324..60dbd12 100644 --- a/src/rove_description/config/basic.rviz +++ b/src/rove_description/config/basic.rviz @@ -5,6 +5,7 @@ Panels: Property Tree Widget: Expanded: - /Global Options1 + - /DepthCloud1/Auto Size1 Splitter Ratio: 0.5 Tree Height: 517 - Class: rviz_common/Selection @@ -24,7 +25,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: LaserScan Visualization Manager: Class: "" Displays: @@ -51,6 +52,11 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true flipper_fl: Alpha: 1 Show Axes: false @@ -153,9 +159,9 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 100 + Max Intensity: 0 Min Color: 0; 0; 0 - Min Intensity: 1 + Min Intensity: 0 Name: LaserScan Position Transformer: XYZ Selectable: true @@ -179,6 +185,18 @@ Visualization Manager: All Enabled: false base_link: Value: true + camera_link: + Value: true + flipper_fl: + Value: true + flipper_fr: + Value: true + flipper_rl: + Value: true + flipper_rr: + Value: true + map: + Value: true odom: Value: true sensor_link: @@ -205,7 +223,33 @@ Visualization Manager: Show Axes: true Show Names: false Tree: - {} + map: + odom: + base_link: + flipper_fl: + {} + flipper_fr: + {} + flipper_rl: + {} + flipper_rr: + {} + sensor_link: + camera_link: + {} + track_l: + track_fl: + {} + track_rl: + {} + track_r: + track_fr: + {} + track_rr: + {} + velodyne_link: + velodyne_laser: + {} Update Interval: 0 Value: true - Alpha: 0.699999988079071 @@ -332,21 +376,31 @@ Visualization Manager: Value: true Axis: Z Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 + Class: rtabmap_rviz_plugins/MapCloud + Cloud decimation: 4 + Cloud from scan: false + Cloud max depth (m): 4 + Cloud min depth (m): 0 + Cloud voxel size (m): 0.10000000149011612 Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 + Color Transformer: RGB8 + Download graph: false + Download map: false + Download namespace: rtabmap Enabled: true + Filter ceiling (m): 10 + Filter floor (m): 0 Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 255 + Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 - Name: 3d map + Name: MapCloud + Node filtering angle (degrees): 30 + Node filtering radius (m): 0 Position Transformer: XYZ - Selectable: true Size (Pixels): 3 - Size (m): 0.009999999776482582 + Size (m): 0.10000000149011612 Style: Flat Squares Topic: Depth: 5 @@ -354,10 +408,49 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /assembled_cloud + Value: /mapData Use Fixed Frame: true Use rainbow: true Value: true + - Alpha: 1 + Auto Size: + Auto Size Factor: 1 + Value: true + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/DepthCloud + Color: 255; 255; 255 + Color Image Topic: /zed/zed_node/rgb/image_rect_color + Color Transformer: RGB8 + Color Transport Hint: raw + Decay Time: 0 + Depth Map Topic: /zed/zed_node/depth/depth_registered + Depth Map Transport Hint: raw + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: DepthCloud + Occlusion Compensation: + Occlusion Time-Out: 30 + Value: false + Position Transformer: XYZ + Queue Size: 5 + Reliability Policy: Best effort + Selectable: true + Size (Pixels): 3 + Style: Flat Squares + Topic Filter: true + Use Fixed Frame: true + Use rainbow: true + Value: false Enabled: true Global Options: Background Color: 48; 48; 48 @@ -404,25 +497,25 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 9.305854797363281 + Distance: 11.600849151611328 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -0.6736313104629517 - Y: -1.0152995586395264 - Z: 2.0505826473236084 + X: -1.209733486175537 + Y: -1.5692439079284668 + Z: 2.5361857414245605 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.600398600101471 + Pitch: 0.9553995728492737 Target Frame: Value: Orbit (rviz) - Yaw: 1.893579363822937 + Yaw: 3.6536216735839844 Saved: ~ Window Geometry: Displays: diff --git a/src/rove_description/urdf/flipper.urdf.xacro b/src/rove_description/urdf/flipper.urdf.xacro index 4f4520a..8e07784 100644 --- a/src/rove_description/urdf/flipper.urdf.xacro +++ b/src/rove_description/urdf/flipper.urdf.xacro @@ -7,86 +7,89 @@ - - - + + + - - + + - - + + - - + + - - - - - - + + + + + + - - - + - + - + - + - + - + 0.65 0.85 - + 20 - + - - - + + + - - flipper_${suffix} - -3.0 - 3.0 - + + flipper_${suffix} + -3.0 + 3.0 + - - - - - - + + + + + + \ No newline at end of file diff --git a/src/rove_description/urdf/rove.urdf.xacro b/src/rove_description/urdf/rove.urdf.xacro index 3dae78e..2f3ac52 100644 --- a/src/rove_description/urdf/rove.urdf.xacro +++ b/src/rove_description/urdf/rove.urdf.xacro @@ -36,7 +36,7 @@ - + diff --git a/src/rove_description/urdf/sensor.urdf.xacro b/src/rove_description/urdf/sensor.urdf.xacro index e9c3c26..3b01c59 100644 --- a/src/rove_description/urdf/sensor.urdf.xacro +++ b/src/rove_description/urdf/sensor.urdf.xacro @@ -11,17 +11,15 @@ - + - - @@ -80,8 +78,8 @@ 1.88 - 256 - 256 + 1344 + 376 R_FLOAT32 @@ -99,8 +97,8 @@ 1.88 - 256 - 256 + 1344 + 376 R_FLOAT32 diff --git a/src/rove_description/urdf/track.urdf.xacro b/src/rove_description/urdf/track.urdf.xacro index e9fe3f3..ee20478 100644 --- a/src/rove_description/urdf/track.urdf.xacro +++ b/src/rove_description/urdf/track.urdf.xacro @@ -61,7 +61,7 @@ - + diff --git a/src/rove_slam/README.md b/src/rove_slam/README.md new file mode 100644 index 0000000..948b5cf --- /dev/null +++ b/src/rove_slam/README.md @@ -0,0 +1,10 @@ +# SLAM options + +**icp_zed.launch.py** : Use lidar to localize the robot in the map. The 2d map is created from both the lidar and the zed camera. The zed camera is used to create a 3d map. The 3d map is photorealistic and can be seen in rviz. + +**slam2d.launch.py** : Use slamtool box native slam to perform slam using laser scan (3d lidar converted to 2d). This method is the least accurate but the fastest. (lowest resource usage) + +**slam3d_full.launch.py** : Use RTABMAP and create a 3d map from the lidar and the zed camera. This method is the most accurate but the slowest. (highest resource usage). The 3d map can't be seen in rviz with color, you need to use RTABMAP viewer to see the 3d color map. (coupled with lidar point). + +**velodyne_3d.launch.py** : Use RTABMAP and create a 3d map from the lidar only. Don't use a lot of resources and more accurate than slam2d. The 3d map isn't colored. + diff --git a/src/rove_slam/launch/icp_zed.launch.py b/src/rove_slam/launch/icp_zed.launch.py new file mode 100644 index 0000000..ad1fe61 --- /dev/null +++ b/src/rove_slam/launch/icp_zed.launch.py @@ -0,0 +1,95 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + + use_sim_time = LaunchConfiguration('use_sim_time') + + parameters=[{ + 'frame_id':'base_link', + 'subscribe_depth':True, + 'subscribe_odom_info':False, + 'subscribe_rgbd':True, + 'publish_tf':True, + 'Odom/Strategy':'0', + 'Odom/ResetCountdown':'15', + 'Odom/GuessSmoothingDelay':'0', + 'Rtabmap/StartNewMapOnLoopClosure':'true', + 'RGBD/CreateOccupancyGrid':'false', + 'RGBD/LinearUpdate':'0', + 'Icp/VoxelSize': '0.5', + 'Icp/PointToPlaneK': '20', + 'Icp/PointToPlaneRadius': '0', + 'Icp/PointToPlane': 'true', + 'Icp/Iterations': '10', + 'Icp/Epsilon': '0.001', + 'Icp/MaxTranslation': '3', + 'Icp/MaxCorrespondenceDistance': '1', + 'Icp/Strategy': '1', + 'Icp/OutlierRatio': '0.7', + 'Icp/CorrespondenceRatio': '0.2', + 'Grid/MaxGroundHeight': '0.1', + 'Grid/MaxObstacleHeight': '1.5', + 'wait_for_transform': 0.2, + 'use_sim_time': use_sim_time, + 'RGBD/AngularUpdate':'0'}] + + + remappings=[ + ('rgb/image', '/zed/zed_node/rgb/image_rect_color'), + ('rgb/camera_info', '/zed/zed_node/depth/camera_info'), + ('/depth/image', '/zed/zed_node/depth/depth_registered'), + ('/odom', '/odometry/local'), + ] + + + + rtabmap_slam = Node( + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=parameters, + remappings=remappings, + arguments=['-d', + 'Grid/MaxGroundHeight', '0.1',]) + + rtabmap_viz_node = Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + parameters=parameters, + remappings=remappings) + + camera_sync = Node( + package='rtabmap_sync', executable='rgbd_sync', output='screen', + parameters=parameters, + remappings=remappings) + + icp_odometry_node = Node( + package='rtabmap_odom', + executable='icp_odometry', + name='icp_odometry', + output='screen', + parameters=[{ + 'odom_frame_id': '/odom', + 'wait_for_transform': 0.2, + 'publish_tf': False, + 'expected_update_rate': 50.0, + 'deskewing': False, + 'use_sim_time': use_sim_time, + }], + remappings=[ + ('scan_cloud', '/velodyne_points'), + ('odom', '/icp_odom') + ], + ) + + + return LaunchDescription([ + icp_odometry_node, + camera_sync, + rtabmap_slam, + rtabmap_viz_node, + ]) diff --git a/src/rove_slam/launch/lidar_zed.launch.py b/src/rove_slam/launch/lidar_zed.launch.py deleted file mode 100644 index 86a5579..0000000 --- a/src/rove_slam/launch/lidar_zed.launch.py +++ /dev/null @@ -1,164 +0,0 @@ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory -import os - -def generate_launch_description(): - - pkg_rove_zed = get_package_share_directory('rove_zed') - - # Launch configuration - use_sim_time = LaunchConfiguration('use_sim_time') - deskewing = False - - zed = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(pkg_rove_zed, 'launch', 'zed_mapping.launch.py'), - ) - ) - - # Nodes - icp_odometry_node = Node( - package='rtabmap_odom', - executable='icp_odometry', - name='icp_odometry', - output='screen', - parameters=[{ - 'odom_frame_id': '/odom', - 'wait_for_transform': 0.2, - 'publish_tf': False, - 'expected_update_rate': 50.0, - 'deskewing': deskewing, - 'use_sim_time': use_sim_time, - }], - remappings=[ - ('scan_cloud', '/velodyne_points'), - ('odom', '/icp_odom') - ], - arguments=[ - 'Icp/PointToPlane', 'true', - 'Icp/Iterations', '10', - 'Icp/VoxelSize', '0.1', - 'Icp/Epsilon', '0.001', - 'Icp/PointToPlaneK', '20', - 'Icp/PointToPlaneRadius', '0', - 'Icp/MaxTranslation', '2', - 'Icp/MaxCorrespondenceDistance', '1', - 'Icp/Strategy', '1', - 'Icp/OutlierRatio', '0.7', - 'Icp/CorrespondenceRatio', '0.01', - 'Odom/ScanKeyFrameThr', '0.4', - 'OdomF2M/ScanSubtractRadius', '0.1', - 'OdomF2M/ScanMaxSize', '15000', - 'OdomF2M/BundleAdjustment', 'false', - ] - ) - - point_cloud_assembler_node = Node( - package='rtabmap_util', - executable='point_cloud_assembler', - name='point_cloud_assembler', - output='screen', - parameters=[{ - 'max_clouds': 10, - 'use_sim_time': use_sim_time, - }], - remappings=[ - ('cloud', '/velodyne_points'), - ('odom', '/odometry/local') - ] - ) - - camera_sync = Node( - package='rtabmap_sync', executable='rgbd_sync', output='screen', - parameters=[{ - 'approx_sync':True - }], - remappings=[ - ('/depth/image', '/zed/zed_node/depth/depth_registered'), - ('/rgb/camera_info' , '/zed/zed_node/depth/camera_info'), - ('/rgb/image', '/zed/zed_node/rgb/image_rect_color') - ]) - - - rtabmap_node = Node( - package='rtabmap_slam', - executable='rtabmap', - name='rtabmap', - output='screen', - parameters=[{ - 'subscribe_depth': False, - 'subscribe_rgb': False, - 'subscribe_scan_cloud': True, - 'approx_sync': True, - 'wait_for_transform': 0.2, - 'use_sim_time': use_sim_time, - }], - remappings=[ - ('scan_cloud', '/assembled_cloud'), - ('odom', '/odometry/local') - - ], - arguments=[ - '-d', # This will delete the previous database (~/.ros/rtabmap.db) - 'RGBD/ProximityMaxGraphDepth', '0', - 'RGBD/ProximityPathMaxNeighbors', '1', - 'RGBD/AngularUpdate', '0.05', - 'RGBD/LinearUpdate', '0.05', - 'RGBD/CreateOccupancyGrid', 'true', - 'Mem/NotLinkedNodesKept', 'false', - 'Mem/STMSize', '30', - 'Mem/LaserScanNormalK', '20', - 'Reg/Strategy', '1', - 'Icp/VoxelSize', '0.5', - 'Icp/PointToPlaneK', '20', - 'Icp/PointToPlaneRadius', '0', - 'Icp/PointToPlane', 'true', - 'Icp/Iterations', '10', - 'Icp/Epsilon', '0.001', - 'Icp/MaxTranslation', '3', - 'Icp/MaxCorrespondenceDistance', '1', - 'Icp/Strategy', '1', - 'Icp/OutlierRatio', '0.7', - 'Icp/CorrespondenceRatio', '0.2', - 'Grid/MaxGroundHeight', '0.1', - 'Grid/MaxObstacleHeight', '1.5', - 'Grid/RayTracing', 'true', - 'Grid/3D', 'false', - 'Reg/Force3DoF', 'true', - 'Optimizer/Slam2D', 'true', - ] - ) - - rtabmap_viz_node = Node( - package='rtabmap_viz', - executable='rtabmap_viz', - name='rtabmap_viz', - output='screen', - parameters=[{ - 'odom_frame_id': 'odom', - 'subscribe_rgbd':True, - 'subscribe_odom_info': True, - 'subscribe_scan_cloud': True, - 'approx_sync': True, - 'use_sim_time': use_sim_time, - }], - remappings=[ - ('scan_cloud', '/assembled_cloud'), - ('odom', '/odometry/local') - ] - ) - - return LaunchDescription([ - icp_odometry_node, - point_cloud_assembler_node, - rtabmap_node, - rtabmap_viz_node, - zed, - camera_sync, - ]) diff --git a/src/rove_slam/launch/rtabmap.launch.py b/src/rove_slam/launch/rtabmap.launch.py deleted file mode 100644 index 2d82ede..0000000 --- a/src/rove_slam/launch/rtabmap.launch.py +++ /dev/null @@ -1,47 +0,0 @@ -import os -from sympy import false -from ament_index_python import get_package_share_directory -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch_ros.actions import Node - -def generate_launch_description(): - - parameters=[ - ['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' ], - # ['odom_frame_id', '/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'], - ['delete_db_on_start', 'true'], - # ['subscribe_scan_cloud', 'true'], https://wiki.ros.org/rtabmap_ros/Tutorials/SetupOnYourRobot#Kinect_.2B-_Odometry_.2B-_2D_laser - # ['scan_cloud_topic', '/velodyne_points'], - # ['RGBD/NeighborLinkRefining', 'true'], - # ['RGBD/ProximityBySpace', 'true'], - # ['RGBD/AngularUpdate', '0.01'], - # ['RGBD/LinearUpdate', '0.01'], - # ['RGBD/OptimizeFromGraphEnd', 'false'], - # ['Grid/FromDepth', 'false'], - # ['Reg/Force3DoF', 'true'], - # ['Reg/Strategy', '1'], - # ['Icp/VoxelSize', '0.05'], - # ['Icp/MaxCorrespondenceDistance', '0.1'], - # ['Optimizer/Slam2D', 'true'], - ] - - pkg_rtabmap = get_package_share_directory('rtabmap_launch') - launch_file_path = os.path.join(pkg_rtabmap, 'launch', 'rtabmap.launch.py') - - return LaunchDescription([ - IncludeLaunchDescription( - launch_description_source=launch_file_path, - launch_arguments=parameters, - ) - ]) diff --git a/src/rove_slam/launch/slam.launch.py b/src/rove_slam/launch/slam2d.launch.py similarity index 100% rename from src/rove_slam/launch/slam.launch.py rename to src/rove_slam/launch/slam2d.launch.py diff --git a/src/rove_slam/launch/slam3d_full.launch.py b/src/rove_slam/launch/slam3d_full.launch.py new file mode 100644 index 0000000..4492c1a --- /dev/null +++ b/src/rove_slam/launch/slam3d_full.launch.py @@ -0,0 +1,125 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +import os + +def generate_launch_description(): + + # Launch configuration + use_sim_time = LaunchConfiguration('use_sim_time') + deskewing = False + + remappings=[ + ('rgb/image', '/zed/zed_node/rgb/image_rect_color'), + ('rgb/camera_info', '/zed/zed_node/depth/camera_info'), + ('/depth/image', '/zed/zed_node/depth/depth_registered'), + ('/odom', '/odometry/local'), + ('scan_cloud', '/assembled_cloud'), + ] + + parameters=[{ + 'frame_id':'base_link', + 'subscribe_depth':True, + 'subscribe_odom_info':False, + 'subscribe_scan_cloud': True, + 'subscribe_rgbd':True, + 'approx_sync': True, + 'publish_tf':True, + 'Odom/Strategy':'0', + 'Odom/ResetCountdown':'15', + 'Odom/GuessSmoothingDelay':'0', + 'Rtabmap/StartNewMapOnLoopClosure':'true', + 'RGBD/ProximityMaxGraphDepth': '0', + 'RGBD/ProximityPathMaxNeighbors': '1', + 'RGBD/AngularUpdate': '0.05', + 'RGBD/LinearUpdate': '0.05', + 'Icp/VoxelSize': '0.5', + 'Icp/PointToPlaneK': '20', + 'Icp/PointToPlaneRadius': '0', + 'Icp/PointToPlane': 'true', + 'Icp/Iterations': '10', + 'Icp/Epsilon': '0.001', + 'Icp/MaxTranslation': '3', + 'Icp/MaxCorrespondenceDistance': '1', + 'Icp/Strategy': '1', + 'Icp/OutlierRatio': '0.7', + 'Icp/CorrespondenceRatio': '0.2', + 'Grid/MaxGroundHeight': '0.1', + 'Grid/MaxObstacleHeight': '1.5', + 'Grid/RayTracing': 'true', + 'wait_for_transform': 0.2, + 'use_sim_time': True,}] + + + # Nodes + icp_odometry_node = Node( + package='rtabmap_odom', + executable='icp_odometry', + name='icp_odometry', + output='screen', + parameters=[{ + 'odom_frame_id': '/odom', + 'wait_for_transform': 0.2, + 'publish_tf': False, + 'expected_update_rate': 50.0, + 'deskewing': deskewing, + 'use_sim_time': use_sim_time, + }], + remappings=[ + ('scan_cloud', '/assembled_cloud'), + ('odom', '/icp_odom') + ], + ) + + + + point_cloud_assembler_node = Node( + package='rtabmap_util', + executable='point_cloud_assembler', + name='point_cloud_assembler', + output='screen', + parameters=[{ + 'max_clouds': 10, + 'fixed_frame_id':'', + 'use_sim_time': use_sim_time, + }], + remappings=[ + ('cloud', '/velodyne_points'), + ('odom', '/odometry/local') + ] + ) + + camera_sync = Node( + package='rtabmap_sync', executable='rgbd_sync', output='screen', + parameters=[{ + 'approx_sync':True + }], + remappings=remappings) + + rtabmap_node = Node( + package='rtabmap_slam', + executable='rtabmap', + name='rtabmap', + output='screen', + parameters=parameters, + remappings=remappings, + arguments=['-d'] + ) + + rtabmap_viz_node = Node( + package='rtabmap_viz', + executable='rtabmap_viz', + name='rtabmap_viz', + output='screen', + parameters=parameters, + remappings=remappings, + ) + + return LaunchDescription([ + icp_odometry_node, + camera_sync, + point_cloud_assembler_node, + rtabmap_node, + #rtabmap_viz_node + ]) diff --git a/src/rove_slam/launch/slam3d.launch.py b/src/rove_slam/launch/velodyne_3d.launch.py similarity index 99% rename from src/rove_slam/launch/slam3d.launch.py rename to src/rove_slam/launch/velodyne_3d.launch.py index fad8ea8..f0636e2 100644 --- a/src/rove_slam/launch/slam3d.launch.py +++ b/src/rove_slam/launch/velodyne_3d.launch.py @@ -30,6 +30,8 @@ def generate_launch_description(): ], ) + + point_cloud_assembler_node = Node( package='rtabmap_util', executable='point_cloud_assembler', diff --git a/src/rove_slam/launch/zed.launch.py b/src/rove_slam/launch/zed.launch.py deleted file mode 100644 index 98f64d3..0000000 --- a/src/rove_slam/launch/zed.launch.py +++ /dev/null @@ -1,62 +0,0 @@ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory -import os - -def generate_launch_description(): - - pkg_rove_zed = get_package_share_directory('rove_zed') - - parameters=[{ - 'odom_frame_id': 'odom', - 'publish_tf': False, - 'frame_id':'base_link', - 'subscribe_depth': True, - 'subscribe_odom_info': False, - 'approx_sync': True, - 'Grid/MaxGroundHeight': '0.1', - 'Grid/MaxObstacleHeight': '1.5', - 'Grid/RayTracing': True, - 'Grid/3D': False, - 'wait_imu_to_init':True}] - - remappings=[ - ('imu', '/imu'), - ('rgb/image', '/zed/zed_node/rgb/image_rect_color'), - ('rgb/camera_info', '/zed/zed_node/depth/camera_info'), - ('/depth/image', '/zed/zed_node/depth/depth_registered'), - ] - - rtabmap_slam = Node( - package='rtabmap_slam', executable='rtabmap', output='screen', - parameters=parameters, - remappings=remappings, - arguments=['-d']) - - rtabmap_viz_node = Node( - package='rtabmap_viz', - executable='rtabmap_viz', - name='rtabmap_viz', - output='screen', - parameters=parameters, - remappings=remappings - ) - - - - odom = Node( - package='rtabmap_odom', executable='rgbd_odometry', output='screen', - parameters=parameters, - remappings=remappings) - - - - return LaunchDescription([ - odom, - rtabmap_slam, - rtabmap_viz_node, - ])