diff --git a/include/htc_vive_tracker_alg.h b/include/htc_vive_tracker_alg.h
index 151625aed14142e0a6896c6bb9934c66593e23e5..5a922b7a0c5f4bd30eaa40e847d019d7441546fa 100644
--- a/include/htc_vive_tracker_alg.h
+++ b/include/htc_vive_tracker_alg.h
@@ -28,8 +28,11 @@
 #include <iri_htc_vive_tracker/HtcViveTrackerConfig.h>
 #include <math.h>
 
+#include "geometry_msgs/PoseStamped.h"
+#include <tf/transform_broadcaster.h>
 //include htc_vive_tracker_alg main library
 #include "htc_vive_tracker.h"
+
 /**
  * \brief IRI ROS Specific Driver Class
  *
@@ -46,6 +49,7 @@ class HtcViveTrackerAlgorithm
     */
     pthread_mutex_t access_;    
 
+
     // private attributes and methods
 
     CHtc_Vive_Tracker htc_vive_;
@@ -57,6 +61,10 @@ class HtcViveTrackerAlgorithm
     * will then use the same variable type Config.
     */
     typedef iri_htc_vive_tracker::HtcViveTrackerConfig Config;
+    const std::string BASE_NAME = "iri_wam_link_base";
+    const std::string WORLD_NAME = "chaperone";
+    const std::string DEVICE_NOT_FOUND_MSG = "Device not found";
+
 
    /**
     * \brief config variable
@@ -136,6 +144,8 @@ class HtcViveTrackerAlgorithm
 
     
     bool TriggerHapticPulse(const std::string & device_name, uint32_t strength);
+
+    geometry_msgs::PoseStamped PoseFromTF(const tf::StampedTransform & stamped_transform);
     // the driver parameters
 
    /**
diff --git a/src/htc_vive_tracker_alg.cpp b/src/htc_vive_tracker_alg.cpp
index f5a7f4d8d954e1edce58a1cd846ff0cd133152c6..65aa8fc1789db0ac6a0c67d1e6150a0b256983da 100644
--- a/src/htc_vive_tracker_alg.cpp
+++ b/src/htc_vive_tracker_alg.cpp
@@ -81,3 +81,17 @@ vr::EVRButtonId HtcViveTrackerAlgorithm::GetPressedButton(const std::string & de
 bool HtcViveTrackerAlgorithm::TriggerHapticPulse(const std::string & device_name, uint32_t strength){
 	return this->htc_vive_.HapticPulse(device_name,0,strength);
 }
+geometry_msgs::PoseStamped HtcViveTrackerAlgorithm::PoseFromTF(const tf::StampedTransform & stamped_transform){
+
+    geometry_msgs::PoseStamped pose_new;
+    pose_new.header.frame_id = this->WORLD_NAME;
+    pose_new.pose.position.x = stamped_transform.getOrigin().x();
+    pose_new.pose.position.y = stamped_transform.getOrigin().y();
+    pose_new.pose.position.z = stamped_transform.getOrigin().z();
+    pose_new.pose.orientation.x = 0; 
+    pose_new.pose.orientation.y = 1; 
+    pose_new.pose.orientation.z = 0; 
+    pose_new.pose.orientation.w = 0; 
+return pose_new;
+
+}