effort_display.h

Go to the documentation of this file.

#ifndef EFFORT_DISPLAY_H
#define EFFORT_DISPLAY_H

#include <boost/circular_buffer.hpp>

#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 urdf
{
    class Model;
}

// MessageFilter to support message with out frame_id
// https://code.ros.org/trac/ros-pkg/ticket/5467
namespace tf
{
#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__)

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

                setTargetFrame(target_frame);

                connectInput(f);
            }

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

        ~MessageFilterJointState()
            {
                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)
                {
                    *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)
            {
                time_tolerance_ = tolerance;
            }

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

        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;
                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();//message->__connection_header ? (*message->__connection_header)["callerid"] : "unknown";
                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, 0) ;
                        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&)
            {
                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::signals::connection tf_connection_;
        message_filters::Connection message_connection_;

        FailureSignal failure_signal_;
        boost::mutex failure_signal_mutex_;
    };
}

#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_( NULL )
    , 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." );
    }

  virtual void onInitialize()
    {
      tf_filter_ = new tf::MessageFilterJointState( *context_->getTFClient(),
                                                    fixed_frame_.toStdString(), 10, update_nh_ );

      tf_filter_->connectInput( sub_ );
      tf_filter_->registerCallback( boost::bind( &MessageFilterJointStateDisplay::incomingMessage, this, _1 ));
      context_->getFrameManager()->registerFilterForTransformStatusCheck( tf_filter_, this );
    }

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

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

protected:
  virtual void updateTopic()
    {
      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();
    }

  virtual void onEnable()
    {
      subscribe();
    }

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

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

  void incomingMessage( const typename 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 typename sensor_msgs::JointState::ConstPtr& msg ) = 0;

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

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

        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();
        virtual ~EffortDisplay();

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

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

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

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

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

        // 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::Property *joints_category_;
        rviz::BoolProperty *all_enabled_property_;
    };
} // end namespace rviz_plugin_tutorials

#endif // EFFORT_DISPLAY_H