diff --git a/rviz_common/include/rviz_common/frame_manager_iface.hpp b/rviz_common/include/rviz_common/frame_manager_iface.hpp index 0ed8c1f1b..56d92d5f3 100644 --- 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 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 index c076996e2..39d3dec83 100644 --- 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 index bf0b91f78..4aa135865 100644 --- 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 index 34772506d..76410d146 100644 --- 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/rviz_default_plugins/src/rviz_default_plugins/displays/interactive_markers/interactive_marker.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/interactive_markers/interactive_marker.cpp index dd77beeb7..3ad7b9435 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/interactive_markers/interactive_marker.cpp +++ b/rviz_default_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 index 9d3002ce6..f785efe5e 100644 --- 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 index 4c5bd7641..2e3b9c3a9 100644 --- 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 index 878613d14..167fad9c3 100644 --- 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 index ec0fc82b1..172c3eaec 100644 --- 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 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 index 1c983f716..200e3f18c 100644 --- 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 index bd3f2c703..e07d55343 100644 --- 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 index f6514a4a5..2b5cef3e1 100644 --- 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 index 36d7ba508..e9a95c09e 100644 --- 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::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::context_->getFrameManager()->getTransform( - header.frame_id, header.stamp, position, + header.frame_id, time_stamp, position, orientation)) { rviz_common::MessageFilterDisplay::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 index 49b819913..aebce1b1d 100644 --- 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;