Explicit time conversions and comparisons (#1087)

Signed-off-by: Guilherme Rodrigues <[email protected]>
ros2 · Nov 13, 2023 · fe52932 · fe52932
1 parent ea2dbb3
commit fe52932
Show file tree

Hide file tree

Showing 14 changed files with 37 additions and 22 deletions.
diff --git a/rviz_common/include/rviz_common/frame_manager_iface.hpp b/rviz_common/include/rviz_common/frame_manager_iface.hpp
@@ -146,7 +146,8 @@ class RVIZ_COMMON_PUBLIC FrameManagerIface : public QObject
     Ogre::Vector3 & position,
     Ogre::Quaternion & orientation)
   {
-    return getTransform(header.frame_id, header.stamp, position, orientation);
+    rclcpp::Time time_stamp(header.stamp, RCL_ROS_TIME);
+    return getTransform(header.frame_id, time_stamp, position, orientation);
   }
 
   /// Return the pose for a frame relative to the fixed frame, in Ogre classes, at the most recent
@@ -199,7 +200,8 @@ class RVIZ_COMMON_PUBLIC FrameManagerIface : public QObject
     Ogre::Vector3 & position,
     Ogre::Quaternion & orientation)
   {
-    return transform(header.frame_id, header.stamp, pose, position, orientation);   // NOLINT
+    rclcpp::Time time_stamp(header.stamp, RCL_ROS_TIME);
+    return transform(header.frame_id, time_stamp, pose, position, orientation);   // NOLINT
     // linter wants #include <algorithm> for transform
   }
 

diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/camera/camera_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/camera/camera_display.cpp
@@ -482,8 +482,9 @@ bool CameraDisplay::updateCamera()
 
   Ogre::Vector3 position;
   Ogre::Quaternion orientation;
+  rclcpp::Time time_stamp(image->header.stamp, RCL_ROS_TIME);
   if (!context_->getFrameManager()->getTransform(
-      image->header.frame_id, image->header.stamp, position, orientation))
+      image->header.frame_id, time_stamp, position, orientation))
   {
     setMissingTransformToFixedFrame(image->header.frame_id);
     return false;
@@ -542,7 +543,7 @@ bool CameraDisplay::timeDifferenceInExactSyncMode(
   const sensor_msgs::msg::Image::ConstSharedPtr & image, rclcpp::Time & rviz_time) const
 {
   return context_->getFrameManager()->getSyncMode() == rviz_common::FrameManagerIface::SyncExact &&
-         rviz_time != image->header.stamp;
+         rviz_time != rclcpp::Time(image->header.stamp, RCL_ROS_TIME);
 }
 
 ImageDimensions CameraDisplay::getImageDimensions(

diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/effort/effort_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/effort/effort_display.cpp
@@ -327,9 +327,11 @@ void EffortDisplay::processMessage(sensor_msgs::msg::JointState::ConstSharedPtr
       continue;  // skip joints..
     }
 
+    rclcpp::Time msg_time(msg->header.stamp, RCL_ROS_TIME);
+
     // update effort property
     joint_info->setEffort(msg->effort[i]);
-    joint_info->last_update_ = msg->header.stamp;
+    joint_info->last_update_ = msg_time;
 
     const urdf::Joint * joint = robot_model_->getJoint(joint_name).get();
     int joint_type = joint->type;
@@ -341,7 +343,7 @@ void EffortDisplay::processMessage(sensor_msgs::msg::JointState::ConstSharedPtr
 
       // Call rviz::FrameManager to get the transform from the fixed frame to the joint's frame.
       if (!context_->getFrameManager()->getTransform(
-          tf_frame_id, msg->header.stamp, position, orientation))
+          tf_frame_id, msg_time, position, orientation))
       {
         setStatus(
           rviz_common::properties::StatusProperty::Error,

diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/grid_cells/grid_cells_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/grid_cells/grid_cells_display.cpp
@@ -131,7 +131,8 @@ void GridCellsDisplay::processMessage(nav_msgs::msg::GridCells::ConstSharedPtr m
 
 bool GridCellsDisplay::setTransform(std_msgs::msg::Header const & header)
 {
-  if (!updateFrame(header.frame_id, header.stamp)) {
+  rclcpp::Time time_stamp(header.stamp, RCL_ROS_TIME);
+  if (!updateFrame(header.frame_id, time_stamp)) {
     setMissingTransformToFixedFrame(header.frame_id, getNameStd());
     return false;
   }

diff --git a/...ault_plugins/src/rviz_default_plugins/displays/interactive_markers/interactive_marker.cpp b/...ault_plugins/src/rviz_default_plugins/displays/interactive_markers/interactive_marker.cpp
@@ -97,9 +97,9 @@ void InteractiveMarker::processMessage(
     orientation.w = 1.0;
   }
 
-  reference_time_ = message.header.stamp;
+  reference_time_ = rclcpp::Time(message.header.stamp, RCL_ROS_TIME);
   reference_frame_ = message.header.frame_id;
-  frame_locked_ = (message.header.stamp == rclcpp::Time());
+  frame_locked_ = (reference_time_ == rclcpp::Time());
 
   requestPoseUpdate(position, orientation);
   context_->queueRender();
@@ -120,9 +120,9 @@ bool InteractiveMarker::processMessage(const visualization_msgs::msg::Interactiv
 
   scale_ = message.scale;
 
+  reference_time_ = rclcpp::Time(message.header.stamp, RCL_ROS_TIME);
   reference_frame_ = message.header.frame_id;
-  reference_time_ = message.header.stamp;
-  frame_locked_ = (message.header.stamp == rclcpp::Time());
+  frame_locked_ = (reference_time_ == rclcpp::Time());
 
   position_ = Ogre::Vector3(
     message.pose.position.x, message.pose.position.y, message.pose.position.z);
@@ -619,7 +619,7 @@ void InteractiveMarker::updateReferencePose()
         geometry_msgs::msg::TransformStamped transform =
           context_->getFrameManager()->getTransformer()->lookupTransform(
           reference_frame_, fixed_frame, tf2::TimePoint());
-        reference_time_ = transform.header.stamp;
+        reference_time_ = rclcpp::Time(transform.header.stamp, RCL_ROS_TIME);
       } catch (...) {
         std::ostringstream oss;
         oss << "Error getting time of latest transform between " << reference_frame_ <<

diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/map/map_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/map/map_display.cpp
@@ -632,7 +632,7 @@ void MapDisplay::transformMap()
   rclcpp::Time transform_time = context_->getClock()->now();
 
   if (transform_timestamp_property_->getBool()) {
-    transform_time = current_map_.header.stamp;
+    transform_time = rclcpp::Time(current_map_.header.stamp, RCL_ROS_TIME);
   }
 
   Ogre::Vector3 position;

diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/marker_base.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/marker_base.cpp
@@ -103,7 +103,7 @@ bool MarkerBase::transform(
   Ogre::Quaternion & orient,
   Ogre::Vector3 & scale)
 {
-  rclcpp::Time stamp = message->header.stamp;
+  rclcpp::Time stamp(message->header.stamp, RCL_ROS_TIME);
   if (message->frame_locked) {
     stamp = rclcpp::Time(0, 0, context_->getClock()->get_clock_type());
   }
@@ -112,8 +112,9 @@ bool MarkerBase::transform(
       message->header.frame_id, stamp, message->pose, pos, orient))
   {
     std::string error;
+    rclcpp::Time message_time(message->header.stamp, RCL_ROS_TIME);
     context_->getFrameManager()->transformHasProblems(
-      message->header.frame_id, message->header.stamp, error);
+      message->header.frame_id, message_time, error);
     if (owner_) {
       owner_->setMarkerStatus(getID(), rviz_common::properties::StatusProperty::Error, error);
     }

diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/point/point_stamped_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/point/point_stamped_display.cpp
@@ -128,7 +128,8 @@ void PointStampedDisplay::processMessage(geometry_msgs::msg::PointStamped::Const
     return;
   }
 
-  if (!updateFrame(msg->header.frame_id, msg->header.stamp)) {
+  rclcpp::Time time_stamp(msg->header.stamp, RCL_ROS_TIME);
+  if (!updateFrame(msg->header.frame_id, time_stamp)) {
     setMissingTransformToFixedFrame(msg->header.frame_id);
     return;
   }

diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_common.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_common.cpp
@@ -510,9 +510,11 @@ void PointCloudCommon::processMessage(const sensor_msgs::msg::PointCloud2::Const
   info->receive_time_ = clock_->now();
 
   if (transformCloud(info, true)) {
+    rclcpp::Time time_stamp(cloud->header.stamp, RCL_ROS_TIME);
+
     std::unique_lock<std::mutex> lock(new_clouds_mutex_);
     new_cloud_infos_.push_back(info);
-    display_->emitTimeSignal(cloud->header.stamp);
+    display_->emitTimeSignal(time_stamp);
   }
 }
 

diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/polygon/polygon_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/polygon/polygon_display.cpp
@@ -104,7 +104,8 @@ void PolygonDisplay::processMessage(geometry_msgs::msg::PolygonStamped::ConstSha
     return;
   }
 
-  if (!updateFrame(msg->header.frame_id, msg->header.stamp)) {
+  rclcpp::Time msg_time(msg->header.stamp, RCL_ROS_TIME);
+  if (!updateFrame(msg->header.frame_id, msg_time)) {
     setMissingTransformToFixedFrame(msg->header.frame_id);
     return;
   }

diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/pose_array/pose_array_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/pose_array/pose_array_display.cpp
@@ -204,7 +204,8 @@ bool PoseArrayDisplay::validateFloats(const geometry_msgs::msg::PoseArray & msg)
 
 bool PoseArrayDisplay::setTransform(std_msgs::msg::Header const & header)
 {
-  if (!updateFrame(header.frame_id, header.stamp)) {
+  rclcpp::Time time_stamp(header.stamp, RCL_ROS_TIME);
+  if (!updateFrame(header.frame_id, time_stamp)) {
     setMissingTransformToFixedFrame(header.frame_id);
     return false;
   }

diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/range/range_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/range/range_display.cpp
@@ -119,11 +119,12 @@ void RangeDisplay::processMessage(const sensor_msgs::msg::Range::ConstSharedPtr
 
   Ogre::Vector3 position;
   Ogre::Quaternion orientation;
+  rclcpp::Time time_stamp(msg->header.stamp, RCL_ROS_TIME);
   float displayed_range = getDisplayedRange(msg);
   auto pose = getPose(displayed_range);
 
   if (!context_->getFrameManager()->transform(
-      msg->header.frame_id, msg->header.stamp, pose, position, orientation))
+      msg->header.frame_id, time_stamp, pose, position, orientation))
   {
     setMissingTransformToFixedFrame(msg->header.frame_id);
     return;

diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/screw/screw_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/screw/screw_display.cpp
@@ -187,8 +187,9 @@ void ScrewDisplay<MessageType>::processMessagePrivate(
   // it fails, we can't do anything else so we return.
   Ogre::Quaternion orientation;
   Ogre::Vector3 position;
+  rclcpp::Time time_stamp(header.stamp, RCL_ROS_TIME);
   if (!rviz_common::MessageFilterDisplay<MessageType>::context_->getFrameManager()->getTransform(
-      header.frame_id, header.stamp, position,
+      header.frame_id, time_stamp, position,
       orientation))
   {
     rviz_common::MessageFilterDisplay<MessageType>::setStatus(

diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/wrench/wrench_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/wrench/wrench_display.cpp
@@ -164,8 +164,9 @@ void WrenchDisplay::processMessage(geometry_msgs::msg::WrenchStamped::ConstShare
 
   Ogre::Quaternion orientation;
   Ogre::Vector3 position;
+  rclcpp::Time time_stamp(msg->header.stamp, RCL_ROS_TIME);
   if (!context_->getFrameManager()->getTransform(
-      msg->header.frame_id, msg->header.stamp, position, orientation))
+      msg->header.frame_id, time_stamp, position, orientation))
   {
     setMissingTransformToFixedFrame(msg->header.frame_id);
     return;