From 636947ef920ec80509dd29c564b4c3bc8cc4edb2 Mon Sep 17 00:00:00 2001 From: njacquemin1993 Date: Fri, 12 Jan 2024 22:07:53 +0100 Subject: [PATCH] Add option to use VRPN timestamps rather than generating them again (#7) * Add option to use VRPN timestamps rather than generating them again * Correctly handle seconds and nanoseconds * Keep vrpn timestamps in vrpn timebase * Switch to CamelCase for consistency * Fix style --- README.md | 1 + config/client.yaml | 1 + include/vrpn_mocap/tracker.hpp | 4 ++++ src/client.cpp | 1 + src/tracker.cpp | 20 +++++++++++++++++--- 5 files changed, 24 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index fdf7661..b4114d1 100644 --- a/README.md +++ b/README.md @@ -55,6 +55,7 @@ file with custom configurations. system default QoS which is reliable (default: `true`) - `multi_sensor (bool)` -- set to true if there are more than one sensor (frame) reporting on the same object (default: `false`) +- `use_vrpn_timestamps (bool)` -- use timestamps coming from VRPN. This ensures that the interval between frames timestamps is not modified (default: `false`) ### Acknowledgement Some ideas are borrowed from the well known diff --git a/config/client.yaml b/config/client.yaml index d870d25..fdfec83 100644 --- a/config/client.yaml +++ b/config/client.yaml @@ -6,3 +6,4 @@ update_freq: 100. refresh_freq: 1. multi_sensor: false + use_vrpn_timestamps: false diff --git a/include/vrpn_mocap/tracker.hpp b/include/vrpn_mocap/tracker.hpp index 855e97d..b0e14ca 100644 --- a/include/vrpn_mocap/tracker.hpp +++ b/include/vrpn_mocap/tracker.hpp @@ -31,6 +31,7 @@ #include "vrpn_Connection.h" #include "vrpn_Tracker.h" +#include "builtin_interfaces/msg/time.hpp" #include "geometry_msgs/msg/accel_stamped.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" @@ -100,6 +101,7 @@ class Tracker : public rclcpp::Node const bool multi_sensor_; const std::string frame_id_; const bool sensor_data_qos_; + const bool use_vrpn_timestamps_; const std::shared_ptr connection_; vrpn_Tracker_Remote vrpn_tracker_; @@ -139,6 +141,8 @@ class Tracker : public rclcpp::Node return pubs->at(sensor_idx); } + builtin_interfaces::msg::Time GetTimestamp(struct timeval vrpn_timestamp); + static void VRPN_CALLBACK HandlePose(void * tracker, const vrpn_TRACKERCB tracker_pose); static void VRPN_CALLBACK HandleTwist(void * tracker, const vrpn_TRACKERVELCB tracker_vel); static void VRPN_CALLBACK HandleAccel(void * tracker, const vrpn_TRACKERACCCB tracker_acc); diff --git a/src/client.cpp b/src/client.cpp index e7b610a..152c7ac 100644 --- a/src/client.cpp +++ b/src/client.cpp @@ -47,6 +47,7 @@ Client::Client(const std::string & name) mainloop_timer_ = this->create_wall_timer(1s / update_freq, std::bind(&Client::MainLoop, this)); this->declare_parameter("sensor_data_qos", true); + this->declare_parameter("use_vrpn_timestamps", false); } std::string Client::ParseHost() diff --git a/src/tracker.cpp b/src/tracker.cpp index c2ae446..261a979 100644 --- a/src/tracker.cpp +++ b/src/tracker.cpp @@ -53,6 +53,7 @@ Tracker::Tracker(const std::string & tracker_name) multi_sensor_(declare_parameter("multi_sensor", false)), frame_id_(declare_parameter("frame_id", "world")), sensor_data_qos_(declare_parameter("sensor_data_qos", true)), + use_vrpn_timestamps_(declare_parameter("use_vrpn_timestamps", false)), vrpn_tracker_(name_.c_str()) { Init(); @@ -70,6 +71,7 @@ Tracker::Tracker( multi_sensor_(base_node.get_parameter("multi_sensor").as_bool()), frame_id_(base_node.get_parameter("frame_id").as_string()), sensor_data_qos_(base_node.get_parameter("sensor_data_qos").as_bool()), + use_vrpn_timestamps_(base_node.get_parameter("use_vrpn_timestamps").as_bool()), vrpn_tracker_(name_.c_str(), connection.get()) { Init(); @@ -96,6 +98,18 @@ void Tracker::Init() void Tracker::MainLoop() {vrpn_tracker_.mainloop();} +builtin_interfaces::msg::Time Tracker::GetTimestamp(struct timeval vrpn_timestamp) +{ + if (this->use_vrpn_timestamps_) { + builtin_interfaces::msg::Time stamp; + stamp.sec = vrpn_timestamp.tv_sec; + stamp.nanosec = vrpn_timestamp.tv_usec * 1000; + + return stamp; + } + return this->get_clock()->now(); +} + void VRPN_CALLBACK Tracker::HandlePose(void * data, const vrpn_TRACKERCB tracker_pose) { Tracker * tracker = static_cast(data); @@ -107,7 +121,7 @@ void VRPN_CALLBACK Tracker::HandlePose(void * data, const vrpn_TRACKERCB tracker // populate message PoseStamped msg; msg.header.frame_id = tracker->frame_id_; - msg.header.stamp = tracker->get_clock()->now(); + msg.header.stamp = tracker->GetTimestamp(tracker_pose.msg_time); msg.pose.position.x = tracker_pose.pos[0]; msg.pose.position.y = tracker_pose.pos[1]; @@ -132,7 +146,7 @@ void VRPN_CALLBACK Tracker::HandleTwist(void * data, const vrpn_TRACKERVELCB tra // populate message TwistStamped msg; msg.header.frame_id = tracker->frame_id_; - msg.header.stamp = tracker->get_clock()->now(); + msg.header.stamp = tracker->GetTimestamp(tracker_twist.msg_time); msg.twist.linear.x = tracker_twist.vel[0]; msg.twist.linear.y = tracker_twist.vel[1]; @@ -161,7 +175,7 @@ void VRPN_CALLBACK Tracker::HandleAccel(void * data, const vrpn_TRACKERACCCB tra // populate message AccelStamped msg; msg.header.frame_id = tracker->frame_id_; - msg.header.stamp = tracker->get_clock()->now(); + msg.header.stamp = tracker->GetTimestamp(tracker_accel.msg_time); msg.accel.linear.x = tracker_accel.acc[0]; msg.accel.linear.y = tracker_accel.acc[1];