diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5a1aaec317922582d646f9c7f6ceab5c0fcb2676..635a7770cb949718fed93496d950cf6c38bf418f 100644
--- a/CMakeLists.txt
+++ b/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
diff --git a/cfg/calibration.cpp b/cfg/calibration.cpp
index c2ad05865de4e315ee8b79844ee0f65a69f07140..dbdd49df8633f89403bd74c4bbc30e0335d35ce0 100644
--- a/cfg/calibration.cpp
+++ b/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 (){
diff --git a/include/file_reader.h b/include/file_reader.h
index e08430becef5e3afb19f6695b5d01feb6a762b17..e188fd8f8536607f19a88c7cfbd56b28df6f3455 100644
--- a/include/file_reader.h
+++ b/include/file_reader.h
@@ -1,18 +1,14 @@
#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);
};
diff --git a/include/hand_eye_calibration_helper.h b/include/hand_eye_calibration_helper.h
new file mode 100644
index 0000000000000000000000000000000000000000..236a096d6b0ca722168045e70707e06c13c7ec8e
--- /dev/null
+++ b/include/hand_eye_calibration_helper.h
@@ -0,0 +1,10 @@
+
+#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);
+
+
+
+};
diff --git a/include/htc_vive_tracker_alg_node.h b/include/htc_vive_tracker_alg_node.h
index 5756cebc9583e5f24ce3aa4e1d8196eb28c38839..724b439d2f13861deab07674ccd6d9836df7a0dd 100644
--- a/include/htc_vive_tracker_alg_node.h
+++ b/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
diff --git a/src/file_reader.cpp b/src/file_reader.cpp
index 68c4777115da82ec2b358fda79933abc55dfa0f7..c97d6c04c752d6e6cef5e9e9371ab45c091603f8 100644
--- a/src/file_reader.cpp
+++ b/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);
-}
diff --git a/src/hand_eye_calibration_helper.cpp b/src/hand_eye_calibration_helper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d7cd58f5df455a37c687d9a360603cc77aa80879
--- /dev/null
+++ b/src/hand_eye_calibration_helper.cpp
@@ -0,0 +1,19 @@
+#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);
+}
diff --git a/src/htc_vive_tracker_alg_node.cpp b/src/htc_vive_tracker_alg_node.cpp
index 2e822f63ba9ab0d138dca4b35db6c18e9fdfe901..5bdadc593ae525c4bee9f9b157df33c5e0208d14 100644
--- a/src/htc_vive_tracker_alg_node.cpp
+++ b/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[])