[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()));
  }

 void add(const MEvent& evt)
   {
     if (target_frames_.empty())
     {
       return;
     }

     namespace mt = ros::message_traits;
     const MConstPtr& message = evt.getMessage();
     std::string frame_id = stripSlash(mt::FrameId<M>::value(*message));
     ros::Time stamp = mt::TimeStamp<M>::value(*message);

     if (frame_id.empty())
     {
       messageDropped(evt, filter_failure_reasons::EmptyFrameID);
       return;
     }

     // iterate through the target frames and add requests for each of them
     MessageInfo info;
     info.handles.reserve(expected_success_count_);
     {
       V_string target_frames_copy;
       // Copy target_frames_ to avoid deadlock from #79
       {
         boost::mutex::scoped_lock frames_lock(target_frames_mutex_);
         target_frames_copy = target_frames_;
       }

       V_string::iterator it = target_frames_copy.begin();
       V_string::iterator end = target_frames_copy.end();
       for (; it != end; ++it)
       {
         const std::string& target_frame = *it;
         tf2::TransformableRequestHandle handle = bc_.addTransformableRequest(callback_handle_, target_frame, frame_id, stamp);
         if (handle == 0xffffffffffffffffULL) // never transformable
         {
           messageDropped(evt, filter_failure_reasons::OutTheBack);
           return;
         }
         else if (handle == )
         {
           ++info.success_count;
         }
         else
         {
           info.handles.push_back(handle);
         }

         if (!time_tolerance_.isZero())
         {
           handle = bc_.addTransformableRequest(callback_handle_, target_frame, frame_id, stamp + time_tolerance_);
           if (handle == 0xffffffffffffffffULL) // never transformable
           {
             messageDropped(evt, filter_failure_reasons::OutTheBack);
             return;
           }
           else if (handle == )
           {
             ++info.success_count;
           }
           else
           {
             info.handles.push_back(handle);
           }
         }
       }
     }

     // We can transform already
     if (info.success_count == expected_success_count_)
     {
       messageReady(evt);
     }
     else
     {
       // If this message is about to push us past our queue size, erase the oldest message
       if (queue_size_ !=  && message_count_ +  > queue_size_)
       {
         // While we're using the reference keep a shared lock on the messages.
         boost::shared_lock< boost::shared_mutex > shared_lock(messages_mutex_);

         ++dropped_message_count_;
         const MessageInfo& front = messages_.front();
         TF2_ROS_MESSAGEFILTER_DEBUG("Removed oldest message because buffer is full, count now %d (frame_id=%s, stamp=%f)", message_count_,
                                 (mt::FrameId<M>::value(*front.event.getMessage())).c_str(), mt::TimeStamp<M>::value(*front.event.getMessage()).toSec());

         V_TransformableRequestHandle::const_iterator it = front.handles.begin();
         V_TransformableRequestHandle::const_iterator end = front.handles.end();
         for (; it != end; ++it)
         {
           bc_.cancelTransformableRequest(*it);
         }

         messageDropped(front.event, filter_failure_reasons::Unknown);
         // Unlock the shared lock and get a unique lock. Upgradeable lock is used in transformable.
         // There can only be one upgrade lock. It's important the cancelTransformableRequest not deadlock with transformable.
         // They both require the transformable_requests_mutex_ in BufferCore.
         shared_lock.unlock();
         // There is a very slight race condition if an older message arrives in this gap.
         boost::unique_lock< boost::shared_mutex > unique_lock(messages_mutex_);
         messages_.pop_front();
          --message_count_;
       }

       // Add the message to our list
       info.event = evt;
       // Lock access to the messages_ before modifying them.
       boost::unique_lock< boost::shared_mutex > unique_lock(messages_mutex_);
       messages_.push_back(info);
       ++message_count_;
     }

     TF2_ROS_MESSAGEFILTER_DEBUG("Added message in frame %s at time %.3f, count now %d", frame_id.c_str(), stamp.toSec(), message_count_);

     ++incoming_message_count_;
   }

完整的类代码：http://docs.ros.org/api/tf/html/c++/message__filter_8h_source.html

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

    #ifndef TF_MESSAGE_FILTER_H
    #define TF_MESSAGE_FILTER_H

    #include <ros/ros.h>
    #include <tf/transform_listener.h>
    #include <tf/tfMessage.h>

    #include <string>
    #include <list>
    #include <vector>
    #include <boost/function.hpp>
    #include <boost/bind.hpp>
    #include <boost/shared_ptr.hpp>
    #include <boost/weak_ptr.hpp>
    #include <boost/thread.hpp>
    #include <boost/signals2.hpp>

    #include <ros/callback_queue.h>

    #include <message_filters/connection.h>
    #include <message_filters/simple_filter.h>

    #define TF_MESSAGEFILTER_DEBUG(fmt, ...) \
      ROS_DEBUG_NAMED("message_filter", "MessageFilter [target=%s]: " fmt, getTargetFramesString().c_str(), __VA_ARGS__)

    #define TF_MESSAGEFILTER_WARN(fmt, ...) \
      ROS_WARN_NAMED("message_filter", "MessageFilter [target=%s]: " fmt, getTargetFramesString().c_str(), __VA_ARGS__)

    namespace tf
    {

    namespace filter_failure_reasons
    {
    enum FilterFailureReason
    {
      Unknown,
      OutTheBack,
      EmptyFrameID,
    };
    }
    typedef filter_failure_reasons::FilterFailureReason FilterFailureReason;

    class MessageFilterBase
    {
    public:
      virtual ~MessageFilterBase(){}
      virtual void clear() = ;
      virtual void setTargetFrame(const std::string& target_frame) = ;
      virtual void setTargetFrames(const std::vector<std::string>& target_frames) = ;
      virtual void setTolerance(const ros::Duration& tolerance) = ;
      virtual void setQueueSize( uint32_t new_queue_size ) = ;
      virtual uint32_t getQueueSize() = ;
    };

   template<class M>
   class MessageFilter : public MessageFilterBase, public message_filters::SimpleFilter<M>
   {
   public:
     typedef boost::shared_ptr<M const> MConstPtr;
     typedef ros::MessageEvent<M const> MEvent;
     typedef boost::function<void(const MConstPtr&, FilterFailureReason)> FailureCallback;
     typedef boost::signals2::signal<void(const MConstPtr&, FilterFailureReason)> FailureSignal;

     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))
     : tf_(tf)
     , nh_(nh)
     , max_rate_(max_rate)
     , queue_size_(queue_size)
     {
       init();

       setTargetFrame(target_frame);
     }

     template<class F>
     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))
     : tf_(tf)
     , nh_(nh)
     , max_rate_(max_rate)
     , queue_size_(queue_size)
     {
       init();

       setTargetFrame(target_frame);

       connectInput(f);
     }

     template<class F>
     void connectInput(F& f)
     {
       message_connection_.disconnect();
       message_connection_ = f.registerCallback(&MessageFilter::incomingMessage, this);
     }

     ~MessageFilter()
     {
       // Explicitly stop callbacks; they could execute after we're destroyed
       max_rate_timer_.stop();
       message_connection_.disconnect();
       tf_.removeTransformsChangedListener(tf_connection_);

       clear();

       TF_MESSAGEFILTER_DEBUG("Successful Transforms: %llu, Failed Transforms: %llu, Discarded due to age: %llu, Transform messages received: %llu, Messages received: %llu, Total dropped: %llu",
                              (long long unsigned int)successful_transform_count_, (long long unsigned int)failed_transform_count_,
                              (long long unsigned int)failed_out_the_back_count_, (long long unsigned int)transform_message_count_,
                              (long long unsigned int)incoming_message_count_, (long long unsigned int)dropped_message_count_);

     }

     void setTargetFrame(const std::string& target_frame)
     {
       std::vector<std::string> frames;
       frames.push_back(target_frame);
       setTargetFrames(frames);
     }

     void setTargetFrames(const std::vector<std::string>& target_frames)
     {
       boost::mutex::scoped_lock list_lock(messages_mutex_);
       boost::mutex::scoped_lock string_lock(target_frames_string_mutex_);

       target_frames_ = target_frames;

       std::stringstream ss;
       for (std::vector<std::string>::iterator it = target_frames_.begin(); it != target_frames_.end(); ++it)
       {
         ss << *it << " ";
       }
       target_frames_string_ = ss.str();
     }
     std::string getTargetFramesString()
     {
       boost::mutex::scoped_lock lock(target_frames_string_mutex_);
       return target_frames_string_;
     };

     void setTolerance(const ros::Duration& tolerance)
     {
       time_tolerance_ = tolerance;
     }

     void clear()
     {
       boost::mutex::scoped_lock lock(messages_mutex_);

       TF_MESSAGEFILTER_DEBUG("%s", "Cleared");

       messages_.clear();
       message_count_ = ;

       warned_about_unresolved_name_ = false;
       warned_about_empty_frame_id_ = false;
     }

     void add(const MEvent& evt)
     {
       boost::mutex::scoped_lock lock(messages_mutex_);

       testMessages();

       if (!testMessage(evt))
       {
         // If this message is about to push us past our queue size, erase the oldest message
         if (queue_size_ !=  && message_count_ +  > queue_size_)
         {
           ++dropped_message_count_;
           const MEvent& front = messages_.front();
           TF_MESSAGEFILTER_DEBUG(
                 "Removed oldest message because buffer is full, count now %d (frame_id=%s, stamp=%f)",
                 message_count_,
                 ros::message_traits::FrameId<M>::value(*front.getMessage()).c_str(),
                 ros::message_traits::TimeStamp<M>::value(*front.getMessage()).toSec());
           signalFailure(messages_.front(), filter_failure_reasons::Unknown);

           messages_.pop_front();
           --message_count_;
         }

         // Add the message to our list
         messages_.push_back(evt);
         ++message_count_;
       }

       TF_MESSAGEFILTER_DEBUG(
             "Added message in frame %s at time %.3f, count now %d",
             ros::message_traits::FrameId<M>::value(*evt.getMessage()).c_str(),
             ros::message_traits::TimeStamp<M>::value(*evt.getMessage()).toSec(),
             message_count_);

       ++incoming_message_count_;
     }

     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()));
     }

     message_filters::Connection registerFailureCallback(const FailureCallback& callback)
     {
       boost::mutex::scoped_lock lock(failure_signal_mutex_);
       return message_filters::Connection(boost::bind(&MessageFilter::disconnectFailure, this, _1), failure_signal_.connect(callback));
     }

     virtual void setQueueSize( uint32_t new_queue_size )
     {
       queue_size_ = new_queue_size;
     }

     virtual uint32_t getQueueSize()
     {
       return queue_size_;
     }

   private:

     void init()
     {
       message_count_ = ;
       new_transforms_ = false;
       successful_transform_count_ = ;
       failed_transform_count_ = ;
       failed_out_the_back_count_ = ;
       transform_message_count_ = ;
       incoming_message_count_ = ;
       dropped_message_count_ = ;
       time_tolerance_ = ros::Duration(0.0);
       warned_about_unresolved_name_ = false;
       warned_about_empty_frame_id_ = false;

       tf_connection_ = tf_.addTransformsChangedListener(boost::bind(&MessageFilter::transformsChanged, this));

       max_rate_timer_ = nh_.createTimer(max_rate_, &MessageFilter::maxRateTimerCallback, this);
     }

     typedef std::list<MEvent> L_Event;

     bool testMessage(const MEvent& evt)
     {
       const MConstPtr& message = evt.getMessage();
       std::string callerid = evt.getPublisherName();
       std::string frame_id = ros::message_traits::FrameId<M>::value(*message);
       ros::Time stamp = ros::message_traits::TimeStamp<M>::value(*message);

       //Throw out messages which have an empty frame_id
       if (frame_id.empty())
       {
         if (!warned_about_empty_frame_id_)
         {
           warned_about_empty_frame_id_ = true;
           TF_MESSAGEFILTER_WARN("Discarding message from [%s] due to empty frame_id.  This message will only print once.", callerid.c_str());
         }
         signalFailure(evt, filter_failure_reasons::EmptyFrameID);
         return true;
       }

       //Throw out messages which are too old
       for (std::vector<std::string>::iterator target_it = target_frames_.begin(); target_it != target_frames_.end(); ++target_it)
       {
         const std::string& target_frame = *target_it;

         if (target_frame != frame_id && stamp != ros::Time())
         {
           ros::Time latest_transform_time ;

           tf_.getLatestCommonTime(frame_id, target_frame, latest_transform_time, ) ;

           if (stamp + tf_.getCacheLength() < latest_transform_time)
           {
             ++failed_out_the_back_count_;
             ++dropped_message_count_;
             TF_MESSAGEFILTER_DEBUG(
                   "Discarding Message, in frame %s, Out of the back of Cache Time "
                   "(stamp: %.3f + cache_length: %.3f < latest_transform_time %.3f. "
                   "Message Count now: %d",
                   ros::message_traits::FrameId<M>::value(*message).c_str(),
                   ros::message_traits::TimeStamp<M>::value(*message).toSec(),
                   tf_.getCacheLength().toSec(), latest_transform_time.toSec(), message_count_);

             last_out_the_back_stamp_ = stamp;
             last_out_the_back_frame_ = frame_id;

             signalFailure(evt, filter_failure_reasons::OutTheBack);
             return true;
           }
         }

       }

       bool ready = !target_frames_.empty();
       for (std::vector<std::string>::iterator target_it = target_frames_.begin(); ready && target_it != target_frames_.end(); ++target_it)
       {
         std::string& target_frame = *target_it;
         if (time_tolerance_ != ros::Duration(0.0))
         {
           ready = ready && (tf_.canTransform(target_frame, frame_id, stamp) &&
                             tf_.canTransform(target_frame, frame_id, stamp + time_tolerance_) );
         }
         else
         {
           ready = ready && tf_.canTransform(target_frame, frame_id, stamp);
         }
       }

       if (ready)
       {
         TF_MESSAGEFILTER_DEBUG("Message ready in frame %s at time %.3f, count now %d", frame_id.c_str(), stamp.toSec(), message_count_);

         ++successful_transform_count_;

         this->signalMessage(evt);
       }
       else
       {
         ++failed_transform_count_;
       }

       return ready;
     }

     void testMessages()
     {
       if (!messages_.empty() && getTargetFramesString() == " ")
       {
         ROS_WARN_NAMED("message_notifier", "MessageFilter [target=%s]: empty target frame", getTargetFramesString().c_str());
       }

       int i = ;

       typename L_Event::iterator it = messages_.begin();
       for (; it != messages_.end(); ++i)
       {
         MEvent& evt = *it;

         if (testMessage(evt))
         {
           --message_count_;
           it = messages_.erase(it);
         }
         else
         {
           ++it;
         }
       }
     }

     void maxRateTimerCallback(const ros::TimerEvent&)
     {
       boost::mutex::scoped_lock list_lock(messages_mutex_);
       if (new_transforms_)
       {
         testMessages();
         new_transforms_ = false;
       }

       checkFailures();
     }

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

     void transformsChanged()
     {
       new_transforms_ = true;

       ++transform_message_count_;
     }

     void checkFailures()
     {
       if (next_failure_warning_.isZero())
       {
         next_failure_warning_ = ros::Time::now() + ros::Duration();
       }

       if (ros::Time::now() >= next_failure_warning_)
       {
         if (incoming_message_count_ - message_count_ == )
         {
           return;
         }

         double dropped_pct = (double)dropped_message_count_ / (double)(incoming_message_count_ - message_count_);
         if (dropped_pct > 0.95)
         {
           TF_MESSAGEFILTER_WARN("Dropped %.2f%% of messages so far. Please turn the [%s.message_notifier] rosconsole logger to DEBUG for more information.", dropped_pct*, ROSCONSOLE_DEFAULT_NAME);
           next_failure_warning_ = ros::Time::now() + ros::Duration();

           if ((double)failed_out_the_back_count_ / (double)dropped_message_count_ > 0.5)
           {
             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());
           }
         }
       }
     }

     void disconnectFailure(const message_filters::Connection& c)
     {
       boost::mutex::scoped_lock lock(failure_signal_mutex_);
       c.getBoostConnection().disconnect();
     }

     void signalFailure(const MEvent& evt, FilterFailureReason reason)
     {
       boost::mutex::scoped_lock lock(failure_signal_mutex_);
       failure_signal_(evt.getMessage(), reason);
     }

     Transformer& tf_;
     ros::NodeHandle nh_;
     ros::Duration max_rate_;
     ros::Timer max_rate_timer_;
     std::vector<std::string> target_frames_;
     std::string target_frames_string_;
     boost::mutex target_frames_string_mutex_;
     uint32_t queue_size_; 

     L_Event messages_;
     uint32_t message_count_;
     boost::mutex messages_mutex_; 

     bool new_messages_;
     volatile bool new_transforms_; 

     bool warned_about_unresolved_name_;
     bool warned_about_empty_frame_id_;

     uint64_t successful_transform_count_;
     uint64_t failed_transform_count_;
     uint64_t failed_out_the_back_count_;
     uint64_t transform_message_count_;
     uint64_t incoming_message_count_;
     uint64_t dropped_message_count_;

     ros::Time last_out_the_back_stamp_;
     std::string last_out_the_back_frame_;

     ros::Time next_failure_warning_;

     ros::Duration time_tolerance_; 

     boost::signals2::connection tf_connection_;
     message_filters::Connection message_connection_;

     FailureSignal failure_signal_;
     boost::mutex failure_signal_mutex_;
   };

   } // namespace tf

   #endif
   

TF_MESSAGE_FILTER

---

 bool
 Buffer::canTransform(const std::string& target_frame, const std::string& source_frame,
                      const ros::Time& time, const ros::Duration timeout, std::string* errstr) const
 {
   if (!checkAndErrorDedicatedThreadPresent(errstr))
     return false;

   // poll for transform if timeout is set
   ros::Time start_time = now_fallback_to_wall();
   while (now_fallback_to_wall() < start_time + timeout &&
          !canTransform(target_frame, source_frame, time) &&
          (now_fallback_to_wall()+ros::Duration(3.0) >= start_time) &&  //don't wait when we detect a bag loop
          (ros::ok() || !ros::isInitialized())) // Make sure we haven't been stopped (won't work for pytf)
     {
       sleep_fallback_to_wall(ros::Duration(0.01));
     }
   bool retval = canTransform(target_frame, source_frame, time, errstr);
   conditionally_append_timeout_info(errstr, start_time, timeout);
   return retval;
 }

 ros::Time now_fallback_to_wall()
 {
   try
   {
     return ros::Time::now();
   }
   catch (ros::TimeNotInitializedException ex)
   {
     ros::WallTime wt = ros::WallTime::now();
     return ros::Time(wt.sec, wt.nsec);
   }
 }

#include "tf2_ros/buffer.h"

2.ROS中的消息发布与订阅机制为啥不需要线程锁？