diff --git a/cfg/calibration.xml b/cfg/calibration.xml
deleted file mode 100644
index 7d50215c490e79033f56051aac4eb7d6752958b4..0000000000000000000000000000000000000000
--- a/cfg/calibration.xml
+++ /dev/null
@@ -1,10 +0,0 @@
-<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>
diff --git a/include/file_reader.h b/include/file_reader.h
index 1c80f6c91e00ccc2762316923a1a72ae8a7a5965..e08430becef5e3afb19f6695b5d01feb6a762b17 100644
--- a/include/file_reader.h
+++ b/include/file_reader.h
@@ -11,8 +11,8 @@ class FileReader{
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 ReadTransformFromXML (const std::string & xml_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);
};
diff --git a/include/htc_vive_tracker_alg_node.h b/include/htc_vive_tracker_alg_node.h
index 13991787a388ab5037430a4a74914afdcc6a68bd..5756cebc9583e5f24ce3aa4e1d8196eb28c38839 100644
--- a/include/htc_vive_tracker_alg_node.h
+++ b/include/htc_vive_tracker_alg_node.h
@@ -46,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_;
@@ -133,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
diff --git a/src/file_reader.cpp b/src/file_reader.cpp
index f95e880f8b63ff2c26ade6122da449cf659b6f58..68c4777115da82ec2b358fda79933abc55dfa0f7 100644
--- a/src/file_reader.cpp
+++ b/src/file_reader.cpp
@@ -1,27 +1,29 @@
#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);
+ 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;
+
+ tf2::Transform transform = GetTransformFromVector(json_values);
return transform;
}
-tf2::Transform FileReader::ReadTransformFromXML (const std::string & xml_file_path){
- std::ifstream xml_file(xml_file_path);
- std::string line;
- while (xml_file.good()){
- getline(xml_file,line);
- }
- tf2::Transform transform;
- return transform;
-
-}
tf2::Transform FileReader::ReadFirstTransformFromCSV (const std::string & csv_file_path){
std::ifstream csv_file(csv_file_path);
@@ -70,10 +72,19 @@ double ValueFromString (const std::string & value_str){
return std::stod (value_str);
}
-tf2::Transform GetBaseWorldFromTransforms ( tf2::Transform hand_eye , tf2::Transform base_hand, tf2::Transform world_eye){
+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);
+}
diff --git a/src/htc_vive_tracker_alg_node.cpp b/src/htc_vive_tracker_alg_node.cpp
index bb6e8469312a8c37c470c260ef74c5d4931d7faf..2e822f63ba9ab0d138dca4b35db6c18e9fdfe901 100644
--- a/src/htc_vive_tracker_alg_node.cpp
+++ b/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[])