diff --git a/.gitignore b/.gitignore index 0a1c75f7..6827ce85 100644 --- a/.gitignore +++ b/.gitignore @@ -31,4 +31,6 @@ container_root/profilers/** **container_root/trav_ws/** **traversability_mapping** container_root/ros_env_vars.sh -container_root/.nv/ \ No newline at end of file +container_root/.nv/ +orb_slam3_ros2_wrapper/datasets/** +!orb_slam3_ros2_wrapper/datasets/PLACE_YOUR_DATASETS_HERE \ No newline at end of file diff --git a/README.md b/README.md index 7d5ae33b..25d27a2b 100644 --- a/README.md +++ b/README.md @@ -32,7 +32,7 @@ sudo chmod +x container_root/shell_scripts/docker_install.sh ## 3. Build the image with ORB_SLAM3 1. Build the image: ```sudo docker build --build-arg USE_CI=false -t orb-slam3-humble:22.04 .``` -2. Add ```xhost +``` to your ```.bashrc``` to support correct x11-forwarding using ```echo "xhost +" >> ~/.bashrc``` +2. Add `xhost +` to your `.bashrc` to support correct X11-forwarding using ```echo "xhost +" >> ~/.bashrc``` 3. ```source ~/.bashrc``` 4. You can see the built images on your machine by running ```sudo docker images```. @@ -44,7 +44,7 @@ Replace step 1. with ```sudo docker build --build-arg USE_CI=false --build-arg T 1. ```cd ORB-SLAM3-ROS2-Docker``` (ignore if you are already in the folder) 2. ```sudo docker compose run orb_slam3_22_humble``` -3. This should take you inside the container. Once you are inside, run the command ```xeyes``` and a pair of eyes should pop-up. If they do, x11 forwarding has correctly been setup on your computer. +3. This should take you inside the container. Once you are inside, run the command ```xeyes``` and a pair of eyes should pop-up. If they do, X11 forwarding has correctly been setup on your computer. ### To run the NVIDIA CUDA version: @@ -68,17 +68,49 @@ cd /root/colcon_ws/ && rm -rf build && colcon build --symlink-install --cmake-ar ## Launching ORB-SLAM3 +This repository supports the following ways to test different configurations.
+ +To try the `mono`, `mono_imu`, `stereo`, `stereo_imu` setups, please refer to the `Running with the Euroc Dataset` below. + +To try the `rgbd`, `rgbd_imu` setups, please refer to the `Running with Gazebo sim simulation` section below. + +The launch files to test each configuration is in `orb_slam3_ros2_docker/launch`
+The default parameters in each of these launch files correspond to the following table. + +| Mode | EuRoC Dataset | Gazebo Simulation | +|------|---------------|-------------------| +| mono | ✅ | ❌ | +| mono_imu | ✅ | ❌ | +| rgbd | ❌ | ✅ | +| rgbd_imu | ❌ | ✅ | +| stereo | ✅ | ❌ | +| stereo_imu | ✅ | ❌ | + Launch the container using steps in (4). If you are inside the container, run the following: -1. ```ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py``` -3. You can adjust the robot namespace in the ```unirobot.launch.py``` file. +1. ```ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py sensor_config:=``` +2. You can adjust the robot namespace in the ```unirobot.launch.py``` file. + +`sensor_config` argument can take the following `mono`, `mono_imu`, `stereo`, `stereo_imu`, `rgbd`, `rgbd_imu` + +## Running this with the Euroc dataset + +Supported modes are `mono`, `mono_imu`, `stereo`, `stereo_imu` + +1. Download the euroc dataset `sudo chmod +x download_euroc.sh && ./download_euroc.sh` +2. Open a new instance of the container you launched in (4) using `docker exec -it bash` +3. `ros2 bag play colcon_ws/src/orb_slam3_ros2_wrapper/datasets/V1_01_easy` + +Once the rosbag is playing, you should be able to run it with the wrapper in parallel. ## Running this with a Gazebo Sim simulation. -1. Setup the ORB-SLAM3 ROS2 Docker using the steps above. Once you do (1) step in the ```Launching ORB-SLAM3``` section, you should see a window popup which is waiting for images. This is partially indicative of the setup correctly done. +Supported modes are `rgbd`, `rgbd_imu` + +1. Setup the ORB-SLAM3 ROS2 Docker using the steps above. Once you complete step (1) in the ```Launching ORB-SLAM3``` section, you should see a window popup which is waiting for images. This is partially indicative of the setup correctly done. 2. Setup the simulation by following the README [here (humble)](https://github.com/suchetanrs/gz-sim-environment/tree/humble) (recommended) or [here (jazzy)](https://github.com/suchetanrs/gz-sim-environment/tree/jazzy) -3. Once you are able to teleop the robot, you should be able to run ORB-SLAM3 with both the containers (simulation and wrapper) running parallely. +3. Once you are able to teleop the robot, you should be able to run ORB-SLAM3 with both the containers (simulation and wrapper) running in parallel. ## Running the map_generator package. @@ -92,10 +124,10 @@ To run the package, these steps can be followed: 4. If you wish to publish the global pointcloud at any point during the SLAM's operation, simply run the python file in the top-right terminal. You should be able to view the global pointcloud in rviz (you can launch RViz with the correct configuration from the bottom-right terminal). ### Potential issues you may face. -The simulation and the wrapper both have their ```ROS_DOMAIN_ID``` set to 55 so they are meant to work out of the box. However, you may face issues if this environment variable is not set properly. Before you start the wrapper, run ```ros2 topic list``` and make sure the topics `/rgb_camera` and `/depth_camera` are visible inside the ORB-SLAM3 container provided the simulation is running along the side. +The simulation and the wrapper both have their ```ROS_DOMAIN_ID``` set to 55 so they are meant to work out of the box. However, you may face issues if this environment variable is not set properly. Before you start the wrapper, run ```ros2 topic list``` and make sure the topics `/rgb_camera` and `/depth_camera` are visible inside the ORB-SLAM3 container provided the simulation is running alongside. ## Services -| Service Name | Purpose | type | +| Service Name | Purpose | Type | |-------------------------|---------------|---------------| | `orb_slam3/get_map_data` | Sends the map_data in the response. | `slam_msgs::srv::GetMap` | | `orb_slam3/get_landmarks_in_view` | Takes an input pose and publishes the feature points visible from that pose. | `slam_msgs::srv::GetLandmarksInView` | @@ -103,7 +135,7 @@ The simulation and the wrapper both have their ```ROS_DOMAIN_ID``` set to 55 so | `orb_slam3/reset_mapping` | Resets the current mapping instance and clears all keyframes. | `std_srvs::srv::SetBool` | ## Published Topics -| Topic Name | Purpose | type | +| Topic Name | Purpose | Type | |-------------------------|---------------|---------------| | `map_points` | Publishes the point cloud representing feature points collected from the SLAM process. This is published when `orb_slam3/get_all_landmarks_in_map` service is called. | `sensor_msgs::msg::PointCloud2` | | `visible_landmarks` | Publishes the point cloud of feature points (landmarks) visible from a given pose. This is published when `orb_slam3/get_landmarks_in_view` service is called. | `sensor_msgs::msg::PointCloud2` | @@ -117,9 +149,9 @@ The simulation and the wrapper both have their ```ROS_DOMAIN_ID``` set to 55 so | `robot_base_frame` | `base_footprint` | The name of the frame attached to the robot's base. | | `global_frame` | `map` | The name of the global frame of reference. It represents a fixed world coordinate frame in which the robot navigates.| | `odom_frame` | `odom` | The name of the odometry frame. | -| `rgb_image_topic_name` | `rgb_camera` | The topic to recieve rgb images. | -| `depth_image_topic_name` | `depth_camera` | The topic to recieve depth images. | -| `imu_topic_name` | `imu` | The topic to recieve IMU messages (Not used in RGB-D mode). | +| `rgb_image_topic_name` | `rgb_camera` | The topic to receive rgb images. | +| `depth_image_topic_name` | `depth_camera` | The topic to receive depth images. | +| `imu_topic_name` | `imu` | The topic to receive IMU messages (Not used in RGB-D mode). | | `visualization` | `true` | A boolean flag to enable or disable visualization. When set to `true`, the ORB-SLAM3 viewer will show up with the tracked points and the keyframe trajectories.| | `odometry_mode` | `false` | A boolean flag to toggle odometry mode. When `false`, the system operates without relying on odometry data, which might be used in scenarios where odometry information is unavailable or unreliable. In this case, it publishes the transform directly between the ```global_frame``` and the ```robot_base_frame```. Further information can be found on the [FAQ](https://github.com/suchetanrs/ORB-SLAM3-ROS2-Docker/wiki/FAQs)| | `publish_tf` | `true` | Publishes the map->odom tf in case `odometry_mode` is set to `true` and map->odom->base_link in case odometry_mode is set to `false`. Further information can be found on the [FAQ](https://github.com/suchetanrs/ORB-SLAM3-ROS2-Docker/wiki/FAQs)| @@ -128,7 +160,7 @@ The simulation and the wrapper both have their ```ROS_DOMAIN_ID``` set to 55 so ## Important notes -ORB-SLAM3 is launched from ```orb_slam3_docker_20_humble/orb_slam3_ros2_wrapper/launch/rgbd.launch.py``` which inturn is launched from ```orb_slam3_docker_20_humble/orb_slam3_ros2_wrapper/launch/unirobot.launch.py``` +ORB-SLAM3 is launched from ```orb_slam3_ros2_wrapper/launch/rgbd.launch.py``` which in turn is launched from ```orb_slam3_ros2_wrapper/launch/unirobot.launch.py``` Currently the ```rgbd.launch.py``` launch file defaults to ```orb_slam3_ros2_wrapper/params/gazebo_rgbd.yaml```. You can modify this with your own parameter file in case you wish to use your own camera. diff --git a/download_euroc.sh b/download_euroc.sh new file mode 100755 index 00000000..c000656c --- /dev/null +++ b/download_euroc.sh @@ -0,0 +1,13 @@ +pip install gdown + +if [ ! -f "orb_slam3_ros2_wrapper/datasets/V1_01_easy.zip" ]; then + gdown --fuzzy https://drive.google.com/file/d/1LFrdiMU6UBjtFfXPHzjJ4L7iDIXcdhvh/view?usp=sharing +else + echo "orb_slam3_ros2_wrapper/datasets/V1_01_easy.zip already exists. Skipping download." +fi + +if [ ! -d "orb_slam3_ros2_wrapper/datasets/V1_01_easy" ]; then + unzip orb_slam3_ros2_wrapper/datasets/V1_01_easy.zip +else + echo "orb_slam3_ros2_wrapper/datasets/V1_01_easy directory already exists. Skipping unzip." +fi diff --git a/orb_slam3_ros2_wrapper/CMakeLists.txt b/orb_slam3_ros2_wrapper/CMakeLists.txt index feb9158a..6d492dfb 100644 --- a/orb_slam3_ros2_wrapper/CMakeLists.txt +++ b/orb_slam3_ros2_wrapper/CMakeLists.txt @@ -144,7 +144,31 @@ if(ORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA) endif() -install(TARGETS orb_slam3_ros2_wrapper_common rgbd rgbd_imu mono_imu stereo +add_executable(mono + src/mono/mono-slam-node.cpp + src/mono/mono.cpp +) +ament_target_dependencies(mono ${dependencies}) +target_link_libraries(mono orb_slam3_ros2_wrapper_common ${PCL_LIBRARIES}) +if(ORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA) + set_target_properties(mono PROPERTIES CUDA_SEPARABLE_COMPILATION ON) + target_compile_definitions(mono PRIVATE ORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA=1) +endif() + + +add_executable(stereo_imu + src/stereo-imu/stereo-imu-slam-node.cpp + src/stereo-imu/stereo_imu.cpp +) +ament_target_dependencies(stereo_imu ${dependencies}) +target_link_libraries(stereo_imu orb_slam3_ros2_wrapper_common ${PCL_LIBRARIES}) +if(ORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA) + set_target_properties(stereo_imu PROPERTIES CUDA_SEPARABLE_COMPILATION ON) + target_compile_definitions(stereo_imu PRIVATE ORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA=1) +endif() + + +install(TARGETS orb_slam3_ros2_wrapper_common rgbd rgbd_imu mono_imu stereo mono stereo_imu DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY launch params diff --git a/orb_slam3_ros2_wrapper/datasets/PLACE_YOUR_DATASETS_HERE b/orb_slam3_ros2_wrapper/datasets/PLACE_YOUR_DATASETS_HERE new file mode 100644 index 00000000..e69de29b diff --git a/orb_slam3_ros2_wrapper/launch/mono.launch.py b/orb_slam3_ros2_wrapper/launch/mono.launch.py new file mode 100644 index 00000000..9656863c --- /dev/null +++ b/orb_slam3_ros2_wrapper/launch/mono.launch.py @@ -0,0 +1,91 @@ +#!/usr/bin/python3 +# -*- coding: utf-8 -*- +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + orb_wrapper_pkg = get_package_share_directory("orb_slam3_ros2_wrapper") + + use_sim_time = LaunchConfiguration("use_sim_time") + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="True", + description="Use simulation (Gazebo) clock if true", + ) + + robot_namespace = LaunchConfiguration("robot_namespace") + robot_namespace_arg = DeclareLaunchArgument( + "robot_namespace", default_value="robot", description="The namespace of the robot" + ) + + orb_slam3_param_file = LaunchConfiguration("orb_slam3_param_file") + declare_orb_slam3_param_file_cmd = DeclareLaunchArgument( + name="orb_slam3_param_file", + default_value="euroc_mono.yaml", # gazebo_mono.yaml + description="Path to the ORB-SLAM3 parameter file", + ) + + ros_params_file = LaunchConfiguration("ros_params_file") + declare_ros_params_file_cmd = DeclareLaunchArgument( + name="ros_params_file", + default_value="euroc-mono-ros-params.yaml", # gazebo-mono-ros-params.yaml + description="Path to the ROS2 parameters file", + ) + + def all_nodes_launch(context, robot_namespace): + params_file = LaunchConfiguration("params_file") + vocabulary_file_path = "/home/orb/ORB_SLAM3/Vocabulary/ORBvoc.txt" + # NOTE: keep settings path consistent with existing launch files. Replace as needed. + config_file_path = "/root/colcon_ws/src/orb_slam3_ros2_wrapper/params/orb_slam3_params/" + orb_slam3_param_file.perform(context) + + declare_params_file_cmd = DeclareLaunchArgument( + "params_file", + default_value=os.path.join(orb_wrapper_pkg, "params", "ros_params", ros_params_file.perform(context)), + description="Full path to the ROS2 parameters file to use for all launched nodes", + ) + + # Optional namespacing hook (kept consistent with other launch files) + base_frame = "" if robot_namespace.perform(context) == "" else robot_namespace.perform(context) + "/" + param_substitutions = { + # "robot_base_frame": base_frame + "base_footprint", + # "odom_frame": base_frame + "odom", + } + + configured_params = RewrittenYaml( + source_file=params_file, + root_key=robot_namespace.perform(context), + param_rewrites=param_substitutions, + convert_types=True, + ) + + orb_slam3_node = Node( + package="orb_slam3_ros2_wrapper", + executable="mono", + output="screen", + namespace=robot_namespace.perform(context), + arguments=[vocabulary_file_path, config_file_path], + parameters=[configured_params], + ) + + return [declare_params_file_cmd, orb_slam3_node] + + opaque_function = OpaqueFunction(function=all_nodes_launch, args=[robot_namespace]) + + return LaunchDescription( + [ + declare_use_sim_time_cmd, + declare_orb_slam3_param_file_cmd, + declare_ros_params_file_cmd, + robot_namespace_arg, + opaque_function, + ] + ) + + diff --git a/orb_slam3_ros2_wrapper/launch/mono_imu.launch.py b/orb_slam3_ros2_wrapper/launch/mono_imu.launch.py index e1fec44d..eb9f8950 100644 --- a/orb_slam3_ros2_wrapper/launch/mono_imu.launch.py +++ b/orb_slam3_ros2_wrapper/launch/mono_imu.launch.py @@ -25,15 +25,29 @@ def generate_launch_description(): "robot_namespace", default_value="robot", description="The namespace of the robot" ) + orb_slam3_param_file = LaunchConfiguration("orb_slam3_param_file") + declare_orb_slam3_param_file_cmd = DeclareLaunchArgument( + name="orb_slam3_param_file", + default_value="euroc_mono_imu.yaml", # gazebo_mono_imu.yaml + description="Path to the ORB-SLAM3 parameter file", + ) + + ros_params_file = LaunchConfiguration("ros_params_file") + declare_ros_params_file_cmd = DeclareLaunchArgument( + name="ros_params_file", + default_value="euroc-mono-imu-ros-params.yaml", # gazebo-mono-imu-ros-params.yaml + description="Path to the ROS2 parameters file", + ) + def all_nodes_launch(context, robot_namespace): params_file = LaunchConfiguration("params_file") vocabulary_file_path = "/home/orb/ORB_SLAM3/Vocabulary/ORBvoc.txt" # NOTE: keep settings path consistent with existing launch files. Replace as needed. - config_file_path = "/root/colcon_ws/src/orb_slam3_ros2_wrapper/params/orb_slam3_params/gazebo_mono_imu.yaml" + config_file_path = "/root/colcon_ws/src/orb_slam3_ros2_wrapper/params/orb_slam3_params/" + orb_slam3_param_file.perform(context) declare_params_file_cmd = DeclareLaunchArgument( "params_file", - default_value=os.path.join(orb_wrapper_pkg, "params", "ros_params", "gazebo-mono-imu-ros-params.yaml"), + default_value=os.path.join(orb_wrapper_pkg, "params", "ros_params", ros_params_file.perform(context)), description="Full path to the ROS2 parameters file to use for all launched nodes", ) @@ -67,6 +81,8 @@ def all_nodes_launch(context, robot_namespace): return LaunchDescription( [ declare_use_sim_time_cmd, + declare_orb_slam3_param_file_cmd, + declare_ros_params_file_cmd, robot_namespace_arg, opaque_function, ] diff --git a/orb_slam3_ros2_wrapper/launch/stereo_imu.launch.py b/orb_slam3_ros2_wrapper/launch/stereo_imu.launch.py new file mode 100644 index 00000000..d380a0ca --- /dev/null +++ b/orb_slam3_ros2_wrapper/launch/stereo_imu.launch.py @@ -0,0 +1,82 @@ +#!/usr/bin/python3 +# -*- coding: utf-8 -*- +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess +from launch.actions import IncludeLaunchDescription, OpaqueFunction +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, FindExecutable, TextSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from nav2_common.launch import RewrittenYaml + +def generate_launch_description(): + +#--------------------------------------------- + + #Essential_paths + orb_wrapper_pkg = get_package_share_directory('orb_slam3_ros2_wrapper') +#--------------------------------------------- + + # LAUNCH ARGS + use_sim_time = LaunchConfiguration('use_sim_time') + declare_use_sim_time_cmd = DeclareLaunchArgument( + name='use_sim_time', + default_value='True', + description='Use simulation (Gazebo) clock if true') + + robot_namespace = LaunchConfiguration('robot_namespace') + robot_namespace_arg = DeclareLaunchArgument('robot_namespace', default_value="robot", + description='The namespace of the robot') +#--------------------------------------------- + + def all_nodes_launch(context, robot_namespace): + params_file = LaunchConfiguration('params_file') + vocabulary_file_path = "/home/orb/ORB_SLAM3/Vocabulary/ORBvoc.txt" + config_file_path = "/root/colcon_ws/src/orb_slam3_ros2_wrapper/params/orb_slam3_params/euroc_stereo_imu.yaml" + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(orb_wrapper_pkg, 'params', 'ros_params', 'euroc-stereo-imu-ros-params.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes') + + base_frame = "" + if(robot_namespace.perform(context) == ""): + base_frame = "" + else: + base_frame = robot_namespace.perform(context) + "/" + + param_substitutions = { + # 'robot_base_frame': base_frame + 'base_footprint', + # 'odom_frame': base_frame + 'odom' + } + + + configured_params = RewrittenYaml( + source_file=params_file, + root_key=robot_namespace.perform(context), + param_rewrites=param_substitutions, + convert_types=True) + + orb_slam3_node = Node( + package="orb_slam3_ros2_wrapper", + executable="stereo_imu", + output="screen", + # prefix=["gdbserver localhost:3000"], + namespace=robot_namespace.perform(context), + arguments=[vocabulary_file_path, config_file_path], + parameters=[configured_params]) + + return [declare_params_file_cmd, orb_slam3_node] + + opaque_function = OpaqueFunction(function=all_nodes_launch, args=[robot_namespace]) +#--------------------------------------------- + + return LaunchDescription([ + declare_use_sim_time_cmd, + robot_namespace_arg, + opaque_function + ]) + diff --git a/orb_slam3_ros2_wrapper/launch/unirobot.launch.py b/orb_slam3_ros2_wrapper/launch/unirobot.launch.py index 53b6c783..ee447a0e 100644 --- a/orb_slam3_ros2_wrapper/launch/unirobot.launch.py +++ b/orb_slam3_ros2_wrapper/launch/unirobot.launch.py @@ -15,7 +15,7 @@ def generate_launch_description(): sensor_config_arg = DeclareLaunchArgument( "sensor_config", default_value="rgbd", - description="Sensor configuration to launch: 'rgbd' (default), 'rgbd_imu', 'mono_imu' or 'stereo'") + description="Sensor configuration to launch: 'rgbd' (default), 'rgbd_imu', 'mono', 'mono_imu', 'stereo' or 'stereo_imu'") orb_slam3_launch_file_dir = os.path.join( get_package_share_directory('orb_slam3_ros2_wrapper'), 'launch') @@ -26,12 +26,18 @@ def generate_launch_description(): rgbd_imu_launch_file_path = os.path.join( orb_slam3_launch_file_dir, 'rgbd_imu.launch.py') + mono_launch_file_path = os.path.join( + orb_slam3_launch_file_dir, 'mono.launch.py') + mono_imu_launch_file_path = os.path.join( orb_slam3_launch_file_dir, 'mono_imu.launch.py') stereo_launch_file_path = os.path.join( orb_slam3_launch_file_dir, 'stereo.launch.py') + stereo_imu_launch_file_path = os.path.join( + orb_slam3_launch_file_dir, 'stereo_imu.launch.py') + rgbd_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource(rgbd_launch_file_path), launch_arguments={"robot_namespace": robot_namespace}.items(), @@ -44,6 +50,12 @@ def generate_launch_description(): condition=IfCondition(PythonExpression(["'", sensor_config, "' == 'rgbd_imu'"])), ) + mono_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(mono_launch_file_path), + launch_arguments={"robot_namespace": robot_namespace}.items(), + condition=IfCondition(PythonExpression(["'", sensor_config, "' == 'mono'"])), + ) + mono_imu_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource(mono_imu_launch_file_path), launch_arguments={"robot_namespace": robot_namespace}.items(), @@ -56,6 +68,12 @@ def generate_launch_description(): condition=IfCondition(PythonExpression(["'", sensor_config, "' == 'stereo'"])), ) + stereo_imu_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(stereo_imu_launch_file_path), + launch_arguments={"robot_namespace": robot_namespace}.items(), + condition=IfCondition(PythonExpression(["'", sensor_config, "' == 'stereo_imu'"])), + ) + monitor_enabled_arg = DeclareLaunchArgument( "monitor_enabled", default_value="true", @@ -81,6 +99,8 @@ def generate_launch_description(): monitor_process, rgbd_launch, rgbd_imu_launch, + mono_launch, mono_imu_launch, stereo_launch, + stereo_imu_launch, ]) diff --git a/orb_slam3_ros2_wrapper/params/zed2i_kratos.yaml b/orb_slam3_ros2_wrapper/params/orb_slam3_params/euroc_mono.yaml similarity index 50% rename from orb_slam3_ros2_wrapper/params/zed2i_kratos.yaml rename to orb_slam3_ros2_wrapper/params/orb_slam3_params/euroc_mono.yaml index e83bb002..3840edad 100644 --- a/orb_slam3_ros2_wrapper/params/zed2i_kratos.yaml +++ b/orb_slam3_ros2_wrapper/params/orb_slam3_params/euroc_mono.yaml @@ -1,74 +1,53 @@ %YAML:1.0 +#-------------------------------------------------------------------------------------------- +# System config +#-------------------------------------------------------------------------------------------- + +# When the variables are commented, the system doesn't load a previous session or not store the current one + +# If the LoadFile doesn't exist, the system give a message and create a new Atlas from scratch +#System.LoadAtlasFromFile: "Session_MH01_MH02_MH03_Mono" + +# The store file is created from the current session, if a file with the same name exists it is deleted +#System.SaveAtlasToFile: "Session_MH01_MH02_MH03_Mono" + #-------------------------------------------------------------------------------------------- # Camera Parameters. Adjust them! #-------------------------------------------------------------------------------------------- +File.version: "1.0" + Camera.type: "PinHole" # Camera calibration and distortion parameters (OpenCV) -# Right Camera calibration and distortion parameters (OpenCV) -# Camera.fx: 477.7099914550781 -# Camera.fy: 477.7099914550781 -# Camera.cx: 325.9525146484375 -# Camera.cy: 171.77325439453125 - -Camera.fx: 477.5061950683594 -Camera.fy: 477.5061950683594 -Camera.cx: 322.66217041015625 -Camera.cy: 171.676025390625 +Camera1.fx: 458.654 +Camera1.fy: 457.296 +Camera1.cx: 367.215 +Camera1.cy: 248.375 +Camera1.k1: -0.28340811 +Camera1.k2: 0.07395907 +Camera1.p1: 0.00019359 +Camera1.p2: 1.76187114e-05 -# distortion parameters -Camera.k1: 0.0 -Camera.k2: 0.0 -Camera.p1: 0.0 -Camera.p2: 0.0 -Camera.k3: 0.0 +Camera.width: 752 +Camera.height: 480 -Camera.width: 640 -Camera.height: 360 +Camera.newWidth: 600 +Camera.newHeight: 350 # Camera frames per second -Camera.fps: 15.0 - -# IR projector baseline times fx (aprox.) -Camera.bf: 37.0 +Camera.fps: 20 # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) -Camera.RGB: 0 - -# Close/Far threshold. Baseline times. -ThDepth: 40.0 - -# Deptmap values factor -DepthMapFactor: 1.0 # 1.0 for ROS_bag - -# Transformation from camera 0 to body-frame (imu) -Tbc: !!opencv-matrix - rows: 4 - cols: 4 - dt: f - data: [-0.9995250378696743, 0.0075019185074052044, -0.02989013031643309, 0.045574835649698026, - 0.029615343885863205, -0.03439736061393144, -0.998969345370175, -0.071161801837997044, - -0.008522328211654736, -0.9993800792498829, 0.03415885127385616, -0.044681254117144367, - 0.0, 0.0, 0.0, 1.0] - -# Do not insert KFs when recently lost -InsertKFsWhenLost: 1 - -# IMU noise (Use those from VINS-mono) -IMU.NoiseGyro: 1e-2 # 3 # 2.44e-4 #1e-3 # rad/s^0.5 -IMU.NoiseAcc: 1e-1 #2 # 1.47e-3 #1e-2 # m/s^1.5 -IMU.GyroWalk: 1e-6 # rad/s^1.5 -IMU.AccWalk: 1e-4 # m/s^2.5 -IMU.Frequency: 92 +Camera.RGB: 1 #-------------------------------------------------------------------------------------------- # ORB Parameters #-------------------------------------------------------------------------------------------- # ORB Extractor: Number of features per image -ORBextractor.nFeatures: 900 +ORBextractor.nFeatures: 1000 # ORB Extractor: Scale factor between levels in the scale pyramid ORBextractor.scaleFactor: 1.2 @@ -85,16 +64,15 @@ ORBextractor.minThFAST: 7 #-------------------------------------------------------------------------------------------- # Viewer Parameters -#-------------------------------------------------------------------------------------------- +#--------------------------------------------------------------------------------------------- Viewer.KeyFrameSize: 0.05 -Viewer.KeyFrameLineWidth: 1 +Viewer.KeyFrameLineWidth: 1.0 Viewer.GraphLineWidth: 0.9 -Viewer.PointSize:2 +Viewer.PointSize: 2.0 Viewer.CameraSize: 0.08 -Viewer.CameraLineWidth: 3 -Viewer.ViewpointX: 0 +Viewer.CameraLineWidth: 3.0 +Viewer.ViewpointX: 0.0 Viewer.ViewpointY: -0.7 Viewer.ViewpointZ: -1.8 -Viewer.ViewpointF: 500 -System.SaveAtlasToFile: ./atlas +Viewer.ViewpointF: 500.0 diff --git a/orb_slam3_ros2_wrapper/params/orbbec_astra.yaml b/orb_slam3_ros2_wrapper/params/orb_slam3_params/euroc_mono_imu.yaml similarity index 51% rename from orb_slam3_ros2_wrapper/params/orbbec_astra.yaml rename to orb_slam3_ros2_wrapper/params/orb_slam3_params/euroc_mono_imu.yaml index 2fe7dc29..c5a98e0c 100644 --- a/orb_slam3_ros2_wrapper/params/orbbec_astra.yaml +++ b/orb_slam3_ros2_wrapper/params/orb_slam3_params/euroc_mono_imu.yaml @@ -3,71 +3,57 @@ #-------------------------------------------------------------------------------------------- # Camera Parameters. Adjust them! #-------------------------------------------------------------------------------------------- +File.version: "1.0" + Camera.type: "PinHole" # Camera calibration and distortion parameters (OpenCV) -# Right Camera calibration and distortion parameters (OpenCV) -# Camera.fx: 528.433756558705 -# Camera.fy: 528.433756558705 -# Camera.cx: 320.5 -# Camera.cy: 240.5 -Camera.fx: 478.9118347167969 -Camera.fy: 478.9118347167969 -Camera.cx: 330.0350341796875 -Camera.cy: 248.18951416015625 - - -# distortion parameters -Camera.k1: -0.0217606 -Camera.k2: 0.0456226 -Camera.p1: -0.000758527 -Camera.p2: 0.000665947 -Camera.k3: 0.0 - -Camera.width: 640 +Camera1.fx: 458.654 +Camera1.fy: 457.296 +Camera1.cx: 367.215 +Camera1.cy: 248.375 + +Camera1.k1: -0.28340811 +Camera1.k2: 0.07395907 +Camera1.p1: 0.00019359 +Camera1.p2: 1.76187114e-05 + +# Camera resolution +Camera.width: 752 Camera.height: 480 -# Camera frames per second -Camera.fps: 30.0 +Camera.newWidth: 600 +Camera.newHeight: 350 -# IR projector baseline times fx (aprox.) -Camera.bf: 37.0 +# Camera frames per second +Camera.fps: 20 # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) Camera.RGB: 1 -# Close/Far threshold. Baseline times. -ThDepth: 40.0 - -# Deptmap values factor -DepthMapFactor: 1000.0 # 1.0 for ROS_bag - -# Transformation from camera 0 to body-frame (imu) -Tbc: !!opencv-matrix +# Transformation from camera to body-frame (imu) +IMU.T_b_c1: !!opencv-matrix rows: 4 cols: 4 dt: f - data: [-0.9995250378696743, 0.0075019185074052044, -0.02989013031643309, 0.045574835649698026, - 0.029615343885863205, -0.03439736061393144, -0.998969345370175, -0.071161801837997044, - -0.008522328211654736, -0.9993800792498829, 0.03415885127385616, -0.044681254117144367, - 0.0, 0.0, 0.0, 1.0] - -# Do not insert KFs when recently lost -InsertKFsWhenLost: 1 - -# IMU noise (Use those from VINS-mono) -IMU.NoiseGyro: 1e-2 # 3 # 2.44e-4 #1e-3 # rad/s^0.5 -IMU.NoiseAcc: 1e-1 #2 # 1.47e-3 #1e-2 # m/s^1.5 -IMU.GyroWalk: 1e-6 # rad/s^1.5 -IMU.AccWalk: 1e-4 # m/s^2.5 -IMU.Frequency: 92 + data: [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975, + 0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768, + -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949, + 0.0, 0.0, 0.0, 1.0] + +# IMU noise +IMU.NoiseGyro: 1.7e-4 #1.6968e-04 +IMU.NoiseAcc: 2.0000e-3 #2.0e-3 +IMU.GyroWalk: 1.9393e-05 +IMU.AccWalk: 3.0000e-03 # 3e-03 +IMU.Frequency: 200.0 #-------------------------------------------------------------------------------------------- # ORB Parameters #-------------------------------------------------------------------------------------------- # ORB Extractor: Number of features per image -ORBextractor.nFeatures: 1250 +ORBextractor.nFeatures: 1000 # 1000 # ORB Extractor: Scale factor between levels in the scale pyramid ORBextractor.scaleFactor: 1.2 @@ -86,14 +72,13 @@ ORBextractor.minThFAST: 7 # Viewer Parameters #-------------------------------------------------------------------------------------------- Viewer.KeyFrameSize: 0.05 -Viewer.KeyFrameLineWidth: 1 +Viewer.KeyFrameLineWidth: 1.0 Viewer.GraphLineWidth: 0.9 -Viewer.PointSize:2 +Viewer.PointSize: 2.0 Viewer.CameraSize: 0.08 -Viewer.CameraLineWidth: 3 -Viewer.ViewpointX: 0 +Viewer.CameraLineWidth: 3.0 +Viewer.ViewpointX: 0.0 Viewer.ViewpointY: -0.7 -Viewer.ViewpointZ: -1.8 -Viewer.ViewpointF: 500 -System.SaveAtlasToFile: ./atlas +Viewer.ViewpointZ: -3.5 # -1.8 +Viewer.ViewpointF: 500.0 diff --git a/orb_slam3_ros2_wrapper/params/orb_slam3_params/euroc_stereo_imu.yaml b/orb_slam3_ros2_wrapper/params/orb_slam3_params/euroc_stereo_imu.yaml new file mode 100644 index 00000000..517da510 --- /dev/null +++ b/orb_slam3_ros2_wrapper/params/orb_slam3_params/euroc_stereo_imu.yaml @@ -0,0 +1,114 @@ +%YAML:1.0 + +#-------------------------------------------------------------------------------------------- +# System config +#-------------------------------------------------------------------------------------------- + +# When the variables are commented, the system doesn't load a previous session or not store the current one + +# If the LoadFile doesn't exist, the system give a message and create a new Atlas from scratch +#System.LoadAtlasFromFile: "Session_MH01_MH02_MH03_Stereo60_Pseudo" + +# The store file is created from the current session, if a file with the same name exists it is deleted +#System.SaveAtlasToFile: "Session_MH01_MH02_MH03_Stereo60_Pseudo" + +#-------------------------------------------------------------------------------------------- +# Camera Parameters. Adjust them! +#-------------------------------------------------------------------------------------------- +File.version: "1.0" + +Camera.type: "PinHole" + +# Camera calibration and distortion parameters (OpenCV) +Camera1.fx: 458.654 +Camera1.fy: 457.296 +Camera1.cx: 367.215 +Camera1.cy: 248.375 + +Camera1.k1: -0.28340811 +Camera1.k2: 0.07395907 +Camera1.p1: 0.00019359 +Camera1.p2: 1.76187114e-05 + +Camera2.fx: 457.587 +Camera2.fy: 456.134 +Camera2.cx: 379.999 +Camera2.cy: 255.238 + +Camera2.k1: -0.28368365 +Camera2.k2: 0.07451284 +Camera2.p1: -0.00010473 +Camera2.p2: -3.55590700e-05 + +Camera.width: 752 +Camera.height: 480 + +# Camera frames per second +Camera.fps: 20 + +# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) +Camera.RGB: 1 + +# Close/Far threshold. Baseline times. +Stereo.ThDepth: 60.0 +Stereo.T_c1_c2: !!opencv-matrix + rows: 4 + cols: 4 + dt: f + data: [0.999997256477797,-0.002317135723275,-0.000343393120620,0.110074137800478, + 0.002312067192432,0.999898048507103,-0.014090668452683,-0.000156612054392, + 0.000376008102320,0.014089835846691,0.999900662638081,0.000889382785432, + 0,0,0,1.000000000000000] + +# Transformation from camera 0 to body-frame (imu) +IMU.T_b_c1: !!opencv-matrix + rows: 4 + cols: 4 + dt: f + data: [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975, + 0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768, + -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949, + 0.0, 0.0, 0.0, 1.0] + +# IMU noise +IMU.NoiseGyro: 1.7e-04 # 1.6968e-04 +IMU.NoiseAcc: 2.0e-03 # 2.0000e-3 +IMU.GyroWalk: 1.9393e-05 +IMU.AccWalk: 3.e-03 # 3.0000e-3 +IMU.Frequency: 200.0 + +#-------------------------------------------------------------------------------------------- +# ORB Parameters +#-------------------------------------------------------------------------------------------- + +# ORB Extractor: Number of features per image +ORBextractor.nFeatures: 1200 + +# ORB Extractor: Scale factor between levels in the scale pyramid +ORBextractor.scaleFactor: 1.2 + +# ORB Extractor: Number of levels in the scale pyramid +ORBextractor.nLevels: 8 + +# ORB Extractor: Fast threshold +# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. +# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST +# You can lower these values if your images have low contrast +ORBextractor.iniThFAST: 20 +ORBextractor.minThFAST: 7 + +#-------------------------------------------------------------------------------------------- +# Viewer Parameters +#-------------------------------------------------------------------------------------------- +Viewer.KeyFrameSize: 0.05 +Viewer.KeyFrameLineWidth: 1.0 +Viewer.GraphLineWidth: 0.9 +Viewer.PointSize: 2.0 +Viewer.CameraSize: 0.08 +Viewer.CameraLineWidth: 3.0 +Viewer.ViewpointX: 0.0 +Viewer.ViewpointY: -0.7 +Viewer.ViewpointZ: -1.8 +Viewer.ViewpointF: 500.0 +Viewer.imageViewScale: 1.0 + diff --git a/orb_slam3_ros2_wrapper/params/ros_params/euroc-mono-imu-ros-params.yaml b/orb_slam3_ros2_wrapper/params/ros_params/euroc-mono-imu-ros-params.yaml new file mode 100644 index 00000000..61f8bf0b --- /dev/null +++ b/orb_slam3_ros2_wrapper/params/ros_params/euroc-mono-imu-ros-params.yaml @@ -0,0 +1,30 @@ +# Add a "/" at the start of the topics to avoid namespacing +# The frame_ids are automatically namespaced in the launch file. + +ORB_SLAM3_IMU_MONO_ROS2: + ros__parameters: + # slam related + robot_base_frame: base_footprint + global_frame: map + odom_frame: odom + + # topics + image_topic_name: /cam0/image_raw + imu_topic_name: /imu0 + + # base_footprint -> camera_link (used to account for camera offset w.r.t. base) + robot_x: 0.7 + robot_y: 0.0 + robot_z: 1.150 + robot_qx: 0.0 + robot_qy: 0.0 + robot_qz: 0.0 + robot_qw: 1.0 + + visualization: true + odometry_mode: false + publish_tf: true + map_data_publish_frequency: 1000 # publish every 1000.0 milliseconds + do_loop_closing: true + + diff --git a/orb_slam3_ros2_wrapper/params/ros_params/euroc-mono-ros-params.yaml b/orb_slam3_ros2_wrapper/params/ros_params/euroc-mono-ros-params.yaml new file mode 100644 index 00000000..379bb49b --- /dev/null +++ b/orb_slam3_ros2_wrapper/params/ros_params/euroc-mono-ros-params.yaml @@ -0,0 +1,29 @@ +# Add a "/" at the start of the topics to avoid namespacing +# The frame_ids are automatically namespaced in the launch file. + +ORB_SLAM3_MONO_ROS2: + ros__parameters: + # slam related + robot_base_frame: base_footprint + global_frame: map + odom_frame: odom + + # topics + image_topic_name: /cam0/image_raw + + # base_footprint -> camera_link (used to account for camera offset w.r.t. base) + robot_x: 0.7 + robot_y: 0.0 + robot_z: 1.150 + robot_qx: 0.0 + robot_qy: 0.0 + robot_qz: 0.0 + robot_qw: 1.0 + + visualization: true + odometry_mode: false + publish_tf: true + map_data_publish_frequency: 1000 # publish every 1000.0 milliseconds + do_loop_closing: true + + diff --git a/orb_slam3_ros2_wrapper/params/ros_params/euroc-stereo-imu-ros-params.yaml b/orb_slam3_ros2_wrapper/params/ros_params/euroc-stereo-imu-ros-params.yaml new file mode 100644 index 00000000..9e7a6fb8 --- /dev/null +++ b/orb_slam3_ros2_wrapper/params/ros_params/euroc-stereo-imu-ros-params.yaml @@ -0,0 +1,31 @@ +# Add a "/" at the start of the topics to avoid namespacing +# The frame_ids are automatically namespaced in the stereo.launch.py file. +# If the robot namespace is "robot_0" then the frame_ids become "robot_0/camera_link" + +ORB_SLAM3_IMU_STEREO_ROS2: + ros__parameters: + # slam related + robot_base_frame: base_footprint + global_frame: map + odom_frame: odom + left_image_topic_name: /cam0/image_raw + right_image_topic_name: /cam1/image_raw + + imu_topic_name: /imu0 + + # the below parameters are used to adjust the initial pose with respect to the map. + # the camera pose maybe translated and rotated with respect to the base_footprint. The wrapper publishes map->base_footprint. Hence this relative transform needs to be accounted. + # the below transform is the transform from base_footpring -> camera_link + robot_x: 0.7 + robot_y: 0.0 + robot_z: 1.150 + robot_qx: 0.0 + robot_qy: 0.0 + robot_qz: 0.0 + robot_qw: 1.0 + + visualization: true + odometry_mode: false + publish_tf: true + map_data_publish_frequency: 1000 # publish every 1000.0 milliseconds + do_loop_closing: true diff --git a/orb_slam3_ros2_wrapper/params/zed2i.yaml b/orb_slam3_ros2_wrapper/params/zed2i.yaml deleted file mode 100644 index 0d7e3513..00000000 --- a/orb_slam3_ros2_wrapper/params/zed2i.yaml +++ /dev/null @@ -1,101 +0,0 @@ -%YAML:1.0 - -#-------------------------------------------------------------------------------------------- -# Camera Parameters. Adjust them! -#-------------------------------------------------------------------------------------------- -Camera.type: "PinHole" - -# Camera calibration and distortion parameters (OpenCV) -# Right Camera calibration and distortion parameters (OpenCV) -Camera.fx: 273.669189453125 -Camera.fy: 273.669189453125 -Camera.cx: 317.84844970703125 -Camera.cy: 194.20004272460938 - - -# distortion parameters -Camera.k1: 0.0 -Camera.k2: 0.0 -Camera.p1: 0.0 -Camera.p2: 0.0 -Camera.k3: 0.0 - -Camera.width: 640 -Camera.height: 360 - -# Camera frames per second -Camera.fps: 15.0 - -# IR projector baseline times fx (aprox.) -Camera.bf: 37.0 - -# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) -Camera.RGB: 0 - -# Close/Far threshold. Baseline times. -ThDepth: 40.0 - -# Deptmap values factor -DepthMapFactor: 1.0 # 1.0 for ROS_bag - -# Transformation from camera 0 to body-frame (imu) -Tbc: !!opencv-matrix - rows: 4 - cols: 4 - dt: f - data: [0.9999745, 0.0065543, -0.0028336, -0.0020000000949949026, - -0.0065462, 0.9999745, 0.0028475, -0.023061001673340797, - 0.0028522, -0.0028289, 0.9999920, 0.00021700002253055573, - 0.0, 0.0, 0.0, 1.0] - -# Do not insert KFs when recently lost -InsertKFsWhenLost: 1 - -# IMU noise (Use those from VINS-mono) -# IMU.NoiseGyro: 1e-2 # 3 # 2.44e-4 #1e-3 # rad/s^0.5 -# IMU.NoiseAcc: 1e-1 #2 # 1.47e-3 #1e-2 # m/s^1.5 -# IMU.GyroWalk: 1e-6 # rad/s^1.5 -# IMU.AccWalk: 1e-4 # m/s^2.5 -# IMU.Frequency: 92 - -IMU.NoiseGyro: 7.04e-8 # computed from angular_velocity_covariance (average over axes), rad/s/√Hz -IMU.NoiseAcc: 8.00e-1 # computed from linear_acceleration_covariance (average over axes), m/s²/√Hz -IMU.GyroWalk: 1e-6 # default value for gyro bias random walk, rad/s^1.5 -IMU.AccWalk: 1e-4 # default value for accelerometer bias random walk, m/s²^1.5 -IMU.Frequency: 200 - -#-------------------------------------------------------------------------------------------- -# ORB Parameters -#-------------------------------------------------------------------------------------------- - -# ORB Extractor: Number of features per image -ORBextractor.nFeatures: 900 - -# ORB Extractor: Scale factor between levels in the scale pyramid -ORBextractor.scaleFactor: 1.2 - -# ORB Extractor: Number of levels in the scale pyramid -ORBextractor.nLevels: 8 - -# ORB Extractor: Fast threshold -# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. -# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST -# You can lower these values if your images have low contrast -ORBextractor.iniThFAST: 20 -ORBextractor.minThFAST: 7 - -#-------------------------------------------------------------------------------------------- -# Viewer Parameters -#-------------------------------------------------------------------------------------------- -Viewer.KeyFrameSize: 0.05 -Viewer.KeyFrameLineWidth: 1 -Viewer.GraphLineWidth: 0.9 -Viewer.PointSize:2 -Viewer.CameraSize: 0.08 -Viewer.CameraLineWidth: 3 -Viewer.ViewpointX: 0 -Viewer.ViewpointY: -0.7 -Viewer.ViewpointZ: -1.8 -Viewer.ViewpointF: 500 -System.SaveAtlasToFile: ./atlas - diff --git a/orb_slam3_ros2_wrapper/src/mono/mono-slam-node.cpp b/orb_slam3_ros2_wrapper/src/mono/mono-slam-node.cpp new file mode 100644 index 00000000..f792a40f --- /dev/null +++ b/orb_slam3_ros2_wrapper/src/mono/mono-slam-node.cpp @@ -0,0 +1,64 @@ +/** + * @file mono-slam-node.cpp + * @brief Implementation of the MonoSlamNode Wrapper class. + */ + +#include "mono-slam-node.hpp" + +#include +#include + +namespace ORB_SLAM3_Wrapper +{ + using namespace WrapperTypeConversions; + + MonoSlamNode::MonoSlamNode(const std::string &strVocFile, + const std::string &strSettingsFile, + ORB_SLAM3::System::eSensor sensor) + : SlamNodeBase("ORB_SLAM3_MONO_ROS2", strVocFile, strSettingsFile, sensor) + { + // Declare parameters (topic names) + this->declare_parameter("image_topic_name", rclcpp::ParameterValue("camera/image_raw")); + + // Image subscriber (frame callback) + imageSub_ = this->create_subscription( + this->get_parameter("image_topic_name").as_string(), + rclcpp::SensorDataQoS(), + std::bind(&MonoSlamNode::ImageCallback, this, std::placeholders::_1)); + + RCLCPP_INFO(this->get_logger(), "Mono node started. Image: %s", + this->get_parameter("image_topic_name").as_string().c_str()); + } + + MonoSlamNode::~MonoSlamNode() + { + imageSub_.reset(); + RCLCPP_INFO(this->get_logger(), "Mono node stopped."); + } + + void MonoSlamNode::ImageCallback(const sensor_msgs::msg::Image::SharedPtr msgImg) + { + cv_bridge::CvImageConstPtr cvImg; + try + { + cvImg = cv_bridge::toCvShare(msgImg); + } + catch (cv_bridge::Exception &e) + { + std::cerr << "cv_bridge exception (mono image)!" << std::endl; + return; + } + + const double tImg = stampToSec(msgImg->header.stamp); + + // Track monocular image without IMU measurements. + auto Tcw = interface()->slam()->TrackMonocular(cvImg->image, tImg); + + // Process and publish on success. + if (interface()->processTrackedPose(Tcw)) + { + this->onTracked(msgImg->header); + } + } +} // namespace ORB_SLAM3_Wrapper + diff --git a/orb_slam3_ros2_wrapper/src/mono/mono-slam-node.hpp b/orb_slam3_ros2_wrapper/src/mono/mono-slam-node.hpp new file mode 100644 index 00000000..1debebd3 --- /dev/null +++ b/orb_slam3_ros2_wrapper/src/mono/mono-slam-node.hpp @@ -0,0 +1,32 @@ +/** + * @file mono-slam-node.hpp + * @brief Definition of the MonoSlamNode Wrapper class. + */ + +#ifndef MONO_SLAM_NODE_HPP_ +#define MONO_SLAM_NODE_HPP_ + +#include + +#include "orb_slam3_ros2_wrapper/slam_node_base.hpp" + +namespace ORB_SLAM3_Wrapper +{ + class MonoSlamNode : public SlamNodeBase + { + public: + MonoSlamNode(const std::string &strVocFile, + const std::string &strSettingsFile, + ORB_SLAM3::System::eSensor sensor); + ~MonoSlamNode(); + + private: + void ImageCallback(const sensor_msgs::msg::Image::SharedPtr msgImg); + + // Image subscription + rclcpp::Subscription::SharedPtr imageSub_; + }; +} // namespace ORB_SLAM3_Wrapper + +#endif // MONO_SLAM_NODE_HPP_ + diff --git a/orb_slam3_ros2_wrapper/src/mono/mono.cpp b/orb_slam3_ros2_wrapper/src/mono/mono.cpp new file mode 100644 index 00000000..c7fb4ce4 --- /dev/null +++ b/orb_slam3_ros2_wrapper/src/mono/mono.cpp @@ -0,0 +1,26 @@ +#include + +#include "rclcpp/rclcpp.hpp" +#include "mono-slam-node.hpp" + +int main(int argc, char **argv) +{ + if (argc < 3) + { + std::cerr << "\nUsage: ros2 run orb_slam3_ros2_wrapper mono path_to_vocabulary path_to_settings" << std::endl; + return 1; + } + + rclcpp::init(argc, argv); + + auto node = std::make_shared( + argv[1], argv[2], ORB_SLAM3::System::MONOCULAR); + + auto executor = std::make_shared(); + executor->add_node(node); + executor->spin(); + rclcpp::shutdown(); + + return 0; +} + diff --git a/orb_slam3_ros2_wrapper/src/stereo-imu/stereo-imu-slam-node.cpp b/orb_slam3_ros2_wrapper/src/stereo-imu/stereo-imu-slam-node.cpp new file mode 100644 index 00000000..a70c85bd --- /dev/null +++ b/orb_slam3_ros2_wrapper/src/stereo-imu/stereo-imu-slam-node.cpp @@ -0,0 +1,129 @@ +/** + * @file stereo-imu-slam-node.cpp + * @brief Implementation of the StereoImuSlamNode Wrapper class. + */ + +#include "stereo-imu-slam-node.hpp" + +#include +#include +#include + +namespace ORB_SLAM3_Wrapper +{ + using namespace WrapperTypeConversions; + + StereoImuSlamNode::StereoImuSlamNode(const std::string &strVocFile, + const std::string &strSettingsFile, + ORB_SLAM3::System::eSensor sensor) + : SlamNodeBase("ORB_SLAM3_IMU_STEREO_ROS2", strVocFile, strSettingsFile, sensor) + { + // Declare parameters (topic names) + this->declare_parameter("left_image_topic_name", rclcpp::ParameterValue("left/image_raw")); + this->declare_parameter("right_image_topic_name", rclcpp::ParameterValue("right/image_raw")); + this->declare_parameter("imu_topic_name", rclcpp::ParameterValue("imu")); + + // ROS Subscribers for stereo images + leftSub_ = std::make_shared>(this, this->get_parameter("left_image_topic_name").as_string()); + rightSub_ = std::make_shared>(this, this->get_parameter("right_image_topic_name").as_string()); + syncApproximate_ = std::make_shared>(approximate_sync_policy(10), *leftSub_, *rightSub_); + syncApproximate_->registerCallback(&StereoImuSlamNode::StereoCallback, this); + + // IMU subscriber in an independent callback group. + imuCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::SubscriptionOptions imuSubOptions; + imuSubOptions.callback_group = imuCallbackGroup_; + + imuSub_ = this->create_subscription( + this->get_parameter("imu_topic_name").as_string(), + rclcpp::SensorDataQoS(), + std::bind(&StereoImuSlamNode::ImuCallback, this, std::placeholders::_1), + imuSubOptions); + + RCLCPP_INFO(this->get_logger(), "Stereo-IMU node started. Left: %s | Right: %s | IMU: %s", + this->get_parameter("left_image_topic_name").as_string().c_str(), + this->get_parameter("right_image_topic_name").as_string().c_str(), + this->get_parameter("imu_topic_name").as_string().c_str()); + } + + StereoImuSlamNode::~StereoImuSlamNode() + { + leftSub_.reset(); + rightSub_.reset(); + syncApproximate_.reset(); + imuSub_.reset(); + RCLCPP_INFO(this->get_logger(), "Stereo-IMU node stopped."); + } + + void StereoImuSlamNode::ImuCallback(const sensor_msgs::msg::Imu::SharedPtr msgImu) + { + std::lock_guard lock(imuMutex_); + imuBuf_.push(msgImu); + } + + void StereoImuSlamNode::StereoCallback(const sensor_msgs::msg::Image::SharedPtr msgLeft, + const sensor_msgs::msg::Image::SharedPtr msgRight) + { + cv_bridge::CvImageConstPtr cvLeft; + cv_bridge::CvImageConstPtr cvRight; + + try + { + cvLeft = cv_bridge::toCvShare(msgLeft); + } + catch (cv_bridge::Exception &e) + { + std::cerr << "cv_bridge exception left!" << std::endl; + return; + } + + try + { + cvRight = cv_bridge::toCvShare(msgRight); + } + catch (cv_bridge::Exception &e) + { + std::cerr << "cv_bridge exception right!" << std::endl; + return; + } + + const double tFrame = std::min(stampToSec(msgLeft->header.stamp), stampToSec(msgRight->header.stamp)); + + // Collect IMU measurements up to this stereo frame timestamp (Euroc-style: everything since last frame). + std::vector vImuMeas; + { + std::lock_guard lock(imuMutex_); + while (!imuBuf_.empty() && stampToSec(imuBuf_.front()->header.stamp) <= tFrame) + { + const double t = stampToSec(imuBuf_.front()->header.stamp); + const cv::Point3f acc( + imuBuf_.front()->linear_acceleration.x, + imuBuf_.front()->linear_acceleration.y, + imuBuf_.front()->linear_acceleration.z); + const cv::Point3f gyr( + imuBuf_.front()->angular_velocity.x, + imuBuf_.front()->angular_velocity.y, + imuBuf_.front()->angular_velocity.z); + + vImuMeas.push_back(ORB_SLAM3::IMU::Point(acc, gyr, t)); + imuBuf_.pop(); + } + } + + if (vImuMeas.empty()) + { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000, + "No IMU measurements available for stereo frame t=%.6f (waiting for IMU...)", tFrame); + return; + } + + // Track stereo images with IMU measurements. + auto Tcw = interface()->slam()->TrackStereo(cvLeft->image, cvRight->image, tFrame, vImuMeas); + + if (interface()->processTrackedPose(Tcw)) + { + this->onTracked(msgLeft->header); + } + } +} // namespace ORB_SLAM3_Wrapper + diff --git a/orb_slam3_ros2_wrapper/src/stereo-imu/stereo-imu-slam-node.hpp b/orb_slam3_ros2_wrapper/src/stereo-imu/stereo-imu-slam-node.hpp new file mode 100644 index 00000000..5af5fa8d --- /dev/null +++ b/orb_slam3_ros2_wrapper/src/stereo-imu/stereo-imu-slam-node.hpp @@ -0,0 +1,51 @@ +/** + * @file stereo-imu-slam-node.hpp + * @brief Definition of the StereoImuSlamNode Wrapper class. + */ + +#ifndef STEREO_IMU_SLAM_NODE_HPP_ +#define STEREO_IMU_SLAM_NODE_HPP_ + +#include +#include + +#include +#include +#include +#include +#include + +#include "orb_slam3_ros2_wrapper/slam_node_base.hpp" + +namespace ORB_SLAM3_Wrapper +{ + class StereoImuSlamNode : public SlamNodeBase + { + public: + StereoImuSlamNode(const std::string &strVocFile, + const std::string &strSettingsFile, + ORB_SLAM3::System::eSensor sensor); + ~StereoImuSlamNode(); + + private: + using approximate_sync_policy = message_filters::sync_policies::ApproximateTime; + + void ImuCallback(const sensor_msgs::msg::Imu::SharedPtr msgImu); + void StereoCallback(const sensor_msgs::msg::Image::SharedPtr msgLeft, + const sensor_msgs::msg::Image::SharedPtr msgRight); + + std::shared_ptr> leftSub_; + std::shared_ptr> rightSub_; + std::shared_ptr> syncApproximate_; + + // Dedicated callback group for IMU subscription (allows independent scheduling vs stereo callback). + rclcpp::CallbackGroup::SharedPtr imuCallbackGroup_; + rclcpp::Subscription::SharedPtr imuSub_; + + std::mutex imuMutex_; + std::queue imuBuf_; + }; +} // namespace ORB_SLAM3_Wrapper + +#endif // STEREO_IMU_SLAM_NODE_HPP_ + diff --git a/orb_slam3_ros2_wrapper/src/stereo-imu/stereo_imu.cpp b/orb_slam3_ros2_wrapper/src/stereo-imu/stereo_imu.cpp new file mode 100644 index 00000000..5c6875da --- /dev/null +++ b/orb_slam3_ros2_wrapper/src/stereo-imu/stereo_imu.cpp @@ -0,0 +1,26 @@ +#include + +#include "rclcpp/rclcpp.hpp" +#include "stereo-imu-slam-node.hpp" + +int main(int argc, char **argv) +{ + if (argc < 3) + { + std::cerr << "\nUsage: ros2 run orb_slam3_ros2_wrapper stereo_imu path_to_vocabulary path_to_settings" << std::endl; + return 1; + } + + rclcpp::init(argc, argv); + + auto node = std::make_shared( + argv[1], argv[2], ORB_SLAM3::System::IMU_STEREO); + + auto executor = std::make_shared(); + executor->add_node(node); + executor->spin(); + rclcpp::shutdown(); + + return 0; +} +