diff --git a/launch/asl_vicon.launch b/launch/asl_vicon.launch index e918207..afd380c 100644 --- a/launch/asl_vicon.launch +++ b/launch/asl_vicon.launch @@ -1,9 +1,11 @@ + + - + diff --git a/package.xml b/package.xml index 323599f..7e5aab5 100644 --- a/package.xml +++ b/package.xml @@ -30,4 +30,6 @@ eigen_conversions glog_catkin + cuckoo_time_translator + diff --git a/src/ros_vrpn_client.cpp b/src/ros_vrpn_client.cpp index d15e6d7..31db440 100644 --- a/src/ros_vrpn_client.cpp +++ b/src/ros_vrpn_client.cpp @@ -54,6 +54,10 @@ #include #include +#include +#include +#include + #include "vicon_odometry_estimator.h" void VRPN_CALLBACK track_target(void*, const vrpn_TRACKERCB tracker); @@ -80,7 +84,12 @@ struct TargetState { enum CoordinateSystem { kVicon, kOptitrack } coordinate_system; // Available timestamping options. -enum TimestampingSystem { kTrackerStamp, kRosStamp } timestamping_system; +enum TimestampingSystem { kTrackerStamp, + kRosStamp, + kTranslation +} timestamping_system; + +std::shared_ptr device_time_translator; // Global target descriptions. TargetState* target_state; @@ -252,6 +261,11 @@ void inline getTimeStamp(const ros::Time& vicon_stamp, ros::Time* timestamp) { *timestamp = ros::Time::now(); break; } + case kTranslation: { + if(device_time_translator) { + *timestamp = device_time_translator->update(vicon_stamp.toNSec(), ros::Time::now()); + } + } } } @@ -296,7 +310,7 @@ void VRPN_CALLBACK track_target(void*, const vrpn_TRACKERCB tracker) { vicon_odometry_estimator->updateEstimate(position_measured_W, orientation_measured_B_W, timestamp); - Eigen::Vector3d position_estimate_W = + Eigen::Vector3d position_estimate_W = vicon_odometry_estimator->getEstimatedPosition(); Eigen::Vector3d velocity_estimate_W = vicon_odometry_estimator->getEstimatedVelocity(); @@ -400,9 +414,27 @@ int main(int argc, char* argv[]) { timestamping_system = TimestampingSystem::kTrackerStamp; } else if (timestamping_system_string == "ros") { timestamping_system = TimestampingSystem::kRosStamp; + } else if (timestamping_system_string == "translation") { + timestamping_system = TimestampingSystem::kTranslation; + + const bool kAppendDeviceTimeSubnamespace = true; + const double kSwitchingTime = 100.0; // s + const uint64_t kOverflow = 0xFFFFFFFFFFFFFFFF; // 64 bit + const double kClockFrequency = 1e9; // nanoseconds + + cuckoo_time_translator::Defaults defaults; + defaults.setFilterAlgorithm( + cuckoo_time_translator::FilterAlgorithm( + cuckoo_time_translator::FilterAlgorithm::Type::ConvexHull)); + defaults.setSwitchTimeSecs(kSwitchingTime); + device_time_translator.reset( + new cuckoo_time_translator::DefaultDeviceTimeUnwrapperAndTranslator( + {kOverflow, kClockFrequency}, + {nh.getNamespace(), kAppendDeviceTimeSubnamespace}, + defaults)); } else { ROS_FATAL( - "ROS param timestamping_system should be either 'tracker' or 'ros'!"); + "ROS param timestamping_system should be either 'tracker' or 'ros' or 'translation'!"); return EXIT_FAILURE; }