Merge branch 'adding_FileReader' into 'master'

Adding file reader

See merge request lfreixas/iri_htc_vive_tracker!1

CMakeLists.txt

@@ -31,12 +31,11 @@ find_package(htc_vive_tracker REQUIRED)
 # )
 
 ## Generate services in the 'srv' folder
-add_service_files(
-   FILES
-   BaseStationSrv.srv
+#add_service_files(
+#   FILES
 #   Service1.srv
 #   Service2.srv
-)
+#)
 
 ## Generate actions in the 'action' folder
 # add_action_files(
@@ -89,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)
+add_executable(${PROJECT_NAME} src/htc_vive_tracker_alg.cpp src/htc_vive_tracker_alg_node.cpp src/file_reader.cpp)
 
 # ******************************************************************** 
 #                   Add the libraries

cfg/calibration.xml

-<rosparam>
-delay: 0.240481138229
-i: 0.014848087221
-k: 0.157088698842
-j: 0.0105477702755
-w: 0.987416538014
-y: -0.00328448660645
-x: 0.00618500813615
-z: 0.0660937172089
-</rosparam>

include/file_reader.h

+#include <sstream> 
+#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);
+
+};

include/htc_vive_tracker_alg_node.h

@@ -32,6 +32,7 @@
 #include <tf2_ros/transform_broadcaster.h>
 #include <geometry_msgs/TransformStamped.h>
 #include <tf2/LinearMath/Quaternion.h>
+#include "file_reader.h"
 // [service client headers]
 
 // [action server client headers]
@@ -45,7 +46,11 @@ 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_;
     float ax_, ay_,az_,angle_rad_;
     bool apply_rotation_;
     double wam_to_chaperone_x_, wam_to_chaperone_y_, wam_to_chaperone_z_;
@@ -132,6 +137,7 @@ class HtcViveTrackerAlgNode : public algorithm_base::IriBaseAlgorithm<HtcViveTra
     void PrintAllDeviceNames();
     void ApplyRotation(tf2::Quaternion & q, float x, float y, float z, float angle);
     tf2::Quaternion ApplyRotationForIRIStandardCoordinates(const tf2::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

+#include "file_reader.h"
+	
+	
+std::vector<std::string> string_values = {"x","y","z","i","j","k","w"};
+tf2::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
+
+	while (json_file.good()){
+		getline(json_file,line,'"');
+		for (int i = 0; i<string_values.size(); ++i){
+			if (line == string_values[i]){
+				getline(json_file,data_value,',');
+				json_values[i] = std::stod(data_value.erase(0,1));
+			}
+		}
+	}
+
+	
+	tf2::Transform transform = GetTransformFromVector(json_values);
+	return transform;
+}
+
+
+tf2::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;
+	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));
+	//	transform_stamped.header.stamp_ = ros::Time  (csv_values[0]);
+	return transform;	
+
+}
+
+
+tf2::Transform FileReader::GetTransformFromVector(const std::vector<double>&values){
+	std::cout<<"size ' "<<values.size()<<std::endl;
+	tf2::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],
+						values[first_value_position+1],
+						values[first_value_position+2]));
+		transform.setRotation(tf2::Quaternion(
+					values[first_value_position+3],
+					values[first_value_position+4],
+					values[first_value_position+5],
+					values[first_value_position+6]
+					));
+		
+	}
+	return transform;
+	
+	
+}
+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/htc_vive_tracker_alg_node.cpp

@@ -4,18 +4,14 @@ HtcViveTrackerAlgNode::HtcViveTrackerAlgNode(void) :
   algorithm_base::IriBaseAlgorithm<HtcViveTrackerAlgorithm>()
 {
    this->loop_rate_ = 20;
-   bool verbose = true;
-   //"delay": -7165.209842681885, 
-   //"rotation": {
-     this->wam_to_chaperone_i_ = -0.004155247641619615;
-     this->wam_to_chaperone_j_ = 0.010017762641695618;
-     this->wam_to_chaperone_k_ = -0.9953359445264848;
-     this->wam_to_chaperone_w_ = 0.09585789420940735;
-   //}, 
-   //"translation": {
-     this->wam_to_chaperone_x_ = 0.0006095892506164251; 
-     this->wam_to_chaperone_y_ = -0.006141146275579224;
-     this->wam_to_chaperone_z_ = 0.07223111590777255;
+   bool verbose = true; 
+   this-> SetValuesWamToChaperone(
+     "/home/pmlab/iri-lab/iri_ws/src/iri_htc_vive_tracker/cfg/calibration.json",
+     
+    "/home/pmlab/iri-lab/iri_ws/src/iri_htc_vive_tracker/cfg/aligned_tf_poses_wam.csv",
+    "/home/pmlab/iri-lab/iri_ws/src/iri_htc_vive_tracker/cfg/aligned_tf_poses_tracker.csv");
+
+
   // }
 
    //init class attributes if necessary
@@ -225,26 +221,22 @@ void HtcViveTrackerAlgNode::ApplyRotation(tf2::Quaternion & orig, float ax, floa
 void HtcViveTrackerAlgNode::BroadcastWAMToChaperoneTransformation (){
 	
     	static tf2_ros::TransformBroadcaster tf_broadcaster;
-    	geometry_msgs::TransformStamped transform_stamped_wam_chaperone;
-	transform_stamped_wam_chaperone.header.stamp = ros::Time::now();
-	transform_stamped_wam_chaperone.header.frame_id = "chaperone";
-	transform_stamped_wam_chaperone.child_frame_id = "iri_wam_link_base";
-	transform_stamped_wam_chaperone.transform.translation.x = this->wam_to_chaperone_x_;
-	transform_stamped_wam_chaperone.transform.translation.y = this->wam_to_chaperone_y_;
-	transform_stamped_wam_chaperone.transform.translation.z = this->wam_to_chaperone_z_;
 
+	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){
 	
-	
-	
-
-	transform_stamped_wam_chaperone.transform.rotation.x = this->wam_to_chaperone_i_;
-	transform_stamped_wam_chaperone.transform.rotation.y = this->wam_to_chaperone_j_;
-	transform_stamped_wam_chaperone.transform.rotation.z = this->wam_to_chaperone_k_;
-	transform_stamped_wam_chaperone.transform.rotation.w = this->wam_to_chaperone_w_;
+	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();
 
-	tf_broadcaster.sendTransform(transform_stamped_wam_chaperone);
- 
 }
 /* main function */
 int main(int argc,char *argv[])