Apollo 10.0
自动驾驶开放平台
apollo::transform::Buffer类 参考

#include <buffer.h>

类 apollo::transform::Buffer 继承关系图:
apollo::transform::Buffer 的协作图:

Public 成员函数

int Init ()
 Constructor for a Buffer object
 
virtual TransformStamped lookupTransform (const std::string &target_frame, const std::string &source_frame, const cyber::Time &time, const float timeout_second=0.01f) const
 Get the transform between two frames by frame ID.
 
virtual TransformStamped lookupTransform (const std::string &target_frame, const cyber::Time &target_time, const std::string &source_frame, const cyber::Time &source_time, const std::string &fixed_frame, const float timeout_second=0.01f) const
 Get the transform between two frames by frame ID assuming fixed frame.
 
virtual bool canTransform (const std::string &target_frame, const std::string &source_frame, const cyber::Time &target_time, const float timeout_second=0.01f, std::string *errstr=nullptr) const
 Test if a transform is possible
 
virtual bool canTransform (const std::string &target_frame, const cyber::Time &target_time, const std::string &source_frame, const cyber::Time &source_time, const std::string &fixed_frame, const float timeout_second=0.01f, std::string *errstr=nullptr) const
 Test if a transform is possible
 
bool GetLatestStaticTF (const std::string &frame_id, const std::string &child_frame_id, TransformStamped *tf)
 
- Public 成员函数 继承自 apollo::transform::BufferInterface
template<typename T >
T & transform (const T &in, T &out, const std::string &target_frame, float timeout=0.0f) const
 
template<typename T >
transform (const T &in, const std::string &target_frame, float timeout=0.0f) const
 
template<typename A , typename B >
B & transform (const A &in, B &out, const std::string &target_frame, float timeout=0.0f) const
 
template<typename T >
T & transform (const T &in, T &out, const std::string &target_frame, const cyber::Time &target_time, const std::string &fixed_frame, float timeout=0.0f) const
 
template<typename T >
transform (const T &in, const std::string &target_frame, const cyber::Time &target_time, const std::string &fixed_frame, float timeout=0.0f) const
 
template<typename A , typename B >
B & transform (const A &in, B &out, const std::string &target_frame, const cyber::Time &target_time, const std::string &fixed_frame, float timeout=0.0f) const
 

详细描述

在文件 buffer.h33 行定义.

成员函数说明

◆ canTransform() [1/2]

bool apollo::transform::Buffer::canTransform ( const std::string &  target_frame,
const cyber::Time target_time,
const std::string &  source_frame,
const cyber::Time source_time,
const std::string &  fixed_frame,
const float  timeout_second = 0.01f,
std::string *  errstr = nullptr 
) const
virtual

Test if a transform is possible

参数
target_frameThe frame into which to transform
target_timeThe time into which to transform
source_frameThe frame from which to transform
source_timeThe time from which to transform
fixed_frameThe frame in which to treat the transform as constant in time
timeoutHow long to block before failing
errstrA pointer to a string which will be filled with why the transform failed, if not nullptr
返回
True if the transform is possible, false otherwise

实现了 apollo::transform::BufferInterface.

在文件 buffer.cc222 行定义.

228 {
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}
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
uint64_t ToNanosecond() const
convert time to nanosecond.
Definition time.cc:83
#define AWARN
Definition log.h:43
bool IsShutdown()
Definition state.h:46
::apollo::cyber::Time Time

◆ canTransform() [2/2]

bool apollo::transform::Buffer::canTransform ( const std::string &  target_frame,
const std::string &  source_frame,
const cyber::Time target_time,
const float  timeout_second = 0.01f,
std::string *  errstr = nullptr 
) const
virtual

Test if a transform is possible

参数
target_frameThe frame into which to transform
source_frameThe frame from which to transform
target_timeThe time at which to transform
timeoutHow long to block before failing
errstrA pointer to a string which will be filled with why the transform failed, if not nullptr
返回
True if the transform is possible, false otherwise

实现了 apollo::transform::BufferInterface.

在文件 buffer.cc195 行定义.

198 {
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 &&
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}

◆ GetLatestStaticTF()

bool apollo::transform::Buffer::GetLatestStaticTF ( const std::string &  frame_id,
const std::string &  child_frame_id,
TransformStamped tf 
)

在文件 buffer.cc123 行定义.

125 {
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}

◆ Init()

int apollo::transform::Buffer::Init ( )

Constructor for a Buffer object

参数
cache_timeHow long to keep a history of transforms
debugWhether to advertise the view_frames service that exposes debugging information from the buffer
返回

在文件 buffer.cc38 行定义.

38 {
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}
static Time Now()
get the current time.
Definition time.cc:57
static const QosProfile QOS_PROFILE_TF_STATIC
std::unique_ptr< Node > CreateNode(const std::string &node_name, const std::string &name_space)
Definition cyber.cc:33

◆ lookupTransform() [1/2]

TransformStamped apollo::transform::Buffer::lookupTransform ( const std::string &  target_frame,
const cyber::Time target_time,
const std::string &  source_frame,
const cyber::Time source_time,
const std::string &  fixed_frame,
const float  timeout_second = 0.01f 
) const
virtual

Get the transform between two frames by frame ID assuming fixed frame.

参数
target_frameThe frame to which data should be transformed
target_timeThe time to which the data should be transformed. (0 will get the latest)
source_frameThe frame where the data originated
source_timeThe time at which the source_frame should be evaluated. (0 will get the latest)
fixed_frameThe frame in which to assume the transform is constant in time.
timeoutHow long to block before failing
返回
The transform between the frames

Possible exceptions tf2::LookupException, tf2::ConnectivityException, tf2::ExtrapolationException, tf2::InvalidArgumentException

实现了 apollo::transform::BufferInterface.

在文件 buffer.cc180 行定义.

185 {
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}

◆ lookupTransform() [2/2]

TransformStamped apollo::transform::Buffer::lookupTransform ( const std::string &  target_frame,
const std::string &  source_frame,
const cyber::Time time,
const float  timeout_second = 0.01f 
) const
virtual

Get the transform between two frames by frame ID.

参数
target_frameThe frame to which data should be transformed
source_frameThe frame where the data originated
timeThe time at which the value of the transform is desired. (0 will get the latest)
timeoutHow long to block before failing
返回
The transform between the frames

Possible exceptions tf2::LookupException, tf2::ConnectivityException, tf2::ExtrapolationException, tf2::InvalidArgumentException

实现了 apollo::transform::BufferInterface.

在文件 buffer.cc168 行定义.

171 {
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}

该类的文档由以下文件生成: