Skip to content

Commit

Permalink
Ros2 tf tree branch (#20)
Browse files Browse the repository at this point in the history 
* Added functionality to the Nodelet to start publishing the TF tree required for mapping

* Added functionality to the Nodelet to start publishing the TF tree required for mapping

* 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

* clang-format action fixes (#21)

Co-authored-by: PranavNedunghat <[email protected]>

---------

Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>
Co-authored-by: PranavNedunghat <[email protected]>
  • Loading branch information
@PranavNedunghat @github-actions
3 people authored Mar 14, 2024
1 parent df29714 commit 9aba039
Show file tree
Hide file tree
Showing 6 changed files with 104 additions and 57 deletions.
2 changes: 2 additions & 0 deletions cmake/ROS2.cmake
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -46,6 +47,7 @@ ament_target_dependencies(odom_transform_node
std_msgs
nav_msgs
sensor_msgs
geometry_msgs
yaml-cpp
rclcpp_components
tf2
Expand Down
1 change: 1 addition & 0 deletions config/transform.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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}
Expand Down
25 changes: 24 additions & 1 deletion include/transform_nodelet_ros2.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down Expand Up @@ -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;
Expand Down
1 change: 1 addition & 0 deletions package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>

Expand Down
16 changes: 8 additions & 8 deletions src/transform.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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));
Expand Down
116 changes: 68 additions & 48 deletions src/transform_nodelet_ros2.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
}

Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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>();
Expand Down Expand Up @@ -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

Expand Down Expand Up @@ -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));
// }
Expand All @@ -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

Expand Down

0 comments on commit 9aba039

Please sign in to comment.