Merge branch 'switch_to_tf_from_tf2' into 'master'

finished switching from tf2 to tf

See merge request lfreixas/iri_htc_vive_tracker!2

CMakeLists.txt

@@ -9,7 +9,7 @@ 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
+tf
 )
 
 ## System dependencies are found with CMake's conventions
@@ -65,7 +65,7 @@ catkin_package(
 # ******************************************************************** 
 #            Add ROS and IRI ROS run time dependencies
 # ******************************************************************** 
- CATKIN_DEPENDS iri_base_algorithm #geometry_msgs std_msgs message_generation
+ CATKIN_DEPENDS iri_base_algorithm 
 # ******************************************************************** 
 #      Add system and labrobotica run time dependencies here
 # ******************************************************************** 
@@ -88,7 +88,7 @@ include_directories(${htc_vive_tracker_INCLUDE_DIR})
 # 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 src/file_reader.cpp)
+add_executable(${PROJECT_NAME} src/htc_vive_tracker_alg.cpp src/htc_vive_tracker_alg_node.cpp src/file_reader.cpp src/hand_eye_calibration_helper.cpp)
 
 # ******************************************************************** 
 #                   Add the libraries

cfg/calibration.cpp

@@ -28,10 +28,8 @@ bool IsNumber (char c){
 	return false;
 }
 void AddValueToField (std::string & num, std::string & field, CalibrationInfo c){
-	std::cout<<"adding "<<num<<" to "<< field<<std::endl;
 	if (field == "x"){
 		c.t.x = std::stod(num);
-		std::cout<<c.t.x<<std::endl;
 	} else if (field == "y"){
 		c.t.y = std::stod(num);
 	} else if (field == "z"){
@@ -65,9 +63,7 @@ CalibrationInfo ReadCalibrationFromJSONFile (const char * file_path){
 			current_field = current_field + c;
 	
 		}
-	//	std::cout<<c<<std::endl;
 	}
-	std::cout<<calibration_info.t.x<<std::endl;
 }
 
 int main (){

include/file_reader.h

 #include <sstream> 
+#include <tf/transform_broadcaster.h>
 #include <string>
 #include <iostream>
 #include <fstream>
-#include <tf2/LinearMath/Transform.h>
 #include <geometry_msgs/TransformStamped.h>
-#include <tf2/LinearMath/Quaternion.h>
 class FileReader{
 	
 	public:
 	double ValueFromString (const std::string & value_str);
-	tf2::Transform GetTransformFromVector (const std::vector<double> & values);
-	tf2::Transform ReadTransformFromJSON (const std::string & json_file_path);
-	tf2::Transform ReadFirstTransformFromCSV (const std::string & csv_file_path);
-	tf2::Transform GetBaseWorldFromTransforms ( 	tf2::Transform hand_eye , tf2::Transform base_hand, tf2::Transform world_eye);
-	tf2::Transform GetBaseFromFilePaths (const std::string & hand_eye_json_path, const std::string &  base_hand_csv, const std::string & world_eye_csv);
-
+	tf::Transform GetTransformFromVector (const std::vector<double> & values);
+	tf::Transform ReadTransformFromJSON (const std::string & json_file_path);
+	tf::Transform ReadFirstTransformFromCSV (const std::string & csv_file_path);
 };

include/hand_eye_calibration_helper.h

+
+#include "file_reader.h"
+class HandEyeHelper{
+    public:
+	tf::Transform GetBaseWorldFromTransforms ( 	tf::Transform hand_eye , tf::Transform base_hand, tf::Transform world_eye);
+	tf::Transform GetBaseFromFilePaths (const std::string & hand_eye_json_path, const std::string &  base_hand_csv, const std::string & world_eye_csv);
+
+
+
+};

include/htc_vive_tracker_alg_node.h

@@ -29,10 +29,9 @@
 #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>
-#include "file_reader.h"
+#include <tf/transform_broadcaster.h>
+#include "hand_eye_calibration_helper.h"
 // [service client headers]
 
 // [action server client headers]
@@ -45,16 +44,17 @@ class HtcViveTrackerAlgNode : public algorithm_base::IriBaseAlgorithm<HtcViveTra
 {
   private:
     // [publisher attributes]
-    //tf2_ros::TransformBroadcaster tf_broadcaster_;
     const std::string BASE_NAME = "iri_wam_link_base";
     const std::string WORLD_NAME = "chaperone";
-    FileReader file_reader_;
     geometry_msgs::TransformStamped transform_stamped_;
-    geometry_msgs::TransformStamped transform_wam_chaperone_;
+    tf::StampedTransform transform_wam_chaperone_;
     float ax_, ay_,az_,angle_rad_;
     bool apply_rotation_;
     double wam_to_chaperone_x_, wam_to_chaperone_y_, wam_to_chaperone_z_;
     double wam_to_chaperone_i_, wam_to_chaperone_j_, wam_to_chaperone_k_, wam_to_chaperone_w_;
+ 
+
+    HandEyeHelper hand_eye_helper_;
     // [subscriber attributes]
 	
     // [service attributes]
@@ -135,9 +135,10 @@ class HtcViveTrackerAlgNode : public algorithm_base::IriBaseAlgorithm<HtcViveTra
     void BroadcastWAMToChaperoneTransformation ();
     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);
+    void ApplyRotation(tf::Quaternion & q, float x, float y, float z, float angle);
+    tf::Quaternion ApplyRotationForIRIStandardCoordinates(const tf::Quaternion & orig);
     void SetValuesWamToChaperone (const std::string & hand_eye_json_path, const std::string &  base_hand_csv, const std::string & world_eye_csv);
+
 };
 
 #endif

src/file_reader.cpp

@@ -2,9 +2,8 @@
 	
 	
 std::vector<std::string> string_values = {"x","y","z","i","j","k","w"};
-tf2::Transform FileReader::ReadTransformFromJSON (const std::string & json_file_path){
+tf::Transform FileReader::ReadTransformFromJSON (const std::string & json_file_path){
 	std::ifstream json_file(json_file_path);
-	//std::cout<"Reading JSON FILE : "<<std::endl;
 	std::string line;
 	std::string data_value;
 	std::vector<double>json_values(string_values.size(),0.0); // x,y,z, qx,qy,qz,qw
@@ -20,22 +19,21 @@ tf2::Transform FileReader::ReadTransformFromJSON (const std::string & json_file_
 	}
 
 	
-	tf2::Transform transform = GetTransformFromVector(json_values);
+	tf::Transform transform = GetTransformFromVector(json_values);
 	return transform;
 }
 
 
-tf2::Transform FileReader::ReadFirstTransformFromCSV (const std::string & csv_file_path){
+tf::Transform FileReader::ReadFirstTransformFromCSV (const std::string & csv_file_path){
 	std::ifstream csv_file(csv_file_path);
 	//only get the first line
 	std::vector<double> csv_values(8,0.0); //time, x,y,z, qx,qy,qz,qw
-	tf2::Transform transform;
+	tf::Transform transform;
 	std::string data_value;
 	for (int i = 0; i < 8 ; ++i){
 		if (csv_file.good()){
 			getline(csv_file,data_value,',');
 			csv_values[i] = std::stod(data_value);
-			std::cout<<csv_values[i]<<std::endl;
 		}
 	}
 	transform = (GetTransformFromVector (csv_values));
@@ -45,18 +43,17 @@ tf2::Transform FileReader::ReadFirstTransformFromCSV (const std::string & csv_fi
 }
 
 
-tf2::Transform FileReader::GetTransformFromVector(const std::vector<double>&values){
-	std::cout<<"size ' "<<values.size()<<std::endl;
-	tf2::Transform transform;
+tf::Transform FileReader::GetTransformFromVector(const std::vector<double>&values){
+	tf::Transform transform;
 	int first_value_position = 0;
 	if (values.size()==7 || values.size()==8){
 		if (values.size() == 8) first_value_position = 1;
 		
 		transform.setOrigin(
-				tf2::Vector3(values[first_value_position],
+				tf::Vector3(values[first_value_position],
 						values[first_value_position+1],
 						values[first_value_position+2]));
-		transform.setRotation(tf2::Quaternion(
+		transform.setRotation(tf::Quaternion(
 					values[first_value_position+3],
 					values[first_value_position+4],
 					values[first_value_position+5],
@@ -71,20 +68,5 @@ tf2::Transform FileReader::GetTransformFromVector(const std::vector<double>&valu
 double ValueFromString (const std::string & value_str){
 	return std::stod (value_str);
 
-}
-tf2::Transform FileReader::GetBaseWorldFromTransforms ( 	tf2::Transform hand_eye , tf2::Transform base_hand, tf2::Transform world_eye){
-	
-	tf2::Transform  t1 = base_hand*hand_eye;
-	t1 = t1*(world_eye.inverse());
-	return t1;
-
 }
 
-tf2::Transform FileReader::GetBaseFromFilePaths (const std::string & hand_eye_json_path, const std::string &  base_hand_csv, const std::string & world_eye_csv){
-	
-	tf2::Transform hand_eye_t = ReadTransformFromJSON (hand_eye_json_path);
-	tf2::Transform base_hand_t = ReadFirstTransformFromCSV (base_hand_csv);
-	tf2::Transform world_eye_t = ReadFirstTransformFromCSV (world_eye_csv);
-	
-	return GetBaseWorldFromTransforms (hand_eye_t, base_hand_t, world_eye_t);
-}

src/hand_eye_calibration_helper.cpp

+#include "hand_eye_calibration_helper.h"
+tf::Transform HandEyeHelper::GetBaseWorldFromTransforms ( 	tf::Transform hand_eye , tf::Transform base_hand, tf::Transform world_eye){
+
+	
+	tf::Transform  t1 = base_hand*hand_eye;
+	t1 = t1*(world_eye.inverse());
+	return t1;
+
+}
+
+tf::Transform HandEyeHelper::GetBaseFromFilePaths (const std::string & hand_eye_json_path, const std::string &  base_hand_csv, const std::string & world_eye_csv){
+	
+	FileReader fr;
+	tf::Transform hand_eye_t = fr.ReadTransformFromJSON (hand_eye_json_path);
+	tf::Transform base_hand_t = fr.ReadFirstTransformFromCSV (base_hand_csv);
+	tf::Transform world_eye_t = fr.ReadFirstTransformFromCSV (world_eye_csv);
+	
+	return GetBaseWorldFromTransforms (hand_eye_t, base_hand_t, world_eye_t);
+}

src/htc_vive_tracker_alg_node.cpp

@@ -142,7 +142,7 @@ void HtcViveTrackerAlgNode::addNodeDiagnostics(void)
 
 void HtcViveTrackerAlgNode::BroadcastPoseRotated(const std::string & device_name){
 
-    static tf2_ros::TransformBroadcaster tf_broadcaster;
+    static tf::TransformBroadcaster tf_broadcaster;
     double pose[3];
     double quaternion[4];
     double roll,pitch,yaw;
@@ -164,7 +164,7 @@ void HtcViveTrackerAlgNode::BroadcastPoseRotated(const std::string & device_name
 	
 	w = quaternion[3]
 	*/
-    	tf2::Quaternion q = tf2::Quaternion(quaternion[0], quaternion[1], quaternion[2], quaternion[3]);
+    	tf::Quaternion q = tf::Quaternion(quaternion[0], quaternion[1], quaternion[2], quaternion[3]);
 	
 	//Apply the necessary rotations so that the coordinate system is the one decided at IRI
 	q = this -> ApplyRotationForIRIStandardCoordinates(q);
@@ -188,15 +188,15 @@ void HtcViveTrackerAlgNode::PrintAllDeviceNames(){
 		}
 	}
 }
-tf2::Quaternion HtcViveTrackerAlgNode::ApplyRotationForIRIStandardCoordinates(const tf2::Quaternion & orig){
-	tf2::Quaternion q_result;
+tf::Quaternion HtcViveTrackerAlgNode::ApplyRotationForIRIStandardCoordinates(const tf::Quaternion & orig){
+	tf::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);
+	tf::Vector3 x_axis(1.0, 0.0, 0.0);	
+	tf::Vector3 z_axis(0.0, 0.0, 1.0);	
+	tf::Quaternion rotation_x90 = tf::Quaternion(x_axis, M_PI/2);
+	tf::Quaternion rotation_x180 = tf::Quaternion(x_axis, M_PI);
+	tf::Quaternion rotation_z90 = tf::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.
@@ -205,11 +205,11 @@ tf2::Quaternion HtcViveTrackerAlgNode::ApplyRotationForIRIStandardCoordinates(co
 	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);
+void HtcViveTrackerAlgNode::ApplyRotation(tf::Quaternion & orig, float ax, float ay, float az, float angle_radians){
+	tf::Vector3 axis(ax,ay,az);
+	tf::Quaternion rotation = tf::Quaternion(axis, angle_radians);
 	
-	tf2::Quaternion finalQ = rotation*orig;
+	tf::Quaternion finalQ = rotation*orig;
 	
 	this->transform_stamped_.transform.rotation.x = finalQ.x();
 	this->transform_stamped_.transform.rotation.y = finalQ.y();
@@ -220,23 +220,20 @@ void HtcViveTrackerAlgNode::ApplyRotation(tf2::Quaternion & orig, float ax, floa
 }
 void HtcViveTrackerAlgNode::BroadcastWAMToChaperoneTransformation (){
 	
-    	static tf2_ros::TransformBroadcaster tf_broadcaster;
+    	static tf::TransformBroadcaster tf_broadcaster;
 
 	tf_broadcaster.sendTransform(this->transform_wam_chaperone_);
  
 }
 void HtcViveTrackerAlgNode::SetValuesWamToChaperone (const std::string & hand_eye_json_path, const std::string &  base_hand_csv, const std::string & world_eye_csv){
+	tf::Transform base_transform = this->hand_eye_helper_.GetBaseFromFilePaths(hand_eye_json_path, base_hand_csv, world_eye_csv);
+	this->transform_wam_chaperone_ = tf::StampedTransform(
+		 base_transform,
+		 ros::Time::now(),
+		 BASE_NAME,
+		WORLD_NAME
+	);
 	
-	this->transform_wam_chaperone_.transform = Tf2ToGeometryMsgTranformConversion (file_reader_.GetBaseFromFilePaths(hand_eye_json_path, base_hand_csv, world_eye_csv));
-	this->transform_wam_chaperone_.header.frame_id = BASE_NAME;
-	this->transform_wam_chaperone_.child_frame_id = WORLD_NAME;
-
-}
-geometry_msgs::Transform Tf2ToGeometryMsgTransformConversion(tf2::Transform & transform_original){
-	geometry_msgs::Transform transform_final;
-	transform_final.translation = transform_original.getOrigin();
-	transform_final.rotation = transform_original.getRotation();
-
 }
 /* main function */
 int main(int argc,char *argv[])