From 9af4d2db73a38266f90b2d563baad54288d154a8 Mon Sep 17 00:00:00 2001 From: Juan Ignacio Ubeira Date: Mon, 13 Feb 2017 15:00:37 -0300 Subject: [PATCH] All timestamps' unit of measure changed to seconds. --- .../include/tango_ros_native/tango_ros_node.h | 2 +- .../tango_ros_native/src/tango_ros_node.cpp | 18 +++++++++--------- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/tango_ros_common/tango_ros_native/include/tango_ros_native/tango_ros_node.h b/tango_ros_common/tango_ros_native/include/tango_ros_native/tango_ros_node.h index 73517fb..140b46b 100644 --- a/tango_ros_common/tango_ros_native/include/tango_ros_native/tango_ros_node.h +++ b/tango_ros_common/tango_ros_native/include/tango_ros_native/tango_ros_node.h @@ -150,7 +150,7 @@ class TangoRosNode { std::mutex color_image_available_mutex_; std::condition_variable color_image_available_; - double time_offset_ = 0.; // Offset between tango time and ros time in ms. + double time_offset_ = 0.; // Offset between tango time and ros time in s. tf::TransformBroadcaster tf_broadcaster_; geometry_msgs::TransformStamped start_of_service_T_device_; diff --git a/tango_ros_common/tango_ros_native/src/tango_ros_node.cpp b/tango_ros_common/tango_ros_native/src/tango_ros_node.cpp index 1a2f43f..ec55824 100644 --- a/tango_ros_common/tango_ros_native/src/tango_ros_node.cpp +++ b/tango_ros_common/tango_ros_native/src/tango_ros_node.cpp @@ -62,7 +62,7 @@ void onFrameAvailableRouter(void* context, TangoCameraId camera_id, } // Converts a TangoPoseData to a geometry_msgs::TransformStamped. // @param pose, TangoPoseData to convert. -// @param time_offset, offset in ms between pose (tango time) and +// @param time_offset, offset in s between pose (tango time) and // transform (ros time). // @param transform, the output TransformStamped. void toTransformStamped(const TangoPoseData& pose, @@ -75,11 +75,11 @@ void toTransformStamped(const TangoPoseData& pose, transform->transform.rotation.y = pose.orientation[1]; transform->transform.rotation.z = pose.orientation[2]; transform->transform.rotation.w = pose.orientation[3]; - transform->header.stamp.fromSec((pose.timestamp + time_offset) / 1e3); + transform->header.stamp.fromSec(pose.timestamp + time_offset); } // Converts a TangoPointCloud to a sensor_msgs::PointCloud2. // @param tango_point_cloud, TangoPointCloud to convert. -// @param time_offset, offset in ms between tango_point_cloud (tango time) and +// @param time_offset, offset in s between tango_point_cloud (tango time) and // point_cloud (ros time). // @param point_cloud, the output PointCloud2. void toPointCloud2(const TangoPointCloud& tango_point_cloud, @@ -110,7 +110,7 @@ void toPointCloud2(const TangoPointCloud& tango_point_cloud, *iter_z = tango_point_cloud.points[i][2]; *iter_c = tango_point_cloud.points[i][3]; } - point_cloud->header.stamp.fromSec((tango_point_cloud.timestamp + time_offset) / 1e3); + point_cloud->header.stamp.fromSec(tango_point_cloud.timestamp + time_offset); } // Convert a point to a laser scan range. // Method taken from the ros package 'pointcloud_to_laserscan': @@ -152,7 +152,7 @@ void toLaserScanRange(double x, double y, double z, double min_height, } // Converts a TangoPointCloud to a sensor_msgs::LaserScan. // @param tango_point_cloud, TangoPointCloud to convert. -// @param time_offset, offset in ms between tango_point_cloud (tango time) and +// @param time_offset, offset in s between tango_point_cloud (tango time) and // laser_scan (ros time). // @param min_height minimum height for a point of the point cloud to be // included in laser scan. @@ -172,7 +172,7 @@ void toLaserScan(const TangoPointCloud& tango_point_cloud, double z = -tango_point_cloud.points[i][1]; toLaserScanRange(x, y, z, min_height, max_height, laser_scan); } - laser_scan->header.stamp.fromSec((tango_point_cloud.timestamp + time_offset) / 1e3); + laser_scan->header.stamp.fromSec(tango_point_cloud.timestamp + time_offset); } // Compresses a cv::Mat image to a sensor_msgs::CompressedImage in JPEG format. // @param image, cv::Mat to compress, in YUV420sp format. @@ -302,7 +302,7 @@ TangoErrorType TangoRosNode::OnTangoServiceConnected() { LOG(ERROR) << "Error, could not get a first valid pose."; return TANGO_INVALID; } - time_offset_ = ros::Time::now().toSec() * 1e3 - pose.timestamp; + time_offset_ = ros::Time::now().toSec() - pose.timestamp; return TANGO_SUCCESS; } @@ -545,7 +545,7 @@ void TangoRosNode::OnFrameAvailable(TangoCameraId camera_id, const TangoImageBuf fisheye_image_available_mutex_.try_lock()) { fisheye_image_ = cv::Mat(buffer->height + buffer->height / 2, buffer->width, CV_8UC1, buffer->data, buffer->stride); // No deep copy. - fisheye_compressed_image_.header.stamp.fromSec((buffer->timestamp + time_offset_) / 1e3); + fisheye_compressed_image_.header.stamp.fromSec(buffer->timestamp + time_offset_); fisheye_compressed_image_.header.seq = buffer->frame_number; fisheye_compressed_image_.header.frame_id = toFrameId(TANGO_COORDINATE_FRAME_CAMERA_FISHEYE); fisheye_compressed_image_.format = ROS_IMAGE_COMPRESSING_FORMAT; @@ -557,7 +557,7 @@ void TangoRosNode::OnFrameAvailable(TangoCameraId camera_id, const TangoImageBuf color_image_available_mutex_.try_lock()) { color_image_ = cv::Mat(buffer->height + buffer->height / 2, buffer->width, CV_8UC1, buffer->data, buffer->stride); // No deep copy. - color_compressed_image_.header.stamp.fromSec((buffer->timestamp + time_offset_) / 1e3); + color_compressed_image_.header.stamp.fromSec(buffer->timestamp + time_offset_); color_compressed_image_.header.seq = buffer->frame_number; color_compressed_image_.header.frame_id = toFrameId(TANGO_COORDINATE_FRAME_CAMERA_COLOR); color_compressed_image_.format = ROS_IMAGE_COMPRESSING_FORMAT;