Apollo 10.0
自动驾驶开放平台
buffer.cc
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2018 The Apollo Authors. All Rights Reserved.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *****************************************************************************/
16
18
19#include "absl/strings/str_cat.h"
20
21#include "cyber/cyber.h"
22#include "cyber/time/clock.h"
24
27
28namespace {
29constexpr float kSecondToNanoFactor = 1e9f;
30constexpr uint64_t kMilliToNanoFactor = 1e6;
31} // namespace
32
33namespace apollo {
34namespace transform {
35
36Buffer::Buffer() : BufferCore() { Init(); }
37
38int Buffer::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");
44 message_subscriber_tf_ = node_->CreateReader<TransformStampeds>(
45 attr, [&](const std::shared_ptr<const TransformStampeds>& msg_evt) {
46 SubscriptionCallbackImpl(msg_evt, false);
47 });
48
50 attr_static.set_channel_name(FLAGS_tf_static_topic);
51 attr_static.mutable_qos_profile()->CopyFrom(
53 message_subscriber_tf_static_ = node_->CreateReader<TransformStampeds>(
54 attr_static, [&](const std::shared_ptr<TransformStampeds>& msg_evt) {
55 SubscriptionCallbackImpl(msg_evt, true);
56 });
57
58 return cyber::SUCC;
59}
60
61void Buffer::SubscriptionCallback(
62 const std::shared_ptr<const TransformStampeds>& msg_evt) {
63 SubscriptionCallbackImpl(msg_evt, false);
64}
65
66void Buffer::StaticSubscriptionCallback(
67 const std::shared_ptr<const TransformStampeds>& msg_evt) {
68 SubscriptionCallbackImpl(msg_evt, true);
69}
70
71void Buffer::SubscriptionCallbackImpl(
72 const std::shared_ptr<const TransformStampeds>& msg_evt, bool is_static) {
73 cyber::Time now = Clock::Now();
74 std::string authority =
75 "cyber_tf"; // msg_evt.getPublisherName(); // lookup the authority
76 if (now.ToNanosecond() < last_update_.ToNanosecond()) {
77 AINFO << "Detected jump back in time. Clearing TF buffer.";
78 clear();
79 // cache static transform stamped again.
80 for (auto& msg : static_msgs_) {
81 setTransform(msg, authority, true);
82 }
83 }
84 last_update_ = now;
85
86 for (int i = 0; i < msg_evt->transforms_size(); i++) {
87 try {
88 geometry_msgs::TransformStamped trans_stamped;
89
90 // header
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();
96
97 // child_frame_id
98 trans_stamped.child_frame_id = msg_evt->transforms(i).child_frame_id();
99
100 // translation
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();
105
106 // rotation
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();
111
112 if (is_static) {
113 static_msgs_.push_back(trans_stamped);
114 }
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();
119 }
120 }
121}
122
123bool Buffer::GetLatestStaticTF(const std::string& frame_id,
124 const std::string& child_frame_id,
125 TransformStamped* tf) {
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));
131 return true;
132 }
133 }
134 return false;
135}
136
137void Buffer::TF2MsgToCyber(
138 const geometry_msgs::TransformStamped& tf2_trans_stamped,
139 TransformStamped& trans_stamped) const {
140 // header
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);
145
146 // child_frame_id
147 trans_stamped.set_child_frame_id(tf2_trans_stamped.child_frame_id);
148
149 // translation
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);
156
157 // rotation
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);
166}
167
168TransformStamped Buffer::lookupTransform(const std::string& target_frame,
169 const std::string& source_frame,
170 const cyber::Time& time,
171 const float timeout_second) const {
172 tf2::Time tf2_time(time.ToNanosecond());
173 geometry_msgs::TransformStamped tf2_trans_stamped =
174 tf2::BufferCore::lookupTransform(target_frame, source_frame, tf2_time);
175 TransformStamped trans_stamped;
176 TF2MsgToCyber(tf2_trans_stamped, trans_stamped);
177 return trans_stamped;
178}
179
180TransformStamped Buffer::lookupTransform(const std::string& target_frame,
181 const cyber::Time& target_time,
182 const std::string& source_frame,
183 const cyber::Time& source_time,
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(),
188 source_frame, source_time.ToNanosecond(),
189 fixed_frame);
190 TransformStamped trans_stamped;
191 TF2MsgToCyber(tf2_trans_stamped, trans_stamped);
192 return trans_stamped;
193}
194
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);
201 uint64_t start_time = Clock::Now().ToNanosecond(); // time.ToNanosecond();
202 while (Clock::Now().ToNanosecond() < start_time + timeout_ns &&
203 !cyber::IsShutdown()) {
204 errstr->clear();
205 bool retval = tf2::BufferCore::canTransform(target_frame, source_frame,
206 time.ToNanosecond(), errstr);
207 if (retval) {
208 return true;
209 } else {
210 if (!cyber::common::GlobalData::Instance()->IsRealityMode()) {
211 break;
212 }
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));
216 }
217 }
218 *errstr = *errstr + ":timeout";
219 return false;
220}
221
222bool Buffer::canTransform(const std::string& target_frame,
223 const cyber::Time& target_time,
224 const std::string& source_frame,
225 const cyber::Time& source_time,
226 const std::string& fixed_frame,
227 const float timeout_second,
228 std::string* errstr) const {
229 // poll for transform if timeout is set
230 uint64_t timeout_ns =
231 static_cast<uint64_t>(timeout_second * kSecondToNanoFactor);
232 uint64_t start_time = Clock::Now().ToNanosecond();
233 while (Clock::Now().ToNanosecond() < start_time + timeout_ns &&
234 !cyber::IsShutdown()) { // Make sure we haven't been stopped
235 errstr->clear();
236 bool retval = tf2::BufferCore::canTransform(
237 target_frame, target_time.ToNanosecond(), source_frame,
238 source_time.ToNanosecond(), fixed_frame, errstr);
239 if (retval) {
240 return true;
241 } else {
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()) {
246 Clock::SetNow(Time(Clock::Now().ToNanosecond() +
247 sleep_time_ms * kMilliToNanoFactor));
248 }
249 }
250 }
251 *errstr = *errstr + ":timeout";
252 return false;
253}
254
255} // namespace transform
256} // namespace apollo
::apollo::cyber::Time Time
Definition buffer.cc:25
Set the behavior of the
a singleton clock that can be used to get the current timestamp.
Definition clock.h:39
static void SetNow(const Time &now)
This is for mock clock mode only.
Definition clock.cc:82
static Time Now()
PRECISION >= 1000000 means the precision is at least 1us.
Definition clock.cc:40
Cyber has builtin time type Time.
Definition time.h:31
uint64_t ToNanosecond() const
convert time to nanosecond.
Definition time.cc:83
static Time Now()
get the current time.
Definition time.cc:57
static const QosProfile QOS_PROFILE_TF_STATIC
#define AERROR
Definition log.h:44
#define AINFO
Definition log.h:42
#define AWARN
Definition log.h:43
bool Init(const char *binary_name, const std::string &dag_info)
Definition init.cc:98
class register implement
Definition arena_queue.h:37