Add method for transforming map tf and pose estimate to a target fram…

…e different from the camera frame
appliedAI-Initiative · Jan 8, 2021 · f8b5bd2 · f8b5bd2
1 parent 7282412
commit f8b5bd2
Show file tree

Hide file tree

Showing 2 changed files with 82 additions and 8 deletions.
diff --git a/ros/include/Node.h b/ros/include/Node.h
@@ -29,6 +29,9 @@
 #include <opencv2/core/core.hpp>
 
 #include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
 #include <dynamic_reconfigure/server.h>
 #include <orb_slam2_ros/dynamic_reconfigureConfig.h>
@@ -70,7 +73,12 @@ class Node
     bool SaveMapSrv (orb_slam2_ros::SaveMap::Request &req, orb_slam2_ros::SaveMap::Response &res);
     void LoadOrbParameters (ORB_SLAM2::ORBParameters& parameters);
 
+    // initialization Transform listener
+    boost::shared_ptr<tf2_ros::Buffer> tfBuffer;
+    boost::shared_ptr<tf2_ros::TransformListener> tfListener;
+
     tf2::Transform TransformFromMat (cv::Mat position_mat);
+    tf2::Transform TransformToTarget (tf2::Transform tf_in, std::string frame_in, std::string frame_target);
     sensor_msgs::PointCloud2 MapPointsToPointCloud (std::vector<ORB_SLAM2::MapPoint*> map_points);
 
     dynamic_reconfigure::Server<orb_slam2_ros::dynamic_reconfigureConfig> dynamic_param_server_;
@@ -89,6 +97,7 @@ class Node
 
     std::string map_frame_id_param_;
     std::string camera_frame_id_param_;
+    std::string target_frame_id_param_;
     std::string map_file_name_param_;
     std::string voc_file_name_param_;
     bool load_map_param_;

diff --git a/ros/src/Node.cc b/ros/src/Node.cc
@@ -27,6 +27,7 @@ void Node::Init () {
   node_handle_.param(name_of_node_+ "/publish_tf", publish_tf_param_, true);
   node_handle_.param<std::string>(name_of_node_+ "/pointcloud_frame_id", map_frame_id_param_, "map");
   node_handle_.param<std::string>(name_of_node_+ "/camera_frame_id", camera_frame_id_param_, "camera_link");
+  node_handle_.param<std::string>(name_of_node_+ "/target_frame_id", target_frame_id_param_, "base_link");
   node_handle_.param<std::string>(name_of_node_ + "/map_file", map_file_name_param_, "map.bin");
   node_handle_.param<std::string>(name_of_node_ + "/voc_file", voc_file_name_param_, "file_not_set");
   node_handle_.param(name_of_node_ + "/load_map", load_map_param_, false);
@@ -44,6 +45,10 @@ void Node::Init () {
   dynamic_param_callback = boost::bind(&Node::ParamsChangedCallback, this, _1, _2);
   dynamic_param_server_.setCallback(dynamic_param_callback);
 
+  // Initialization transformation listener
+  tfBuffer.reset(new tf2_ros::Buffer);
+  tfListener.reset(new tf2_ros::TransformListener(*tfBuffer));
+
   rendered_image_publisher_ = image_transport_.advertise (name_of_node_+"/debug_image", 1);
   if (publish_pointcloud_param_) {
     map_points_publisher_ = node_handle_.advertise<sensor_msgs::PointCloud2> (name_of_node_+"/map_points", 1);
@@ -60,7 +65,7 @@ void Node::Update () {
   cv::Mat position = orb_slam_->GetCurrentPosition();
 
   if (!position.empty()) {
-    if(publish_tf_param_){
+    if (publish_tf_param_){
       PublishPositionAsTransform(position);
     }
 
@@ -83,29 +88,89 @@ void Node::PublishMapPoints (std::vector<ORB_SLAM2::MapPoint*> map_points) {
   map_points_publisher_.publish (cloud);
 }
 
+tf2::Transform Node::TransformToTarget (tf2::Transform tf_in, std::string frame_in, std::string frame_target) {
+  // Transform tf_in from frame_in to frame_target
+  tf2::Transform tf_map2orig = tf_in;
+  tf2::Transform tf_orig2target;
+  tf2::Transform tf_map2target;
+
+  tf2::Stamped<tf2::Transform> transformStamped_temp;
+  try {
+    // Get the transform from camera to target
+    geometry_msgs::TransformStamped tf_msg = tfBuffer->lookupTransform(frame_in, frame_target, ros::Time(0));
+    // Convert to tf2
+    tf2::fromMsg(tf_msg, transformStamped_temp);
+    tf_orig2target.setBasis(transformStamped_temp.getBasis());
+    tf_orig2target.setOrigin(transformStamped_temp.getOrigin());
+
+  } catch (tf2::TransformException &ex) {
+    ROS_WARN("%s",ex.what());
+    //ros::Duration(1.0).sleep();
+    tf_orig2target.setIdentity();
+  }
+
+  /* 
+    // Print debug info
+    double roll, pitch, yaw;
+    // Print debug map2orig
+    tf2::Matrix3x3(tf_map2orig.getRotation()).getRPY(roll, pitch, yaw);
+    ROS_INFO("Static transform Map to Orig [%s -> %s]",
+                    map_frame_id_param_.c_str(), frame_in.c_str());
+    ROS_INFO(" * Translation: {%.3f,%.3f,%.3f}",
+                    tf_map2orig.getOrigin().x(), tf_map2orig.getOrigin().y(), tf_map2orig.getOrigin().z());
+    ROS_INFO(" * Rotation: {%.3f,%.3f,%.3f}",
+                    RAD2DEG(roll), RAD2DEG(pitch), RAD2DEG(yaw));
+    // Print debug tf_orig2target
+    tf2::Matrix3x3(tf_orig2target.getRotation()).getRPY(roll, pitch, yaw);
+    ROS_INFO("Static transform Orig to Target [%s -> %s]",
+                    frame_in.c_str(), frame_target.c_str());
+    ROS_INFO(" * Translation: {%.3f,%.3f,%.3f}",
+                    tf_orig2target.getOrigin().x(), tf_orig2target.getOrigin().y(), tf_orig2target.getOrigin().z());
+    ROS_INFO(" * Rotation: {%.3f,%.3f,%.3f}",
+                    RAD2DEG(roll), RAD2DEG(pitch), RAD2DEG(yaw));
+    // Print debug map2target
+    tf2::Matrix3x3(tf_map2target.getRotation()).getRPY(roll, pitch, yaw);
+    ROS_INFO("Static transform Map to Target [%s -> %s]",
+                    map_frame_id_param_.c_str(), frame_target.c_str());
+    ROS_INFO(" * Translation: {%.3f,%.3f,%.3f}",
+                    tf_map2target.getOrigin().x(), tf_map2target.getOrigin().y(), tf_map2target.getOrigin().z());
+    ROS_INFO(" * Rotation: {%.3f,%.3f,%.3f}",
+                    RAD2DEG(roll), RAD2DEG(pitch), RAD2DEG(yaw));
+  */
+
+  // Transform from map to target
+  tf_map2target = tf_map2orig * tf_orig2target;
+  return tf_map2target;
+}
 
 void Node::PublishPositionAsTransform (cv::Mat position) {
   // Get transform from map to camera frame
   tf2::Transform tf_transform = TransformFromMat(position);
 
+  // Make transform from camera frame to target frame
+  tf2::Transform tf_map2target = TransformToTarget(tf_transform, camera_frame_id_param_, target_frame_id_param_);
+
   // Make message
-  tf2::Stamped<tf2::Transform> tf_transform_stamped;
-  tf_transform_stamped = tf2::Stamped<tf2::Transform>(tf_transform, current_frame_time_, map_frame_id_param_);
-  geometry_msgs::TransformStamped msg = tf2::toMsg(tf_transform_stamped);
-  msg.child_frame_id = camera_frame_id_param_;
+  tf2::Stamped<tf2::Transform> tf_map2target_stamped;
+  tf_map2target_stamped = tf2::Stamped<tf2::Transform>(tf_map2target, current_frame_time_, map_frame_id_param_);
+  geometry_msgs::TransformStamped msg = tf2::toMsg(tf_map2target_stamped);
+  msg.child_frame_id = target_frame_id_param_;
   // Broadcast tf
   static tf2_ros::TransformBroadcaster tf_broadcaster;
   tf_broadcaster.sendTransform(msg);
 }
 
 void Node::PublishPositionAsPoseStamped (cv::Mat position) {
   tf2::Transform tf_position = TransformFromMat(position);
+
+  // Make transform from camera frame to target frame
+  tf2::Transform tf_position_target = TransformToTarget(tf_position, camera_frame_id_param_, target_frame_id_param_);
 
   // Make message
-  tf2::Stamped<tf2::Transform> tf_position_stamped;
-  tf_position_stamped = tf2::Stamped<tf2::Transform>(tf_position, current_frame_time_, map_frame_id_param_);
+  tf2::Stamped<tf2::Transform> tf_position_target_stamped;
+  tf_position_target_stamped = tf2::Stamped<tf2::Transform>(tf_position_target, current_frame_time_, map_frame_id_param_);
   geometry_msgs::PoseStamped pose_msg;
-  tf2::toMsg(tf_position_stamped, pose_msg);
+  tf2::toMsg(tf_position_target_stamped, pose_msg);
   pose_publisher_.publish(pose_msg);
 }