From 72530346e26929a1bbda4ff7a71e494d7b57906f Mon Sep 17 00:00:00 2001 From: PranavNedunghat Date: Fri, 8 Mar 2024 20:05:44 -0500 Subject: [PATCH 1/4] Added functionality to the Nodelet to start publishing the TF tree required for mapping --- cmake/ROS2.cmake | 2 ++ config/transform.yaml | 1 + include/transform_nodelet_ros2.h | 25 ++++++++++++++++++++++++- package.xml | 1 + src/transform.cpp | 4 ++-- src/transform_nodelet_ros2.cpp | 25 +++++++++++++++++++------ 6 files changed, 49 insertions(+), 9 deletions(-) diff --git a/cmake/ROS2.cmake b/cmake/ROS2.cmake index aa24604..442ad8d 100644 --- a/cmake/ROS2.cmake +++ b/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 diff --git a/config/transform.yaml b/config/transform.yaml index 37c9f60..8dedae1 100644 --- a/config/transform.yaml +++ b/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} diff --git a/include/transform_nodelet_ros2.h b/include/transform_nodelet_ros2.h index 82f2474..002addc 100644 --- a/include/transform_nodelet_ros2.h +++ b/include/transform_nodelet_ros2.h @@ -4,6 +4,11 @@ #include #include #include +#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,8 +63,11 @@ namespace transform_nodelet_ns rclcpp::Publisher::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{}; @@ -67,6 +75,21 @@ namespace transform_nodelet_ns 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 skew_x(const Eigen::Matrix &w) { Eigen::Matrix w_x; diff --git a/package.xml b/package.xml index 29812b1..1f998c2 100644 --- a/package.xml +++ b/package.xml @@ -28,6 +28,7 @@ tf2_ros std_msgs sensor_msgs + geometry_msgs nav_msgs rclcpp_components diff --git a/src/transform.cpp b/src/transform.cpp index 13d83db..162f137 100644 --- a/src/transform.cpp +++ b/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 ( 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)); diff --git a/src/transform_nodelet_ros2.cpp b/src/transform_nodelet_ros2.cpp index eb098f2..9035caa 100644 --- a/src/transform_nodelet_ros2.cpp +++ b/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(); odom_rate = config["odom_rate"].as(); + publish_tf = config["publish_tf"].as(); //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); @@ -102,7 +103,6 @@ void OvtransformNodeletClass::setupTransformationMatrix(const std::string transf // 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(); @@ -184,13 +184,12 @@ void OvtransformNodeletClass::odomCallback(const nav_msgs::msg::Odometry::Shared auto odomBinW = std::make_unique(); odomBinW->header.frame_id = "world"; auto odomBinB0 = std::make_unique(); - 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 @@ -254,7 +253,7 @@ void OvtransformNodeletClass::odomCallback(const nav_msgs::msg::Odometry::Shared odomBinB0->pose.pose.position.z = position_BinB0(2); // The TWIST component (angular and linear velocities) - odomBinW->child_frame_id = "body"; + 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 @@ -263,7 +262,7 @@ void OvtransformNodeletClass::odomCallback(const nav_msgs::msg::Odometry::Shared odomBinW->twist.twist.angular.z = w_BinB(2); ; // we do not estimate this... - odomBinB0->child_frame_id = "body"; + 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 @@ -286,6 +285,20 @@ void OvtransformNodeletClass::odomCallback(const nav_msgs::msg::Odometry::Shared pub_odomworld->publish(std::move(odomBinW)); pub_odomworldB0->publish(std::move(odomBinB0)); + // If publish tf is set, publish both odom transforms : BinB0 and Binworld frames. + // TODO: Check to see if there are any subscribers before publishing this data. It might slow down the network. + if (publish_tf) { + nav_msgs::msg::Odometry frame_odom = *odomBinW; + geometry_msgs::msg::TransformStamped trans = get_stamped_transform_from_odom(frame_odom); //Get the transform stamped message from the function. + trans.header.frame_id = "world"; + trans.child_frame_id = mav_name + "/body"; + mTfBr.sendTransform(trans); //Publish the tranform odom w.r.t world frame + frame_odom = *odomBinB0; + trans = get_stamped_transform_from_odom(frame_odom); //Get the transform stamped message from the function. + trans.header.frame_id = mav_name + "/odom"; + trans.child_frame_id = mav_name + "/body"; + mTfBr.sendTransform(trans);// Publish the transform of odom w.r.t initial odom value. + } } } // namespace transform_nodelet_ns From edf76bb022bcaedd0c4e1eb78015ddc4b841d812 Mon Sep 17 00:00:00 2001 From: PranavNedunghat Date: Sat, 9 Mar 2024 17:36:23 -0500 Subject: [PATCH 2/4] Added functionality to the Nodelet to start publishing the TF tree required for mapping --- src/transform_nodelet_ros2.cpp | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/src/transform_nodelet_ros2.cpp b/src/transform_nodelet_ros2.cpp index 9035caa..f81997b 100644 --- a/src/transform_nodelet_ros2.cpp +++ b/src/transform_nodelet_ros2.cpp @@ -98,13 +98,12 @@ void OvtransformNodeletClass::setupTransformationMatrix(const std::string transf T_B0_W["r" + std::to_string(i)]["c" + std::to_string(j)].as()); } } - + 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(); // TODO: Check this part with Vicon later if (init_world_with_vicon) { - 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 viconOdomWTopic = "/" + mav_name + viconOdomWTopic1; auto sharedInitBodyOdominW = std::make_shared(); @@ -282,19 +281,19 @@ 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)); // } - + nav_msgs::msg::Odometry frame_odom_W = *odomBinW; //Storing the data before its moved out of the smart pointer when its published. + nav_msgs::msg::Odometry frame_odom_B0 = *odomBinB0; pub_odomworld->publish(std::move(odomBinW)); pub_odomworldB0->publish(std::move(odomBinB0)); // If publish tf is set, publish both odom transforms : BinB0 and Binworld frames. // TODO: Check to see if there are any subscribers before publishing this data. It might slow down the network. - if (publish_tf) { - nav_msgs::msg::Odometry frame_odom = *odomBinW; - geometry_msgs::msg::TransformStamped trans = get_stamped_transform_from_odom(frame_odom); //Get the transform stamped message from the function. + if (publish_tf) { + + geometry_msgs::msg::TransformStamped trans = get_stamped_transform_from_odom(frame_odom_W); //Get the transform stamped message from the function. trans.header.frame_id = "world"; trans.child_frame_id = mav_name + "/body"; mTfBr.sendTransform(trans); //Publish the tranform odom w.r.t world frame - frame_odom = *odomBinB0; - trans = get_stamped_transform_from_odom(frame_odom); //Get the transform stamped message from the function. + trans = get_stamped_transform_from_odom(frame_odom_B0); //Get the transform stamped message from the function. trans.header.frame_id = mav_name + "/odom"; trans.child_frame_id = mav_name + "/body"; mTfBr.sendTransform(trans);// Publish the transform of odom w.r.t initial odom value. From 0eda9a70e79b16307de815bae7fb1c728230e758 Mon Sep 17 00:00:00 2001 From: PranavNedunghat Date: Mon, 11 Mar 2024 15:53:13 -0400 Subject: [PATCH 3/4] Added a check to make sure odom transform only publishes the tf tree when someone is listening to rhe respective topic. Also, we need to add functionality to accomodate multi robot frame transforms between world and map and odom --- src/transform_nodelet_ros2.cpp | 118 ++++++++++++++++++--------------- 1 file changed, 64 insertions(+), 54 deletions(-) diff --git a/src/transform_nodelet_ros2.cpp b/src/transform_nodelet_ros2.cpp index f81997b..924427e 100644 --- a/src/transform_nodelet_ros2.cpp +++ b/src/transform_nodelet_ros2.cpp @@ -233,44 +233,70 @@ 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; - + //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); - - 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 = 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... - - 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... - - odomBinW->pose.covariance = odomIinM.pose.covariance; - odomBinB0->pose.covariance = odomIinM.pose.covariance; + 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)); // } @@ -281,23 +307,7 @@ 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)); // } - nav_msgs::msg::Odometry frame_odom_W = *odomBinW; //Storing the data before its moved out of the smart pointer when its published. - nav_msgs::msg::Odometry frame_odom_B0 = *odomBinB0; - pub_odomworld->publish(std::move(odomBinW)); - pub_odomworldB0->publish(std::move(odomBinB0)); - // If publish tf is set, publish both odom transforms : BinB0 and Binworld frames. - // TODO: Check to see if there are any subscribers before publishing this data. It might slow down the network. - if (publish_tf) { - - geometry_msgs::msg::TransformStamped trans = get_stamped_transform_from_odom(frame_odom_W); //Get the transform stamped message from the function. - trans.header.frame_id = "world"; - trans.child_frame_id = mav_name + "/body"; - mTfBr.sendTransform(trans); //Publish the tranform odom w.r.t world frame - trans = get_stamped_transform_from_odom(frame_odom_B0); //Get the transform stamped message from the function. - trans.header.frame_id = mav_name + "/odom"; - trans.child_frame_id = mav_name + "/body"; - mTfBr.sendTransform(trans);// Publish the transform of odom w.r.t initial odom value. - } + } } // namespace transform_nodelet_ns From e0bacfb468be64c4768ff8a87c3b4900151c58a6 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Wed, 13 Mar 2024 20:43:52 -0400 Subject: [PATCH 4/4] clang-format action fixes (#21) Co-authored-by: PranavNedunghat --- src/transform.cpp | 16 ++-- src/transform_nodelet_ros2.cpp | 130 ++++++++++++++++----------------- 2 files changed, 72 insertions(+), 74 deletions(-) diff --git a/src/transform.cpp b/src/transform.cpp index 162f137..82eaa0d 100644 --- a/src/transform.cpp +++ b/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)); diff --git a/src/transform_nodelet_ros2.cpp b/src/transform_nodelet_ros2.cpp index 924427e..350a74f 100644 --- a/src/transform_nodelet_ros2.cpp +++ b/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),mTfBr(this) { + : Node("odom_transform_nodelet", transform_node), mTfBr(this) { onInit(); // Calling the onInit function } @@ -52,7 +52,7 @@ void OvtransformNodeletClass::setup(const std::string transform_config_path) { imu_rate = config["imu_rate"].as(); odom_rate = config["odom_rate"].as(); - publish_tf = config["publish_tf"].as(); //Get whether we should publish tf or not + publish_tf = config["publish_tf"].as(); // 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); @@ -233,69 +233,68 @@ 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; - //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)); + // 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)); @@ -307,7 +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)); // } - } } // namespace transform_nodelet_ns