diff --git a/README.md b/README.md index cce17e6b..adb5305b 100644 --- a/README.md +++ b/README.md @@ -2,6 +2,10 @@ This repository contains a dockerized comprehensive wrapper for ORB-SLAM3 on ROS 2 Humble for Ubuntu 22.04. +# Important Note: + +Please use the `Release 1.0.0` version of this repository for the latest stable version. The `master` has the latest features but can be unstable and hasn't been tested extensively. + ## Build status ![Humble Docker Build](https://github.com/suchetanrs/ORB-SLAM3-ROS2-Docker/actions/workflows/build-humble-image.yml/badge.svg) diff --git a/container_root/.bash_history b/container_root/.bash_history index eb9a5c34..b5dc7977 100644 --- a/container_root/.bash_history +++ b/container_root/.bash_history @@ -498,3 +498,52 @@ ls tmux ls ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py +ls +clear +cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash +ls +cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash +clear +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py--show-args +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py --show-args +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py sensor_config:=rgbd_imu +cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash +clwar +clear +ls +cd +ls +./launch_slam.sh +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py --show-args +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py sensor_config:=rgbd_imu +cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py sensor_config:=rgbd_imu +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py sensor_config:=mono_imu +cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py sensor_config:=mono_imu +l +./launch_slam.sh +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py sensor_config:=rgbd_imu +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py sensor_config:=mono_imu +ls +cd colcon_ws/ +ls +rm -rf build/ install/ log/ +ls +cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash +ros2 topic hz /imu +ls +clear +ls +clear +ls +cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash +ls +clear +ls +cd +ls +./launch_slam.sh diff --git a/orb_slam3_ros2_wrapper/CMakeLists.txt b/orb_slam3_ros2_wrapper/CMakeLists.txt index a7dc3b92..38161df5 100644 --- a/orb_slam3_ros2_wrapper/CMakeLists.txt +++ b/orb_slam3_ros2_wrapper/CMakeLists.txt @@ -55,24 +55,63 @@ link_directories( include ) -add_executable(rgbd +set(dependencies + rclcpp + sensor_msgs + cv_bridge + message_filters + ORB_SLAM3 + Pangolin + tf2_ros + tf2_eigen + slam_msgs + pcl_ros + pcl_conversions + PCL + nav_msgs + std_srvs +) + +add_library(orb_slam3_ros2_wrapper_common SHARED src/type_conversion.cpp src/orb_slam3_interface.cpp src/time_profiler.cpp + src/slam_node_base.cpp +) +ament_target_dependencies(orb_slam3_ros2_wrapper_common ${dependencies}) + + +add_executable(rgbd src/rgbd/rgbd-slam-node.cpp src/rgbd/rgbd.cpp ) -ament_target_dependencies(rgbd rclcpp sensor_msgs cv_bridge message_filters ORB_SLAM3 Pangolin tf2_ros tf2_eigen slam_msgs pcl_ros pcl_conversions PCL nav_msgs std_srvs) - -# add_executable(test1 -# src/ft.cpp -# src/test_frame.cpp -# ) -# ament_target_dependencies(rgbd rclcpp sensor_msgs cv_bridge message_filters ORB_SLAM3 Pangolin tf2_ros tf2_eigen slam_msgs pcl_ros pcl_conversions PCL nav_msgs) -# ament_target_dependencies(test1 rclcpp sensor_msgs cv_bridge message_filters ORB_SLAM3 Pangolin tf2_ros tf2_eigen slam_msgs pcl_ros pcl_conversions PCL nav_msgs) -# test1 -target_link_libraries(rgbd ${PCL_LIBRARIES}) -install(TARGETS rgbd +ament_target_dependencies(rgbd ${dependencies}) +target_link_libraries(rgbd orb_slam3_ros2_wrapper_common ${PCL_LIBRARIES}) + + +add_executable(rgbd_imu + src/rgbd-imu/rgbd-imu-slam-node.cpp + src/rgbd-imu/rgbd_imu.cpp +) +ament_target_dependencies(rgbd_imu ${dependencies}) +target_link_libraries(rgbd_imu orb_slam3_ros2_wrapper_common ${PCL_LIBRARIES}) + + +add_executable(mono_imu + src/mono-imu/mono-imu-slam-node.cpp + src/mono-imu/mono_imu.cpp +) +ament_target_dependencies(mono_imu ${dependencies}) +target_link_libraries(mono_imu orb_slam3_ros2_wrapper_common ${PCL_LIBRARIES}) + +add_executable(stereo + src/stereo/stereo-slam-node.cpp + src/stereo/stereo.cpp +) +ament_target_dependencies(stereo ${dependencies}) +target_link_libraries(stereo orb_slam3_ros2_wrapper_common ${PCL_LIBRARIES}) + +install(TARGETS orb_slam3_ros2_wrapper_common rgbd rgbd_imu mono_imu stereo DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY launch params diff --git a/orb_slam3_ros2_wrapper/include/orb_slam3_ros2_wrapper/orb_slam3_interface.hpp b/orb_slam3_ros2_wrapper/include/orb_slam3_ros2_wrapper/orb_slam3_interface.hpp index 150c61be..129f0d5e 100644 --- a/orb_slam3_ros2_wrapper/include/orb_slam3_ros2_wrapper/orb_slam3_interface.hpp +++ b/orb_slam3_ros2_wrapper/include/orb_slam3_ros2_wrapper/orb_slam3_interface.hpp @@ -8,6 +8,10 @@ #include #include +#include +#include +#include +#include #include #include @@ -17,6 +21,10 @@ #include "nav_msgs/msg/odometry.hpp" #include "sensor_msgs/msg/imu.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" +#include "std_msgs/msg/header.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" #include #include @@ -68,7 +76,7 @@ namespace ORB_SLAM3_Wrapper */ void mapDataToMsg(slam_msgs::msg::MapData &mapDataMsg, bool currentMapKFOnly, bool includeMapPoints = false, std::vector kFIDforMapPoints = std::vector()); - void correctTrackedPose(Sophus::SE3f &s); + void correctTrackedPose(const Sophus::SE3f &s); void getDirectMapToRobotTF(std_msgs::msg::Header headerToUse, geometry_msgs::msg::TransformStamped &tf); @@ -89,16 +97,11 @@ namespace ORB_SLAM3_Wrapper void mapPointsVisibleFromPose(Sophus::SE3f& cameraPose, std::vector& points, int maxLandmarks, float maxDistance, float maxAngle); - void handleIMU(const sensor_msgs::msg::Imu::SharedPtr msgIMU); + ORB_SLAM3::System* slam() { return mSLAM_.get(); } - bool trackRGBDi(const sensor_msgs::msg::Image::SharedPtr msgRGB, const sensor_msgs::msg::Image::SharedPtr msgD, Sophus::SE3f &Tcw); + ORB_SLAM3::System::eSensor sensor() const { return sensor_; } - bool trackRGBD(const sensor_msgs::msg::Image::SharedPtr msgRGB, const sensor_msgs::msg::Image::SharedPtr msgD, Sophus::SE3f &Tcw); - - std::shared_ptr getTypeConversionPtr() - { - return typeConversions_; - }; + bool processTrackedPose(const Sophus::SE3f& Tcw); void resetLocalMapping(); @@ -109,16 +112,12 @@ namespace ORB_SLAM3_Wrapper private: std::shared_ptr mSLAM_; - std::shared_ptr typeConversions_; ORB_SLAM3::Atlas *orbAtlas_; std::string strVocFile_; std::string strSettingsFile_; ORB_SLAM3::System::eSensor sensor_; bool bUseViewer_; bool loopClosing_; - - queue imuBuf_; - std::mutex bufMutex_; std::mutex mapDataMutex_; std::mutex currentMapPointsMutex_; diff --git a/orb_slam3_ros2_wrapper/include/orb_slam3_ros2_wrapper/slam_node_base.hpp b/orb_slam3_ros2_wrapper/include/orb_slam3_ros2_wrapper/slam_node_base.hpp new file mode 100644 index 00000000..afef4aa4 --- /dev/null +++ b/orb_slam3_ros2_wrapper/include/orb_slam3_ros2_wrapper/slam_node_base.hpp @@ -0,0 +1,120 @@ +/** + * @file slam_node_base.hpp + * @brief Common ROS 2 wrapper base class for ORB-SLAM3 sensor nodes. + */ +#ifndef ORB_SLAM3_ROS2_WRAPPER__SLAM_NODE_BASE_HPP_ +#define ORB_SLAM3_ROS2_WRAPPER__SLAM_NODE_BASE_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include "std_srvs/srv/set_bool.hpp" + +#include +#include +#include +#include +#include + +#include "orb_slam3_ros2_wrapper/type_conversion.hpp" +#include "orb_slam3_ros2_wrapper/orb_slam3_interface.hpp" + +namespace ORB_SLAM3_Wrapper +{ + + class SlamNodeBase : public rclcpp::Node + { + public: + SlamNodeBase(const std::string &node_name, + const std::string &strVocFile, + const std::string &strSettingsFile, + ORB_SLAM3::System::eSensor sensor); + + ~SlamNodeBase() override; + + protected: + // Called by derived sensor nodes when tracking succeeded for a frame. + void onTracked(const std_msgs::msg::Header &stamp_source_header); + + std::shared_ptr interface() { return interface_; } + + private: + void publishMapData(); + + void publishMapPointCloud(std::shared_ptr request_header, + std::shared_ptr request, + std::shared_ptr response); + + void resetActiveMapSrv(std::shared_ptr request_header, + std::shared_ptr request, + std::shared_ptr response); + + void getMapServer(std::shared_ptr request_header, + std::shared_ptr request, + std::shared_ptr response); + + void getMapPointsInViewServer(std::shared_ptr request_header, + std::shared_ptr request, + std::shared_ptr response); + + private: + // ROS Publishers and Subscribers + rclcpp::Subscription::SharedPtr odomSub_; + rclcpp::Publisher::SharedPtr mapDataPub_; + rclcpp::Publisher::SharedPtr mapPointsPub_; + rclcpp::Publisher::SharedPtr visibleLandmarksPub_; + rclcpp::Publisher::SharedPtr visibleLandmarksPose_; + rclcpp::Publisher::SharedPtr robotPoseMapFrame_; + rclcpp::Publisher::SharedPtr slamInfoPub_; + // TF + std::shared_ptr tfBroadcaster_; + std::shared_ptr tfListener_; + std::shared_ptr tfBuffer_; + // ROS Services + rclcpp::Service::SharedPtr getMapDataService_; + rclcpp::Service::SharedPtr getMapPointsService_; + rclcpp::Service::SharedPtr mapPointsService_; + rclcpp::Service::SharedPtr resetLocalMapSrv_; + // ROS Timers + rclcpp::TimerBase::SharedPtr mapDataTimer_; + rclcpp::CallbackGroup::SharedPtr mapDataCallbackGroup_; + rclcpp::CallbackGroup::SharedPtr mapPointsCallbackGroup_; + rclcpp::CallbackGroup::SharedPtr pointsInViewCallbackGroup_; + // ROS Params + std::string robot_base_frame_id_; + std::string odom_frame_id_; + std::string global_frame_; + double robot_x_, robot_y_, robot_z_, robot_qx_, robot_qy_, robot_qz_, robot_qw_; + bool isTracked_ = false; + bool odometry_mode_; + bool publish_tf_; + double frequency_tracker_count_ = 0; + int map_data_publish_frequency_; + bool do_loop_closing_; + std::chrono::_V2::system_clock::time_point frequency_tracker_clock_; + std::shared_ptr interface_; + geometry_msgs::msg::TransformStamped tfMapOdom_; + }; + +} // namespace ORB_SLAM3_Wrapper + +#endif // ORB_SLAM3_ROS2_WRAPPER__SLAM_NODE_BASE_HPP_ diff --git a/orb_slam3_ros2_wrapper/include/orb_slam3_ros2_wrapper/type_conversion.hpp b/orb_slam3_ros2_wrapper/include/orb_slam3_ros2_wrapper/type_conversion.hpp index 112cd149..af4210c1 100644 --- a/orb_slam3_ros2_wrapper/include/orb_slam3_ros2_wrapper/type_conversion.hpp +++ b/orb_slam3_ros2_wrapper/include/orb_slam3_ros2_wrapper/type_conversion.hpp @@ -27,9 +27,8 @@ namespace ORB_SLAM3_Wrapper { - class WrapperTypeConversions + namespace WrapperTypeConversions { - public: // **************************************DATA TYPE CONVERSIONS************************************* /** * @brief Converts a ROS timestamp to seconds. @@ -114,7 +113,7 @@ namespace ORB_SLAM3_Wrapper * @return Transformed pose. */ template - T transformPoseWithReference(Eigen::Affine3f &, Sophus::SE3f &); + T transformPoseWithReference(Eigen::Affine3f &, const Sophus::SE3f &); /** * @brief Transforms a pose using a reference pose and SE3 transform. diff --git a/orb_slam3_ros2_wrapper/launch/mono_imu.launch.py b/orb_slam3_ros2_wrapper/launch/mono_imu.launch.py new file mode 100644 index 00000000..e1fec44d --- /dev/null +++ b/orb_slam3_ros2_wrapper/launch/mono_imu.launch.py @@ -0,0 +1,75 @@ +#!/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" + ) + + 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" + + declare_params_file_cmd = DeclareLaunchArgument( + "params_file", + default_value=os.path.join(orb_wrapper_pkg, "params", "ros_params", "gazebo-mono-imu-ros-params.yaml"), + 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_imu", + 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, + robot_namespace_arg, + opaque_function, + ] + ) + + diff --git a/orb_slam3_ros2_wrapper/launch/rgbd.launch.py b/orb_slam3_ros2_wrapper/launch/rgbd.launch.py index 8e57d186..cfeb025f 100644 --- a/orb_slam3_ros2_wrapper/launch/rgbd.launch.py +++ b/orb_slam3_ros2_wrapper/launch/rgbd.launch.py @@ -36,10 +36,10 @@ def generate_launch_description(): 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/gazebo_rgbd.yaml" + config_file_path = "/root/colcon_ws/src/orb_slam3_ros2_wrapper/params/orb_slam3_params/gazebo_rgbd.yaml" declare_params_file_cmd = DeclareLaunchArgument( 'params_file', - default_value=os.path.join(orb_wrapper_pkg, 'params', 'rgbd-ros-params.yaml'), + default_value=os.path.join(orb_wrapper_pkg, 'params', 'ros_params', 'gazebo-rgbd-ros-params.yaml'), description='Full path to the ROS2 parameters file to use for all launched nodes') base_frame = "" diff --git a/orb_slam3_ros2_wrapper/launch/rgbd_imu.launch.py b/orb_slam3_ros2_wrapper/launch/rgbd_imu.launch.py new file mode 100644 index 00000000..f9d9146e --- /dev/null +++ b/orb_slam3_ros2_wrapper/launch/rgbd_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/gazebo_rgbd_imu.yaml" + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(orb_wrapper_pkg, 'params','ros_params', 'gazebo-rgbd-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='rgbd_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/stereo.launch.py b/orb_slam3_ros2_wrapper/launch/stereo.launch.py new file mode 100644 index 00000000..9c43efc3 --- /dev/null +++ b/orb_slam3_ros2_wrapper/launch/stereo.launch.py @@ -0,0 +1,81 @@ +#!/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/gazebo_stereo.yaml" + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(orb_wrapper_pkg, 'params', 'ros_params', 'gazebo-stereo-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", + 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 73277ede..4fc7305c 100644 --- a/orb_slam3_ros2_wrapper/launch/unirobot.launch.py +++ b/orb_slam3_ros2_wrapper/launch/unirobot.launch.py @@ -1,28 +1,64 @@ import os from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable -from launch.actions import DeclareLaunchArgument, ExecuteProcess +from launch.actions import IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PythonExpression from ament_index_python.packages import get_package_share_directory def generate_launch_description(): - # Set environment variables robot_namespace = "" + sensor_config = LaunchConfiguration("sensor_config") + sensor_config_arg = DeclareLaunchArgument( + "sensor_config", + default_value="rgbd", + description="Sensor configuration to launch: 'rgbd' (default), 'rgbd_imu', 'mono_imu' or 'stereo'") orb_slam3_launch_file_dir = os.path.join( get_package_share_directory('orb_slam3_ros2_wrapper'), 'launch') - orb_slam3_launch_file_path = os.path.join( - orb_slam3_launch_file_dir, 'rgbd.launch.py') + rgbd_launch_file_path = os.path.join( + orb_slam3_launch_file_dir, 'rgbd.launch.py') - # Launch the rgbd.launch.py file - orb_slam3_launch_description = IncludeLaunchDescription( - PythonLaunchDescriptionSource(orb_slam3_launch_file_path), - launch_arguments={"robot_namespace": robot_namespace}.items(), + rgbd_imu_launch_file_path = os.path.join( + orb_slam3_launch_file_dir, 'rgbd_imu.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') + + rgbd_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(rgbd_launch_file_path), + launch_arguments={"robot_namespace": robot_namespace}.items(), + condition=IfCondition(PythonExpression(["'", sensor_config, "' == 'rgbd'"])), + ) + + rgbd_imu_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(rgbd_imu_launch_file_path), + launch_arguments={"robot_namespace": robot_namespace}.items(), + condition=IfCondition(PythonExpression(["'", sensor_config, "' == 'rgbd_imu'"])), + ) + + mono_imu_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(mono_imu_launch_file_path), + launch_arguments={"robot_namespace": robot_namespace}.items(), + condition=IfCondition(PythonExpression(["'", sensor_config, "' == 'mono_imu'"])), + ) + + stereo_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(stereo_launch_file_path), + launch_arguments={"robot_namespace": robot_namespace}.items(), + condition=IfCondition(PythonExpression(["'", sensor_config, "' == 'stereo'"])), ) return LaunchDescription([ - orb_slam3_launch_description + sensor_config_arg, + rgbd_launch, + rgbd_imu_launch, + mono_imu_launch, + stereo_launch, ]) diff --git a/orb_slam3_ros2_wrapper/params/orb_slam3_params/gazebo_mono_imu.yaml b/orb_slam3_ros2_wrapper/params/orb_slam3_params/gazebo_mono_imu.yaml new file mode 100644 index 00000000..4a1468f2 --- /dev/null +++ b/orb_slam3_ros2_wrapper/params/orb_slam3_params/gazebo_mono_imu.yaml @@ -0,0 +1,92 @@ +%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: 432.496042035043 +Camera.fy: 432.496042035043 +Camera.cx: 320.0 +Camera.cy: 240.0 + + +# 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: 480 + +# Camera frames per second +Camera.fps: 30.0 + +# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) +Camera.RGB: 0 + +# Transformation from body-frame (imu) to left camera +Tbc: !!opencv-matrix + rows: 4 + cols: 4 + dt: f + data: [1.000, 0.000, 0.000, 0.000, + 0.000, 1.000, 0.000, 0.000, + 0.000, 0.000, 1.000, 0.000, + 0.000, 0.000, 0.000, 1.000] + +# Do not insert KFs when recently lost +IMU.InsertKFsWhenLost: 0 + +# IMU noise (Use those from VINS-mono) +IMU.NoiseGyro: 1e-9 # 3 # 2.44e-4 #1e-3 # rad/s^0.5 +IMU.NoiseAcc: 1e-9 #2 # 1.47e-3 #1e-2 # m/s^1.5 +IMU.GyroWalk: 1e-9 # rad/s^1.5 +IMU.AccWalk: 1e-9 # m/s^2.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 Parameters +#-------------------------------------------------------------------------------------------- + +System.SaveAtlasToFile: ./atlas + diff --git a/orb_slam3_ros2_wrapper/params/gazebo_rgbd.yaml b/orb_slam3_ros2_wrapper/params/orb_slam3_params/gazebo_rgbd.yaml similarity index 75% rename from orb_slam3_ros2_wrapper/params/gazebo_rgbd.yaml rename to orb_slam3_ros2_wrapper/params/orb_slam3_params/gazebo_rgbd.yaml index de1e3038..e98e1115 100644 --- a/orb_slam3_ros2_wrapper/params/gazebo_rgbd.yaml +++ b/orb_slam3_ros2_wrapper/params/orb_slam3_params/gazebo_rgbd.yaml @@ -38,26 +38,6 @@ 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 - #-------------------------------------------------------------------------------------------- # ORB Parameters #-------------------------------------------------------------------------------------------- @@ -91,5 +71,11 @@ Viewer.ViewpointX: 0 Viewer.ViewpointY: -0.7 Viewer.ViewpointZ: -1.8 Viewer.ViewpointF: 500 + + +#-------------------------------------------------------------------------------------------- +# System Parameters +#-------------------------------------------------------------------------------------------- + System.SaveAtlasToFile: ./atlas diff --git a/orb_slam3_ros2_wrapper/params/orb_slam3_params/gazebo_rgbd_imu.yaml b/orb_slam3_ros2_wrapper/params/orb_slam3_params/gazebo_rgbd_imu.yaml new file mode 100644 index 00000000..90004528 --- /dev/null +++ b/orb_slam3_ros2_wrapper/params/orb_slam3_params/gazebo_rgbd_imu.yaml @@ -0,0 +1,102 @@ +%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: 432.496042035043 +Camera.fy: 432.496042035043 +Camera.cx: 320.0 +Camera.cy: 240.0 + + +# 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: 480 + +# Camera frames per second +Camera.fps: 30.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 body-frame (imu) to left camera +Tbc: !!opencv-matrix + rows: 4 + cols: 4 + dt: f + data: [1.000, 0.000, 0.000, 0.000, + 0.000, 1.000, 0.000, 0.000, + 0.000, 0.000, 1.000, 0.000, + 0.000, 0.000, 0.000, 1.000] + + +# Do not insert KFs when recently lost +IMU.InsertKFsWhenLost: 0 + +# IMU noise (Use those from VINS-mono) +IMU.NoiseGyro: 1e-9 # 3 # 2.44e-4 #1e-3 # rad/s^0.5 +IMU.NoiseAcc: 1e-9 #2 # 1.47e-3 #1e-2 # m/s^1.5 +IMU.GyroWalk: 1e-9 # rad/s^1.5 +IMU.AccWalk: 1e-9 # m/s^2.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 Parameters +#-------------------------------------------------------------------------------------------- + +System.SaveAtlasToFile: ./atlas + diff --git a/orb_slam3_ros2_wrapper/params/orb_slam3_params/gazebo_stereo.yaml b/orb_slam3_ros2_wrapper/params/orb_slam3_params/gazebo_stereo.yaml new file mode 100644 index 00000000..531833b5 --- /dev/null +++ b/orb_slam3_ros2_wrapper/params/orb_slam3_params/gazebo_stereo.yaml @@ -0,0 +1,96 @@ +%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 + +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] + +#-------------------------------------------------------------------------------------------- +# 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/gazebo-mono-imu-ros-params.yaml b/orb_slam3_ros2_wrapper/params/ros_params/gazebo-mono-imu-ros-params.yaml new file mode 100644 index 00000000..8e68c709 --- /dev/null +++ b/orb_slam3_ros2_wrapper/params/ros_params/gazebo-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: rgb_camera + imu_topic_name: imu + + # 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/gazebo-rgbd-imu-ros-params.yaml b/orb_slam3_ros2_wrapper/params/ros_params/gazebo-rgbd-imu-ros-params.yaml new file mode 100644 index 00000000..e8c471e2 --- /dev/null +++ b/orb_slam3_ros2_wrapper/params/ros_params/gazebo-rgbd-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 rgbd.launch.py file. +# If the robot namespace is "robot_0" then the frame_ids become "robot_0/camera_link" + +ORB_SLAM3_IMU_RGBD_ROS2: + ros__parameters: + # slam related + robot_base_frame: base_footprint + global_frame: map + odom_frame: odom + rgb_image_topic_name: rgb_camera + depth_image_topic_name: depth_camera + imu_topic_name: imu + + # 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/rgbd-ros-params.yaml b/orb_slam3_ros2_wrapper/params/ros_params/gazebo-rgbd-ros-params.yaml similarity index 92% rename from orb_slam3_ros2_wrapper/params/rgbd-ros-params.yaml rename to orb_slam3_ros2_wrapper/params/ros_params/gazebo-rgbd-ros-params.yaml index 0e716955..784aa54d 100644 --- a/orb_slam3_ros2_wrapper/params/rgbd-ros-params.yaml +++ b/orb_slam3_ros2_wrapper/params/ros_params/gazebo-rgbd-ros-params.yaml @@ -10,7 +10,6 @@ ORB_SLAM3_RGBD_ROS2: odom_frame: odom rgb_image_topic_name: rgb_camera depth_image_topic_name: depth_camera - imu_topic_name: imu #Not used in this variant. Try RGB_IMU to use this information. # 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. diff --git a/orb_slam3_ros2_wrapper/params/ros_params/gazebo-stereo-ros-params.yaml b/orb_slam3_ros2_wrapper/params/ros_params/gazebo-stereo-ros-params.yaml new file mode 100644 index 00000000..7220f3ce --- /dev/null +++ b/orb_slam3_ros2_wrapper/params/ros_params/gazebo-stereo-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 stereo.launch.py file. +# If the robot namespace is "robot_0" then the frame_ids become "robot_0/camera_link" + +ORB_SLAM3_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 + + # 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/src/mono-imu/mono-imu-slam-node.cpp b/orb_slam3_ros2_wrapper/src/mono-imu/mono-imu-slam-node.cpp new file mode 100644 index 00000000..655931f4 --- /dev/null +++ b/orb_slam3_ros2_wrapper/src/mono-imu/mono-imu-slam-node.cpp @@ -0,0 +1,113 @@ +/** + * @file mono-imu-slam-node.cpp + * @brief Implementation of the MonoImuSlamNode Wrapper class. + */ + +#include "mono-imu-slam-node.hpp" + +#include +#include + +namespace ORB_SLAM3_Wrapper +{ + using namespace WrapperTypeConversions; + + MonoImuSlamNode::MonoImuSlamNode(const std::string &strVocFile, + const std::string &strSettingsFile, + ORB_SLAM3::System::eSensor sensor) + : SlamNodeBase("ORB_SLAM3_IMU_MONO_ROS2", strVocFile, strSettingsFile, sensor) + { + // Declare parameters (topic names) + this->declare_parameter("image_topic_name", rclcpp::ParameterValue("camera/image_raw")); + this->declare_parameter("imu_topic_name", rclcpp::ParameterValue("imu")); + + // Image subscriber (frame callback) + imageSub_ = this->create_subscription( + this->get_parameter("image_topic_name").as_string(), + rclcpp::SensorDataQoS(), + std::bind(&MonoImuSlamNode::ImageCallback, this, std::placeholders::_1)); + + // 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(&MonoImuSlamNode::ImuCallback, this, std::placeholders::_1), + imuSubOptions); + + RCLCPP_INFO(this->get_logger(), "Mono-IMU node started. Image: %s | IMU: %s", + this->get_parameter("image_topic_name").as_string().c_str(), + this->get_parameter("imu_topic_name").as_string().c_str()); + } + + MonoImuSlamNode::~MonoImuSlamNode() + { + imageSub_.reset(); + imuSub_.reset(); + RCLCPP_INFO(this->get_logger(), "Mono-IMU node stopped."); + } + + void MonoImuSlamNode::ImuCallback(const sensor_msgs::msg::Imu::SharedPtr msgImu) + { + std::lock_guard lock(imuMutex_); + imuBuf_.push(msgImu); + } + + void MonoImuSlamNode::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); + + // Collect IMU measurements up to this image timestamp (Euroc-style: everything since last frame). + std::vector vImuMeas; + { + std::lock_guard lock(imuMutex_); + while (!imuBuf_.empty() && stampToSec(imuBuf_.front()->header.stamp) <= tImg) + { + 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 image t=%.6f (waiting for IMU...)", tImg); + return; + } + + // Track monocular image with IMU measurements. + auto Tcw = interface()->slam()->TrackMonocular(cvImg->image, tImg, vImuMeas); + + // 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-imu/mono-imu-slam-node.hpp b/orb_slam3_ros2_wrapper/src/mono-imu/mono-imu-slam-node.hpp new file mode 100644 index 00000000..e99fbb1c --- /dev/null +++ b/orb_slam3_ros2_wrapper/src/mono-imu/mono-imu-slam-node.hpp @@ -0,0 +1,45 @@ +/** + * @file mono-imu-slam-node.hpp + * @brief Definition of the MonoImuSlamNode Wrapper class. + */ + +#ifndef MONO_IMU_SLAM_NODE_HPP_ +#define MONO_IMU_SLAM_NODE_HPP_ + +#include +#include + +#include +#include + +#include "orb_slam3_ros2_wrapper/slam_node_base.hpp" + +namespace ORB_SLAM3_Wrapper +{ + class MonoImuSlamNode : public SlamNodeBase + { + public: + MonoImuSlamNode(const std::string &strVocFile, + const std::string &strSettingsFile, + ORB_SLAM3::System::eSensor sensor); + ~MonoImuSlamNode(); + + private: + void ImuCallback(const sensor_msgs::msg::Imu::SharedPtr msgImu); + void ImageCallback(const sensor_msgs::msg::Image::SharedPtr msgImg); + + // Image subscription + rclcpp::Subscription::SharedPtr imageSub_; + + // Dedicated callback group for IMU subscription (allows independent scheduling vs image callback). + rclcpp::CallbackGroup::SharedPtr imuCallbackGroup_; + rclcpp::Subscription::SharedPtr imuSub_; + + std::mutex imuMutex_; + std::queue imuBuf_; + }; +} // namespace ORB_SLAM3_Wrapper + +#endif // MONO_IMU_SLAM_NODE_HPP_ + + diff --git a/orb_slam3_ros2_wrapper/src/mono-imu/mono_imu.cpp b/orb_slam3_ros2_wrapper/src/mono-imu/mono_imu.cpp new file mode 100644 index 00000000..3970b6dd --- /dev/null +++ b/orb_slam3_ros2_wrapper/src/mono-imu/mono_imu.cpp @@ -0,0 +1,27 @@ +#include + +#include "rclcpp/rclcpp.hpp" +#include "mono-imu-slam-node.hpp" + +int main(int argc, char **argv) +{ + if (argc < 3) + { + std::cerr << "\nUsage: ros2 run orb_slam3_ros2_wrapper mono_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_MONOCULAR); + + auto executor = std::make_shared(); + executor->add_node(node); + executor->spin(); + rclcpp::shutdown(); + + return 0; +} + + diff --git a/orb_slam3_ros2_wrapper/src/orb_slam3_interface.cpp b/orb_slam3_ros2_wrapper/src/orb_slam3_interface.cpp index c002f22c..657ffe2e 100644 --- a/orb_slam3_ros2_wrapper/src/orb_slam3_interface.cpp +++ b/orb_slam3_ros2_wrapper/src/orb_slam3_interface.cpp @@ -7,6 +7,7 @@ namespace ORB_SLAM3_Wrapper { + using namespace WrapperTypeConversions; ORBSLAM3Interface::ORBSLAM3Interface(const std::string &strVocFile, const std::string &strSettingsFile, ORB_SLAM3::System::eSensor sensor, @@ -28,7 +29,7 @@ namespace ORB_SLAM3_Wrapper { std::cout << "Interface constructor started" << endl; mSLAM_ = std::make_shared(strVocFile_, strSettingsFile_, sensor_, bUseViewer_, loopClosing); - typeConversions_ = std::make_shared(); + orbAtlas_ = mSLAM_->GetAtlas(); time_profiler_ = TimeProfiler::getInstance(); std::cout << "Interface constructor complete" << endl; robotBase_to_cameraLink_ = Eigen::Affine3f( @@ -42,10 +43,8 @@ namespace ORB_SLAM3_Wrapper std::cout << "Interface destructor" << endl; mSLAM_->Shutdown(); mSLAM_.reset(); - typeConversions_.reset(); mapReferencePoses_.clear(); allKFs_.clear(); - delete time_profiler_; } std::unordered_map ORBSLAM3Interface::makeKFIdPair(std::vector mapsList) @@ -99,7 +98,7 @@ namespace ORB_SLAM3_Wrapper // std::cout << "Map ID: " << mapsList[c]->GetId() << " origin kf ID: " << mapsList[c]->GetOriginKF()->mnId << " init kf id: " << mapsList[c]->GetInitKFid() << " max kf id: " << mapsList[c]->GetMaxKFid() << std::endl; if (c == 0) { - auto poseWithoutOffset = typeConversions_->se3ToAffine(mapsList[c]->GetOriginKF()->GetPose()); + auto poseWithoutOffset = se3ToAffine(mapsList[c]->GetOriginKF()->GetPose()); // transform from map_orb -> camera mapReferencePoses_[mapsList[c]] = poseWithoutOffset; } @@ -126,7 +125,7 @@ namespace ORB_SLAM3_Wrapper } auto parentMapORBPose = allKFs_[parentMapID]->GetPose(); // transform from map_orb -> camera - mapReferencePoses_[mapsList[c]] = typeConversions_->transformPoseWithReference(mapReferencePoses_[allKFs_[parentMapID]->GetMap()], parentMapORBPose); + mapReferencePoses_[mapsList[c]] = transformPoseWithReference(mapReferencePoses_[allKFs_[parentMapID]->GetMap()], parentMapORBPose); } } // used for reset map service. To contonue tracking where it was left off. @@ -150,14 +149,14 @@ namespace ORB_SLAM3_Wrapper { if (!mapPoint->isBad()) { - auto worldPos = typeConversions_->vector3fORBToROS(mapPoint->GetWorldPos()); + auto worldPos = vector3fORBToROS(mapPoint->GetWorldPos()); mapReferencesMutex_.lock(); if (allKFs_.count(KF->mnId) == 0) { mapReferencesMutex_.unlock(); continue; } - auto mapPointWorld = typeConversions_->transformPointWithReference(mapReferencePoses_[allKFs_[KF->mnId]->GetMap()], worldPos); + auto mapPointWorld = transformPointWithReference(mapReferencePoses_[allKFs_[KF->mnId]->GetMap()], worldPos); // robotBase_to_cameraLink_ is map_ros->map_orb mapPointWorld = robotBase_to_cameraLink_ * mapPointWorld; mapReferencesMutex_.unlock(); @@ -165,14 +164,14 @@ namespace ORB_SLAM3_Wrapper } } } - mapPointCloud = typeConversions_->MapPointsToPCL(trackedMapPoints); + mapPointCloud = MapPointsToPCL(trackedMapPoints); } void ORBSLAM3Interface::mapPointsVisibleFromPose(geometry_msgs::msg::Pose cameraPose, std::vector& points, int maxLandmarks, float maxDistance, float maxAngle) { - auto T_mapros_base = typeConversions_->poseToAffine(cameraPose); + auto T_mapros_base = poseToAffine(cameraPose); auto T_maporb_cam = robotBase_to_cameraLink_.inverse() * T_mapros_base * robotBase_to_cameraLink_; - auto camPose = typeConversions_->se3ROSToORB(T_maporb_cam); + auto camPose = se3ROSToORB(T_maporb_cam); mapPointsVisibleFromPose(camPose, points, maxLandmarks, maxDistance, maxAngle); } @@ -187,7 +186,7 @@ namespace ORB_SLAM3_Wrapper auto Twc = Tcw.inverse(); std::cout << "Input camera pose: " << std::endl; - auto eulerAngles = typeConversions_->rotationORBToEulerROS(Twc.rotationMatrix()); + auto eulerAngles = rotationORBToEulerROS(Twc.rotationMatrix()); std::cout << "Translation:" << Twc.translation().transpose() << std::endl; std::cout << "Roll ROS coord: " << eulerAngles(0) << std::endl; std::cout << "Pitch ROS coord: " << eulerAngles(1) << std::endl; @@ -218,7 +217,7 @@ namespace ORB_SLAM3_Wrapper for(auto pKFMp : mapKFs_) { auto rel_T_CamToKF = Tcw * pKFMp->GetPoseInverse(); - auto relEulerAngles = typeConversions_->rotationORBToEulerROS(rel_T_CamToKF.rotationMatrix()); + auto relEulerAngles = rotationORBToEulerROS(rel_T_CamToKF.rotationMatrix()); float distBwKfs = rel_T_CamToKF.translation().norm(); // checks ...... @@ -323,11 +322,11 @@ namespace ORB_SLAM3_Wrapper { if (!mapPoint->isBad()) { - auto worldPos = typeConversions_->vector3fORBToROS(mapPoint->GetWorldPos()); + auto worldPos = vector3fORBToROS(mapPoint->GetWorldPos()); mapReferencesMutex_.lock(); - auto mapPointWorld = typeConversions_->transformPointWithReference(mapReferencePoses_[allKFs_[kFId]->GetMap()], worldPos); + auto mapPointWorld = transformPointWithReference(mapReferencePoses_[allKFs_[kFId]->GetMap()], worldPos); mapPointWorld = robotBase_to_cameraLink_ * mapPointWorld; - auto mapPointWorldMsg = typeConversions_->eigenToPointMsg(mapPointWorld); + auto mapPointWorldMsg = eigenToPointMsg(mapPointWorld); mapReferencesMutex_.unlock(); pushedKf.word_pts.push_back(mapPointWorldMsg); } @@ -340,14 +339,13 @@ namespace ORB_SLAM3_Wrapper } } } - mapDataMutex_.unlock(); } - void ORBSLAM3Interface::correctTrackedPose(Sophus::SE3f &s) + void ORBSLAM3Interface::correctTrackedPose(const Sophus::SE3f &s) { std::lock_guard lock(latestTrackedPoseMutex_); mapReferencesMutex_.lock(); - latestTrackedPoseORB_camera_ = typeConversions_->transformPoseWithReference( + latestTrackedPoseORB_camera_ = transformPoseWithReference( mapReferencePoses_[orbAtlas_->GetCurrentMap()], s); latestTrackedPose_ = robotBase_to_cameraLink_ * latestTrackedPoseORB_camera_ * robotBase_to_cameraLink_.inverse(); mapReferencesMutex_.unlock(); @@ -369,7 +367,7 @@ namespace ORB_SLAM3_Wrapper std::lock_guard lock(latestTrackedPoseMutex_); // get transform between map and odom and send the transform. auto poseInMap = latestTrackedPose_; - geometry_msgs::msg::Pose poseInMapROS = typeConversions_->affine3fToPose(poseInMap); + geometry_msgs::msg::Pose poseInMapROS = affine3fToPose(poseInMap); rclcpp::Duration transformTimeout_ = rclcpp::Duration::from_seconds(0.5); rclcpp::Time odomTimestamp = headerToUse.stamp; tf.header.stamp = odomTimestamp + transformTimeout_; @@ -401,7 +399,7 @@ namespace ORB_SLAM3_Wrapper // get transform between map and odom and send the transform. std::lock_guard lock(latestTrackedPoseMutex_); auto tfMapOdom = latestTrackedPose_ * latestOdomTransform_.inverse(); - geometry_msgs::msg::Pose poseMapOdom = typeConversions_->affine3fToPose(tfMapOdom); + geometry_msgs::msg::Pose poseMapOdom = affine3fToPose(tfMapOdom); rclcpp::Duration transformTimeout_ = rclcpp::Duration::from_seconds(0.5); rclcpp::Time odomTimestamp = odomToBaseTf.header.stamp; tf.header.stamp = odomTimestamp + transformTimeout_; @@ -418,7 +416,7 @@ namespace ORB_SLAM3_Wrapper { std::lock_guard lock(latestTrackedPoseMutex_); pose.header.frame_id = globalFrame_; - pose.pose = typeConversions_->affine3fToPose(latestTrackedPose_); + pose.pose = affine3fToPose(latestTrackedPose_); } void ORBSLAM3Interface::getOptimizedPoseGraph(slam_msgs::msg::MapGraph &graph, bool currentMapKFOnly) @@ -431,12 +429,12 @@ namespace ORB_SLAM3_Wrapper Sophus::SE3f kfPose = kf->GetPose(); geometry_msgs::msg::PoseStamped kfPoseStamped; mapReferencesMutex_.lock(); - auto affinePose = typeConversions_->transformPoseWithReference(mapReferencePoses_[kf->GetMap()], kfPose); + auto affinePose = transformPoseWithReference(mapReferencePoses_[kf->GetMap()], kfPose); affinePose = robotBase_to_cameraLink_ * affinePose * robotBase_to_cameraLink_.inverse(); - kfPoseStamped.pose = typeConversions_->affine3fToPose(affinePose); + kfPoseStamped.pose = affine3fToPose(affinePose); mapReferencesMutex_.unlock(); kfPoseStamped.header.frame_id = globalFrame_; - kfPoseStamped.header.stamp = typeConversions_->secToStamp(kf->mTimeStamp); + kfPoseStamped.header.stamp = secToStamp(kf->mTimeStamp); graph.poses.push_back(kfPoseStamped); graph.poses_id.push_back(kf->mnId); } @@ -453,11 +451,11 @@ namespace ORB_SLAM3_Wrapper mapReferencesMutex_.unlock(); Sophus::SE3f kFPose = pKFcurr->GetPose(); geometry_msgs::msg::PoseStamped poseStamped; - auto affinePose = typeConversions_->transformPoseWithReference(currReferencePose_, kFPose); + auto affinePose = transformPoseWithReference(currReferencePose_, kFPose); affinePose = robotBase_to_cameraLink_ * affinePose * robotBase_to_cameraLink_.inverse(); - poseStamped.pose = typeConversions_->affine3fToPose(affinePose); + poseStamped.pose = affine3fToPose(affinePose); poseStamped.header.frame_id = globalFrame_; - poseStamped.header.stamp = typeConversions_->secToStamp(pKFcurr->mTimeStamp); + poseStamped.header.stamp = secToStamp(pKFcurr->mTimeStamp); // push to pose graph. graph.poses.push_back(poseStamped); graph.poses_id.push_back(pKFcurr->mnId); @@ -465,123 +463,8 @@ namespace ORB_SLAM3_Wrapper } } - void ORBSLAM3Interface::handleIMU(const sensor_msgs::msg::Imu::SharedPtr msgIMU) - { - bufMutex_.lock(); - imuBuf_.push(msgIMU); - bufMutex_.unlock(); - } - - bool ORBSLAM3Interface::trackRGBDi(const sensor_msgs::msg::Image::SharedPtr msgRGB, const sensor_msgs::msg::Image::SharedPtr msgD, Sophus::SE3f &Tcw) + bool ORBSLAM3Interface::processTrackedPose(const Sophus::SE3f& Tcw) { - orbAtlas_ = mSLAM_->GetAtlas(); - cv_bridge::CvImageConstPtr cvRGB; - cv_bridge::CvImageConstPtr cvD; - // Copy the ros rgb image message to cv::Mat. - try - { - cvRGB = cv_bridge::toCvShare(msgRGB); - } - catch (cv_bridge::Exception &e) - { - std::cerr << "cv_bridge exception RGB!" << endl; - return false; - } - - // Copy the ros depth image message to cv::Mat. - try - { - cvD = cv_bridge::toCvShare(msgD); - } - catch (cv_bridge::Exception &e) - { - std::cerr << "cv_bridge exception D!" << endl; - return false; - } - - vector vImuMeas; - bufMutex_.lock(); - if (!imuBuf_.empty()) - { - // Load imu measurements from buffer - vImuMeas.clear(); - while (!imuBuf_.empty() && typeConversions_->stampToSec(imuBuf_.front()->header.stamp) <= std::min(typeConversions_->stampToSec(msgRGB->header.stamp), typeConversions_->stampToSec(msgD->header.stamp))) - { - double t = typeConversions_->stampToSec(imuBuf_.front()->header.stamp); - cv::Point3f acc(imuBuf_.front()->linear_acceleration.x, imuBuf_.front()->linear_acceleration.y, imuBuf_.front()->linear_acceleration.z); - 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(); - } - } - bufMutex_.unlock(); - if (imuBuf_.size() > 0) - { - // track the frame. - Tcw = mSLAM_->TrackRGBD(cvRGB->image, cvD->image, typeConversions_->stampToSec(msgRGB->header.stamp), vImuMeas); - auto currentTrackingState = mSLAM_->GetTrackingState(); - auto orbLoopClosing = mSLAM_->GetLoopClosing(); - if (loopClosing_ && orbLoopClosing->mergeDetected()) - { - // do not publish any values during map merging. This is because the reference poses change. - std::cout << "Waiting for merge to finish." << endl; - return false; - } - if (currentTrackingState == 2) - { - calculateReferencePoses(); - correctTrackedPose(Tcw); - hasTracked_ = true; - return true; - } - else - { - switch (currentTrackingState) - { - case 0: - std::cerr << "ORB-SLAM failed: No images yet." << endl; - break; - case 1: - std::cerr << "ORB-SLAM failed: Not initialized." << endl; - break; - case 3: - std::cerr << "ORB-SLAM failed: Tracking LOST." << endl; - break; - } - return false; - } - } - return false; - } - - bool ORBSLAM3Interface::trackRGBD(const sensor_msgs::msg::Image::SharedPtr msgRGB, const sensor_msgs::msg::Image::SharedPtr msgD, Sophus::SE3f &Tcw) - { - orbAtlas_ = mSLAM_->GetAtlas(); - cv_bridge::CvImageConstPtr cvRGB; - cv_bridge::CvImageConstPtr cvD; - // Copy the ros rgb image message to cv::Mat. - try - { - cvRGB = cv_bridge::toCvShare(msgRGB); - } - catch (cv_bridge::Exception &e) - { - std::cerr << "cv_bridge exception RGB!" << endl; - return false; - } - - // Copy the ros depth image message to cv::Mat. - try - { - cvD = cv_bridge::toCvShare(msgD); - } - catch (cv_bridge::Exception &e) - { - std::cerr << "cv_bridge exception D!" << endl; - return false; - } - // track the frame. - Tcw = mSLAM_->TrackRGBD(cvRGB->image, cvD->image, typeConversions_->stampToSec(msgRGB->header.stamp)); auto currentTrackingState = mSLAM_->GetTrackingState(); auto orbLoopClosing = mSLAM_->GetLoopClosing(); if (loopClosing_ && orbLoopClosing->mergeDetected()) diff --git a/orb_slam3_ros2_wrapper/src/rgbd-imu/rgbd-imu-slam-node.cpp b/orb_slam3_ros2_wrapper/src/rgbd-imu/rgbd-imu-slam-node.cpp new file mode 100644 index 00000000..621879e4 --- /dev/null +++ b/orb_slam3_ros2_wrapper/src/rgbd-imu/rgbd-imu-slam-node.cpp @@ -0,0 +1,123 @@ +/** + * @file rgbd-imu-slam-node.cpp + * @brief Implementation of the RgbdImuSlamNode Wrapper class. + * @author Suchetan R S (rssuchetan@gmail.com) + */ +#include "rgbd-imu-slam-node.hpp" + +#include +#include + +namespace ORB_SLAM3_Wrapper +{ + using namespace WrapperTypeConversions; + + RgbdImuSlamNode::RgbdImuSlamNode(const std::string &strVocFile, + const std::string &strSettingsFile, + ORB_SLAM3::System::eSensor sensor) + : SlamNodeBase("ORB_SLAM3_IMU_RGBD_ROS2", strVocFile, strSettingsFile, sensor) + { + // Declare parameters (topic names) + this->declare_parameter("rgb_image_topic_name", rclcpp::ParameterValue("camera/image_raw")); + this->declare_parameter("depth_image_topic_name", rclcpp::ParameterValue("depth/image_raw")); + this->declare_parameter("imu_topic_name", rclcpp::ParameterValue("imu")); + + // Subscribers: RGB + Depth synchronized, IMU buffered + rgbSub_ = std::make_shared>(this, this->get_parameter("rgb_image_topic_name").as_string()); + depthSub_ = std::make_shared>(this, this->get_parameter("depth_image_topic_name").as_string()); + syncApproximate_ = std::make_shared>(approximate_sync_policy(10), *rgbSub_, *depthSub_); + syncApproximate_->registerCallback(&RgbdImuSlamNode::RGBDCallback, 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(&RgbdImuSlamNode::ImuCallback, this, std::placeholders::_1), + imuSubOptions); + + RCLCPP_INFO(this->get_logger(), "RGBD-IMU node started. RGB: %s | Depth: %s | IMU: %s", + this->get_parameter("rgb_image_topic_name").as_string().c_str(), + this->get_parameter("depth_image_topic_name").as_string().c_str(), + this->get_parameter("imu_topic_name").as_string().c_str()); + } + + RgbdImuSlamNode::~RgbdImuSlamNode() + { + rgbSub_.reset(); + depthSub_.reset(); + syncApproximate_.reset(); + imuSub_.reset(); + RCLCPP_INFO(this->get_logger(), "RGBD-IMU node stopped."); + } + + void RgbdImuSlamNode::ImuCallback(const sensor_msgs::msg::Imu::SharedPtr msgImu) + { + std::lock_guard lock(imuMutex_); + imuBuf_.push(msgImu); + } + + void RgbdImuSlamNode::RGBDCallback(const sensor_msgs::msg::Image::SharedPtr msgRGB, const sensor_msgs::msg::Image::SharedPtr msgD) + { + cv_bridge::CvImageConstPtr cvRGB; + cv_bridge::CvImageConstPtr cvD; + // Copy the ros rgb image message to cv::Mat. + try + { + cvRGB = cv_bridge::toCvShare(msgRGB); + } + catch (cv_bridge::Exception &e) + { + std::cerr << "cv_bridge exception RGB!" << endl; + return; + } + + // Copy the ros depth image message to cv::Mat. + try + { + cvD = cv_bridge::toCvShare(msgD); + } + catch (cv_bridge::Exception &e) + { + std::cerr << "cv_bridge exception D!" << endl; + return; + } + + vector vImuMeas; + { + std::lock_guard lock(imuMutex_); + if (!imuBuf_.empty()) + { + // Load imu measurements from buffer + vImuMeas.clear(); + const double tFrame = std::min(stampToSec(msgRGB->header.stamp), stampToSec(msgD->header.stamp)); + while (!imuBuf_.empty() && stampToSec(imuBuf_.front()->header.stamp) <= tFrame) + { + double t = stampToSec(imuBuf_.front()->header.stamp); + cv::Point3f acc(imuBuf_.front()->linear_acceleration.x, imuBuf_.front()->linear_acceleration.y, imuBuf_.front()->linear_acceleration.z); + 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(this->get_logger(), "No IMU measurements between two RGBD frames!"); + return; + } + + // track the frame. + auto Tcw = interface()->slam()->TrackRGBD(cvRGB->image, cvD->image, stampToSec(msgRGB->header.stamp), vImuMeas); + + // process the tracked pose. + if (interface()->processTrackedPose(Tcw)) + { + // Use RGB timestamp as the source stamp for TF/pose publishing. + this->onTracked(msgRGB->header); + } + } +} diff --git a/orb_slam3_ros2_wrapper/src/rgbd-imu/rgbd-imu-slam-node.hpp b/orb_slam3_ros2_wrapper/src/rgbd-imu/rgbd-imu-slam-node.hpp new file mode 100644 index 00000000..df63db19 --- /dev/null +++ b/orb_slam3_ros2_wrapper/src/rgbd-imu/rgbd-imu-slam-node.hpp @@ -0,0 +1,61 @@ +/** + * @file rgbd-imu-slam-node.hpp + * @brief Definition of the RgbdImuSlamNode Wrapper class. + * @author Suchetan R S (rssuchetan@gmail.com) + */ + +#ifndef RGBD_IMU_SLAM_NODE_HPP_ +#define RGBD_IMU_SLAM_NODE_HPP_ + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include "orb_slam3_ros2_wrapper/slam_node_base.hpp" + +namespace ORB_SLAM3_Wrapper +{ + class RgbdImuSlamNode : public SlamNodeBase + { + public: + RgbdImuSlamNode(const std::string &strVocFile, + const std::string &strSettingsFile, + ORB_SLAM3::System::eSensor sensor); + ~RgbdImuSlamNode(); + + private: + typedef message_filters::sync_policies::ApproximateTime approximate_sync_policy; + + // ROS 2 Callbacks + void ImuCallback(const sensor_msgs::msg::Imu::SharedPtr msgImu); + void RGBDCallback(const sensor_msgs::msg::Image::SharedPtr msgRGB, + const sensor_msgs::msg::Image::SharedPtr msgD); + + /** + * Member variables + */ + // RGBD Sensor specifics + std::shared_ptr> rgbSub_; + std::shared_ptr> depthSub_; + std::shared_ptr> syncApproximate_; + + // Dedicated callback group for IMU subscription (allows independent scheduling vs RGBD callback). + rclcpp::CallbackGroup::SharedPtr imuCallbackGroup_; + rclcpp::Subscription::SharedPtr imuSub_; + std::mutex imuMutex_; + std::queue imuBuf_; + }; +} +#endif + + diff --git a/orb_slam3_ros2_wrapper/src/rgbd-imu/rgbd_imu.cpp b/orb_slam3_ros2_wrapper/src/rgbd-imu/rgbd_imu.cpp new file mode 100644 index 00000000..d3a2faa6 --- /dev/null +++ b/orb_slam3_ros2_wrapper/src/rgbd-imu/rgbd_imu.cpp @@ -0,0 +1,28 @@ +#include + +#include "rclcpp/rclcpp.hpp" +#include "rgbd-imu-slam-node.hpp" + +int main(int argc, char **argv) +{ + if (argc < 3) + { + std::cerr << "\nUsage: ros2 run orb_slam3_ros2_wrapper imu_rgbd 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_RGBD); + + auto executor = std::make_shared(); + executor->add_node(node); + executor->spin(); + rclcpp::shutdown(); + + return 0; +} + + + diff --git a/orb_slam3_ros2_wrapper/src/rgbd/rgbd-slam-node.cpp b/orb_slam3_ros2_wrapper/src/rgbd/rgbd-slam-node.cpp index 433a6fb9..1becd91c 100644 --- a/orb_slam3_ros2_wrapper/src/rgbd/rgbd-slam-node.cpp +++ b/orb_slam3_ros2_wrapper/src/rgbd/rgbd-slam-node.cpp @@ -6,18 +6,19 @@ #include "rgbd-slam-node.hpp" #include +#include namespace ORB_SLAM3_Wrapper { + using namespace WrapperTypeConversions; RgbdSlamNode::RgbdSlamNode(const std::string &strVocFile, const std::string &strSettingsFile, ORB_SLAM3::System::eSensor sensor) - : Node("ORB_SLAM3_RGBD_ROS2") + : SlamNodeBase("ORB_SLAM3_RGBD_ROS2", strVocFile, strSettingsFile, sensor) { // Declare parameters (topic names) this->declare_parameter("rgb_image_topic_name", rclcpp::ParameterValue("camera/image_raw")); this->declare_parameter("depth_image_topic_name", rclcpp::ParameterValue("depth/image_raw")); - this->declare_parameter("imu_topic_name", rclcpp::ParameterValue("imu")); // ROS Subscribers rgbSub_ = std::make_shared>(this, this->get_parameter("rgb_image_topic_name").as_string()); @@ -25,99 +26,6 @@ namespace ORB_SLAM3_Wrapper syncApproximate_ = std::make_shared>(approximate_sync_policy(10), *rgbSub_, *depthSub_); syncApproximate_->registerCallback(&RgbdSlamNode::RGBDCallback, this); - imuSub_ = this->create_subscription(this->get_parameter("imu_topic_name").as_string(), 1000, std::bind(&RgbdSlamNode::ImuCallback, this, std::placeholders::_1)); - - // ROS Publishers - //---- the following is published when a service is called - mapPointsPub_ = this->create_publisher("map_points", 10); - visibleLandmarksPub_ = this->create_publisher("visible_landmarks", 10); - visibleLandmarksPose_ = this->create_publisher("visible_landmarks_pose", 10); - slamInfoPub_ = this->create_publisher("slam_info", 10); - //---- the following is published continously - mapDataPub_ = this->create_publisher("map_data", 10); - robotPoseMapFrame_ = this->create_publisher("robot_pose_slam", 10); - - // Services - getMapDataService_ = this->create_service("orb_slam3/get_map_data", std::bind(&RgbdSlamNode::getMapServer, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - pointsInViewCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - getMapPointsService_ = this->create_service("orb_slam3/get_landmarks_in_view", std::bind(&RgbdSlamNode::getMapPointsInViewServer, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, pointsInViewCallbackGroup_); - mapPointsCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - mapPointsService_ = this->create_service("orb_slam3/get_all_landmarks_in_map", std::bind(&RgbdSlamNode::publishMapPointCloud, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, mapPointsCallbackGroup_); - resetLocalMapSrv_ = this->create_service("orb_slam3/reset_mapping", std::bind(&RgbdSlamNode::resetActiveMapSrv, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, mapPointsCallbackGroup_); - - // TF - tfBroadcaster_ = std::make_shared(this); - tfBuffer_ = std::make_shared(this->get_clock()); - tfListener_ = std::make_shared(*tfBuffer_); - - bool bUseViewer; - this->declare_parameter("visualization", rclcpp::ParameterValue(true)); - this->get_parameter("visualization", bUseViewer); - - this->declare_parameter("robot_base_frame", "base_footprint"); - this->get_parameter("robot_base_frame", robot_base_frame_id_); - - this->declare_parameter("global_frame", "map"); - this->get_parameter("global_frame", global_frame_); - - this->declare_parameter("odom_frame", "odom"); - this->get_parameter("odom_frame", odom_frame_id_); - - this->declare_parameter("robot_x", rclcpp::ParameterValue(1.0)); - this->get_parameter("robot_x", robot_x_); - - this->declare_parameter("robot_y", rclcpp::ParameterValue(1.0)); - this->get_parameter("robot_y", robot_y_); - - this->declare_parameter("robot_z", rclcpp::ParameterValue(1.0)); - this->get_parameter("robot_z", robot_z_); - - // Declare and get the quaternion components - this->declare_parameter("robot_qx", rclcpp::ParameterValue(0.0)); - this->get_parameter("robot_qx", robot_qx_); - - this->declare_parameter("robot_qy", rclcpp::ParameterValue(0.0)); - this->get_parameter("robot_qy", robot_qy_); - - this->declare_parameter("robot_qz", rclcpp::ParameterValue(0.0)); - this->get_parameter("robot_qz", robot_qz_); - - this->declare_parameter("robot_qw", rclcpp::ParameterValue(1.0)); - this->get_parameter("robot_qw", robot_qw_); - - // Create and populate the Pose message - geometry_msgs::msg::Pose initial_pose; - initial_pose.position.x = robot_x_; - initial_pose.position.y = robot_y_; - initial_pose.position.z = robot_z_; - initial_pose.orientation.x = robot_qx_; - initial_pose.orientation.y = robot_qy_; - initial_pose.orientation.z = robot_qz_; - initial_pose.orientation.w = robot_qw_; - - this->declare_parameter("odometry_mode", rclcpp::ParameterValue(false)); - this->get_parameter("odometry_mode", odometry_mode_); - - this->declare_parameter("publish_tf", rclcpp::ParameterValue(true)); - this->get_parameter("publish_tf", publish_tf_); - - this->declare_parameter("map_data_publish_frequency", rclcpp::ParameterValue(1000)); - this->get_parameter("map_data_publish_frequency", map_data_publish_frequency_); - - this->declare_parameter("do_loop_closing", rclcpp::ParameterValue(true)); - this->get_parameter("do_loop_closing", do_loop_closing_); - - // Timers - mapDataCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - mapDataTimer_ = this->create_wall_timer(std::chrono::milliseconds(map_data_publish_frequency_), std::bind(&RgbdSlamNode::publishMapData, this), mapDataCallbackGroup_); - - interface_ = std::make_shared(strVocFile, strSettingsFile, - sensor, bUseViewer, do_loop_closing_, initial_pose, global_frame_, odom_frame_id_, robot_base_frame_id_); - - frequency_tracker_count_ = 0; - frequency_tracker_clock_ = std::chrono::high_resolution_clock::now(); - RCLCPP_INFO(this->get_logger(), "CONSTRUCTOR END!"); } @@ -125,177 +33,42 @@ namespace ORB_SLAM3_Wrapper { rgbSub_.reset(); depthSub_.reset(); - imuSub_.reset(); - odomSub_.reset(); - interface_.reset(); RCLCPP_INFO(this->get_logger(), "DESTRUCTOR!"); } - void RgbdSlamNode::ImuCallback(const sensor_msgs::msg::Imu::SharedPtr msgIMU) - { - RCLCPP_DEBUG_STREAM(this->get_logger(), "ImuCallback"); - // push value to imu buffer. - interface_->handleIMU(msgIMU); - } - void RgbdSlamNode::RGBDCallback(const sensor_msgs::msg::Image::SharedPtr msgRGB, const sensor_msgs::msg::Image::SharedPtr msgD) { - Sophus::SE3f Tcw; - if (interface_->trackRGBD(msgRGB, msgD, Tcw)) + cv_bridge::CvImageConstPtr cvRGB; + cv_bridge::CvImageConstPtr cvD; + // Copy the ros rgb image message to cv::Mat. + try { - isTracked_ = true; - // if tf publishing is enabled, move into this block. - if (publish_tf_) - { - // populate map to base_footprint tf if odometry is not being used to get transform map -> base_link - if (!odometry_mode_) - { - auto tfMapRobot = geometry_msgs::msg::TransformStamped(); - tfMapRobot.header.stamp = msgRGB->header.stamp; - tfMapRobot.header.frame_id = global_frame_; - tfMapRobot.child_frame_id = odom_frame_id_; - interface_->getDirectMapToRobotTF(msgRGB->header, tfMapRobot); - tfBroadcaster_->sendTransform(tfMapRobot); - } - // populate map to odom tf if odometry is being used to get transform map -> odom -> base_link - else - { - try - { - auto msgOdom = tfBuffer_->lookupTransform(odom_frame_id_, robot_base_frame_id_, tf2::TimePointZero); - interface_->getMapToOdomTF(msgOdom, tfMapOdom_); - tfBroadcaster_->sendTransform(tfMapOdom_); - } - catch (tf2::TransformException &ex) - { - RCLCPP_WARN(this->get_logger(), "Could not transform %s to %s: %s", odom_frame_id_.c_str(), robot_base_frame_id_.c_str(), ex.what()); - RCLCPP_ERROR(this->get_logger(), "You have set the parameter `odometry_mode` to true. This requires the transform between `odom_frame` and `robot_base_frame` to exist. If you do not have odometry information, set odometry_mode to false."); - return; - } - } - } - geometry_msgs::msg::PoseStamped pose; - interface_->getRobotPose(pose); - pose.header.stamp = msgRGB->header.stamp; - robotPoseMapFrame_->publish(pose); - - ++frequency_tracker_count_; + cvRGB = cv_bridge::toCvShare(msgRGB); } - } - - void RgbdSlamNode::publishMapPointCloud(std::shared_ptr request_header, - std::shared_ptr request, - std::shared_ptr response) - { - if (isTracked_) + catch (cv_bridge::Exception &e) { - // Using high resolution clock to measure time - auto start = std::chrono::high_resolution_clock::now(); - - sensor_msgs::msg::PointCloud2 mapPCL; - - auto t1 = std::chrono::high_resolution_clock::now(); - auto time_create_mapPCL = std::chrono::duration_cast>(t1 - start).count(); - RCLCPP_DEBUG_STREAM(this->get_logger(), "Time to create mapPCL object: " << time_create_mapPCL << " seconds"); - - interface_->getCurrentMapPoints(mapPCL); - - if (mapPCL.data.size() == 0) - return; - - auto t2 = std::chrono::high_resolution_clock::now(); - auto time_get_map_points = std::chrono::duration_cast>(t2 - t1).count(); - RCLCPP_DEBUG_STREAM(this->get_logger(), "Time to get current map points: " << time_get_map_points << " seconds"); - - mapPointsPub_->publish(mapPCL); - auto t3 = std::chrono::high_resolution_clock::now(); - auto time_publish_map_points = std::chrono::duration_cast>(t3 - t2).count(); - RCLCPP_DEBUG_STREAM(this->get_logger(), "Time to publish map points: " << time_publish_map_points << " seconds"); - RCLCPP_DEBUG_STREAM(this->get_logger(), "======================="); - - // Calculate the time taken for each line - - // Print the time taken for each line - response->landmarks = mapPCL; + std::cerr << "cv_bridge exception RGB!" << endl; + return; } - } - void RgbdSlamNode::resetActiveMapSrv(std::shared_ptr request_header, - std::shared_ptr request, - std::shared_ptr response) - { - interface_->resetLocalMapping(); - } - - void RgbdSlamNode::publishMapData() - { - if (isTracked_) + // Copy the ros depth image message to cv::Mat. + try { - auto start = std::chrono::high_resolution_clock::now(); - slam_msgs::msg::SlamInfo slamInfoMsg; - RCLCPP_DEBUG_STREAM(this->get_logger(), "Publishing map data"); - double tracking_freq = frequency_tracker_count_ / std::chrono::duration_cast>(std::chrono::high_resolution_clock::now() - frequency_tracker_clock_).count(); - RCLCPP_INFO_STREAM(this->get_logger(), "Current ORB-SLAM3 tracking frequency: " << tracking_freq << " frames / sec"); - frequency_tracker_clock_ = std::chrono::high_resolution_clock::now(); - frequency_tracker_count_ = 0; - // publish the map data (current active keyframes etc) - slam_msgs::msg::MapData mapDataMsg; - interface_->mapDataToMsg(mapDataMsg, true, false); - mapDataPub_->publish(mapDataMsg); - slamInfoMsg.num_maps = interface_->getNumberOfMaps(); - slamInfoMsg.num_keyframes_in_current_map = mapDataMsg.graph.poses_id.size(); - slamInfoMsg.tracking_frequency = tracking_freq; - auto t1 = std::chrono::high_resolution_clock::now(); - auto time_publishMapData = std::chrono::duration_cast>(t1 - start).count(); - RCLCPP_DEBUG_STREAM(this->get_logger(), "Time to create mapdata: " << time_publishMapData << " seconds"); - RCLCPP_INFO_STREAM(this->get_logger(), "*************************"); - slamInfoPub_->publish(slamInfoMsg); + cvD = cv_bridge::toCvShare(msgD); } - } - - void RgbdSlamNode::getMapServer(std::shared_ptr request_header, - std::shared_ptr request, - std::shared_ptr response) - { - RCLCPP_INFO(this->get_logger(), "GetMap2 service called."); - slam_msgs::msg::MapData mapDataMsg; - interface_->mapDataToMsg(mapDataMsg, false, request->tracked_points, request->kf_id_for_landmarks); - response->data = mapDataMsg; - } - - void RgbdSlamNode::getMapPointsInViewServer(std::shared_ptr request_header, - std::shared_ptr request, - std::shared_ptr response) - { - RCLCPP_INFO(this->get_logger(), "GetMapPointsInView service called."); - std::vector landmarks; - std::vector points; - interface_->mapPointsVisibleFromPose(request->pose, points, 1000, request->max_dist_pose_observation, request->max_angle_pose_observation); - auto affineMapToPos = interface_->getTypeConversionPtr()->poseToAffine(request->pose); - auto affinePosToMap = affineMapToPos.inverse(); - // Populate the pose of the points vector into the ros message - for (const auto& point : points) { - slam_msgs::msg::MapPoint landmark; - Eigen::Vector3f landmark_position = point->GetWorldPos(); - auto position = interface_->getTypeConversionPtr()->vector3fORBToROS(landmark_position); - position = interface_->getTypeConversionPtr()->transformPointWithReference(affinePosToMap, position); - // RCLCPP_INFO_STREAM(this->get_logger(), "x: " << position.x() << " y: " << position.y() << " z: " << position.z()); - landmark.position.x = position.x(); - landmark.position.y = position.y(); - landmark.position.z = position.z(); - landmarks.push_back(landmark); + catch (cv_bridge::Exception &e) + { + std::cerr << "cv_bridge exception D!" << endl; + return; + } + // track the frame. + auto Tcw = interface()->slam()->TrackRGBD(cvRGB->image, cvD->image, stampToSec(msgRGB->header.stamp)); + + // process the tracked pose. + if (interface()->processTrackedPose(Tcw)) + { + // Use RGB timestamp as the source stamp for TF/pose publishing. + this->onTracked(msgRGB->header); } - response->map_points = landmarks; - auto cloud = interface_->getTypeConversionPtr()->MapPointsToPCL(points); - visibleLandmarksPub_->publish(cloud); - - // Convert the pose in request to PoseStamped and publish - geometry_msgs::msg::PoseStamped pose_stamped; - pose_stamped.header.stamp = this->now(); - pose_stamped.header.frame_id = "map"; // Assuming the frame is "map", adjust if needed - pose_stamped.pose = request->pose; - - // Publish the PoseStamped - visibleLandmarksPose_->publish(pose_stamped); } } diff --git a/orb_slam3_ros2_wrapper/src/rgbd/rgbd-slam-node.hpp b/orb_slam3_ros2_wrapper/src/rgbd/rgbd-slam-node.hpp index a9aded01..485d7931 100644 --- a/orb_slam3_ros2_wrapper/src/rgbd/rgbd-slam-node.hpp +++ b/orb_slam3_ros2_wrapper/src/rgbd/rgbd-slam-node.hpp @@ -11,38 +11,20 @@ #include #include #include +#include +#include -#include #include -#include -#include - -#include -#include -#include -#include - -#include -#include #include #include #include -#include "std_srvs/srv/set_bool.hpp" - -#include -#include -#include -#include -#include - -#include "orb_slam3_ros2_wrapper/type_conversion.hpp" -#include "orb_slam3_ros2_wrapper/orb_slam3_interface.hpp" +#include "orb_slam3_ros2_wrapper/slam_node_base.hpp" namespace ORB_SLAM3_Wrapper { - class RgbdSlamNode : public rclcpp::Node + class RgbdSlamNode : public SlamNodeBase { public: RgbdSlamNode(const std::string &strVocFile, @@ -54,38 +36,9 @@ namespace ORB_SLAM3_Wrapper typedef message_filters::sync_policies::ApproximateTime approximate_sync_policy; // ROS 2 Callbacks. - void ImuCallback(const sensor_msgs::msg::Imu::SharedPtr msgIMU); void RGBDCallback(const sensor_msgs::msg::Image::SharedPtr msgRGB, const sensor_msgs::msg::Image::SharedPtr msgD); - /** - * @brief Publishes map data. (Keyframes and all poses in the current active map.) - * @param orb_atlas Pointer to the Atlas object. - * @param last_init_kf_id ID of the last initialized keyframe. - */ - void publishMapData(); - - void publishMapPointCloud(std::shared_ptr request_header, - std::shared_ptr request, - std::shared_ptr response); - - void resetActiveMapSrv(std::shared_ptr request_header, - std::shared_ptr request, - std::shared_ptr response); - - /** - * @brief Callback function for GetMap service. - * @param request_header Request header. - * @param request Request message. - * @param response Response message. - */ - void getMapServer(std::shared_ptr request_header, - std::shared_ptr request, - std::shared_ptr response); - - void getMapPointsInViewServer(std::shared_ptr request_header, - std::shared_ptr request, - std::shared_ptr response); /** * Member variables */ @@ -93,45 +46,6 @@ namespace ORB_SLAM3_Wrapper std::shared_ptr> rgbSub_; std::shared_ptr> depthSub_; std::shared_ptr> syncApproximate_; - rclcpp::Subscription::SharedPtr imuSub_; - // ROS Publishers and Subscribers - rclcpp::Subscription::SharedPtr odomSub_; - rclcpp::Publisher::SharedPtr mapDataPub_; - rclcpp::Publisher::SharedPtr mapPointsPub_; - rclcpp::Publisher::SharedPtr visibleLandmarksPub_; - rclcpp::Publisher::SharedPtr visibleLandmarksPose_; - rclcpp::Publisher::SharedPtr robotPoseMapFrame_; - rclcpp::Publisher::SharedPtr slamInfoPub_; - // TF - std::shared_ptr tfBroadcaster_; - std::shared_ptr tfListener_; - std::shared_ptr tfBuffer_; - // ROS Services - rclcpp::Service::SharedPtr getMapDataService_; - rclcpp::Service::SharedPtr getMapPointsService_; - rclcpp::Service::SharedPtr mapPointsService_; - rclcpp::Service::SharedPtr resetLocalMapSrv_; - // ROS Timers - rclcpp::TimerBase::SharedPtr mapDataTimer_; - rclcpp::CallbackGroup::SharedPtr mapDataCallbackGroup_; - rclcpp::CallbackGroup::SharedPtr mapPointsCallbackGroup_; - rclcpp::CallbackGroup::SharedPtr pointsInViewCallbackGroup_; - // ROS Params - std::string robot_base_frame_id_; - std::string odom_frame_id_; - std::string global_frame_; - double robot_x_, robot_y_, robot_z_, robot_qx_, robot_qy_, robot_qz_, robot_qw_; - bool isTracked_ = false; - bool odometry_mode_; - bool publish_tf_; - double frequency_tracker_count_ = 0; - int map_data_publish_frequency_; - bool do_loop_closing_; - std::chrono::_V2::system_clock::time_point frequency_tracker_clock_; - - ORB_SLAM3_Wrapper::WrapperTypeConversions typeConversion_; - std::shared_ptr interface_; - geometry_msgs::msg::TransformStamped tfMapOdom_; }; } #endif diff --git a/orb_slam3_ros2_wrapper/src/slam_node_base.cpp b/orb_slam3_ros2_wrapper/src/slam_node_base.cpp new file mode 100644 index 00000000..978e4a69 --- /dev/null +++ b/orb_slam3_ros2_wrapper/src/slam_node_base.cpp @@ -0,0 +1,276 @@ +/** + * @file slam_node_base.cpp + * @brief Implementation of the common ROS 2 wrapper base class. + */ + +#include "orb_slam3_ros2_wrapper/slam_node_base.hpp" + +#include + +namespace ORB_SLAM3_Wrapper +{ + using namespace WrapperTypeConversions; + + SlamNodeBase::SlamNodeBase(const std::string &node_name, + const std::string &strVocFile, + const std::string &strSettingsFile, + ORB_SLAM3::System::eSensor sensor) + : rclcpp::Node(node_name) + { + + // ROS Publishers + //---- the following is published when a service is called + mapPointsPub_ = this->create_publisher("map_points", 10); + visibleLandmarksPub_ = this->create_publisher("visible_landmarks", 10); + visibleLandmarksPose_ = this->create_publisher("visible_landmarks_pose", 10); + slamInfoPub_ = this->create_publisher("slam_info", 10); + //---- the following is published continously + mapDataPub_ = this->create_publisher("map_data", 10); + robotPoseMapFrame_ = this->create_publisher("robot_pose_slam", 10); + + // Services + getMapDataService_ = this->create_service("orb_slam3/get_map_data", std::bind(&SlamNodeBase::getMapServer, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + pointsInViewCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + getMapPointsService_ = this->create_service("orb_slam3/get_landmarks_in_view", std::bind(&SlamNodeBase::getMapPointsInViewServer, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, pointsInViewCallbackGroup_); + mapPointsCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + mapPointsService_ = this->create_service("orb_slam3/get_all_landmarks_in_map", std::bind(&SlamNodeBase::publishMapPointCloud, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, mapPointsCallbackGroup_); + resetLocalMapSrv_ = this->create_service("orb_slam3/reset_mapping", std::bind(&SlamNodeBase::resetActiveMapSrv, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, mapPointsCallbackGroup_); + + // TF + tfBroadcaster_ = std::make_shared(this); + tfBuffer_ = std::make_shared(this->get_clock()); + tfListener_ = std::make_shared(*tfBuffer_); + + bool bUseViewer; + this->declare_parameter("visualization", rclcpp::ParameterValue(true)); + this->get_parameter("visualization", bUseViewer); + + this->declare_parameter("robot_base_frame", "base_footprint"); + this->get_parameter("robot_base_frame", robot_base_frame_id_); + + this->declare_parameter("global_frame", "map"); + this->get_parameter("global_frame", global_frame_); + + this->declare_parameter("odom_frame", "odom"); + this->get_parameter("odom_frame", odom_frame_id_); + + this->declare_parameter("robot_x", rclcpp::ParameterValue(1.0)); + this->get_parameter("robot_x", robot_x_); + + this->declare_parameter("robot_y", rclcpp::ParameterValue(1.0)); + this->get_parameter("robot_y", robot_y_); + + this->declare_parameter("robot_z", rclcpp::ParameterValue(1.0)); + this->get_parameter("robot_z", robot_z_); + + // Declare and get the quaternion components + this->declare_parameter("robot_qx", rclcpp::ParameterValue(0.0)); + this->get_parameter("robot_qx", robot_qx_); + + this->declare_parameter("robot_qy", rclcpp::ParameterValue(0.0)); + this->get_parameter("robot_qy", robot_qy_); + + this->declare_parameter("robot_qz", rclcpp::ParameterValue(0.0)); + this->get_parameter("robot_qz", robot_qz_); + + this->declare_parameter("robot_qw", rclcpp::ParameterValue(1.0)); + this->get_parameter("robot_qw", robot_qw_); + + // Create and populate the Pose message + geometry_msgs::msg::Pose initial_pose; + initial_pose.position.x = robot_x_; + initial_pose.position.y = robot_y_; + initial_pose.position.z = robot_z_; + initial_pose.orientation.x = robot_qx_; + initial_pose.orientation.y = robot_qy_; + initial_pose.orientation.z = robot_qz_; + initial_pose.orientation.w = robot_qw_; + + this->declare_parameter("odometry_mode", rclcpp::ParameterValue(false)); + this->get_parameter("odometry_mode", odometry_mode_); + + this->declare_parameter("publish_tf", rclcpp::ParameterValue(true)); + this->get_parameter("publish_tf", publish_tf_); + + this->declare_parameter("map_data_publish_frequency", rclcpp::ParameterValue(1000)); + this->get_parameter("map_data_publish_frequency", map_data_publish_frequency_); + + this->declare_parameter("do_loop_closing", rclcpp::ParameterValue(true)); + this->get_parameter("do_loop_closing", do_loop_closing_); + + // Timers + mapDataCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + mapDataTimer_ = this->create_wall_timer(std::chrono::milliseconds(map_data_publish_frequency_), std::bind(&SlamNodeBase::publishMapData, this), mapDataCallbackGroup_); + + interface_ = std::make_shared(strVocFile, strSettingsFile, + sensor, bUseViewer, do_loop_closing_, initial_pose, global_frame_, odom_frame_id_, robot_base_frame_id_); + + frequency_tracker_count_ = 0; + frequency_tracker_clock_ = std::chrono::high_resolution_clock::now(); + } + + SlamNodeBase::~SlamNodeBase() + { + interface_.reset(); + } + + void SlamNodeBase::onTracked(const std_msgs::msg::Header &stamp_source_header) + { + isTracked_ = true; + // if tf publishing is enabled, move into this block. + if (publish_tf_) + { + // populate map to base_footprint tf if odometry is not being used to get transform map -> base_link + if (!odometry_mode_) + { + auto tfMapRobot = geometry_msgs::msg::TransformStamped(); + tfMapRobot.header.stamp = stamp_source_header.stamp; + tfMapRobot.header.frame_id = global_frame_; + tfMapRobot.child_frame_id = odom_frame_id_; + interface_->getDirectMapToRobotTF(stamp_source_header, tfMapRobot); + tfBroadcaster_->sendTransform(tfMapRobot); + } + // populate map to odom tf if odometry is being used to get transform map -> odom -> base_link + else + { + try + { + auto msgOdom = tfBuffer_->lookupTransform(odom_frame_id_, robot_base_frame_id_, tf2::TimePointZero); + interface_->getMapToOdomTF(msgOdom, tfMapOdom_); + tfBroadcaster_->sendTransform(tfMapOdom_); + } + catch (tf2::TransformException &ex) + { + RCLCPP_WARN(this->get_logger(), "Could not transform %s to %s: %s", odom_frame_id_.c_str(), robot_base_frame_id_.c_str(), ex.what()); + RCLCPP_ERROR(this->get_logger(), "You have set the parameter `odometry_mode` to true. This requires the transform between `odom_frame` and `robot_base_frame` to exist. If you do not have odometry information, set odometry_mode to false."); + return; + } + } + } + geometry_msgs::msg::PoseStamped pose; + interface_->getRobotPose(pose); + pose.header.stamp = stamp_source_header.stamp; + robotPoseMapFrame_->publish(pose); + + ++frequency_tracker_count_; + } + + void SlamNodeBase::publishMapPointCloud(std::shared_ptr /*request_header*/, + std::shared_ptr /*request*/, + std::shared_ptr response) + { + if (isTracked_) + { + // Using high resolution clock to measure time + auto start = std::chrono::high_resolution_clock::now(); + + sensor_msgs::msg::PointCloud2 mapPCL; + + auto t1 = std::chrono::high_resolution_clock::now(); + auto time_create_mapPCL = std::chrono::duration_cast>(t1 - start).count(); + RCLCPP_DEBUG_STREAM(this->get_logger(), "Time to create mapPCL object: " << time_create_mapPCL << " seconds"); + + interface_->getCurrentMapPoints(mapPCL); + + if (mapPCL.data.size() == 0) + return; + + auto t2 = std::chrono::high_resolution_clock::now(); + auto time_get_map_points = std::chrono::duration_cast>(t2 - t1).count(); + RCLCPP_DEBUG_STREAM(this->get_logger(), "Time to get current map points: " << time_get_map_points << " seconds"); + + mapPointsPub_->publish(mapPCL); + auto t3 = std::chrono::high_resolution_clock::now(); + auto time_publish_map_points = std::chrono::duration_cast>(t3 - t2).count(); + RCLCPP_DEBUG_STREAM(this->get_logger(), "Time to publish map points: " << time_publish_map_points << " seconds"); + RCLCPP_DEBUG_STREAM(this->get_logger(), "======================="); + + // Calculate the time taken for each line + + // Print the time taken for each line + response->landmarks = mapPCL; + } + } + + void SlamNodeBase::resetActiveMapSrv(std::shared_ptr /*request_header*/, + std::shared_ptr /*request*/, + std::shared_ptr /*response*/) + { + interface_->resetLocalMapping(); + } + + void SlamNodeBase::publishMapData() + { + if (isTracked_) + { + auto start = std::chrono::high_resolution_clock::now(); + slam_msgs::msg::SlamInfo slamInfoMsg; + RCLCPP_DEBUG_STREAM(this->get_logger(), "Publishing map data"); + double tracking_freq = frequency_tracker_count_ / std::chrono::duration_cast>(std::chrono::high_resolution_clock::now() - frequency_tracker_clock_).count(); + RCLCPP_INFO_STREAM(this->get_logger(), "Current ORB-SLAM3 tracking frequency: " << tracking_freq << " frames / sec"); + frequency_tracker_clock_ = std::chrono::high_resolution_clock::now(); + frequency_tracker_count_ = 0; + // publish the map data (current active keyframes etc) + slam_msgs::msg::MapData mapDataMsg; + interface_->mapDataToMsg(mapDataMsg, true, false); + mapDataPub_->publish(mapDataMsg); + slamInfoMsg.num_maps = interface_->getNumberOfMaps(); + slamInfoMsg.num_keyframes_in_current_map = mapDataMsg.graph.poses_id.size(); + slamInfoMsg.tracking_frequency = tracking_freq; + auto t1 = std::chrono::high_resolution_clock::now(); + auto time_publishMapData = std::chrono::duration_cast>(t1 - start).count(); + RCLCPP_DEBUG_STREAM(this->get_logger(), "Time to create mapdata: " << time_publishMapData << " seconds"); + RCLCPP_INFO_STREAM(this->get_logger(), "*************************"); + slamInfoPub_->publish(slamInfoMsg); + } + } + + void SlamNodeBase::getMapServer(std::shared_ptr /*request_header*/, + std::shared_ptr request, + std::shared_ptr response) + { + RCLCPP_INFO(this->get_logger(), "GetMap2 service called."); + slam_msgs::msg::MapData mapDataMsg; + interface_->mapDataToMsg(mapDataMsg, false, request->tracked_points, request->kf_id_for_landmarks); + response->data = mapDataMsg; + } + + void SlamNodeBase::getMapPointsInViewServer(std::shared_ptr /*request_header*/, + std::shared_ptr request, + std::shared_ptr response) + { + RCLCPP_INFO(this->get_logger(), "GetMapPointsInView service called."); + std::vector landmarks; + std::vector points; + interface_->mapPointsVisibleFromPose(request->pose, points, 1000, request->max_dist_pose_observation, request->max_angle_pose_observation); + auto affineMapToPos = poseToAffine(request->pose); + auto affinePosToMap = affineMapToPos.inverse(); + // Populate the pose of the points vector into the ros message + for (const auto &point : points) + { + slam_msgs::msg::MapPoint landmark; + Eigen::Vector3f landmark_position = point->GetWorldPos(); + auto position = vector3fORBToROS(landmark_position); + position = transformPointWithReference(affinePosToMap, position); + // RCLCPP_INFO_STREAM(this->get_logger(), "x: " << position.x() << " y: " << position.y() << " z: " << position.z()); + landmark.position.x = position.x(); + landmark.position.y = position.y(); + landmark.position.z = position.z(); + landmarks.push_back(landmark); + } + response->map_points = landmarks; + auto cloud = MapPointsToPCL(points); + visibleLandmarksPub_->publish(cloud); + + // Convert the pose in request to PoseStamped and publish + geometry_msgs::msg::PoseStamped pose_stamped; + pose_stamped.header.stamp = this->now(); + pose_stamped.header.frame_id = "map"; // Assuming the frame is "map", adjust if needed + pose_stamped.pose = request->pose; + + // Publish the PoseStamped + visibleLandmarksPose_->publish(pose_stamped); + } + +} // namespace ORB_SLAM3_Wrapper diff --git a/orb_slam3_ros2_wrapper/src/stereo/stereo-slam-node.cpp b/orb_slam3_ros2_wrapper/src/stereo/stereo-slam-node.cpp new file mode 100644 index 00000000..11d67fa8 --- /dev/null +++ b/orb_slam3_ros2_wrapper/src/stereo/stereo-slam-node.cpp @@ -0,0 +1,79 @@ +/** + * @file stereo-slam-node.cpp + * @brief Implementation of the StereoSlamNode Wrapper class. + */ + +#include "stereo-slam-node.hpp" + +#include +#include +#include + +namespace ORB_SLAM3_Wrapper +{ + using namespace WrapperTypeConversions; + + StereoSlamNode::StereoSlamNode(const std::string &strVocFile, + const std::string &strSettingsFile, + ORB_SLAM3::System::eSensor sensor) + : SlamNodeBase("ORB_SLAM3_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")); + + // ROS Subscribers + 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(&StereoSlamNode::StereoCallback, this); + + RCLCPP_INFO(this->get_logger(), "Stereo node started. Left: %s | Right: %s", + this->get_parameter("left_image_topic_name").as_string().c_str(), + this->get_parameter("right_image_topic_name").as_string().c_str()); + } + + StereoSlamNode::~StereoSlamNode() + { + leftSub_.reset(); + rightSub_.reset(); + syncApproximate_.reset(); + RCLCPP_INFO(this->get_logger(), "Stereo node stopped."); + } + + void StereoSlamNode::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)); + + auto Tcw = interface()->slam()->TrackStereo(cvLeft->image, cvRight->image, tFrame); + + if (interface()->processTrackedPose(Tcw)) + { + this->onTracked(msgLeft->header); + } + } +} // namespace ORB_SLAM3_Wrapper diff --git a/orb_slam3_ros2_wrapper/src/stereo/stereo-slam-node.hpp b/orb_slam3_ros2_wrapper/src/stereo/stereo-slam-node.hpp new file mode 100644 index 00000000..5ddfae8e --- /dev/null +++ b/orb_slam3_ros2_wrapper/src/stereo/stereo-slam-node.hpp @@ -0,0 +1,38 @@ +/** + * @file stereo-slam-node.hpp + * @brief Definition of the StereoSlamNode Wrapper class. + */ + +#ifndef STEREO_SLAM_NODE_HPP_ +#define STEREO_SLAM_NODE_HPP_ + +#include +#include +#include +#include + +#include "orb_slam3_ros2_wrapper/slam_node_base.hpp" + +namespace ORB_SLAM3_Wrapper +{ + class StereoSlamNode : public SlamNodeBase + { + public: + StereoSlamNode(const std::string &strVocFile, + const std::string &strSettingsFile, + ORB_SLAM3::System::eSensor sensor); + ~StereoSlamNode(); + + private: + using approximate_sync_policy = message_filters::sync_policies::ApproximateTime; + + 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_; + }; +} // namespace ORB_SLAM3_Wrapper + +#endif // STEREO_SLAM_NODE_HPP_ diff --git a/orb_slam3_ros2_wrapper/src/stereo/stereo.cpp b/orb_slam3_ros2_wrapper/src/stereo/stereo.cpp new file mode 100644 index 00000000..579658a0 --- /dev/null +++ b/orb_slam3_ros2_wrapper/src/stereo/stereo.cpp @@ -0,0 +1,25 @@ +#include + +#include "rclcpp/rclcpp.hpp" +#include "stereo-slam-node.hpp" + +int main(int argc, char **argv) +{ + if (argc < 3) + { + std::cerr << "\nUsage: ros2 run orb_slam3_ros2_wrapper stereo 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::STEREO); + + auto executor = std::make_shared(); + executor->add_node(node); + executor->spin(); + rclcpp::shutdown(); + + return 0; +} diff --git a/orb_slam3_ros2_wrapper/src/type_conversion.cpp b/orb_slam3_ros2_wrapper/src/type_conversion.cpp index def4c58c..75a63758 100644 --- a/orb_slam3_ros2_wrapper/src/type_conversion.cpp +++ b/orb_slam3_ros2_wrapper/src/type_conversion.cpp @@ -7,15 +7,21 @@ #include "orb_slam3_ros2_wrapper/type_conversion.hpp" #include "sophus/se3.hpp" +#include +#include +#include + namespace ORB_SLAM3_Wrapper { - double WrapperTypeConversions::stampToSec(builtin_interfaces::msg::Time stamp) +namespace WrapperTypeConversions +{ + double stampToSec(builtin_interfaces::msg::Time stamp) { double seconds = stamp.sec + (stamp.nanosec * pow(10, -9)); return seconds; } - builtin_interfaces::msg::Time WrapperTypeConversions::secToStamp(double seconds) + builtin_interfaces::msg::Time secToStamp(double seconds) { builtin_interfaces::msg::Time stamp; stamp.sec = static_cast(std::floor(seconds)); @@ -23,7 +29,7 @@ namespace ORB_SLAM3_Wrapper return stamp; } - geometry_msgs::msg::Point WrapperTypeConversions::eigenToPointMsg(Eigen::Vector3f &e) + geometry_msgs::msg::Point eigenToPointMsg(Eigen::Vector3f &e) { geometry_msgs::msg::Point p; p.x = e.x(); @@ -32,7 +38,7 @@ namespace ORB_SLAM3_Wrapper return p; } - geometry_msgs::msg::Quaternion WrapperTypeConversions::eigenToQuaternionMsg(Eigen::Quaternionf &e) + geometry_msgs::msg::Quaternion eigenToQuaternionMsg(Eigen::Quaternionf &e) { geometry_msgs::msg::Quaternion q; q.w = e.w(); @@ -42,7 +48,7 @@ namespace ORB_SLAM3_Wrapper return q; } - Eigen::Affine3f WrapperTypeConversions::se3ORBToROS(const Sophus::SE3f &s) + Eigen::Affine3f se3ORBToROS(const Sophus::SE3f &s) { Eigen::Matrix3f tfCameraRotation = s.rotationMatrix(); Eigen::Vector3f tfCameraTranslation = s.translation(); @@ -71,7 +77,7 @@ namespace ORB_SLAM3_Wrapper return affineMatrix; } - Sophus::SE3f WrapperTypeConversions::se3ROSToORB(const Eigen::Affine3f &affineMatrix) + Sophus::SE3f se3ROSToORB(const Eigen::Affine3f &affineMatrix) { // Extract rotation and translation from the affine matrix Eigen::Matrix3f tfCameraRotation = affineMatrix.rotation(); @@ -99,7 +105,7 @@ namespace ORB_SLAM3_Wrapper return Sophus::SE3f(tfCameraRotation, tfCameraTranslation); } - Eigen::Vector3f WrapperTypeConversions::vector3fORBToROS(const Eigen::Vector3f &s) + Eigen::Vector3f vector3fORBToROS(const Eigen::Vector3f &s) { Eigen::Matrix3f tfCameraRotation = Eigen::Matrix3f::Identity(); Eigen::Vector3f tfCameraTranslation = s; @@ -120,13 +126,13 @@ namespace ORB_SLAM3_Wrapper return tfCameraTranslation; } - Eigen::Affine3f WrapperTypeConversions::se3ToAffine(const Sophus::SE3f &s) + Eigen::Affine3f se3ToAffine(const Sophus::SE3f &s) { Eigen::Affine3f affineTf = se3ORBToROS(s); return affineTf; } - Eigen::Affine3f WrapperTypeConversions::poseToAffine(const geometry_msgs::msg::Pose &pose) + Eigen::Affine3f poseToAffine(const geometry_msgs::msg::Pose &pose) { auto translation = Eigen::Translation3f( static_cast(pose.position.x), @@ -140,7 +146,7 @@ namespace ORB_SLAM3_Wrapper return translation * quaternion; } - Eigen::Vector3f WrapperTypeConversions::rotationORBToEulerROS(const Eigen::Matrix3f& rotation) + Eigen::Vector3f rotationORBToEulerROS(const Eigen::Matrix3f& rotation) { auto eulerAngles = rotation.eulerAngles(2, 0, 1); eulerAngles[1] = -eulerAngles[1]; @@ -152,14 +158,14 @@ namespace ORB_SLAM3_Wrapper return eulerAngles; } - geometry_msgs::msg::Pose WrapperTypeConversions::se3ToPoseMsg(const Sophus::SE3f &s) + geometry_msgs::msg::Pose se3ToPoseMsg(const Sophus::SE3f &s) { Eigen::Affine3f poseTransform = se3ORBToROS(s); geometry_msgs::msg::Pose pose = affine3fToPose(poseTransform); return pose; } - geometry_msgs::msg::Pose WrapperTypeConversions::affine3fToPose(const Eigen::Affine3f & in) + geometry_msgs::msg::Pose affine3fToPose(const Eigen::Affine3f & in) { geometry_msgs::msg::Pose msg; msg.position.x = in.translation().x(); @@ -173,7 +179,7 @@ namespace ORB_SLAM3_Wrapper return msg; } - sensor_msgs::msg::PointCloud2 WrapperTypeConversions::MapPointsToPCL(std::vector& mapPoints) + sensor_msgs::msg::PointCloud2 MapPointsToPCL(std::vector& mapPoints) { const int numChannels = 3; // x y z @@ -222,7 +228,7 @@ namespace ORB_SLAM3_Wrapper } - sensor_msgs::msg::PointCloud2 WrapperTypeConversions::MapPointsToPCL(std::vector& mapPoints) + sensor_msgs::msg::PointCloud2 MapPointsToPCL(std::vector& mapPoints) { const int numChannels = 3; // x y z @@ -271,7 +277,7 @@ namespace ORB_SLAM3_Wrapper } template <> - geometry_msgs::msg::Pose WrapperTypeConversions::transformPoseWithReference(Eigen::Affine3f &affineMapToRef, Sophus::SE3f &transform) + geometry_msgs::msg::Pose transformPoseWithReference(Eigen::Affine3f &affineMapToRef, const Sophus::SE3f &transform) { // Convert SE3 to affine. auto affine_map_to_pose = affineMapToRef * se3ToAffine(transform); @@ -279,7 +285,7 @@ namespace ORB_SLAM3_Wrapper } template <> - Eigen::Affine3f WrapperTypeConversions::transformPoseWithReference(Eigen::Affine3f &affineMapToRef, Sophus::SE3f &transform) + Eigen::Affine3f transformPoseWithReference(Eigen::Affine3f &affineMapToRef, const Sophus::SE3f &transform) { // Convert SE3 to affine. auto affineMapToPose = affineMapToRef * se3ToAffine(transform); @@ -287,16 +293,17 @@ namespace ORB_SLAM3_Wrapper } template <> - geometry_msgs::msg::Point WrapperTypeConversions::transformPointWithReference(Eigen::Affine3f &affineMapToRef, Eigen::Vector3f &point) + geometry_msgs::msg::Point transformPointWithReference(Eigen::Affine3f &affineMapToRef, Eigen::Vector3f &point) { auto affine_map_to_pose = affineMapToRef.cast() * point; return eigenToPointMsg(affine_map_to_pose); } template <> - Eigen::Vector3f WrapperTypeConversions::transformPointWithReference(Eigen::Affine3f &affineMapToRef, Eigen::Vector3f &point) + Eigen::Vector3f transformPointWithReference(Eigen::Affine3f &affineMapToRef, Eigen::Vector3f &point) { auto affine_map_to_pose = affineMapToRef.cast() * point; return affine_map_to_pose; } +} } \ No newline at end of file diff --git a/orb_slam3_ros2_wrapper/tests/typeConversionsTests.cpp b/orb_slam3_ros2_wrapper/tests/typeConversionsTests.cpp index 52259016..bbf25cc8 100644 --- a/orb_slam3_ros2_wrapper/tests/typeConversionsTests.cpp +++ b/orb_slam3_ros2_wrapper/tests/typeConversionsTests.cpp @@ -6,7 +6,7 @@ #include TEST(TypeConversionsTest, SE3ConversionRoundTrip) { - ORB_SLAM3_Wrapper::WrapperTypeConversions typeConversion_; + using namespace ORB_SLAM3_Wrapper::WrapperTypeConversions; // Create a random number generator std::random_device rd; std::mt19937 gen(rd()); @@ -23,10 +23,10 @@ TEST(TypeConversionsTest, SE3ConversionRoundTrip) { randomAffine.translate(Eigen::Vector3f(dis2(gen), dis2(gen), dis2(gen))); // Convert ROS (Affine3f) to ORB (SE3f) - Sophus::SE3f orbSE3 = typeConversion_.se3ROSToORB(randomAffine); + Sophus::SE3f orbSE3 = se3ROSToORB(randomAffine); // Convert back to ROS (Affine3f) - Eigen::Affine3f convertedAffine = typeConversion_.se3ORBToROS(orbSE3); + Eigen::Affine3f convertedAffine = se3ORBToROS(orbSE3); // Compare the original and converted matrices ASSERT_TRUE(randomAffine.isApprox(convertedAffine, 1e-5))