TF Fixes

README.md

@@ -100,10 +100,9 @@ The simulation and the wrapper both have their ```ROS_DOMAIN_ID``` set to 55 so
 | `rgb_image_topic_name` | `rgb_camera` | The topic to recieve rgb images. |
 | `depth_image_topic_name` | `depth_camera` | The topic to recieve depth images. |
 | `imu_topic_name` | `imu` | The topic to recieve IMU messages (Not used in RGB-D mode). |
-| `odom_topic_name` | `odom` | The topic to recieve on board odometry messages (Used only if `odometry_mode` is set to true). |
 | `visualization`         | `true`        | A boolean flag to enable or disable visualization. When set to `true`, the ORB-SLAM3 viewer will show up with the tracked points and the keyframe trajectories.|
-| `odometry_mode`      | `false`       | A boolean flag to toggle odometry mode. When `false`, the system operates without relying on odometry data, which might be used in scenarios where odometry information is unavailable or unreliable. In this case, it publishes the transform directly between the ```global_frame``` and the ```robot_base_frame```|
-| `publish_tf`               | `true`         | Publishes the map->odom tf in case `odometry_mode` is set to `true` and map->odom->base_link in case odometry_mode is set to `false`.|
+| `odometry_mode`      | `false`       | A boolean flag to toggle odometry mode. When `false`, the system operates without relying on odometry data, which might be used in scenarios where odometry information is unavailable or unreliable. In this case, it publishes the transform directly between the ```global_frame``` and the ```robot_base_frame```. Further information can be found on the [FAQ](https://github.com/suchetanrs/ORB-SLAM3-ROS2-Docker/wiki/FAQs)|
+| `publish_tf`               | `true`         | Publishes the map->odom tf in case `odometry_mode` is set to `true` and map->odom->base_link in case odometry_mode is set to `false`. Further information can be found on the [FAQ](https://github.com/suchetanrs/ORB-SLAM3-ROS2-Docker/wiki/FAQs)|
 | `map_data_publish_frequency`| `1000`         | Time interval at which map_data should be published (ms).|
 | `do_loop_closing`| `true`         | Enable or disable loop closing in ORB-SLAM3. This will also disable re-localisation and multi-map if `false`|
 

orb_slam3_ros2_wrapper/include/orb_slam3_ros2_wrapper/orb_slam3_interface.hpp

@@ -70,9 +70,9 @@ namespace ORB_SLAM3_Wrapper
 
         void correctTrackedPose(Sophus::SE3f &s);
 
-        void getDirectOdomToRobotTF(std_msgs::msg::Header headerToUse, geometry_msgs::msg::TransformStamped &tf);
+        void getDirectMapToRobotTF(std_msgs::msg::Header headerToUse, geometry_msgs::msg::TransformStamped &tf);
 
-        void getMapToOdomTF(const nav_msgs::msg::Odometry::SharedPtr msgOdom, geometry_msgs::msg::TransformStamped &tf);
+        void getMapToOdomTF(const geometry_msgs::msg::TransformStamped& odomToBaseTf, geometry_msgs::msg::TransformStamped &tf);
 
         void getRobotPose(geometry_msgs::msg::PoseStamped& pose);
 

orb_slam3_ros2_wrapper/params/rgbd-ros-params.yaml

@@ -11,7 +11,6 @@ ORB_SLAM3_RGBD_ROS2:
     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.
-    odom_topic_name: ground_truth_pose #Used if odometry_mode parameter is set to true. (Set to ground_truth_pose for pose from simulation example. You can choose the topic you want)
 
     # 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.
@@ -28,4 +27,4 @@ ORB_SLAM3_RGBD_ROS2:
     odometry_mode: false
     publish_tf: true
     map_data_publish_frequency: 1000 # publish every 1000.0 milliseconds
-    do_loop_closing: true
+    do_loop_closing: true

orb_slam3_ros2_wrapper/src/orb_slam3_interface.cpp

@@ -362,29 +362,27 @@ namespace ORB_SLAM3_Wrapper
         mapReferencesMutex_.unlock();
     };
 
-    void ORBSLAM3Interface::getDirectOdomToRobotTF(std_msgs::msg::Header headerToUse, geometry_msgs::msg::TransformStamped &tf)
+    void ORBSLAM3Interface::getDirectMapToRobotTF(std_msgs::msg::Header headerToUse, geometry_msgs::msg::TransformStamped &tf)
     {
-        tf.header.frame_id = odomFrame_;
-        tf.child_frame_id = robotFrame_;
         if (hasTracked_)
         {
             std::lock_guard<std::mutex> lock(latestTrackedPoseMutex_);
             // get transform between map and odom and send the transform.
-            auto tfMapOdom = latestTrackedPose_;
-            geometry_msgs::msg::Pose poseMapOdom = typeConversions_->affine3fToPose(tfMapOdom);
+            auto poseInMap = latestTrackedPose_;
+            geometry_msgs::msg::Pose poseInMapROS = typeConversions_->affine3fToPose(poseInMap);
             rclcpp::Duration transformTimeout_ = rclcpp::Duration::from_seconds(0.5);
             rclcpp::Time odomTimestamp = headerToUse.stamp;
             tf.header.stamp = odomTimestamp + transformTimeout_;
             tf.header.frame_id = globalFrame_;
             tf.child_frame_id = robotFrame_;
-            tf.transform.translation.x = poseMapOdom.position.x;
-            tf.transform.translation.y = poseMapOdom.position.y;
-            tf.transform.translation.z = poseMapOdom.position.z;
-            tf.transform.rotation = poseMapOdom.orientation;
+            tf.transform.translation.x = poseInMapROS.position.x;
+            tf.transform.translation.y = poseInMapROS.position.y;
+            tf.transform.translation.z = poseInMapROS.position.z;
+            tf.transform.rotation = poseInMapROS.orientation;
         }
     }
 
-    void ORBSLAM3Interface::getMapToOdomTF(const nav_msgs::msg::Odometry::SharedPtr msgOdom, geometry_msgs::msg::TransformStamped &tf)
+    void ORBSLAM3Interface::getMapToOdomTF(const geometry_msgs::msg::TransformStamped& odomToBaseTf, geometry_msgs::msg::TransformStamped &tf)
     {
         // tf.header.stamp;
         tf.header.frame_id = globalFrame_;
@@ -393,19 +391,19 @@ namespace ORB_SLAM3_Wrapper
         {
             // convert odom value to Eigen::Affine3f
             auto latestOdomTransform_ = Eigen::Affine3f(
-                Eigen::Translation3f(msgOdom->pose.pose.position.x,
-                                     msgOdom->pose.pose.position.y,
-                                     msgOdom->pose.pose.position.z) *
-                Eigen::Quaternionf(msgOdom->pose.pose.orientation.w,
-                                   msgOdom->pose.pose.orientation.x,
-                                   msgOdom->pose.pose.orientation.y,
-                                   msgOdom->pose.pose.orientation.z));
+                Eigen::Translation3f(odomToBaseTf.transform.translation.x,
+                                     odomToBaseTf.transform.translation.y,
+                                     odomToBaseTf.transform.translation.z) *
+                Eigen::Quaternionf(odomToBaseTf.transform.rotation.w,
+                                   odomToBaseTf.transform.rotation.x,
+                                   odomToBaseTf.transform.rotation.y,
+                                   odomToBaseTf.transform.rotation.z));
             // get transform between map and odom and send the transform.
             std::lock_guard<std::mutex> lock(latestTrackedPoseMutex_);
             auto tfMapOdom = latestTrackedPose_ * latestOdomTransform_.inverse();
             geometry_msgs::msg::Pose poseMapOdom = typeConversions_->affine3fToPose(tfMapOdom);
             rclcpp::Duration transformTimeout_ = rclcpp::Duration::from_seconds(0.5);
-            rclcpp::Time odomTimestamp = msgOdom->header.stamp;
+            rclcpp::Time odomTimestamp = odomToBaseTf.header.stamp;
             tf.header.stamp = odomTimestamp + transformTimeout_;
             tf.header.frame_id = globalFrame_;
             tf.child_frame_id = odomFrame_;

orb_slam3_ros2_wrapper/src/rgbd/rgbd-slam-node.cpp

@@ -18,7 +18,6 @@ namespace ORB_SLAM3_Wrapper
         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"));
-        this->declare_parameter("odom_topic_name", rclcpp::ParameterValue("odom"));
 
         // ROS Subscribers
         rgbSub_ = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::Image>>(this, this->get_parameter("rgb_image_topic_name").as_string());
@@ -27,7 +26,6 @@ namespace ORB_SLAM3_Wrapper
         syncApproximate_->registerCallback(&RgbdSlamNode::RGBDCallback, this);
 
         imuSub_ = this->create_subscription<sensor_msgs::msg::Imu>(this->get_parameter("imu_topic_name").as_string(), 1000, std::bind(&RgbdSlamNode::ImuCallback, this, std::placeholders::_1));
-        odomSub_ = this->create_subscription<nav_msgs::msg::Odometry>(this->get_parameter("odom_topic_name").as_string(), 1000, std::bind(&RgbdSlamNode::OdomCallback, this, std::placeholders::_1));
 
         // ROS Publishers
         //---- the following is published when a service is called
@@ -140,37 +138,41 @@ namespace ORB_SLAM3_Wrapper
         interface_->handleIMU(msgIMU);
     }
 
-    void RgbdSlamNode::OdomCallback(const nav_msgs::msg::Odometry::SharedPtr msgOdom)
-    {
-        if (odometry_mode_)
-        {   // populate map to odom tf if odometry is being used
-            RCLCPP_DEBUG_STREAM(this->get_logger(), "OdomCallback");
-            interface_->getMapToOdomTF(msgOdom, tfMapOdom_);
-        }
-        else
-            RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 4000, "Odometry msg recorded but no odometry mode is true, set to false to use this odometry");
-    }
-
     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))
         {
             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
+                // populate map to base_footprint tf if odometry is not being used to get transform map -> base_link
                 if (!odometry_mode_)
                 {
-                    tfMapOdom_ = geometry_msgs::msg::TransformStamped();
-                    tfMapOdom_.header.stamp = msgRGB->header.stamp;
-                    tfMapOdom_.header.frame_id = global_frame_;
-                    tfMapOdom_.child_frame_id = odom_frame_id_;
-                    tfBroadcaster_->sendTransform(tfMapOdom_);
-                    interface_->getDirectOdomToRobotTF(msgRGB->header, tfMapOdom_);
+                    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;
+                    }
                 }
-                // publish the tf if publish_tf_ is true
-                tfBroadcaster_->sendTransform(tfMapOdom_);
             }
             geometry_msgs::msg::PoseStamped pose;
             interface_->getRobotPose(pose);

orb_slam3_ros2_wrapper/src/rgbd/rgbd-slam-node.hpp

@@ -55,7 +55,6 @@ namespace ORB_SLAM3_Wrapper
 
         // ROS 2 Callbacks.
         void ImuCallback(const sensor_msgs::msg::Imu::SharedPtr msgIMU);
-        void OdomCallback(const nav_msgs::msg::Odometry::SharedPtr msgOdom);
         void RGBDCallback(const sensor_msgs::msg::Image::SharedPtr msgRGB,
                           const sensor_msgs::msg::Image::SharedPtr msgD);