Update to use tf2

ros/include/Node.h

@@ -25,10 +25,11 @@
 #include <ros/ros.h>
 #include <ros/time.h>
 #include <image_transport/image_transport.h>
-#include <tf/transform_broadcaster.h>
 #include <cv_bridge/cv_bridge.h>
 #include <opencv2/core/core.hpp>
 
+#include <tf2_ros/transform_broadcaster.h>
+
 #include <dynamic_reconfigure/server.h>
 #include <orb_slam2_ros/dynamic_reconfigureConfig.h>
 
@@ -69,7 +70,7 @@ class Node
     bool SaveMapSrv (orb_slam2_ros::SaveMap::Request &req, orb_slam2_ros::SaveMap::Response &res);
     void LoadOrbParameters (ORB_SLAM2::ORBParameters& parameters);
 
-    tf::Transform TransformFromMat (cv::Mat position_mat);
+    tf2::Transform TransformFromMat (cv::Mat position_mat);
     sensor_msgs::PointCloud2 MapPointsToPointCloud (std::vector<ORB_SLAM2::MapPoint*> map_points);
 
     dynamic_reconfigure::Server<orb_slam2_ros::dynamic_reconfigureConfig> dynamic_param_server_;

ros/src/Node.cc

@@ -60,10 +60,12 @@ void Node::Update () {
   cv::Mat position = orb_slam_->GetCurrentPosition();
 
   if (!position.empty()) {
-    PublishPositionAsTransform (position);
+    if(publish_tf_param_){
+      PublishPositionAsTransform(position);
+    }
 
     if (publish_pose_param_) {
-      PublishPositionAsPoseStamped (position);
+      PublishPositionAsPoseStamped(position);
     }
   }
 
@@ -83,18 +85,27 @@ void Node::PublishMapPoints (std::vector<ORB_SLAM2::MapPoint*> map_points) {
 
 
 void Node::PublishPositionAsTransform (cv::Mat position) {
-  if(publish_tf_param_){
-      tf::Transform transform = TransformFromMat (position);
-      static tf::TransformBroadcaster tf_broadcaster;
-      tf_broadcaster.sendTransform(tf::StampedTransform(transform, current_frame_time_, map_frame_id_param_, camera_frame_id_param_));
-  }
+  // Get transform from map to camera frame
+  tf2::Transform tf_transform = TransformFromMat(position);
+
+  // 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_;
+  // Broadcast tf
+  static tf2_ros::TransformBroadcaster tf_broadcaster;
+  tf_broadcaster.sendTransform(msg);
 }
 
 void Node::PublishPositionAsPoseStamped (cv::Mat position) {
-  tf::Transform grasp_tf = TransformFromMat (position);
-  tf::Stamped<tf::Pose> grasp_tf_pose(grasp_tf, current_frame_time_, map_frame_id_param_);
+  tf2::Transform tf_position = TransformFromMat(position);
+
+  // 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_);
   geometry_msgs::PoseStamped pose_msg;
-  tf::poseStampedTFToMsg (grasp_tf_pose, pose_msg);
+  tf2::toMsg(tf_position_stamped, pose_msg);
   pose_publisher_.publish(pose_msg);
 }
 
@@ -108,23 +119,23 @@ void Node::PublishRenderedImage (cv::Mat image) {
 }
 
 
-tf::Transform Node::TransformFromMat (cv::Mat position_mat) {
+tf2::Transform Node::TransformFromMat (cv::Mat position_mat) {
   cv::Mat rotation(3,3,CV_32F);
   cv::Mat translation(3,1,CV_32F);
 
   rotation = position_mat.rowRange(0,3).colRange(0,3);
   translation = position_mat.rowRange(0,3).col(3);
 
 
-  tf::Matrix3x3 tf_camera_rotation (rotation.at<float> (0,0), rotation.at<float> (0,1), rotation.at<float> (0,2),
+  tf2::Matrix3x3 tf_camera_rotation (rotation.at<float> (0,0), rotation.at<float> (0,1), rotation.at<float> (0,2),
                                     rotation.at<float> (1,0), rotation.at<float> (1,1), rotation.at<float> (1,2),
                                     rotation.at<float> (2,0), rotation.at<float> (2,1), rotation.at<float> (2,2)
                                    );
 
-  tf::Vector3 tf_camera_translation (translation.at<float> (0), translation.at<float> (1), translation.at<float> (2));
+  tf2::Vector3 tf_camera_translation (translation.at<float> (0), translation.at<float> (1), translation.at<float> (2));
 
   //Coordinate transformation matrix from orb coordinate system to ros coordinate system
-  const tf::Matrix3x3 tf_orb_to_ros (0, 0, 1,
+  const tf2::Matrix3x3 tf_orb_to_ros (0, 0, 1,
                                     -1, 0, 0,
                                      0,-1, 0);
 
@@ -140,7 +151,7 @@ tf::Transform Node::TransformFromMat (cv::Mat position_mat) {
   tf_camera_rotation = tf_orb_to_ros*tf_camera_rotation;
   tf_camera_translation = tf_orb_to_ros*tf_camera_translation;
 
-  return tf::Transform (tf_camera_rotation, tf_camera_translation);
+  return tf2::Transform (tf_camera_rotation, tf_camera_translation);
 }