tami/MYNT-EYE-S-SDK
6
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Timestamp synchronization through one association.

Browse Source
This commit is contained in:
nico 2018-07-02 12:20:24 +08:00
parent 1c1503aae2
commit becccd9b8f
1 changed files with 28 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
View File

@ -44,6 +44,19 @@ MYNTEYE_BEGIN_NAMESPACE
namespace enc = sensor_msgs::image_encodings; namespace enc = sensor_msgs::image_encodings;
ros::Time HardTime2SoftTime(double _hard_time) {
static bool isInited = false;
static double soft_time_begin(0), hard_time_begin(0);
if (false == isInited) {
soft_time_begin = ros::Time::now().toSec();
hard_time_begin = _hard_time;
isInited = true;
}
return ros::Time(soft_time_begin + (_hard_time - hard_time_begin) * 0.00001f);
}
class ROSWrapperNodelet : public nodelet::Nodelet { class ROSWrapperNodelet : public nodelet::Nodelet {
public: public:
ROSWrapperNodelet() {} ROSWrapperNodelet() {}
@ -242,10 +255,11 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
void publishTopics() { void publishTopics() {
api_->SetStreamCallback( api_->SetStreamCallback(
Stream::LEFT, [this](const api::StreamData &data) { Stream::LEFT, [this](const api::StreamData &data) {
static double ros_time_beg = ros::Time::now().toSec(); ros::Time stamp = HardTime2SoftTime(data.img->timestamp);
static double img_time_beg = data.img->timestamp; // static double ros_time_beg = ros::Time::now().toSec();
ros::Time stamp( // static double img_time_beg = data.img->timestamp;
ros_time_beg + (data.img->timestamp - img_time_beg) * 0.00001f); // ros::Time stamp(
// ros_time_beg + (data.img->timestamp - img_time_beg) * 0.00001f);
// static double img_time_prev = -1; // static double img_time_prev = -1;
// NODELET_INFO_STREAM("ros_time_beg: " << FULL_PRECISION << // NODELET_INFO_STREAM("ros_time_beg: " << FULL_PRECISION <<
@ -268,10 +282,11 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
api_->SetStreamCallback( api_->SetStreamCallback(
Stream::RIGHT, [this](const api::StreamData &data) { Stream::RIGHT, [this](const api::StreamData &data) {
static double ros_time_beg = ros::Time::now().toSec(); ros::Time stamp = HardTime2SoftTime(data.img->timestamp);
static double img_time_beg = data.img->timestamp; // static double ros_time_beg = ros::Time::now().toSec();
ros::Time stamp( // static double img_time_beg = data.img->timestamp;
ros_time_beg + (data.img->timestamp - img_time_beg) * 0.00001f); // ros::Time stamp(
// ros_time_beg + (data.img->timestamp - img_time_beg) * 0.00001f);
++right_count_; ++right_count_;
publishCamera(Stream::RIGHT, data, right_count_, stamp); publishCamera(Stream::RIGHT, data, right_count_, stamp);
@ -303,10 +318,11 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
}); });
api_->SetMotionCallback([this](const api::MotionData &data) { api_->SetMotionCallback([this](const api::MotionData &data) {
static double ros_time_beg = ros::Time::now().toSec(); ros::Time stamp = HardTime2SoftTime(data.imu->timestamp);
static double imu_time_beg = data.imu->timestamp; // static double ros_time_beg = ros::Time::now().toSec();
ros::Time stamp( // static double imu_time_beg = data.imu->timestamp;
ros_time_beg + (data.imu->timestamp - imu_time_beg) * 0.00001f); // ros::Time stamp(
// ros_time_beg + (data.imu->timestamp - imu_time_beg) * 0.00001f);
// static double imu_time_prev = -1; // static double imu_time_prev = -1;
// NODELET_INFO_STREAM("ros_time_beg: " << FULL_PRECISION << ros_time_beg // NODELET_INFO_STREAM("ros_time_beg: " << FULL_PRECISION << ros_time_beg