From 4e0faf9b1a50686ccfdffe208d816eb4380cbefc Mon Sep 17 00:00:00 2001 From: Guilherme Rodrigues Date: Mon, 13 Nov 2023 10:44:10 +0000 Subject: [PATCH] Explicit Time comparissons To conform with potential changes introduced in https://github.com/ros2/rclcpp/pull/2293 Signed-off-by: Guilherme Rodrigues --- src/interactive_marker_server.cpp | 2 +- src/message_context.cpp | 7 ++++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/interactive_marker_server.cpp b/src/interactive_marker_server.cpp index 4c374406..53e14ece 100644 --- a/src/interactive_marker_server.cpp +++ b/src/interactive_marker_server.cpp @@ -418,7 +418,7 @@ void InteractiveMarkerServer::processFeedback( marker_context.last_client_id = feedback->client_id; if (feedback->event_type == visualization_msgs::msg::InteractiveMarkerFeedback::POSE_UPDATE) { - if (marker_context.int_marker.header.stamp == rclcpp::Time()) { + if (marker_context.int_marker.header.stamp == builtin_interfaces::msg::Time()) { // keep the old header doSetPose( pending_updates_.find( diff --git a/src/message_context.cpp b/src/message_context.cpp index aa204758..dac454b4 100644 --- a/src/message_context.cpp +++ b/src/message_context.cpp @@ -100,7 +100,7 @@ bool MessageContext::getTransform( header.frame_id.c_str(), target_frame_.c_str(), rclcpp::Time(header.stamp).seconds()); // if timestamp is given, transform message into target frame - if (header.stamp != rclcpp::Time(0)) { + if (header.stamp != builtin_interfaces::msg::Time()) { geometry_msgs::msg::PoseStamped pose_stamped_msg; pose_stamped_msg.header = header; pose_stamped_msg.pose = pose_msg; @@ -117,9 +117,10 @@ bool MessageContext::getTransform( geometry_msgs::msg::TransformStamped transform = tf_buffer_core_->lookupTransform( target_frame_, header.frame_id, tf2::TimePoint()); - rclcpp::Time latest_time = transform.header.stamp; + rclcpp::Time latest_time(transform.header.stamp, RCL_ROS_TIME); + rclcpp::Time source_time(header.stamp, RCL_ROS_TIME); - if (latest_time != rclcpp::Time(0) && latest_time > header.stamp) { + if (latest_time != rclcpp::Time(0) && latest_time > source_time) { std::ostringstream oss; oss << "The message contains an old timestamp and cannot be transformed " << "('" << header.frame_id << "' to '" << target_frame_ << "' at time " <<