[ROS]一些传感器数据读取融合问题的思考

思考问题：

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泛型类执行的基本流程。

 /**
   * \brief 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);
  }

 

  /**
   * \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<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
TF_MESSAGE_FILTER

 

 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中的消息发布与订阅机制为啥不需要线程锁？

 

posted @ 2017-05-06 20:42  太一吾鱼水  阅读(5986)  评论(2)  编辑  收藏  举报