initial commit

CMakeLists.txt

+cmake_minimum_required(VERSION 2.8.3)
+project(iri_htc_vive_tracker)
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED)
+# ******************************************************************** 
+#                 Add catkin additional components here
+# ******************************************************************** 
+find_package(catkin REQUIRED COMPONENTS roscpp rospy iri_base_algorithm std_msgs geometry_msgs message_generation 
+tf2 tf2_ros
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+# ******************************************************************** 
+#           Add system and labrobotica dependencies here
+# ******************************************************************** 
+# find_package(<dependency> REQUIRED)
+find_package(htc_vive_tracker REQUIRED)
+# ******************************************************************** 
+#           Add topic, service and action definition here
+# ******************************************************************** 
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+add_service_files(
+   FILES
+   BaseStationSrv.srv
+#   Service1.srv
+#   Service2.srv
+)
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+   DEPENDENCIES
+   std_msgs  # Or other packages containing msgs
+   geometry_msgs
+)
+
+# ******************************************************************** 
+#                 Add the dynamic reconfigure file 
+# ******************************************************************** 
+generate_dynamic_reconfigure_options(cfg/HtcViveTracker.cfg)
+
+# ******************************************************************** 
+#                 Add run time dependencies here
+# ******************************************************************** 
+catkin_package(
+#  INCLUDE_DIRS 
+#  LIBRARIES 
+# ******************************************************************** 
+#            Add ROS and IRI ROS run time dependencies
+# ******************************************************************** 
+ CATKIN_DEPENDS iri_base_algorithm #geometry_msgs std_msgs message_generation
+# ******************************************************************** 
+#      Add system and labrobotica run time dependencies here
+# ******************************************************************** 
+  DEPENDS htc_vive_tracker
+)
+
+###########
+## Build ##
+###########
+
+# ******************************************************************** 
+#                   Add the include directories 
+# ******************************************************************** 
+include_directories(include)
+include_directories(${catkin_INCLUDE_DIRS})
+include_directories(${htc_vive_tracker_INCLUDE_DIR})
+# include_directories(${<dependency>_INCLUDE_DIR})
+
+## Declare a cpp library
+# add_library(${PROJECT_NAME} <list of source files>)
+
+## Declare a cpp executable
+add_executable(${PROJECT_NAME} src/htc_vive_tracker_alg.cpp src/htc_vive_tracker_alg_node.cpp)
+
+# ******************************************************************** 
+#                   Add the libraries
+# ******************************************************************** 
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} ${htc_vive_tracker_LIBRARY})
+# target_link_libraries(${PROJECT_NAME} ${<dependency>_LIBRARY})
+
+# ******************************************************************** 
+#               Add message headers dependencies 
+# ******************************************************************** 
+# add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
+# ******************************************************************** 
+#               Add dynamic reconfigure dependencies 
+# ******************************************************************** 
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS})

cfg/HtcViveTracker.cfg

+#! /usr/bin/env python
+#*  All rights reserved.
+#*
+#*  Redistribution and use in source and binary forms, with or without
+#*  modification, are permitted provided that the following conditions
+#*  are met:
+#*
+#*   * Redistributions of source code must retain the above copyright
+#*     notice, this list of conditions and the following disclaimer.
+#*   * Redistributions in binary form must reproduce the above
+#*     copyright notice, this list of conditions and the following
+#*     disclaimer in the documentation and/or other materials provided
+#*     with the distribution.
+#*   * Neither the name of the Willow Garage nor the names of its
+#*     contributors may be used to endorse or promote products derived
+#*     from this software without specific prior written permission.
+#*
+#*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+#*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+#*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+#*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+#*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+#*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+#*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+#*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+#*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+#*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+#*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+#*  POSSIBILITY OF SUCH DAMAGE.
+#***********************************************************
+
+# Author: 
+
+PACKAGE='iri_htc_vive_tracker'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+#       Name                       Type       Reconfiguration level            Description                       Default   Min   Max
+#gen.add("velocity_scale_factor",  double_t,  0,                               "Maximum velocity scale factor",  0.5,      0.0,  1.0)
+gen.add("device_name",  str_t,  0,                               "Name of device to get pose",  "tracker_1")
+gen.add("print_devices",  bool_t,  0,                               "Print list of all detected devices",  False)
+#gen.add("apply_rotation",  bool_t,  0,                               "Apply rotation to quaternion",  False)
+#gen.add("x_axis",  double_t,  0,                               "x axis for rotation",  0)
+#gen.add("y_axis",  double_t,  0,                               "y axis for rotation",  0)
+#gen.add("z_axis",  double_t,  0,                               "z axis for rotation",  0)
+#gen.add("angle_degrees",  double_t,  0,                               "angle in degrees",  0)
+
+exit(gen.generate(PACKAGE, "HtcViveTrackerAlgorithm", "HtcViveTracker"))

include/htc_vive_tracker_alg.h

+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
+// Author 
+// All rights reserved.
+//
+// This file is part of iri-ros-pkg
+// iri-ros-pkg is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+// 
+// IMPORTANT NOTE: This code has been generated through a script from the 
+// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
+// of the scripts. ROS topics can be easly add by using those scripts. Please
+// refer to the IRI wiki page for more information:
+// http://wikiri.upc.es/index.php/Robotics_Lab
+
+#ifndef _htc_vive_tracker_alg_h_
+#define _htc_vive_tracker_alg_h_
+
+#include <iri_htc_vive_tracker/HtcViveTrackerConfig.h>
+
+//include htc_vive_tracker_alg main library
+#include "htc_vive_tracker.h"
+/**
+ * \brief IRI ROS Specific Driver Class
+ *
+ *
+ */
+class HtcViveTrackerAlgorithm
+{
+  protected:
+   /**
+    * \brief define config type
+    *
+    * Define a Config type with the HtcViveTrackerConfig. All driver implementations
+    * will then use the same variable type Config.
+    */
+    pthread_mutex_t access_;    
+
+    // private attributes and methods
+
+    CHtc_Vive_Tracker htc_vive_;
+  public:
+   /**
+    * \brief define config type
+    *
+    * Define a Config type with the HtcViveTrackerConfig. All driver implementations
+    * will then use the same variable type Config.
+    */
+    typedef iri_htc_vive_tracker::HtcViveTrackerConfig Config;
+
+   /**
+    * \brief config variable
+    *
+    * This variable has all the driver parameters defined in the cfg config file.
+    * Is updated everytime function config_update() is called.
+    */
+    Config config_;
+
+   /**
+    * \brief constructor
+    *
+    * In this constructor parameters related to the specific driver can be
+    * initalized. Those parameters can be also set in the openDriver() function.
+    * Attributes from the main node driver class IriBaseDriver such as loop_rate,
+    * may be also overload here.
+    */
+    HtcViveTrackerAlgorithm(void);
+
+   /**
+    * \brief Lock Algorithm
+    *
+    * Locks access to the Algorithm class
+    */
+    void lock(void) { pthread_mutex_lock(&this->access_); };
+
+   /**
+    * \brief Unlock Algorithm
+    *
+    * Unlocks access to the Algorithm class
+    */
+    void unlock(void) { pthread_mutex_unlock(&this->access_); };
+
+   /**
+    * \brief Tries Access to Algorithm
+    *
+    * Tries access to Algorithm
+    * 
+    * \return true if the lock was adquired, false otherwise
+    */
+    bool try_enter(void) 
+    { 
+      if(pthread_mutex_trylock(&this->access_)==0)
+        return true;
+      else
+        return false;
+    };
+
+   /**
+    * \brief config update
+    *
+    * In this function the driver parameters must be updated with the input
+    * config variable. Then the new configuration state will be stored in the 
+    * Config attribute.
+    *
+    * \param new_cfg the new driver configuration state
+    *
+    * \param level level in which the update is taken place
+    */
+    void config_update(Config& config, uint32_t level=0);
+
+    // here define all htc_vive_tracker_alg interface methods to retrieve and set
+ 
+    bool InitVR (bool verbose);
+    bool GetDevicePositionQuaternion (const std::string & device_name, double (&pose)[3], double (&quaternion)[4]);
+    bool GetDevicePositionEuler (const std::string & device_name, double (&pose)[3], double &roll, double & pitch, double & yaw);
+    bool GetDeviceVelocity (const std::string & device_name, double (&linear_v)[3], double (&angular_v)[3]);
+
+    bool GetDeviceNames(std::vector<std::string>&device_names);
+
+    bool GetChaperoneDimensions(std::vector<std::vector<float> > & corners, float & sizeX, float & sizeY);
+    // the driver parameters
+
+   /**
+    * \brief Destructor
+    *
+    * This destructor is called when the object is about to be destroyed.
+    *
+    */
+    ~HtcViveTrackerAlgorithm(void);
+
+    
+    // Calls the initialization functions for HTC vive. 
+
+	
+};
+
+#endif

include/htc_vive_tracker_alg_node.h

+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
+// Author 
+// All rights reserved.
+//
+// This file is part of iri-ros-pkg
+// iri-ros-pkg is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+// 
+// IMPORTANT NOTE: This code has been generated through a script from the 
+// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
+// of the scripts. ROS topics can be easly add by using those scripts. Please
+// refer to the IRI wiki page for more information:
+// http://wikiri.upc.es/index.php/Robotics_Lab
+
+#ifndef _htc_vive_tracker_alg_node_h_
+#define _htc_vive_tracker_alg_node_h_
+
+#include <iri_base_algorithm/iri_base_algorithm.h>
+#include "htc_vive_tracker_alg.h"
+
+// [publisher subscriber headers]
+#include <tf2_ros/transform_broadcaster.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <tf2/LinearMath/Quaternion.h>
+// [service client headers]
+
+// [action server client headers]
+
+/**
+ * \brief IRI ROS Specific Algorithm Class
+ *
+ */
+class HtcViveTrackerAlgNode : public algorithm_base::IriBaseAlgorithm<HtcViveTrackerAlgorithm>
+{
+  private:
+    // [publisher attributes]
+    //tf2_ros::TransformBroadcaster tf_broadcaster_;
+    geometry_msgs::TransformStamped transform_stamped_;
+    float ax_, ay_,az_,angle_rad_;
+    bool apply_rotation_;
+    // [subscriber attributes]
+	
+    // [service attributes]
+
+    // [client attributes]
+
+    // [action server attributes]
+
+    // [action client attributes]
+
+   /**
+    * \brief config variable
+    *
+    * This variable has all the driver parameters defined in the cfg config file.
+    * Is updated everytime function config_update() is called.
+    */
+    Config config_;
+    std::string device_name_;
+  public:
+   /**
+    * \brief Constructor
+    * 
+    * This constructor initializes specific class attributes and all ROS
+    * communications variables to enable message exchange.
+    */
+    HtcViveTrackerAlgNode(void);
+
+   /**
+    * \brief Destructor
+    * 
+    * This destructor frees all necessary dynamic memory allocated within this
+    * this class.
+    */
+    ~HtcViveTrackerAlgNode(void);
+
+    bool CheckHMDValuesValid ();
+  protected:
+   /**
+    * \brief main node thread
+    *
+    * This is the main thread node function. Code written here will be executed
+    * in every node loop while the algorithm is on running state. Loop frequency 
+    * can be tuned by modifying loop_rate attribute.
+    *
+    * Here data related to the process loop or to ROS topics (mainly data structs
+    * related to the MSG and SRV files) must be updated. ROS publisher objects 
+    * must publish their data in this process. ROS client servers may also
+    * request data to the corresponding server topics.
+    */
+    void mainNodeThread(void);
+
+   /**
+    * \brief dynamic reconfigure server callback
+    * 
+    * This method is called whenever a new configuration is received through
+    * the dynamic reconfigure. The derivated generic algorithm class must 
+    * implement it.
+    *
+    * \param config an object with new configuration from all algorithm 
+    *               parameters defined in the config file.
+    * \param level  integer referring the level in which the configuration
+    *               has been changed.
+    */
+    void node_config_update(Config &config, uint32_t level);
+
+   /**
+    * \brief node add diagnostics
+    *
+    * In this abstract function additional ROS diagnostics applied to the 
+    * specific algorithms may be added.
+    */
+    void addNodeDiagnostics(void);
+
+    // [diagnostic functions]
+    
+    // [test functions]
+    void BroadcastPoseRotated(const std::string & device_name);
+    void PrintQuaternionPose (const std::string & device_name);
+    void PrintAllDeviceNames();
+    void ApplyRotation(tf2::Quaternion & q, float x, float y, float z, float angle);
+    tf2::Quaternion ApplyRotationForIRIStandardCoordinates(const tf2::Quaternion & orig);
+};
+
+#endif

package.xml

+<?xml version="1.0"?>
+<package format="2">
+  <name>iri_htc_vive_tracker</name>
+  <version>0.0.0</version>
+  <description>The iri_htc_vive_tracker package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="pmlab@todo.todo">pmlab</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>LGPL</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/iri_htc_vive_tracker</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>iri_base_algorithm</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>iri_base_algorithm</build_export_depend>
+  <exec_depend>iri_base_algorithm</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/htc_vive_tracker_alg.cpp

+#include "htc_vive_tracker_alg.h"
+
+HtcViveTrackerAlgorithm::HtcViveTrackerAlgorithm(void)
+{
+  pthread_mutex_init(&this->access_,NULL);
+}
+
+HtcViveTrackerAlgorithm::~HtcViveTrackerAlgorithm(void)
+{
+  pthread_mutex_destroy(&this->access_);
+}
+
+void HtcViveTrackerAlgorithm::config_update(Config& config, uint32_t level)
+{
+  this->lock();
+
+  // save the current configuration
+  this->config_=config;
+  
+  this->unlock();
+}
+
+
+// HtcViveTrackerAlgorithm Public API
+bool HtcViveTrackerAlgorithm::InitVR(bool verbose){
+	return this->htc_vive_.InitializeVR(verbose);
+}
+bool HtcViveTrackerAlgorithm::GetDevicePositionQuaternion (const std::string & device_name, double (&pose)[3], double (&quaternion)[4]){
+  if (this->htc_vive_.IsDeviceDetected(device_name)){
+	return this->htc_vive_.GetDevicePoseQuaternion (device_name, pose, quaternion);
+  }
+  else return false;
+}
+
+bool HtcViveTrackerAlgorithm::GetDevicePositionEuler (const std::string & device_name, double (&pose)[3], double & roll, double & pitch, double & yaw) {
+  if (this->htc_vive_.IsDeviceDetected(device_name)){
+	return this->htc_vive_.GetDevicePoseEuler (device_name, pose, roll, pitch, yaw);
+  }
+  else return false;
+
+}
+bool HtcViveTrackerAlgorithm::GetDeviceVelocity (const std::string & device_name, double (&linear_v)[3], double (&angular_v)[3]){
+  if (this->htc_vive_.IsDeviceDetected(device_name)){
+	return this->htc_vive_.GetDeviceVelocity (device_name, linear_v,angular_v);
+  }
+  else return false;
+}
+bool HtcViveTrackerAlgorithm::GetDeviceNames(std::vector<std::string>&device_names){
+
+	device_names = this->htc_vive_.GetAllDeviceNames();
+	return true;
+}
+
+bool HtcViveTrackerAlgorithm::GetChaperoneDimensions(std::vector<std::vector<float> > & corners, float & sizeX, float & sizeY){
+	
+	return this->htc_vive_.GetChaperoneDimensions(corners,sizeX,sizeY);
+}

src/htc_vive_tracker_alg_node.cpp

+#include "htc_vive_tracker_alg_node.h"
+
+HtcViveTrackerAlgNode::HtcViveTrackerAlgNode(void) :
+  algorithm_base::IriBaseAlgorithm<HtcViveTrackerAlgorithm>()
+{
+  //init class attributes if necessary
+  this->loop_rate_ = 5;//in [Hz]
+  if (!this->alg_.InitVR(true)){
+  	ROS_ERROR("Problem with initialization. Check other error messages");
+  }
+  // [init publishers]
+  
+  // [init subscribers]
+  
+  // [init services]
+  
+  // [init clients]
+  
+  // [init action servers]
+  
+  // [init action clients]
+}
+
+HtcViveTrackerAlgNode::~HtcViveTrackerAlgNode(void)
+{
+  // [free dynamic memory]
+}
+bool AreArraysEqual (const double * array1, const double * array2){
+	//if (array1.size() != array2.size()) return false;
+	int i = 0;
+	while (array1[i]!=NULL){
+		if (array1[i] != array2[i]) return false;
+		i++;
+	}
+	return true;
+
+}
+bool HtcViveTrackerAlgNode::CheckHMDValuesValid (){
+    double position[3];
+    double quat [4];
+    double position2[3];
+    double quat2 [4];
+    if (this->alg_.GetDevicePositionQuaternion ("hmd_1", position,quat)){
+ 	if (position[0]==0 && position[1] == 0 && position[2] == 0){
+		if (quat[0]==0 && quat[1] == 0 && quat[2] == 0 && quat[3] == 1){
+			return false;
+		}
+	}
+	else {
+		if (this->alg_.GetDevicePositionQuaternion ("tracking_reference_2", position2, quat2)){
+			if (AreArraysEqual (position, position2)){
+				return false;
+			}
+		}
+		
+	}
+    }
+} 
+void HtcViveTrackerAlgNode::PrintQuaternionPose(const std::string & device){
+    double pose[3];
+    double q[4];
+    if (this->alg_.GetDevicePositionQuaternion(device,pose,q)){
+	for (int i = 0; i<3; ++i) std::cout<<pose[i]<<" ";
+	std::cout<<std::endl;
+	for (int i = 0; i<4; ++i) std::cout<<q[i]<<" ";
+	std::cout<<std::endl;
+    } 
+}
+void HtcViveTrackerAlgNode::mainNodeThread(void)
+{
+
+ //PrintQuaternionPose("tracker_1");
+ // BroadcastPose(this->device_name_);
+  std::vector<std::string> names;
+  if (this->alg_.GetDeviceNames(names)){
+	for (int i = 0; i<names.size(); ++i){
+		if (names[i]=="hmd_1" && !this->CheckHMDValuesValid()){
+			continue;
+		}
+		else{
+  			ROS_INFO("Broadcasting pose of %s",names[i].c_str());
+			BroadcastPoseRotated (names[i]);
+		}
+	}
+  }
+  // [fill msg structures]
+  
+  // [fill srv structure and make request to the server]
+
+  // [fill action structure and make request to the action server]
+
+  // [publish messages]
+}
+
+/*  [subscriber callbacks] */
+
+/*  [service callbacks] */
+
+/*  [action callbacks] */
+
+/*  [action requests] */
+
+void HtcViveTrackerAlgNode::node_config_update(Config &config, uint32_t level)
+{
+  this->alg_.lock();
+  this->device_name_ = config.device_name;
+  ROS_INFO("device name updated to %s",this->device_name_.c_str());
+  if (config.print_devices){
+	this->PrintAllDeviceNames();
+  }/*
+  if (config.apply_rotation){
+	this->apply_rotation_ = true;
+	this->ax_ = config.x_axis;
+	this->ay_ = config.y_axis;
+	this->az_ = config.z_axis;
+	this->angle_rad_ = config.angle_degrees*M_PI/180;
+
+  } else {
+	this->apply_rotation_=false;
+   }*/
+  this->config_=config;
+  this->alg_.unlock();
+}
+
+void HtcViveTrackerAlgNode::addNodeDiagnostics(void)
+{
+}
+
+void HtcViveTrackerAlgNode::BroadcastPoseRotated(const std::string & device_name){
+
+    static tf2_ros::TransformBroadcaster tf_broadcaster;
+    double pose[3];
+    double quaternion[4];
+    double roll,pitch,yaw;
+	
+    if (this->alg_.GetDevicePositionQuaternion(device_name,pose,quaternion)){
+    //if (this->alg_.GetDevicePositionEuler(device_name,pose,roll,pitch,yaw)){
+	this->transform_stamped_.header.stamp = ros::Time::now();
+	this->transform_stamped_.header.frame_id = "chaperone";
+	transform_stamped_.child_frame_id = device_name;
+	transform_stamped_.transform.translation.x = pose[0];
+	transform_stamped_.transform.translation.y = pose[1];
+	transform_stamped_.transform.translation.z = pose[2];
+
+    	tf2::Quaternion q = tf2::Quaternion(quaternion[0], quaternion[1], quaternion[2], quaternion[3]);
+	
+	q = this -> ApplyRotationForIRIStandardCoordinates(q);
+
+	transform_stamped_.transform.rotation.x = q.x();
+	transform_stamped_.transform.rotation.y = q.y();
+	transform_stamped_.transform.rotation.z = q.z();
+	transform_stamped_.transform.rotation.w = q.w();
+
+
+	tf_broadcaster.sendTransform(transform_stamped_);
+    }
+
+}
+void HtcViveTrackerAlgNode::PrintAllDeviceNames(){
+	std::vector<std::string> names;
+	if (this->alg_.GetDeviceNames(names)){
+		std::cout<<"Devices : "<<std::endl;
+		for (int i=0; i<names.size(); ++i){
+			if (names[i]!="") std::cout<<"  * "<<names[i]<<std::endl;
+		}
+	}
+}
+tf2::Quaternion HtcViveTrackerAlgNode::ApplyRotationForIRIStandardCoordinates(const tf2::Quaternion & orig){
+	tf2::Quaternion q_result;
+
+	//Define rotations by axis + angle
+	tf2::Vector3 x_axis(1.0, 0.0, 0.0);	
+	tf2::Vector3 z_axis(0.0, 0.0, 1.0);	
+	tf2::Quaternion rotation_x90 = tf2::Quaternion(x_axis, M_PI/2);
+	tf2::Quaternion rotation_x180 = tf2::Quaternion(x_axis, M_PI);
+	tf2::Quaternion rotation_z90 = tf2::Quaternion(z_axis, M_PI/2);
+
+	//Apply two pre-rotations, in order to match axis in device
+	//Apply two post-rotations, in order to match axis to coordinate system.
+	q_result = rotation_z90*rotation_x90*orig*rotation_x180*rotation_z90;
+	
+	return q_result;
+	
+}
+void HtcViveTrackerAlgNode::ApplyRotation(tf2::Quaternion & orig, float ax, float ay, float az, float angle_radians){
+	tf2::Vector3 axis(ax,ay,az);
+	tf2::Quaternion rotation = tf2::Quaternion(axis, angle_radians);
+	
+	tf2::Quaternion finalQ = rotation*orig;
+	
+	this->transform_stamped_.transform.rotation.x = finalQ.x();
+	this->transform_stamped_.transform.rotation.y = finalQ.y();
+	this->transform_stamped_.transform.rotation.z = finalQ.z();
+	this->transform_stamped_.transform.rotation.w = finalQ.w();
+	
+	
+}
+/* main function */
+int main(int argc,char *argv[])
+{
+  return algorithm_base::main<HtcViveTrackerAlgNode>(argc, argv, "htc_vive_tracker_alg_node");
+}