effort_display.h

Go to the documentation of this file.

#ifndef EFFORT_DISPLAY_H
#define EFFORT_DISPLAY_H

#ifndef Q_MOC_RUN
#include <boost/circular_buffer.hpp>
#endif

#include <sensor_msgs/JointState.h>
#include <rviz/message_filter_display.h>

namespace Ogre
{
class SceneNode;
}

namespace rviz
{
class FloatProperty;
class IntProperty;
class StringProperty;
class CategoryProperty;
class BoolProperty;
class RosTopicProperty;
} // namespace rviz

namespace urdf
{
class Model;
}

// MessageFilter to support message with out frame_id
// https://code.ros.org/trac/ros-pkg/ticket/5467
namespace tf
{
#ifdef TF_MESSAGEFILTER_DEBUG
#undef TF_MESSAGEFILTER_DEBUG
#endif
#define TF_MESSAGEFILTER_DEBUG(fmt, ...)                                                                \
  ROS_DEBUG_NAMED("message_filter", "MessageFilter [target=%s]: " fmt, getTargetFramesString().c_str(), \
                  __VA_ARGS__)

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

class MessageFilterJointState : public MessageFilter<sensor_msgs::JointState>
{
  typedef sensor_msgs::JointState 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;

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

  MessageFilterJointState(Transformer& tf,
                          const std::string& target_frame,
                          uint32_t queue_size,
                          ros::NodeHandle nh = ros::NodeHandle(),
                          ros::Duration max_rate = ros::Duration(0.01))
    : MessageFilter<sensor_msgs::JointState>(tf, target_frame, queue_size, nh, max_rate)
    , tf_(tf)
    , nh_(nh)
    , max_rate_(max_rate)
    , queue_size_(queue_size)
  {
    init();

    setTargetFrame(target_frame);
  }

  template <class F>
  MessageFilterJointState(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))
    : MessageFilter<sensor_msgs::JointState>(f, tf, target_frame, queue_size, nh, max_rate)
    , 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(&MessageFilterJointState::incomingMessage, this);
  }

  ~MessageFilterJointState() override
  {
    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) override
  {
    std::vector<std::string> frames;
    frames.push_back(target_frame);
    setTargetFrames(frames);
  }

  void setTargetFrames(const std::vector<std::string>& target_frames) override
  {
    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)
    {
      *it = tf::resolve(tf_.getTFPrefix(), *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) override
  {
    time_tolerance_ = tolerance;
  }

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

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

    messages_.clear();
    message_count_ = 0;

    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_ != 0 && message_count_ + 1 > 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_, front.getMessage()->header.frame_id.c_str(),
            front.getMessage()->header.stamp.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",
                           evt.getMessage()->header.frame_id.c_str(),
                           evt.getMessage()->header.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";
    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(&MessageFilterJointState::disconnectFailure, this, _1),
                                       failure_signal_.connect(callback));
  }

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

  uint32_t getQueueSize() override
  {
    return queue_size_;
  }

private:
  void init()
  {
    message_count_ = 0;
    new_transforms_ = false;
    successful_transform_count_ = 0;
    failed_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_unresolved_name_ = false;
    warned_about_empty_frame_id_ = false;

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

    max_rate_timer_ = nh_.createTimer(max_rate_, &MessageFilterJointState::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);

    // FIXED
    if (frame_id.empty())
      frame_id = *(target_frames_.begin());
    // 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;
    }

    if (frame_id[0] != '/')
    {
      std::string unresolved = frame_id;
      frame_id = tf::resolve(tf_.getTFPrefix(), frame_id);

      if (!warned_about_unresolved_name_)
      {
        warned_about_unresolved_name_ = true;
        ROS_WARN("Message from [%s] has a non-fully-qualified frame_id [%s]. Resolved locally to [%s].  "
                 "This is will likely not work in multi-robot systems.  This message will only print "
                 "once.",
                 callerid.c_str(), unresolved.c_str(), frame_id.c_str());
      }
    }

    // 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(0))
      {
        ros::Time latest_transform_time;

        tf_.getLatestCommonTime(frame_id, target_frame, latest_transform_time, nullptr);
        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",
              message->header.frame_id.c_str(), message->header.stamp.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_;

      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 = 0;

    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& /*unused*/)
  {
    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(15);
    }

    if (ros::Time::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)
      {
        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);
        next_failure_warning_ = ros::Time::now() + ros::Duration(60);

        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

#ifndef Q_MOC_RUN
#include <message_filters/subscriber.h>
#include <tf/message_filter.h>
#endif

#include "rviz/display_context.h"
#include "rviz/frame_manager.h"
#include "rviz/properties/ros_topic_property.h"

namespace rviz
{
class MessageFilterJointStateDisplay : public _RosTopicDisplay
{
  // No Q_OBJECT macro here, moc does not support Q_OBJECT in a templated class.
public:
  typedef MessageFilterJointStateDisplay MFDClass;

  MessageFilterJointStateDisplay() : tf_filter_(nullptr), messages_received_(0)
  {
    QString message_type =
        QString::fromStdString(ros::message_traits::datatype<sensor_msgs::JointState>());
    topic_property_->setMessageType(message_type);
    topic_property_->setDescription(message_type + " topic to subscribe to.");
  }

  void onInitialize() override
  {
// TODO(wjwwood): remove this and use tf2 interface instead
#ifndef _WIN32
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#endif

    auto tf_client = context_->getTFClient();

#ifndef _WIN32
#pragma GCC diagnostic pop
#endif
    tf_filter_ = new tf::MessageFilterJointState(*tf_client, fixed_frame_.toStdString(), 10, update_nh_);

    tf_filter_->connectInput(sub_);
    tf_filter_->registerCallback(boost::bind(&MessageFilterJointStateDisplay::incomingMessage, this, _1));
// TODO(wjwwood): remove this and use tf2 interface instead
#ifndef _WIN32
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#endif

    context_->getFrameManager()->registerFilterForTransformStatusCheck(tf_filter_, this);

#ifndef _WIN32
#pragma GCC diagnostic pop
#endif
  }

  ~MessageFilterJointStateDisplay() override
  {
    unsubscribe();
    delete tf_filter_;
  }

  void reset() override
  {
    Display::reset();
    tf_filter_->clear();
    messages_received_ = 0;
  }

protected:
  void updateTopic() override
  {
    unsubscribe();
    reset();
    subscribe();
    context_->queueRender();
  }

  virtual void subscribe()
  {
    if (!isEnabled())
    {
      return;
    }

    try
    {
      sub_.subscribe(update_nh_, topic_property_->getTopicStd(), 10);
      setStatus(StatusProperty::Ok, "Topic", "OK");
    }
    catch (ros::Exception& e)
    {
      setStatus(StatusProperty::Error, "Topic", QString("Error subscribing: ") + e.what());
    }
  }

  virtual void unsubscribe()
  {
    sub_.unsubscribe();
  }

  void onEnable() override
  {
    subscribe();
  }

  void onDisable() override
  {
    unsubscribe();
    reset();
  }

  void fixedFrameChanged() override
  {
    tf_filter_->setTargetFrame(fixed_frame_.toStdString());
    reset();
  }

  void incomingMessage(const sensor_msgs::JointState::ConstPtr& msg)
  {
    if (!msg)
    {
      return;
    }

    ++messages_received_;
    setStatus(StatusProperty::Ok, "Topic", QString::number(messages_received_) + " messages received");

    processMessage(msg);
  }

  virtual void processMessage(const sensor_msgs::JointState::ConstPtr& msg) = 0;

  message_filters::Subscriber<sensor_msgs::JointState> sub_;
  tf::MessageFilterJointState* tf_filter_;
  uint32_t messages_received_;
};
} // namespace rviz

namespace rviz
{
class JointInfo : public QObject
{
  Q_OBJECT
public:
  JointInfo(const std::string name, rviz::Property* parent_category);
  ~JointInfo() override;

  void setEffort(double e);
  double getEffort();
  void setMaxEffort(double m);
  double getMaxEffort();
  bool getEnabled() const;

  ros::Time last_update_;

public Q_SLOTS:
  void updateVisibility();

private:
  std::string name_;
  double effort_, max_effort_;

  rviz::Property* category_;
  rviz::FloatProperty* effort_property_;
  rviz::FloatProperty* max_effort_property_;
};

typedef std::set<JointInfo*> S_JointInfo;
typedef std::vector<std::string> V_string;

class EffortVisual;

class EffortDisplay : public rviz::MessageFilterJointStateDisplay
{
  Q_OBJECT
public:
  // Constructor.  pluginlib::ClassLoader creates instances by calling
  // the default constructor, so make sure you have one.
  EffortDisplay();
  ~EffortDisplay() override;

  // Overrides of public virtual functions from the Display class.
  void onInitialize() override;
  void reset() override;

private Q_SLOTS:
  // Helper function to apply color and alpha to all visuals.
  void updateColorAndAlpha();
  void updateHistoryLength();
  void updateRobotDescription();
  void updateTfPrefix();

  JointInfo* getJointInfo(const std::string& joint);
  JointInfo* createJoint(const std::string& joint);

protected:
  // overrides from Display
  void onEnable() override;
  void onDisable() override;

  // load
  using MessageFilterJointStateDisplay::load;
  void load();
  void clear();

  // The object for urdf model
  boost::shared_ptr<urdf::Model> robot_model_;

  //
  std::string robot_description_;

private:
  void processMessage(const sensor_msgs::JointState::ConstPtr& msg) override;

  // Storage for the list of visuals.  It is a circular buffer where
  // data gets popped from the front (oldest) and pushed to the back (newest)
  boost::circular_buffer<boost::shared_ptr<EffortVisual> > visuals_;

  typedef std::map<std::string, JointInfo*> M_JointInfo;
  M_JointInfo joints_;

  // Property objects for user-editable properties.
  rviz::FloatProperty *alpha_property_, *width_property_, *scale_property_;
  rviz::IntProperty* history_length_property_;

  rviz::StringProperty* robot_description_property_;
  rviz::StringProperty* tf_prefix_property_;
  rviz::Property* joints_category_;
  rviz::BoolProperty* all_enabled_property_;
};
} // namespace rviz

#endif // EFFORT_DISPLAY_H