思考问题：
1. 如何实现传感器数据的融合，或者说时间同步？ 比如里程计读数和雷达数据融合？
void SlamGMapping::startLiveSlam()
{
entropy_publisher_ = private_nh_.advertise<std_msgs::Float64>("entropy", 1, true);
sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);
scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);
scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));
transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));
}
ROS中message_filters 和 tf::MessageFilter 需要关注一下。
 |
 |
message_filters is a utility library for use with roscpp and rospy. It collects commonly used message "filtering" algorithms into a common space. A message filter is defined as something which a message arrives into and may or may not be spit back out of at a later point in time.
An example is the time synchronizer, which takes in messages of different types from multiple sources, and outputs them only if it has received a message on each of those sources with the same timestamp.
以下为MessageFilter泛型类执行的基本流程。
/**
* rief Connect this filter's input to another filter's output. If this filter is already connected, disconnects first.
*/
template<class F> void connectInput(F& f)
{
message_connection_.disconnect();
message_connection_ = f.registerCallback(&MessageFilter::incomingMessage, this);
}
void incomingMessage(const ros::MessageEvent<M const>& evt)
{
add(evt);
}
/**
* rief Manually add a message into this filter.
*
ote If the message (or any other messages in the queue) are immediately transformable this will immediately call through to the output callback, possibly
* multiple times
*/
void add(const MConstPtr& message)
{
boost::shared_ptr<std::map<std::string, std::string> > header(new std::map<std::string, std::string>);
(*header)["callerid"] = "unknown";
add(MEvent(message, header, ros::Time::now()));
}
1 void add(const MEvent& evt) 2 { 3 if (target_frames_.empty()) 4 { 5 return; 6 } 7 8 namespace mt = ros::message_traits; 9 const MConstPtr& message = evt.getMessage(); 10 std::string frame_id = stripSlash(mt::FrameId<M>::value(*message)); 11 ros::Time stamp = mt::TimeStamp<M>::value(*message); 12 13 if (frame_id.empty()) 14 { 15 messageDropped(evt, filter_failure_reasons::EmptyFrameID); 16 return; 17 } 18 19 // iterate through the target frames and add requests for each of them 20 MessageInfo info; 21 info.handles.reserve(expected_success_count_); 22 { 23 V_string target_frames_copy; 24 // Copy target_frames_ to avoid deadlock from #79 25 { 26 boost::mutex::scoped_lock frames_lock(target_frames_mutex_); 27 target_frames_copy = target_frames_; 28 } 29 30 V_string::iterator it = target_frames_copy.begin(); 31 V_string::iterator end = target_frames_copy.end(); 32 for (; it != end; ++it) 33 { 34 const std::string& target_frame = *it; 35 tf2::TransformableRequestHandle handle = bc_.addTransformableRequest(callback_handle_, target_frame, frame_id, stamp); 36 if (handle == 0xffffffffffffffffULL) // never transformable 37 { 38 messageDropped(evt, filter_failure_reasons::OutTheBack); 39 return; 40 } 41 else if (handle == 0) 42 { 43 ++info.success_count; 44 } 45 else 46 { 47 info.handles.push_back(handle); 48 } 49 50 if (!time_tolerance_.isZero()) 51 { 52 handle = bc_.addTransformableRequest(callback_handle_, target_frame, frame_id, stamp + time_tolerance_); 53 if (handle == 0xffffffffffffffffULL) // never transformable 54 { 55 messageDropped(evt, filter_failure_reasons::OutTheBack); 56 return; 57 } 58 else if (handle == 0) 59 { 60 ++info.success_count; 61 } 62 else 63 { 64 info.handles.push_back(handle); 65 } 66 } 67 } 68 } 69 70 71 // We can transform already 72 if (info.success_count == expected_success_count_) 73 { 74 messageReady(evt); 75 } 76 else 77 { 78 // If this message is about to push us past our queue size, erase the oldest message 79 if (queue_size_ != 0 && message_count_ + 1 > queue_size_) 80 { 81 // While we're using the reference keep a shared lock on the messages. 82 boost::shared_lock< boost::shared_mutex > shared_lock(messages_mutex_); 83 84 ++dropped_message_count_; 85 const MessageInfo& front = messages_.front(); 86 TF2_ROS_MESSAGEFILTER_DEBUG("Removed oldest message because buffer is full, count now %d (frame_id=%s, stamp=%f)", message_count_, 87 (mt::FrameId<M>::value(*front.event.getMessage())).c_str(), mt::TimeStamp<M>::value(*front.event.getMessage()).toSec()); 88 89 V_TransformableRequestHandle::const_iterator it = front.handles.begin(); 90 V_TransformableRequestHandle::const_iterator end = front.handles.end(); 91 for (; it != end; ++it) 92 { 93 bc_.cancelTransformableRequest(*it); 94 } 95 96 messageDropped(front.event, filter_failure_reasons::Unknown); 97 // Unlock the shared lock and get a unique lock. Upgradeable lock is used in transformable. 98 // There can only be one upgrade lock. It's important the cancelTransformableRequest not deadlock with transformable. 99 // They both require the transformable_requests_mutex_ in BufferCore. 100 shared_lock.unlock(); 101 // There is a very slight race condition if an older message arrives in this gap. 102 boost::unique_lock< boost::shared_mutex > unique_lock(messages_mutex_); 103 messages_.pop_front(); 104 --message_count_; 105 } 106 107 // Add the message to our list 108 info.event = evt; 109 // Lock access to the messages_ before modifying them. 110 boost::unique_lock< boost::shared_mutex > unique_lock(messages_mutex_); 111 messages_.push_back(info); 112 ++message_count_; 113 } 114 115 TF2_ROS_MESSAGEFILTER_DEBUG("Added message in frame %s at time %.3f, count now %d", frame_id.c_str(), stamp.toSec(), message_count_); 116 117 ++incoming_message_count_; 118 }
完整的类代码：http://docs.ros.org/api/tf/html/c++/message__filter_8h_source.html
1 /* 2 * Copyright (c) 2008, Willow Garage, Inc. 3 * All rights reserved. 4 * 5 * Redistribution and use in source and binary forms, with or without 6 * modification, are permitted provided that the following conditions are met: 7 * 8 * * Redistributions of source code must retain the above copyright 9 * notice, this list of conditions and the following disclaimer. 10 * * Redistributions in binary form must reproduce the above copyright 11 * notice, this list of conditions and the following disclaimer in the 12 * documentation and/or other materials provided with the distribution. 13 * * Neither the name of the Willow Garage, Inc. nor the names of its 14 * contributors may be used to endorse or promote products derived from 15 * this software without specific prior written permission. 16 * 17 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 * POSSIBILITY OF SUCH DAMAGE. 28 */ 29 32 #ifndef TF_MESSAGE_FILTER_H 33 #define TF_MESSAGE_FILTER_H 34 35 #include <ros/ros.h> 36 #include <tf/transform_listener.h> 37 #include <tf/tfMessage.h> 38 39 #include <string> 40 #include <list> 41 #include <vector> 42 #include <boost/function.hpp> 43 #include <boost/bind.hpp> 44 #include <boost/shared_ptr.hpp> 45 #include <boost/weak_ptr.hpp> 46 #include <boost/thread.hpp> 47 #include <boost/signals2.hpp> 48 49 #include <ros/callback_queue.h> 50 51 #include <message_filters/connection.h> 52 #include <message_filters/simple_filter.h> 53 54 #define TF_MESSAGEFILTER_DEBUG(fmt, ...) 55 ROS_DEBUG_NAMED("message_filter", "MessageFilter [target=%s]: " fmt, getTargetFramesString().c_str(), __VA_ARGS__) 56 57 #define TF_MESSAGEFILTER_WARN(fmt, ...) 58 ROS_WARN_NAMED("message_filter", "MessageFilter [target=%s]: " fmt, getTargetFramesString().c_str(), __VA_ARGS__) 59 60 namespace tf 61 { 62 63 namespace filter_failure_reasons 64 { 65 enum FilterFailureReason 66 { 68 Unknown, 70 OutTheBack, 72 EmptyFrameID, 73 }; 74 } 75 typedef filter_failure_reasons::FilterFailureReason FilterFailureReason; 76 77 class MessageFilterBase 78 { 79 public: 80 virtual ~MessageFilterBase(){} 81 virtual void clear() = 0; 82 virtual void setTargetFrame(const std::string& target_frame) = 0; 83 virtual void setTargetFrames(const std::vector<std::string>& target_frames) = 0; 84 virtual void setTolerance(const ros::Duration& tolerance) = 0; 85 virtual void setQueueSize( uint32_t new_queue_size ) = 0; 86 virtual uint32_t getQueueSize() = 0; 87 }; 88 105 template<class M> 106 class MessageFilter : public MessageFilterBase, public message_filters::SimpleFilter<M> 107 { 108 public: 109 typedef boost::shared_ptr<M const> MConstPtr; 110 typedef ros::MessageEvent<M const> MEvent; 111 typedef boost::function<void(const MConstPtr&, FilterFailureReason)> FailureCallback; 112 typedef boost::signals2::signal<void(const MConstPtr&, FilterFailureReason)> FailureSignal; 113 123 MessageFilter(Transformer& tf, const std::string& target_frame, uint32_t queue_size, ros::NodeHandle nh = ros::NodeHandle(), ros::Duration max_rate = ros::Duration(0.01)) 124 : tf_(tf) 125 , nh_(nh) 126 , max_rate_(max_rate) 127 , queue_size_(queue_size) 128 { 129 init(); 130 131 setTargetFrame(target_frame); 132 } 133 144 template<class F> 145 MessageFilter(F& f, Transformer& tf, const std::string& target_frame, uint32_t queue_size, ros::NodeHandle nh = ros::NodeHandle(), ros::Duration max_rate = ros::Duration(0.01)) 146 : tf_(tf) 147 , nh_(nh) 148 , max_rate_(max_rate) 149 , queue_size_(queue_size) 150 { 151 init(); 152 153 setTargetFrame(target_frame); 154 155 connectInput(f); 156 } 157 161 template<class F> 162 void connectInput(F& f) 163 { 164 message_connection_.disconnect(); 165 message_connection_ = f.registerCallback(&MessageFilter::incomingMessage, this); 166 } 167 171 ~MessageFilter() 172 { 173 // Explicitly stop callbacks; they could execute after we're destroyed 174 max_rate_timer_.stop(); 175 message_connection_.disconnect(); 176 tf_.removeTransformsChangedListener(tf_connection_); 177 178 clear(); 179 180 TF_MESSAGEFILTER_DEBUG("Successful Transforms: %llu, Failed Transforms: %llu, Discarded due to age: %llu, Transform messages received: %llu, Messages received: %llu, Total dropped: %llu", 181 (long long unsigned int)successful_transform_count_, (long long unsigned int)failed_transform_count_, 182 (long long unsigned int)failed_out_the_back_count_, (long long unsigned int)transform_message_count_, 183 (long long unsigned int)incoming_message_count_, (long long unsigned int)dropped_message_count_); 184 185 } 186 190 void setTargetFrame(const std::string& target_frame) 191 { 192 std::vector<std::string> frames; 193 frames.push_back(target_frame); 194 setTargetFrames(frames); 195 } 196 200 void setTargetFrames(const std::vector<std::string>& target_frames) 201 { 202 boost::mutex::scoped_lock list_lock(messages_mutex_); 203 boost::mutex::scoped_lock string_lock(target_frames_string_mutex_); 204 205 target_frames_ = target_frames; 206 207 std::stringstream ss; 208 for (std::vector<std::string>::iterator it = target_frames_.begin(); it != target_frames_.end(); ++it) 209 { 210 ss << *it << " "; 211 } 212 target_frames_string_ = ss.str(); 213 } 217 std::string getTargetFramesString() 218 { 219 boost::mutex::scoped_lock lock(target_frames_string_mutex_); 220 return target_frames_string_; 221 }; 222 226 void setTolerance(const ros::Duration& tolerance) 227 { 228 time_tolerance_ = tolerance; 229 } 230 234 void clear() 235 { 236 boost::mutex::scoped_lock lock(messages_mutex_); 237 238 TF_MESSAGEFILTER_DEBUG("%s", "Cleared"); 239 240 messages_.clear(); 241 message_count_ = 0; 242 243 warned_about_unresolved_name_ = false; 244 warned_about_empty_frame_id_ = false; 245 } 246 247 void add(const MEvent& evt) 248 { 249 boost::mutex::scoped_lock lock(messages_mutex_); 250 251 testMessages(); 252 253 if (!testMessage(evt)) 254 { 255 // If this message is about to push us past our queue size, erase the oldest message 256 if (queue_size_ != 0 && message_count_ + 1 > queue_size_) 257 { 258 ++dropped_message_count_; 259 const MEvent& front = messages_.front(); 260 TF_MESSAGEFILTER_DEBUG( 261 "Removed oldest message because buffer is full, count now %d (frame_id=%s, stamp=%f)", 262 message_count_, 263 ros::message_traits::FrameId<M>::value(*front.getMessage()).c_str(), 264 ros::message_traits::TimeStamp<M>::value(*front.getMessage()).toSec()); 265 signalFailure(messages_.front(), filter_failure_reasons::Unknown); 266 267 messages_.pop_front(); 268 --message_count_; 269 } 270 271 // Add the message to our list 272 messages_.push_back(evt); 273 ++message_count_; 274 } 275 276 TF_MESSAGEFILTER_DEBUG( 277 "Added message in frame %s at time %.3f, count now %d", 278 ros::message_traits::FrameId<M>::value(*evt.getMessage()).c_str(), 279 ros::message_traits::TimeStamp<M>::value(*evt.getMessage()).toSec(), 280 message_count_); 281 282 ++incoming_message_count_; 283 } 284 290 void add(const MConstPtr& message) 291 { 292 293 boost::shared_ptr<std::map<std::string, std::string> > header(new std::map<std::string, std::string>); 294 (*header)["callerid"] = "unknown"; 295 add(MEvent(message, header, ros::Time::now())); 296 } 297 302 message_filters::Connection registerFailureCallback(const FailureCallback& callback) 303 { 304 boost::mutex::scoped_lock lock(failure_signal_mutex_); 305 return message_filters::Connection(boost::bind(&MessageFilter::disconnectFailure, this, _1), failure_signal_.connect(callback)); 306 } 307 308 virtual void setQueueSize( uint32_t new_queue_size ) 309 { 310 queue_size_ = new_queue_size; 311 } 312 313 virtual uint32_t getQueueSize() 314 { 315 return queue_size_; 316 } 317 318 private: 319 320 void init() 321 { 322 message_count_ = 0; 323 new_transforms_ = false; 324 successful_transform_count_ = 0; 325 failed_transform_count_ = 0; 326 failed_out_the_back_count_ = 0; 327 transform_message_count_ = 0; 328 incoming_message_count_ = 0; 329 dropped_message_count_ = 0; 330 time_tolerance_ = ros::Duration(0.0); 331 warned_about_unresolved_name_ = false; 332 warned_about_empty_frame_id_ = false; 333 334 tf_connection_ = tf_.addTransformsChangedListener(boost::bind(&MessageFilter::transformsChanged, this)); 335 336 max_rate_timer_ = nh_.createTimer(max_rate_, &MessageFilter::maxRateTimerCallback, this); 337 } 338 339 typedef std::list<MEvent> L_Event; 340 341 bool testMessage(const MEvent& evt) 342 { 343 const MConstPtr& message = evt.getMessage(); 344 std::string callerid = evt.getPublisherName(); 345 std::string frame_id = ros::message_traits::FrameId<M>::value(*message); 346 ros::Time stamp = ros::message_traits::TimeStamp<M>::value(*message); 347 348 //Throw out messages which have an empty frame_id 349 if (frame_id.empty()) 350 { 351 if (!warned_about_empty_frame_id_) 352 { 353 warned_about_empty_frame_id_ = true; 354 TF_MESSAGEFILTER_WARN("Discarding message from [%s] due to empty frame_id. This message will only print once.", callerid.c_str()); 355 } 356 signalFailure(evt, filter_failure_reasons::EmptyFrameID); 357 return true; 358 } 359 360 361 //Throw out messages which are too old 363 for (std::vector<std::string>::iterator target_it = target_frames_.begin(); target_it != target_frames_.end(); ++target_it) 364 { 365 const std::string& target_frame = *target_it; 366 367 if (target_frame != frame_id && stamp != ros::Time(0)) 368 { 369 ros::Time latest_transform_time ; 370 371 tf_.getLatestCommonTime(frame_id, target_frame, latest_transform_time, 0) ; 372 373 if (stamp + tf_.getCacheLength() < latest_transform_time) 374 { 375 ++failed_out_the_back_count_; 376 ++dropped_message_count_; 377 TF_MESSAGEFILTER_DEBUG( 378 "Discarding Message, in frame %s, Out of the back of Cache Time " 379 "(stamp: %.3f + cache_length: %.3f < latest_transform_time %.3f. " 380 "Message Count now: %d", 381 ros::message_traits::FrameId<M>::value(*message).c_str(), 382 ros::message_traits::TimeStamp<M>::value(*message).toSec(), 383 tf_.getCacheLength().toSec(), latest_transform_time.toSec(), message_count_); 384 385 last_out_the_back_stamp_ = stamp; 386 last_out_the_back_frame_ = frame_id; 387 388 signalFailure(evt, filter_failure_reasons::OutTheBack); 389 return true; 390 } 391 } 392 393 } 394 395 bool ready = !target_frames_.empty(); 396 for (std::vector<std::string>::iterator target_it = target_frames_.begin(); ready && target_it != target_frames_.end(); ++target_it) 397 { 398 std::string& target_frame = *target_it; 399 if (time_tolerance_ != ros::Duration(0.0)) 400 { 401 ready = ready && (tf_.canTransform(target_frame, frame_id, stamp) && 402 tf_.canTransform(target_frame, frame_id, stamp + time_tolerance_) ); 403 } 404 else 405 { 406 ready = ready && tf_.canTransform(target_frame, frame_id, stamp); 407 } 408 } 409 410 if (ready) 411 { 412 TF_MESSAGEFILTER_DEBUG("Message ready in frame %s at time %.3f, count now %d", frame_id.c_str(), stamp.toSec(), message_count_); 413 414 ++successful_transform_count_; 415 416 this->signalMessage(evt); 417 } 418 else 419 { 420 ++failed_transform_count_; 421 } 422 423 return ready; 424 } 425 426 void testMessages() 427 { 428 if (!messages_.empty() && getTargetFramesString() == " ") 429 { 430 ROS_WARN_NAMED("message_notifier", "MessageFilter [target=%s]: empty target frame", getTargetFramesString().c_str()); 431 } 432 433 int i = 0; 434 435 typename L_Event::iterator it = messages_.begin(); 436 for (; it != messages_.end(); ++i) 437 { 438 MEvent& evt = *it; 439 440 if (testMessage(evt)) 441 { 442 --message_count_; 443 it = messages_.erase(it); 444 } 445 else 446 { 447 ++it; 448 } 449 } 450 } 451 452 void maxRateTimerCallback(const ros::TimerEvent&) 453 { 454 boost::mutex::scoped_lock list_lock(messages_mutex_); 455 if (new_transforms_) 456 { 457 testMessages(); 458 new_transforms_ = false; 459 } 460 461 checkFailures(); 462 } 463 467 void incomingMessage(const ros::MessageEvent<M const>& evt) 468 { 469 add(evt); 470 } 471 472 void transformsChanged() 473 { 474 new_transforms_ = true; 475 476 ++transform_message_count_; 477 } 478 479 void checkFailures() 480 { 481 if (next_failure_warning_.isZero()) 482 { 483 next_failure_warning_ = ros::Time::now() + ros::Duration(15); 484 } 485 486 if (ros::Time::now() >= next_failure_warning_) 487 { 488 if (incoming_message_count_ - message_count_ == 0) 489 { 490 return; 491 } 492 493 double dropped_pct = (double)dropped_message_count_ / (double)(incoming_message_count_ - message_count_); 494 if (dropped_pct > 0.95) 495 { 496 TF_MESSAGEFILTER_WARN("Dropped %.2f%% of messages so far. Please turn the [%s.message_notifier] rosconsole logger to DEBUG for more information.", dropped_pct*100, ROSCONSOLE_DEFAULT_NAME); 497 next_failure_warning_ = ros::Time::now() + ros::Duration(60); 498 499 if ((double)failed_out_the_back_count_ / (double)dropped_message_count_ > 0.5) 500 { 501 TF_MESSAGEFILTER_WARN(" The majority of dropped messages were due to messages growing older than the TF cache time. The last message's timestamp was: %f, and the last frame_id was: %s", last_out_the_back_stamp_.toSec(), last_out_the_back_frame_.c_str()); 502 } 503 } 504 } 505 } 506 507 void disconnectFailure(const message_filters::Connection& c) 508 { 509 boost::mutex::scoped_lock lock(failure_signal_mutex_); 510 c.getBoostConnection().disconnect(); 511 } 512 513 void signalFailure(const MEvent& evt, FilterFailureReason reason) 514 { 515 boost::mutex::scoped_lock lock(failure_signal_mutex_); 516 failure_signal_(evt.getMessage(), reason); 517 } 518 519 Transformer& tf_; 520 ros::NodeHandle nh_; 521 ros::Duration max_rate_; 522 ros::Timer max_rate_timer_; 523 std::vector<std::string> target_frames_; 524 std::string target_frames_string_; 525 boost::mutex target_frames_string_mutex_; 526 uint32_t queue_size_; 527 528 L_Event messages_; 529 uint32_t message_count_; 530 boost::mutex messages_mutex_; 531 532 bool new_messages_; 533 volatile bool new_transforms_; 534 535 bool warned_about_unresolved_name_; 536 bool warned_about_empty_frame_id_; 537 538 uint64_t successful_transform_count_; 539 uint64_t failed_transform_count_; 540 uint64_t failed_out_the_back_count_; 541 uint64_t transform_message_count_; 542 uint64_t incoming_message_count_; 543 uint64_t dropped_message_count_; 544 545 ros::Time last_out_the_back_stamp_; 546 std::string last_out_the_back_frame_; 547 548 ros::Time next_failure_warning_; 549 550 ros::Duration time_tolerance_; 551 552 boost::signals2::connection tf_connection_; 553 message_filters::Connection message_connection_; 554 555 FailureSignal failure_signal_; 556 boost::mutex failure_signal_mutex_; 557 }; 558 559 560 } // namespace tf 561 562 #endif 563
1 bool 2 Buffer::canTransform(const std::string& target_frame, const std::string& source_frame, 3 const ros::Time& time, const ros::Duration timeout, std::string* errstr) const 4 { 5 if (!checkAndErrorDedicatedThreadPresent(errstr)) 6 return false; 7 8 // poll for transform if timeout is set 9 ros::Time start_time = now_fallback_to_wall(); 10 while (now_fallback_to_wall() < start_time + timeout && 11 !canTransform(target_frame, source_frame, time) && 12 (now_fallback_to_wall()+ros::Duration(3.0) >= start_time) && //don't wait when we detect a bag loop 13 (ros::ok() || !ros::isInitialized())) // Make sure we haven't been stopped (won't work for pytf) 14 { 15 sleep_fallback_to_wall(ros::Duration(0.01)); 16 } 17 bool retval = canTransform(target_frame, source_frame, time, errstr); 18 conditionally_append_timeout_info(errstr, start_time, timeout); 19 return retval; 20 } 21 22 ros::Time now_fallback_to_wall() 23 { 24 try 25 { 26 return ros::Time::now(); 27 } 28 catch (ros::TimeNotInitializedException ex) 29 { 30 ros::WallTime wt = ros::WallTime::now(); 31 return ros::Time(wt.sec, wt.nsec); 32 } 33 }
#include "tf2_ros/buffer.h"
2.ROS中的消息发布与订阅机制为啥不需要线程锁？