From 178f7e5d4a330bbe4783b23f1324bf112da30ec5 Mon Sep 17 00:00:00 2001
From: MarcGyongyosi <marc.gyongyosi@u.northwestern.edu>
Date: Mon, 8 Jun 2015 12:30:01 -0500
Subject: [PATCH] added pose publisher in Trackin.cc and Tracking.h

---
 include/Tracking.h |  5 +++++
 src/Tracking.cc    | 15 +++++++++++++++
 2 files changed, 20 insertions(+)

diff --git a/include/Tracking.h b/include/Tracking.h
index 8355312e..77e201a0 100644
--- a/include/Tracking.h
+++ b/include/Tracking.h
@@ -38,6 +38,7 @@
 #include "MapPublisher.h"
 
 #include<tf/transform_broadcaster.h>
+#include "geometry_msgs/PoseStamped.h"
 
 
 namespace ORB_SLAM
@@ -180,6 +181,10 @@ class Tracking
 
     // Transfor broadcaster (for visualization in rviz)
     tf::TransformBroadcaster mTfBr;
+
+    // Pose broadcaster
+    ros::NodeHandle n;
+    ros::Publisher PosPub = n.advertise<geometry_msgs::PoseStamped>("ORB_SLAM/pose", 5);
 };
 
 } //namespace ORB_SLAM
diff --git a/src/Tracking.cc b/src/Tracking.cc
index 6af74003..24546dc5 100644
--- a/src/Tracking.cc
+++ b/src/Tracking.cc
@@ -37,6 +37,8 @@
 #include<fstream>
 
 
+
+
 using namespace std;
 
 namespace ORB_SLAM
@@ -312,6 +314,19 @@ void Tracking::GrabImage(const sensor_msgs::ImageConstPtr& msg)
         tf::Transform tfTcw(M,V);
 
         mTfBr.sendTransform(tf::StampedTransform(tfTcw,ros::Time::now(), "ORB_SLAM/World", "ORB_SLAM/Camera"));
+
+	geometry_msgs::PoseStamped poseMSG;
+	poseMSG.pose.position.x = tfTcw.getOrigin().x();
+	poseMSG.pose.position.y = tfTcw.getOrigin().y();
+	poseMSG.pose.position.z = tfTcw.getOrigin().z();
+	poseMSG.pose.orientation.x = tfTcw.getRotation().x();
+	poseMSG.pose.orientation.y = tfTcw.getRotation().y();
+	poseMSG.pose.orientation.z = tfTcw.getRotation().z();
+	poseMSG.pose.orientation.w = tfTcw.getRotation().w();
+	poseMSG.header.frame_id = "VSLAM";
+	poseMSG.header.stamp = ros::Time::now();
+	PosPub.publish(poseMSG);
+	
     }
 
 }