From ae77d18a5ee92a292b5e2bc5af8a51fc48707311 Mon Sep 17 00:00:00 2001 From: Timo Horstschaefer Date: Tue, 27 Jun 2017 17:12:50 +0200 Subject: [PATCH 1/2] Use fixed-point arithmetic for time stamps --- davis_ros_driver/src/driver.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/davis_ros_driver/src/driver.cpp b/davis_ros_driver/src/driver.cpp index 3a63ef1..5e2c591 100644 --- a/davis_ros_driver/src/driver.cpp +++ b/davis_ros_driver/src/driver.cpp @@ -485,7 +485,8 @@ void DavisRosDriver::readout() dvs_msgs::Event e; e.x = caerPolarityEventGetX(event); e.y = caerPolarityEventGetY(event); - e.ts = reset_time_ + ros::Duration(caerPolarityEventGetTimestamp64(event, polarity) / 1.e6); + e.ts = reset_time_ + + ros::Duration().fromNSec(caerPolarityEventGetTimestamp64(event, polarity) * 1000); e.polarity = caerPolarityEventGetPolarity(event); event_array_msg->events.push_back(e); @@ -555,7 +556,8 @@ void DavisRosDriver::readout() msg.orientation_covariance[0] = -1.0; // time - msg.header.stamp = reset_time_ + ros::Duration(caerIMU6EventGetTimestamp64(event, imu) / 1.e6); + msg.header.stamp = reset_time_ + + ros::Duration().fromNSec(caerIMU6EventGetTimestamp64(event, imu) * 1000); // frame msg.header.frame_id = "base_link"; @@ -615,7 +617,8 @@ void DavisRosDriver::readout() } // time - msg.header.stamp = reset_time_ + ros::Duration(caerFrameEventGetTimestamp64(event, frame) / 1.e6); + msg.header.stamp = reset_time_ + + ros::Duration().fromNSec(caerFrameEventGetTimestamp64(event, frame) * 1000); image_pub_.publish(msg); } From 8aea50821f590aba2b8c7630ee9882be72b1e9be Mon Sep 17 00:00:00 2001 From: Timo Horstschaefer Date: Wed, 28 Jun 2017 17:04:13 +0200 Subject: [PATCH 2/2] fixed-point timestamps in DVS driver --- dvs_ros_driver/src/driver.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/dvs_ros_driver/src/driver.cpp b/dvs_ros_driver/src/driver.cpp index da2d648..cb87769 100644 --- a/dvs_ros_driver/src/driver.cpp +++ b/dvs_ros_driver/src/driver.cpp @@ -247,7 +247,8 @@ void DvsRosDriver::readout() dvs_msgs::Event e; e.x = caerPolarityEventGetX(event); e.y = caerPolarityEventGetY(event); - e.ts = reset_time_ + ros::Duration(caerPolarityEventGetTimestamp64(event, polarity) / 1.e6); + e.ts = reset_time_ + + ros::Duration().fromNSec(caerPolarityEventGetTimestamp64(event, polarity) * 1000); e.polarity = caerPolarityEventGetPolarity(event); event_array_msg->events.push_back(e);