diff --git a/CMakeLists.txt b/CMakeLists.txt index b270d38c5fbb122f643d4ae2d2c8ff89dbbdaddc..5a1aaec317922582d646f9c7f6ceab5c0fcb2676 100644 --- a/CMakeLists.txt +++ b/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 diff --git a/include/file_reader.h b/include/file_reader.h new file mode 100644 index 0000000000000000000000000000000000000000..1c80f6c91e00ccc2762316923a1a72ae8a7a5965 --- /dev/null +++ b/include/file_reader.h @@ -0,0 +1,18 @@ +#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 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); + +}; diff --git a/include/htc_vive_tracker_alg_node.h b/include/htc_vive_tracker_alg_node.h index b989ad7f628612c6a403049d3fcf73e6af45aa0c..13991787a388ab5037430a4a74914afdcc6a68bd 100644 --- a/include/htc_vive_tracker_alg_node.h +++ b/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] diff --git a/src/file_reader.cpp b/src/file_reader.cpp new file mode 100644 index 0000000000000000000000000000000000000000..f95e880f8b63ff2c26ade6122da449cf659b6f58 --- /dev/null +++ b/src/file_reader.cpp @@ -0,0 +1,79 @@ +#include "file_reader.h" + + +tf2::Transform FileReader::ReadTransformFromJSON (const std::string & json_file_path){ + std::ifstream json_file(json_file_path); + std::string line; + while (json_file.good()){ + getline(json_file,line); + } + + tf2::Transform transform; + 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); + //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 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; + +}