From e11d64be1974cfa3203e6a02b49c58049f6f6dc4 Mon Sep 17 00:00:00 2001
From: pmlab <lfreixas@iri.upc.edu>
Date: Mon, 8 Oct 2018 15:24:44 +0200
Subject: [PATCH] added PoseFromTF function
---
include/htc_vive_tracker_alg.h | 10 ++++++++++
src/htc_vive_tracker_alg.cpp | 14 ++++++++++++++
2 files changed, 24 insertions(+)
diff --git a/include/htc_vive_tracker_alg.h b/include/htc_vive_tracker_alg.h
index 151625a..5a922b7 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 f5a7f4d..65aa8fc 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;
+
+}
--
GitLab