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..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 eb098f2..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) { + : 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); @@ -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()); } } - + 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) { - 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 viconOdomWTopic = "/" + mav_name + viconOdomWTopic1; auto sharedInitBodyOdominW = std::make_shared(); @@ -184,13 +183,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 @@ -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