Ros2 tf tree branch

cmake/ROS2.cmake

@@ -21,6 +21,7 @@ find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(yaml-cpp REQUIRED)
@@ -46,6 +47,7 @@ ament_target_dependencies(odom_transform_node
     std_msgs
     nav_msgs
     sensor_msgs
+    geometry_msgs
     yaml-cpp
     rclcpp_components
     tf2

config/transform.yaml

@@ -3,6 +3,7 @@ odom_rate: 150.0
 init_world_with_vicon: false
 viconOdomWTopic: "/$(optenv MAV_NAME quadrotor)/odom"
 viconOdomWTopic_ros2: "/odom"
+publish_tf: true
 
 T_cam_imu:
   r0: {c0: 1.0, c1: 0.0, c2: 0.0, c3: -0.04402011600452833}

include/transform_nodelet_ros2.h

@@ -4,6 +4,11 @@
 #include <nav_msgs/msg/odometry.hpp>
 #include <chrono>
 #include <Eigen/Eigen>
+#include "tf2_ros/transform_broadcaster.h"
+#include "geometry_msgs/msg/transform_stamped.h"
+#include "geometry_msgs/msg/quaternion.hpp"
+#include "geometry_msgs/msg/vector3.h"
+
 
 namespace transform_nodelet_ns
 {
@@ -58,15 +63,33 @@ namespace transform_nodelet_ns
             rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub_odomworld;
             bool got_init_tf = false;
             bool init_world_with_vicon = false;
+            bool publish_tf = true;
             int skip_count = 0;
-
+            std::string mav_name;
+
+            tf2_ros::TransformBroadcaster mTfBr; //tf object responsible for publishing frames between world and body or odom and body
             double pub_frequency = 0.0;
             std::chrono::high_resolution_clock::time_point last_timestamp{};
             std::chrono::high_resolution_clock::time_point current_timestamp{};
             float imu_rate = 0;
             float odom_rate = 0;
 
     };
+    // This function creates the tf message to be published. Takes input from odom and outputs a msg of type geometry_msgs::msg::TransformStamped 
+    inline geometry_msgs::msg::TransformStamped get_stamped_transform_from_odom(nav_msgs::msg::Odometry &odom) {
+            auto q_ItoC = odom.pose.pose.orientation;
+            auto p_CinI = odom.pose.pose.position;
+            geometry_msgs::msg::TransformStamped trans;
+            trans.header.stamp = odom.header.stamp;
+            //geometry_msgs::msg::Quaternion quat = q_ItoC;//(q_ItoC.x, q_ItoC.y, q_ItoC.z, q_ItoC.w)
+            trans.transform.rotation = q_ItoC;
+            geometry_msgs::msg::Vector3 orig; //(p_CinI.x, p_CinI.y, p_CinI.z)
+            orig.x = p_CinI.x;
+            orig.y = p_CinI.y;
+            orig.z = p_CinI.z;
+            trans.transform.translation = orig;
+            return trans;
+    }
 
     inline Eigen::Matrix<double, 3, 3> skew_x(const Eigen::Matrix<double, 3, 1> &w) {
         Eigen::Matrix<double, 3, 3> w_x;

package.xml

@@ -28,6 +28,7 @@
   <depend condition="$ROS_VERSION == 2">tf2_ros</depend>
   <depend condition="$ROS_VERSION == 2">std_msgs</depend>
   <depend condition="$ROS_VERSION == 2">sensor_msgs</depend>
+  <depend condition="$ROS_VERSION == 2">geometry_msgs</depend>
   <depend condition="$ROS_VERSION == 2">nav_msgs</depend>
   <depend condition="$ROS_VERSION == 2">rclcpp_components</depend>
 

src/transform.cpp

@@ -187,14 +187,14 @@ void Transform_calculator::odomCallback(const nav_msgs::Odometry::ConstPtr &msg_
         odomBinW->pose.covariance = odomIinM.pose.covariance;
 
         pub_odomworld.publish(odomBinW);
-        /*
-            if (publish_tf) {
-                tf::StampedTransform trans = get_stamped_transform_from_odom(odomBinW);
-                trans.frame_id_ = "world";
-                trans.child_frame_id_ = mav_name + "/body";
-                mTfBr.sendTransform(trans);
-            }
-        */
+
+        if (publish_tf) {
+            tf::StampedTransform trans = get_stamped_transform_from_odom(odomBinW);
+            trans.frame_id_ = "world";
+            trans.child_frame_id_ = mav_name + "/body";
+            mTfBr.sendTransform(trans);
+        }
+
         // if ( odomBinW.pose.covariance(0) > 0.05){
         //   PRINT_ERROR(RED "Drift detected: pose covariance of x-x is too high %.6f\n",
         //   odomBinW.pose.covariance(0));

src/transform_nodelet_ros2.cpp

@@ -10,7 +10,7 @@ namespace transform_nodelet_ns {
 
 // Creating constructor
 OvtransformNodeletClass::OvtransformNodeletClass(const rclcpp::NodeOptions& transform_node)
-    : Node("odom_transform_nodelet", transform_node) {
+    : Node("odom_transform_nodelet", transform_node), mTfBr(this) {
     onInit();  // Calling the onInit function
 }
 
@@ -52,6 +52,7 @@ void OvtransformNodeletClass::setup(const std::string transform_config_path) {
 
     imu_rate = config["imu_rate"].as<float>();
     odom_rate = config["odom_rate"].as<float>();
+    publish_tf = config["publish_tf"].as<bool>();  // Get whether we should publish tf or not
 
     RCLCPP_INFO(this->get_logger(), "[odom_transform] imu_rate: %f", imu_rate);
     RCLCPP_INFO(this->get_logger(), "[odom_transform] odom_rate: %f", odom_rate);
@@ -97,14 +98,12 @@ void OvtransformNodeletClass::setupTransformationMatrix(const std::string transf
                         T_B0_W["r" + std::to_string(i)]["c" + std::to_string(j)].as<double>());
         }
     }
-
+    mav_name = this->declare_parameter("mav_name", std::string("Value"));
+    this->get_parameter("mav_name", mav_name);
     init_world_with_vicon = config["init_world_with_vicon"].as<bool>();
 
     // TODO: Check this part with Vicon later
     if (init_world_with_vicon) {
-        std::string mav_name;
-        mav_name = this->declare_parameter("mav_name", std::string("Value"));
-        this->get_parameter("mav_name", mav_name);
         std::string viconOdomWTopic1 = config["viconOdomWTopic_ros2"].as<std::string>();
         std::string viconOdomWTopic = "/" + mav_name + viconOdomWTopic1;
         auto sharedInitBodyOdominW = std::make_shared<nav_msgs::msg::Odometry>();
@@ -184,13 +183,12 @@ void OvtransformNodeletClass::odomCallback(const nav_msgs::msg::Odometry::Shared
     auto odomBinW = std::make_unique<nav_msgs::msg::Odometry>();
     odomBinW->header.frame_id = "world";
     auto odomBinB0 = std::make_unique<nav_msgs::msg::Odometry>();
-    odomBinB0->header.frame_id = "odom";
+    odomBinB0->header.frame_id = mav_name + "/odom";
     odomBinW->header.stamp = odomIinM.header.stamp;
     odomBinB0->header.stamp = odomIinM.header.stamp;
 
     q_GinI_eigen << odomIinM.pose.pose.orientation.x, odomIinM.pose.pose.orientation.y,
         odomIinM.pose.pose.orientation.z, odomIinM.pose.pose.orientation.w;
-    ;
 
     T_ItoM.block(0, 0, 3, 3) = quat_2_Rot(q_GinI_eigen).transpose();  // this is right-handed JPL->right-handed
 
@@ -235,44 +233,69 @@ void OvtransformNodeletClass::odomCallback(const nav_msgs::msg::Odometry::Shared
 
     Eigen::Vector3d v_BinB0 = R_BtoB0 * v_BinB;
     Eigen::Vector3d v_BinW = R_BtoW * v_BinB;
-
-    // The POSE component (orientation and position)
-    odomBinW->pose.pose.orientation.x = q_BinW.x();
-    odomBinW->pose.pose.orientation.y = q_BinW.y();
-    odomBinW->pose.pose.orientation.z = q_BinW.z();
-    odomBinW->pose.pose.orientation.w = q_BinW.w();
-    odomBinW->pose.pose.position.x = position_BinW(0);
-    odomBinW->pose.pose.position.y = position_BinW(1);
-    odomBinW->pose.pose.position.z = position_BinW(2);
-
-    odomBinB0->pose.pose.orientation.x = q_BinB0.x();
-    odomBinB0->pose.pose.orientation.y = q_BinB0.y();
-    odomBinB0->pose.pose.orientation.z = q_BinB0.z();
-    odomBinB0->pose.pose.orientation.w = q_BinB0.w();
-    odomBinB0->pose.pose.position.x = position_BinB0(0);
-    odomBinB0->pose.pose.position.y = position_BinB0(1);
-    odomBinB0->pose.pose.position.z = position_BinB0(2);
-
-    // The TWIST component (angular and linear velocities)
-    odomBinW->child_frame_id = "body";
-    odomBinW->twist.twist.linear.x = v_BinW(0);   // vel in world frame
-    odomBinW->twist.twist.linear.y = v_BinW(1);   // vel in world frame
-    odomBinW->twist.twist.linear.z = v_BinW(2);   // vel in world frame
-    odomBinW->twist.twist.angular.x = w_BinB(0);  // we do not estimate this...
-    odomBinW->twist.twist.angular.y = w_BinB(1);  // we do not estimate this...
-    odomBinW->twist.twist.angular.z = w_BinB(2);
-    ;  // we do not estimate this...
-
-    odomBinB0->child_frame_id = "body";
-    odomBinB0->twist.twist.linear.x = v_BinB0(0);  // vel in world frame
-    odomBinB0->twist.twist.linear.y = v_BinB0(1);  // vel in world frame
-    odomBinB0->twist.twist.linear.z = v_BinB0(2);  // vel in world frame
-    odomBinB0->twist.twist.angular.x = w_BinB(0);  // we do not estimate this...
-    odomBinB0->twist.twist.angular.y = w_BinB(1);  // we do not estimate this...
-    odomBinB0->twist.twist.angular.z = w_BinB(2);  // we do not estimate this...
-
-    odomBinW->pose.covariance = odomIinM.pose.covariance;
-    odomBinB0->pose.covariance = odomIinM.pose.covariance;
+    // Check to see if there are any subscribers before publishing the odometry BinWorld
+    // TODO: To incorporate multi-robot operations, and enable loop closure, we need to define the relation between
+    // world, map and race/odom. So the existing pipeline needs to be modified.
+    // TODO: There needs to be a way to define the relative trasnformations between multiple drones/agents through
+    // the config file in arpl_autonomy
+    if (pub_odomworld->get_subscription_count() > 0) {
+        // The POSE component (orientation and position)
+        odomBinW->pose.pose.orientation.x = q_BinW.x();
+        odomBinW->pose.pose.orientation.y = q_BinW.y();
+        odomBinW->pose.pose.orientation.z = q_BinW.z();
+        odomBinW->pose.pose.orientation.w = q_BinW.w();
+        odomBinW->pose.pose.position.x = position_BinW(0);
+        odomBinW->pose.pose.position.y = position_BinW(1);
+        odomBinW->pose.pose.position.z = position_BinW(2);
+
+        // The TWIST component (angular and linear velocities)
+        odomBinW->child_frame_id = mav_name + "/body";
+        odomBinW->twist.twist.linear.x = v_BinW(0);   // vel in world frame
+        odomBinW->twist.twist.linear.y = v_BinW(1);   // vel in world frame
+        odomBinW->twist.twist.linear.z = v_BinW(2);   // vel in world frame
+        odomBinW->twist.twist.angular.x = w_BinB(0);  // we do not estimate this...
+        odomBinW->twist.twist.angular.y = w_BinB(1);  // we do not estimate this...
+        odomBinW->twist.twist.angular.z = w_BinB(2);
+        ;  // we do not estimate this...
+        odomBinW->pose.covariance = odomIinM.pose.covariance;
+        // If publish tf is set, publish odom transforms tf frames : Binworld frame.
+        if (publish_tf) {
+            geometry_msgs::msg::TransformStamped trans_w = get_stamped_transform_from_odom(
+                *odomBinW);  // Get the transform stamped message from the function.
+            trans_w.header.frame_id = "world";
+            trans_w.child_frame_id = mav_name + "/body";
+            mTfBr.sendTransform(trans_w);  // Publish the tranform odom w.r.t world frame
+        }
+        pub_odomworld->publish(std::move(odomBinW));
+    }
+    // Check to see if there are any subscribers before publishing the odometry BinB0
+    if (pub_odomworldB0->get_subscription_count() > 0) {
+        odomBinB0->pose.pose.orientation.x = q_BinB0.x();
+        odomBinB0->pose.pose.orientation.y = q_BinB0.y();
+        odomBinB0->pose.pose.orientation.z = q_BinB0.z();
+        odomBinB0->pose.pose.orientation.w = q_BinB0.w();
+        odomBinB0->pose.pose.position.x = position_BinB0(0);
+        odomBinB0->pose.pose.position.y = position_BinB0(1);
+        odomBinB0->pose.pose.position.z = position_BinB0(2);
+
+        odomBinB0->child_frame_id = mav_name + "/body";
+        odomBinB0->twist.twist.linear.x = v_BinB0(0);  // vel in world frame
+        odomBinB0->twist.twist.linear.y = v_BinB0(1);  // vel in world frame
+        odomBinB0->twist.twist.linear.z = v_BinB0(2);  // vel in world frame
+        odomBinB0->twist.twist.angular.x = w_BinB(0);  // we do not estimate this...
+        odomBinB0->twist.twist.angular.y = w_BinB(1);  // we do not estimate this...
+        odomBinB0->twist.twist.angular.z = w_BinB(2);  // we do not estimate this...
+        odomBinB0->pose.covariance = odomIinM.pose.covariance;
+        // If publish tf is set, publish odom transforms tf frames : BinB0 frame.
+        if (publish_tf) {
+            geometry_msgs::msg::TransformStamped trans_B0 = get_stamped_transform_from_odom(
+                *odomBinB0);  // Get the transform stamped message from the function.
+            trans_B0.header.frame_id = mav_name + "/odom";
+            trans_B0.child_frame_id = mav_name + "/body";
+            mTfBr.sendTransform(trans_B0);  // Publish the transform of odom w.r.t initial odom value.
+        }
+        pub_odomworldB0->publish(std::move(odomBinB0));
+    }
     // if ( odomBinW.pose.covariance(0) > 0.05){
     //   PRINT_ERROR(RED "Drift detected: pose covariance of x-x is too high %.6f\n", odomBinW.pose.covariance(0));
     // }
@@ -283,9 +306,6 @@ void OvtransformNodeletClass::odomCallback(const nav_msgs::msg::Odometry::Shared
     //   PRINT_ERROR(RED "Drift detected: pose covariance of z-z is too high %.6f\n",
     //   odomBinW.pose.covariance(14));
     // }
-
-    pub_odomworld->publish(std::move(odomBinW));
-    pub_odomworldB0->publish(std::move(odomBinB0));
 }
 }  // namespace transform_nodelet_ns