Support for RGBD-IMU, Mono-IMU, Stereo sensor configs

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)
 

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 

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

orb_slam3_ros2_wrapper/include/orb_slam3_ros2_wrapper/orb_slam3_interface.hpp

@@ -8,6 +8,10 @@
 
 #include <iostream>
 #include <algorithm>
+#include <mutex>
+#include <string>
+#include <unordered_map>
+#include <vector>
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
@@ -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 <slam_msgs/msg/map_data.hpp>
 #include <slam_msgs/msg/map_graph.hpp>
@@ -68,7 +76,7 @@ namespace ORB_SLAM3_Wrapper
          */
         void mapDataToMsg(slam_msgs::msg::MapData &mapDataMsg, bool currentMapKFOnly, bool includeMapPoints = false, std::vector<int> kFIDforMapPoints = std::vector<int>());
 
-        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<ORB_SLAM3::MapPoint*>& 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<WrapperTypeConversions> getTypeConversionPtr()
-        {
-            return typeConversions_;
-        };
+        bool processTrackedPose(const Sophus::SE3f& Tcw);
 
         void resetLocalMapping();
 
@@ -109,16 +112,12 @@ namespace ORB_SLAM3_Wrapper
 
     private:
         std::shared_ptr<ORB_SLAM3::System> mSLAM_;
-        std::shared_ptr<WrapperTypeConversions> typeConversions_;
         ORB_SLAM3::Atlas *orbAtlas_;
         std::string strVocFile_;
         std::string strSettingsFile_;
         ORB_SLAM3::System::eSensor sensor_;
         bool bUseViewer_;
         bool loopClosing_;
-
-        queue<sensor_msgs::msg::Imu::SharedPtr> imuBuf_;
-        std::mutex bufMutex_;
         std::mutex mapDataMutex_;
         std::mutex currentMapPointsMutex_;
 

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 <iostream>
+#include <algorithm>
+#include <fstream>
+#include <chrono>
+
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <geometry_msgs/msg/pose.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/pose_array.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+
+#include "std_srvs/srv/set_bool.hpp"
+
+#include <slam_msgs/msg/map_data.hpp>
+#include <slam_msgs/msg/slam_info.hpp>
+#include <slam_msgs/srv/get_map.hpp>
+#include <slam_msgs/srv/get_landmarks_in_view.hpp>
+#include <slam_msgs/srv/get_all_landmarks_in_map.hpp>
+
+#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<ORBSLAM3Interface> interface() { return interface_; }
+
+    private:
+        void publishMapData();
+
+        void publishMapPointCloud(std::shared_ptr<rmw_request_id_t> request_header,
+                                  std::shared_ptr<slam_msgs::srv::GetAllLandmarksInMap::Request> request,
+                                  std::shared_ptr<slam_msgs::srv::GetAllLandmarksInMap::Response> response);
+
+        void resetActiveMapSrv(std::shared_ptr<rmw_request_id_t> request_header,
+                               std::shared_ptr<std_srvs::srv::SetBool::Request> request,
+                               std::shared_ptr<std_srvs::srv::SetBool::Response> response);
+
+        void getMapServer(std::shared_ptr<rmw_request_id_t> request_header,
+                          std::shared_ptr<slam_msgs::srv::GetMap::Request> request,
+                          std::shared_ptr<slam_msgs::srv::GetMap::Response> response);
+
+        void getMapPointsInViewServer(std::shared_ptr<rmw_request_id_t> request_header,
+                                      std::shared_ptr<slam_msgs::srv::GetLandmarksInView::Request> request,
+                                      std::shared_ptr<slam_msgs::srv::GetLandmarksInView::Response> response);
+
+    private:
+        // ROS Publishers and Subscribers
+        rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odomSub_;
+        rclcpp::Publisher<slam_msgs::msg::MapData>::SharedPtr mapDataPub_;
+        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr mapPointsPub_;
+        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr visibleLandmarksPub_;
+        rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr visibleLandmarksPose_;
+        rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr robotPoseMapFrame_;
+        rclcpp::Publisher<slam_msgs::msg::SlamInfo>::SharedPtr slamInfoPub_;
+        // TF
+        std::shared_ptr<tf2_ros::TransformBroadcaster> tfBroadcaster_;
+        std::shared_ptr<tf2_ros::TransformListener> tfListener_;
+        std::shared_ptr<tf2_ros::Buffer> tfBuffer_;
+        // ROS Services
+        rclcpp::Service<slam_msgs::srv::GetMap>::SharedPtr getMapDataService_;
+        rclcpp::Service<slam_msgs::srv::GetLandmarksInView>::SharedPtr getMapPointsService_;
+        rclcpp::Service<slam_msgs::srv::GetAllLandmarksInMap>::SharedPtr mapPointsService_;
+        rclcpp::Service<std_srvs::srv::SetBool>::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<ORB_SLAM3_Wrapper::ORBSLAM3Interface> interface_;
+        geometry_msgs::msg::TransformStamped tfMapOdom_;
+    };
+
+} // namespace ORB_SLAM3_Wrapper
+
+#endif // ORB_SLAM3_ROS2_WRAPPER__SLAM_NODE_BASE_HPP_

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 <typename T>
-        T transformPoseWithReference(Eigen::Affine3f &, Sophus::SE3f &);
+        T transformPoseWithReference(Eigen::Affine3f &, const Sophus::SE3f &);
 
         /**
          * @brief Transforms a pose using a reference pose and SE3 transform.