| 1003 | } |
| 1004 | |
| 1005 | void Publisher::publishStaticTf(const geometry_msgs::msg::Transform& pose, |
| 1006 | const std::string& parent_frame_id, |
| 1007 | const std::string& child_frame_id) { |
| 1008 | static tf2_ros::StaticTransformBroadcaster static_broadcaster(node_); |
| 1009 | geometry_msgs::msg::TransformStamped static_transform_stamped; |
| 1010 | static_transform_stamped.header.stamp = _t; |
| 1011 | if ((node_->get_clock()->now().seconds() - _t.seconds()) > 10.0) |
| 1012 | static_transform_stamped.header.stamp = node_->get_clock()->now(); |
| 1013 | |
| 1014 | // TODO(Toni): Warning: using rclcpp::Time::now(), will that bring issues? |
| 1015 | static_transform_stamped.header.stamp = node_->now(); |
| 1016 | static_transform_stamped.header.frame_id = parent_frame_id; |
| 1017 | static_transform_stamped.child_frame_id = child_frame_id; |
| 1018 | static_transform_stamped.transform = pose; |
| 1019 | static_broadcaster.sendTransform(static_transform_stamped); |
| 1020 | } |
| 1021 | |
| 1022 | } // namespace okvis |