19#include "absl/strings/str_cat.h"
29constexpr float kSecondToNanoFactor = 1e9f;
30constexpr uint64_t kMilliToNanoFactor = 1e6;
36Buffer::Buffer() : BufferCore() {
Init(); }
39 const std::string node_name =
40 absl::StrCat(
"transform_listener_",
Time::Now().ToNanosecond());
41 node_ = cyber::CreateNode(node_name);
43 attr.set_channel_name(
"/tf");
45 attr, [&](
const std::shared_ptr<const TransformStampeds>& msg_evt) {
46 SubscriptionCallbackImpl(msg_evt,
false);
50 attr_static.set_channel_name(FLAGS_tf_static_topic);
51 attr_static.mutable_qos_profile()->CopyFrom(
54 attr_static, [&](
const std::shared_ptr<TransformStampeds>& msg_evt) {
55 SubscriptionCallbackImpl(msg_evt,
true);
61void Buffer::SubscriptionCallback(
62 const std::shared_ptr<const TransformStampeds>& msg_evt) {
63 SubscriptionCallbackImpl(msg_evt,
false);
66void Buffer::StaticSubscriptionCallback(
67 const std::shared_ptr<const TransformStampeds>& msg_evt) {
68 SubscriptionCallbackImpl(msg_evt,
true);
71void Buffer::SubscriptionCallbackImpl(
72 const std::shared_ptr<const TransformStampeds>& msg_evt,
bool is_static) {
74 std::string authority =
76 if (now.ToNanosecond() < last_update_.ToNanosecond()) {
77 AINFO <<
"Detected jump back in time. Clearing TF buffer.";
80 for (
auto& msg : static_msgs_) {
81 setTransform(msg, authority,
true);
86 for (
int i = 0; i < msg_evt->transforms_size(); i++) {
88 geometry_msgs::TransformStamped trans_stamped;
91 const auto& header = msg_evt->transforms(i).header();
92 trans_stamped.header.stamp =
93 static_cast<uint64_t
>(header.timestamp_sec() * kSecondToNanoFactor);
94 trans_stamped.header.frame_id = header.frame_id();
95 trans_stamped.header.seq = header.sequence_num();
98 trans_stamped.child_frame_id = msg_evt->transforms(i).child_frame_id();
101 const auto& transform = msg_evt->transforms(i).transform();
102 trans_stamped.transform.translation.x = transform.translation().x();
103 trans_stamped.transform.translation.y = transform.translation().y();
104 trans_stamped.transform.translation.z = transform.translation().z();
107 trans_stamped.transform.rotation.x = transform.rotation().qx();
108 trans_stamped.transform.rotation.y = transform.rotation().qy();
109 trans_stamped.transform.rotation.z = transform.rotation().qz();
110 trans_stamped.transform.rotation.w = transform.rotation().qw();
113 static_msgs_.push_back(trans_stamped);
115 setTransform(trans_stamped, authority, is_static);
116 }
catch (tf2::TransformException& ex) {
117 std::string temp = ex.what();
118 AERROR <<
"Failure to set received transform:" << temp.c_str();
123bool Buffer::GetLatestStaticTF(
const std::string& frame_id,
124 const std::string& child_frame_id,
126 for (
auto reverse_iter = static_msgs_.rbegin();
127 reverse_iter != static_msgs_.rend(); ++reverse_iter) {
128 if ((*reverse_iter).header.frame_id == frame_id &&
129 (*reverse_iter).child_frame_id == child_frame_id) {
130 TF2MsgToCyber((*reverse_iter), (*tf));
137void Buffer::TF2MsgToCyber(
138 const geometry_msgs::TransformStamped& tf2_trans_stamped,
141 trans_stamped.mutable_header()->set_timestamp_sec(
142 static_cast<double>(tf2_trans_stamped.header.stamp) / 1e9);
143 trans_stamped.mutable_header()->set_frame_id(
144 tf2_trans_stamped.header.frame_id);
147 trans_stamped.set_child_frame_id(tf2_trans_stamped.child_frame_id);
150 trans_stamped.mutable_transform()->mutable_translation()->set_x(
151 tf2_trans_stamped.transform.translation.x);
152 trans_stamped.mutable_transform()->mutable_translation()->set_y(
153 tf2_trans_stamped.transform.translation.y);
154 trans_stamped.mutable_transform()->mutable_translation()->set_z(
155 tf2_trans_stamped.transform.translation.z);
158 trans_stamped.mutable_transform()->mutable_rotation()->set_qx(
159 tf2_trans_stamped.transform.rotation.x);
160 trans_stamped.mutable_transform()->mutable_rotation()->set_qy(
161 tf2_trans_stamped.transform.rotation.y);
162 trans_stamped.mutable_transform()->mutable_rotation()->set_qz(
163 tf2_trans_stamped.transform.rotation.z);
164 trans_stamped.mutable_transform()->mutable_rotation()->set_qw(
165 tf2_trans_stamped.transform.rotation.w);
169 const std::string& source_frame,
171 const float timeout_second)
const {
173 geometry_msgs::TransformStamped tf2_trans_stamped =
174 tf2::BufferCore::lookupTransform(target_frame, source_frame, tf2_time);
176 TF2MsgToCyber(tf2_trans_stamped, trans_stamped);
177 return trans_stamped;
182 const std::string& source_frame,
184 const std::string& fixed_frame,
185 const float timeout_second)
const {
186 geometry_msgs::TransformStamped tf2_trans_stamped =
187 tf2::BufferCore::lookupTransform(target_frame, target_time.
ToNanosecond(),
191 TF2MsgToCyber(tf2_trans_stamped, trans_stamped);
192 return trans_stamped;
195bool Buffer::canTransform(
const std::string& target_frame,
196 const std::string& source_frame,
197 const cyber::Time& time,
const float timeout_second,
198 std::string* errstr)
const {
199 uint64_t timeout_ns =
200 static_cast<uint64_t
>(timeout_second * kSecondToNanoFactor);
202 while (
Clock::Now().ToNanosecond() < start_time + timeout_ns &&
203 !cyber::IsShutdown()) {
205 bool retval = tf2::BufferCore::canTransform(target_frame, source_frame,
210 if (!cyber::common::GlobalData::Instance()->IsRealityMode()) {
213 const int sleep_time_ms = 3;
214 AWARN <<
"BufferCore::canTransform failed: " << *errstr;
215 std::this_thread::sleep_for(std::chrono::milliseconds(sleep_time_ms));
218 *errstr = *errstr +
":timeout";
222bool Buffer::canTransform(
const std::string& target_frame,
224 const std::string& source_frame,
226 const std::string& fixed_frame,
227 const float timeout_second,
228 std::string* errstr)
const {
230 uint64_t timeout_ns =
231 static_cast<uint64_t
>(timeout_second * kSecondToNanoFactor);
233 while (
Clock::Now().ToNanosecond() < start_time + timeout_ns &&
234 !cyber::IsShutdown()) {
236 bool retval = tf2::BufferCore::canTransform(
242 const int sleep_time_ms = 3;
243 AWARN <<
"BufferCore::canTransform failed: " << *errstr;
244 std::this_thread::sleep_for(std::chrono::milliseconds(sleep_time_ms));
245 if (!cyber::common::GlobalData::Instance()->IsRealityMode()) {
247 sleep_time_ms * kMilliToNanoFactor));
251 *errstr = *errstr +
":timeout";
::apollo::cyber::Time Time
a singleton clock that can be used to get the current timestamp.
static void SetNow(const Time &now)
This is for mock clock mode only.
static Time Now()
PRECISION >= 1000000 means the precision is at least 1us.
Cyber has builtin time type Time.
uint64_t ToNanosecond() const
convert time to nanosecond.
static Time Now()
get the current time.
static const QosProfile QOS_PROFILE_TF_STATIC
bool Init(const char *binary_name, const std::string &dag_info)