included publish pose (which was done by tf to pose)

include/htc_vive_tracker_alg_node.h

@@ -30,9 +30,11 @@
 #include "iri_htc_vive_tracker/GetButtonPressed.h"
 #include "iri_htc_vive_tracker/TriggerHapticPulse.h"
 
+#include "tf/transform_listener.h"
+
+
 // [publisher subscriber headers]
 #include <geometry_msgs/TransformStamped.h>
-#include <tf/transform_broadcaster.h>
 // [service client headers]
 
 // [action server client headers]
@@ -45,15 +47,14 @@ class HtcViveTrackerAlgNode : public algorithm_base::IriBaseAlgorithm<HtcViveTra
 {
   private:
     // [publisher attributes]
-    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";
-
     geometry_msgs::TransformStamped transform_stamped_;
     //Transformation from base to world. In this case, WAM to CHAPERONE
  
     // [subscriber attributes]
 	
+    ros::Publisher pose_publisher_;
+    tf::TransformListener tf_listener_;
+        //
     // [service attributes]
 
     // [client attributes]
@@ -72,7 +73,11 @@ class HtcViveTrackerAlgNode : public algorithm_base::IriBaseAlgorithm<HtcViveTra
 
     uint32_t haptic_pulse_strength_;
     bool publish_hmd_;
- 
+    bool frame_names_set;
+
+    std::string target_frame_name_;
+    std::string source_frame_name_;
+
     ros::ServiceServer trigger_pulse_server_;
     bool trigger_pulse_serverCallback(iri_htc_vive_tracker::TriggerHapticPulse::Request &req, iri_htc_vive_tracker::TriggerHapticPulse::Response &res);
 

launch/publish_wam_chaperone_link.launch

 <launch>
+  <arg name="target_frame" default="tracker_1" />
+
        <!-- HTC Vive Transformation -->
        <!--static_transform_publisher x y z qx qy qz qw frame_id child_frame_id  period_in_ms -->
         <node pkg="tf"
@@ -18,5 +20,7 @@
 	<node pkg="iri_htc_vive_tracker"
 		type = "iri_htc_vive_tracker"
 		name = "iri_htc_vive_tracker" >
+         <param name="target_frame" type="string" value="$(arg target_frame)"/>
+
 	</node>
 </launch>

launch/wam_follow_tracker.launch

 <launch>
        <!-- Initialize htc vive and publish transformation -->
-	<include file="$(find iri_htc_vive_tracker)/launch/publish_wam_chaperone_link.launch"/>
+	<include file="$(find iri_htc_vive_tracker)/launch/publish_wam_chaperone_link.launch">
+	    <!-- Initialize tf-to-pose node from chaperone to tracker1-->
+    	<arg name="target_frame" value ="tracker_1"/>
 
-	<!-- Initialize tf-to-pose node from chaperone to tracker1-->
-	<!-- Also remaps topic to /iri_wam/pose_surface-->
-	<include file="$(find iri_tf_to_pose)/launch/htcvive.launch">
-	<arg name="source_frame" value ="chaperone"/>
-	<arg name="target_frame" value ="tracker_1"/>
 	</include>
+        <remap from="iri_htc_vive_tracker/new_pose" to="iri_wam/pose_surface" />
 </launch>

src/htc_vive_tracker_alg_node.cpp

@@ -22,6 +22,20 @@ HtcViveTrackerAlgNode::HtcViveTrackerAlgNode(void) :
    	ROS_ERROR("Problem with initialization. Check other error messages");
    }
 
+    this->frame_names_set = true;
+    if (this->public_node_handle_.hasParam("target_frame_name")){
+        this->public_node_handle_.getParam("target_frame_name",this->target_frame_name_);
+    }
+    else {
+        ROS_INFO("No param set for target_frame");
+        this->frame_names_set = false;
+    }
+    
+    this->source_frame_name_ = "chaperone";
+
+    pose_publisher_ = this->public_node_handle_.advertise<geometry_msgs::PoseStamped>("new_pose",100);
+
+
   // [init publishers]
   
   
@@ -68,6 +82,25 @@ void HtcViveTrackerAlgNode::mainNodeThread(void)
 		}
 	}
   }
+
+  if (this->frame_names_set){
+    tf::StampedTransform stamped_transform;
+    try{
+        bool is_transform_possible = this->tf_listener_.canTransform(this->source_frame_name_, this->target_frame_name_, ros::Time(0));
+        if (is_transform_possible) {
+            this->tf_listener_.lookupTransform(this->source_frame_name_, this->target_frame_name_, ros::Time(0), stamped_transform);
+            geometry_msgs::PoseStamped tf_pose = this->alg_.PoseFromTF(stamped_transform);
+            this->pose_publisher_.publish(tf_pose);
+        }
+        else {
+            ROS_INFO ("Transform not possible");
+        }
+    }
+    catch (tf::TransformException &ex) {
+          ROS_ERROR("%s",ex.what());
+          ros::Duration(1.0).sleep();
+    }
+   }
   
   // [fill msg structures]
   
@@ -114,7 +147,7 @@ void HtcViveTrackerAlgNode::BroadcastPoseRotated(const std::string & device_name
     if (this->alg_.GetDevicePositionQuaternion(device_name,pose,quaternion)) {
 
 	this->transform_stamped_.header.stamp = ros::Time::now();
-	this->transform_stamped_.header.frame_id = WORLD_NAME;
+	this->transform_stamped_.header.frame_id = this->alg_.WORLD_NAME;
 	transform_stamped_.child_frame_id = device_name;
 	transform_stamped_.transform.translation.x = pose[0];
 	transform_stamped_.transform.translation.y = pose[1];
@@ -171,7 +204,7 @@ tf::Quaternion HtcViveTrackerAlgNode::ApplyRotationForIRIStandardCoordinates(con
 bool HtcViveTrackerAlgNode::trigger_pulse_serverCallback(iri_htc_vive_tracker::TriggerHapticPulse::Request &req, iri_htc_vive_tracker::TriggerHapticPulse::Response &res) {
 	res.success = this->alg_.TriggerHapticPulse(req.device_name, this->haptic_pulse_strength_);
 	if (!res.success) {
-		res.message = DEVICE_NOT_FOUND_MSG;
+		res.message = this->alg_.DEVICE_NOT_FOUND_MSG;
 	}
 
 	return true;
@@ -182,7 +215,7 @@ bool HtcViveTrackerAlgNode::get_button_serverCallback(iri_htc_vive_tracker::GetB
 	vr::EVRButtonId button_pressed =  this->alg_.GetPressedButton(req.device_name);
 	res.button_pressed = (int) button_pressed;
 	res.success = this->alg_.IsDeviceDetected (req.device_name);
-	if (!res.success) res.message = DEVICE_NOT_FOUND_MSG;
+	if (!res.success) res.message = this->alg_.DEVICE_NOT_FOUND_MSG;
 	return true;
 }
 /* main function */