message_filter.h

Go to the documentation of this file.

/*
 * Copyright (c) 2010, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef TF2_ROS_MESSAGE_FILTER_H
#define TF2_ROS_MESSAGE_FILTER_H

#include <tf2/buffer_core.h>

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

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

#include <ros/node_handle.h>
#include <ros/callback_queue_interface.h>
#include <ros/init.h>

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

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

namespace tf2_ros
{

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

class MessageFilterBase
{
public:
  typedef std::vector<std::string> V_string;

  virtual ~MessageFilterBase(){}
  virtual void clear() = 0;
  virtual void setTargetFrame(const std::string& target_frame) = 0;
  virtual void setTargetFrames(const V_string& target_frames) = 0;
  virtual void setTolerance(const ros::Duration& tolerance) = 0;
};

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::signal<void(const MConstPtr&, FilterFailureReason)> FailureSignal;

  // If you hit this assert your message does not have a header, or does not have the HasHeader trait defined for it
  ROS_STATIC_ASSERT(ros::message_traits::HasHeader<M>::value);

  MessageFilter(tf2::BufferCore& bc, const std::string& target_frame, uint32_t queue_size, const ros::NodeHandle& nh)
  : bc_(bc)
  , queue_size_(queue_size)
  , callback_queue_(nh.getCallbackQueue())
  {
    init();

    setTargetFrame(target_frame);
  }

  template<class F>
  MessageFilter(F& f, tf2::BufferCore& bc, const std::string& target_frame, uint32_t queue_size, const ros::NodeHandle& nh)
  : bc_(bc)
  , queue_size_(queue_size)
  , callback_queue_(nh.getCallbackQueue())
  {
    init();

    setTargetFrame(target_frame);

    connectInput(f);
  }

  MessageFilter(tf2::BufferCore& bc, const std::string& target_frame, uint32_t queue_size, ros::CallbackQueueInterface* cbqueue)
  : bc_(bc)
  , queue_size_(queue_size)
  , callback_queue_(cbqueue)
  {
    init();

    setTargetFrame(target_frame);
  }

  template<class F>
  MessageFilter(F& f, tf2::BufferCore& bc, const std::string& target_frame, uint32_t queue_size, ros::CallbackQueueInterface* cbqueue)
  : bc_(bc)
  , queue_size_(queue_size)
  , callback_queue_(cbqueue)
  {
    init();

    setTargetFrame(target_frame);

    connectInput(f);
  }

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

  ~MessageFilter()
  {
    message_connection_.disconnect();

    clear();

    TF2_ROS_MESSAGEFILTER_DEBUG("Successful 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_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)
  {
    V_string frames;
    frames.push_back(target_frame);
    setTargetFrames(frames);
  }

  void setTargetFrames(const V_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;
    expected_success_count_ = target_frames_.size() + (time_tolerance_.isZero() ? 0 : 1);

    std::stringstream ss;
    for (V_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)
  {
    boost::mutex::scoped_lock list_lock(messages_mutex_);
    time_tolerance_ = tolerance;
    expected_success_count_ = target_frames_.size() + (time_tolerance_.isZero() ? 0 : 1);
  }

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

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

    bc_.removeTransformableCallback(callback_handle_);
    callback_handle_ = bc_.addTransformableCallback(boost::bind(&MessageFilter::transformable, this, _1, _2, _3, _4, _5));

    messages_.clear();
    message_count_ = 0;

    warned_about_empty_frame_id_ = false;
  }

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

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

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

    boost::mutex::scoped_lock lock(messages_mutex_);
    // iterate through the target frames and add requests for each of them
    MessageInfo info;
    info.handles.reserve(expected_success_count_);
    {
      V_string::iterator it = target_frames_.begin();
      V_string::iterator end = target_frames_.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 == 0)
        {
          ++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 == 0)
          {
            ++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_ != 0 && message_count_ + 1 > queue_size_)
      {
        ++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>::pointer(*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);

        messages_.pop_front();
        --message_count_;
      }

      // Add the message to our list
      info.event = evt;
      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_;
  }

  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";
    ros::WallTime n = ros::WallTime::now();
    ros::Time t(n.sec, n.nsec);
    add(MEvent(message, header, t));
  }

  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_ = 0;
    successful_transform_count_ = 0;
    failed_out_the_back_count_ = 0;
    transform_message_count_ = 0;
    incoming_message_count_ = 0;
    dropped_message_count_ = 0;
    time_tolerance_ = ros::Duration(0.0);
    warned_about_empty_frame_id_ = false;
    expected_success_count_ = 1;

    callback_handle_ = bc_.addTransformableCallback(boost::bind(&MessageFilter::transformable, this, _1, _2, _3, _4, _5));
  }

  void transformable(tf2::TransformableRequestHandle request_handle, const std::string& target_frame, const std::string& source_frame,
                     ros::Time time, tf2::TransformableResult result)
  {
    namespace mt = ros::message_traits;

    boost::mutex::scoped_lock lock(messages_mutex_);

    // find the message this request is associated with
    typename L_MessageInfo::iterator msg_it = messages_.begin();
    typename L_MessageInfo::iterator msg_end = messages_.end();
    for (; msg_it != msg_end; ++msg_it)
    {
      MessageInfo& info = *msg_it;
      V_TransformableRequestHandle::const_iterator handle_it = std::find(info.handles.begin(), info.handles.end(), request_handle);
      if (handle_it != info.handles.end())
      {
        // found msg_it
        ++info.success_count;
        break;
      }
    }

    if (msg_it == msg_end)
    {
      return;
    }

    const MessageInfo& info = *msg_it;
    if (info.success_count < expected_success_count_)
    {
      return;
    }

    bool can_transform = true;
    const MConstPtr& message = info.event.getMessage();
    const std::string& frame_id = *mt::FrameId<M>::pointer(*message);
    ros::Time stamp = mt::TimeStamp<M>::value(*message);

    if (result == tf2::TransformAvailable)
    {
      // make sure we can still perform all the necessary transforms
      typename V_string::iterator it = target_frames_.begin();
      typename V_string::iterator end = target_frames_.end();
      for (; it != end; ++it)
      {
        const std::string& target = *it;
        if (!bc_.canTransform(target, frame_id, stamp))
        {
          can_transform = false;
          break;
        }

        if (!time_tolerance_.isZero())
        {
          if (!bc_.canTransform(target, frame_id, stamp + time_tolerance_))
          {
            can_transform = false;
            break;
          }
        }
      }
    }
    else
    {
      can_transform = false;
    }

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

      ++successful_transform_count_;

      messageReady(info.event);

    }
    else
    {
      ++dropped_message_count_;

      TF2_ROS_MESSAGEFILTER_DEBUG("Discarding message in frame %s at time %.3f, count now %d", frame_id.c_str(), stamp.toSec(), message_count_ - 1);
      messageDropped(info.event, filter_failure_reasons::Unknown);
    }

    messages_.erase(msg_it);
    --message_count_;
  }

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

  void checkFailures()
  {
    if (next_failure_warning_.isZero())
    {
      next_failure_warning_ = ros::WallTime::now() + ros::WallDuration(15);
    }

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

      double dropped_pct = (double)dropped_message_count_ / (double)(incoming_message_count_ - message_count_);
      if (dropped_pct > 0.95)
      {
        TF2_ROS_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);
        next_failure_warning_ = ros::WallTime::now() + ros::WallDuration(60);

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

  struct CBQueueCallback : public ros::CallbackInterface
  {
    CBQueueCallback(MessageFilter* filter, const MEvent& event, bool success, FilterFailureReason reason)
    : event_(event)
    , filter_(filter)
    , reason_(reason)
    , success_(success)
    {}


    virtual CallResult call()
    {
      if (success_)
      {
        filter_->signalMessage(event_);
      }
      else
      {
        filter_->signalFailure(event_, reason_);
      }

      return Success;
    }

  private:
    MEvent event_;
    MessageFilter* filter_;
    FilterFailureReason reason_;
    bool success_;
  };

  void messageDropped(const MEvent& evt, FilterFailureReason reason)
  {
    if (callback_queue_)
    {
      ros::CallbackInterfacePtr cb(new CBQueueCallback(this, evt, false, reason));
      callback_queue_->addCallback(cb, (uint64_t)this);
    }
    else
    {
      signalFailure(evt, reason);
    }
  }

  void messageReady(const MEvent& evt)
  {
    if (callback_queue_)
    {
      ros::CallbackInterfacePtr cb(new CBQueueCallback(this, evt, true, filter_failure_reasons::Unknown));
      callback_queue_->addCallback(cb, (uint64_t)this);
    }
    else
    {
      signalMessage(evt);
    }
  }

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

  tf2::BufferCore& bc_; 
  V_string target_frames_; 
  std::string target_frames_string_;
  boost::mutex target_frames_string_mutex_;
  uint32_t queue_size_; 
  tf2::TransformableCallbackHandle callback_handle_;

  typedef std::vector<tf2::TransformableRequestHandle> V_TransformableRequestHandle;
  struct MessageInfo
  {
    MessageInfo()
    : success_count(0)
    {}

    MEvent event;
    V_TransformableRequestHandle handles;
    uint32_t success_count;
  };
  typedef std::list<MessageInfo> L_MessageInfo;
  L_MessageInfo messages_;
  uint32_t message_count_; 
  boost::mutex messages_mutex_; 
  uint32_t expected_success_count_;

  bool warned_about_empty_frame_id_;

  uint64_t successful_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::WallTime next_failure_warning_;

  ros::Duration time_tolerance_; 

  message_filters::Connection message_connection_;

  FailureSignal failure_signal_;
  boost::mutex failure_signal_mutex_;

  ros::CallbackQueueInterface* callback_queue_;
};

} // namespace tf2

#endif