思考问题：

1. 如何实现传感器数据的融合，或者说时间同步？ 比如里程计读数和雷达数据融合？

void SlamGMapping::startLiveSlam(){  entropy_publisher_ = private_nh_.advertise
     
      ("entropy", 1, true);  sst_ = node_.advertise
      
       ("map", 1, true);  sstm_ = node_.advertise
       
        ("map_metadata", 1, true);  ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);  scan_filter_sub_ = new message_filters::Subscriber
        
         (node_, "scan", 5);  scan_filter_ = new tf::MessageFilter
         
          (*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 is a utility library for use with  and . 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泛型类执行的基本流程。

/**   * \brief Connect this filter's input to another filter's output.  If this filter is already connected, disconnects first.   */  template
     
        void connectInput(F& f)  {    message_connection_.disconnect();    message_connection_ = f.registerCallback(&MessageFilter::incomingMessage, this);  }
     

 

void incomingMessage(const ros::MessageEvent
     
      & evt)  {    add(evt);  }
     

 

/**   * \brief Manually add a message into this filter.   * \note 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
     
      
        > header(new std::map
       
        );    (*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
     
      ::value(*message)); 11     ros::Time stamp = mt::TimeStamp
      
       ::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
       
        ::value(*front.event.getMessage())).c_str(), mt::TimeStamp
        
         ::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 list108       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 
      
          36 #include 
       
           37 #include 
        
            38    39 #include 
         
             40 #include 
          
              41 #include 
           
             42 #include 
            
              43 #include 
             
               44 #include 
              
                45 #include 
               
                 46 #include 
                
                  47 #include 
                 
                   48 49 #include 
                  
                    50 51 #include 
                   
                     52 #include 
                    
                      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
                     
                      & 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
                      
                        106 class MessageFilter : public MessageFilterBase, public message_filters::SimpleFilter
                       
                         107 { 108 public: 109 typedef boost::shared_ptr
                        
                          MConstPtr; 110 typedef ros::MessageEvent
                         
                           MEvent; 111 typedef boost::function
                          
                            FailureCallback; 112 typedef boost::signals2::signal
                           
                             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
                            
                              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
                             
                               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
                              
                                frames; 193 frames.push_back(target_frame); 194 setTargetFrames(frames); 195 } 196 200 void setTargetFrames(const std::vector
                               
                                & 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
                                
                                 ::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
                                 
                                  ::value(*front.getMessage()).c_str(), 264 ros::message_traits::TimeStamp
                                  
                                   ::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
                                   
                                    ::value(*evt.getMessage()).c_str(), 279 ros::message_traits::TimeStamp
                                    
                                     ::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
                                     
                                      
                                        > header(new std::map
                                       
                                        ); 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
                                        
                                          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
                                         
                                          ::value(*message); 346 ros::Time stamp = ros::message_traits::TimeStamp
                                          
                                           ::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
                                           
                                            ::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
                                            
                                             ::value(*message).c_str(), 382 ros::message_traits::TimeStamp
                                             
                                              ::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
                                              
                                               ::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
                                               
                                                & 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
                                                
                                                  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 loop13          (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   try25   {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中的消息发布与订阅机制为啥不需要线程锁？