00001 #ifndef EFFORT_DISPLAY_H
00002 #define EFFORT_DISPLAY_H
00003
00004 #include <boost/circular_buffer.hpp>
00005
00006 #include <sensor_msgs/JointState.h>
00007 #include <rviz/message_filter_display.h>
00008
00009 namespace Ogre
00010 {
00011 class SceneNode;
00012 }
00013
00014 namespace rviz
00015 {
00016 class FloatProperty;
00017 class IntProperty;
00018 class StringProperty;
00019 class CategoryProperty;
00020 class BoolProperty;
00021 class RosTopicProperty;
00022 }
00023
00024 namespace urdf
00025 {
00026 class Model;
00027 }
00028
00029
00030
00031 namespace tf
00032 {
00033 #ifdef TF_MESSAGEFILTER_DEBUG
00034 # undef TF_MESSAGEFILTER_DEBUG
00035 #endif
00036 #define TF_MESSAGEFILTER_DEBUG(fmt, ...) \
00037 ROS_DEBUG_NAMED("message_filter", "MessageFilter [target=%s]: "fmt, getTargetFramesString().c_str(), __VA_ARGS__)
00038
00039 #ifdef TF_MESSAGEFILTER_WARN
00040 # undef TF_MESSAGEFILTER_WARN
00041 #endif
00042 #define TF_MESSAGEFILTER_WARN(fmt, ...) \
00043 ROS_WARN_NAMED("message_filter", "MessageFilter [target=%s]: "fmt, getTargetFramesString().c_str(), __VA_ARGS__)
00044
00045 class MessageFilterJointState : public MessageFilter<sensor_msgs::JointState>
00046 {
00047 typedef sensor_msgs::JointState M;
00048 public:
00049 typedef boost::shared_ptr<M const> MConstPtr;
00050 typedef ros::MessageEvent<M const> MEvent;
00051 typedef boost::function<void(const MConstPtr&, FilterFailureReason)> FailureCallback;
00052 #ifdef RVIZ_USE_BOOST_SIGNAL_1
00053 typedef boost::signal<void(const MConstPtr&, FilterFailureReason)> FailureSignal;
00054 #else
00055 typedef boost::signals2::signal<void(const MConstPtr&, FilterFailureReason)> FailureSignal;
00056 #endif
00057
00058
00059 ROS_STATIC_ASSERT(ros::message_traits::HasHeader<M>::value);
00060
00070 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))
00071 : MessageFilter<sensor_msgs::JointState>(tf, target_frame, queue_size, nh, max_rate)
00072 , tf_(tf)
00073 , nh_(nh)
00074 , max_rate_(max_rate)
00075 , queue_size_(queue_size)
00076 {
00077 init();
00078
00079 setTargetFrame(target_frame);
00080 }
00081
00092 template<class F>
00093 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))
00094 : tf_(tf)
00095 , nh_(nh)
00096 , max_rate_(max_rate)
00097 , queue_size_(queue_size)
00098 , MessageFilter<sensor_msgs::JointState>(f, tf, target_frame, queue_size, nh, max_rate)
00099 {
00100 init();
00101
00102 setTargetFrame(target_frame);
00103
00104 connectInput(f);
00105 }
00106
00110 template<class F>
00111 void connectInput(F& f)
00112 {
00113 message_connection_.disconnect();
00114 message_connection_ = f.registerCallback(&MessageFilterJointState::incomingMessage, this);
00115 }
00116
00120 ~MessageFilterJointState()
00121 {
00122 message_connection_.disconnect();
00123 tf_.removeTransformsChangedListener(tf_connection_);
00124
00125 clear();
00126
00127 TF_MESSAGEFILTER_DEBUG("Successful Transforms: %llu, Failed Transforms: %llu, Discarded due to age: %llu, Transform messages received: %llu, Messages received: %llu, Total dropped: %llu",
00128 (long long unsigned int)successful_transform_count_, (long long unsigned int)failed_transform_count_,
00129 (long long unsigned int)failed_out_the_back_count_, (long long unsigned int)transform_message_count_,
00130 (long long unsigned int)incoming_message_count_, (long long unsigned int)dropped_message_count_);
00131
00132 }
00133
00137 void setTargetFrame(const std::string& target_frame)
00138 {
00139 std::vector<std::string> frames;
00140 frames.push_back(target_frame);
00141 setTargetFrames(frames);
00142 }
00143
00147 void setTargetFrames(const std::vector<std::string>& target_frames)
00148 {
00149 boost::mutex::scoped_lock list_lock(messages_mutex_);
00150 boost::mutex::scoped_lock string_lock(target_frames_string_mutex_);
00151
00152 target_frames_ = target_frames;
00153
00154 std::stringstream ss;
00155 for (std::vector<std::string>::iterator it = target_frames_.begin(); it != target_frames_.end(); ++it)
00156 {
00157 *it = tf::resolve(tf_.getTFPrefix(), *it);
00158 ss << *it << " ";
00159 }
00160 target_frames_string_ = ss.str();
00161 }
00165 std::string getTargetFramesString()
00166 {
00167 boost::mutex::scoped_lock lock(target_frames_string_mutex_);
00168 return target_frames_string_;
00169 };
00170
00174 void setTolerance(const ros::Duration& tolerance)
00175 {
00176 time_tolerance_ = tolerance;
00177 }
00178
00182 void clear()
00183 {
00184 boost::mutex::scoped_lock lock(messages_mutex_);
00185
00186 TF_MESSAGEFILTER_DEBUG("%s", "Cleared");
00187
00188 messages_.clear();
00189 message_count_ = 0;
00190
00191 warned_about_unresolved_name_ = false;
00192 warned_about_empty_frame_id_ = false;
00193 }
00194
00195 void add(const MEvent& evt)
00196 {
00197 boost::mutex::scoped_lock lock(messages_mutex_);
00198
00199 testMessages();
00200
00201 if (!testMessage(evt))
00202 {
00203
00204 if (queue_size_ != 0 && message_count_ + 1 > queue_size_)
00205 {
00206 ++dropped_message_count_;
00207 const MEvent& front = messages_.front();
00208 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());
00209 signalFailure(messages_.front(), filter_failure_reasons::Unknown);
00210
00211 messages_.pop_front();
00212 --message_count_;
00213 }
00214
00215
00216 messages_.push_back(evt);
00217 ++message_count_;
00218 }
00219
00220 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_);
00221
00222 ++incoming_message_count_;
00223 }
00224
00230 void add(const MConstPtr& message)
00231 {
00232 boost::shared_ptr<std::map<std::string, std::string> > header(new std::map<std::string, std::string>);
00233 (*header)["callerid"] = "unknown";
00234 add(MEvent(message, header, ros::Time::now()));
00235 }
00236
00241 message_filters::Connection registerFailureCallback(const FailureCallback& callback)
00242 {
00243 boost::mutex::scoped_lock lock(failure_signal_mutex_);
00244 return message_filters::Connection(boost::bind(&MessageFilterJointState::disconnectFailure, this, _1), failure_signal_.connect(callback));
00245 }
00246
00247 virtual void setQueueSize( uint32_t new_queue_size )
00248 {
00249 queue_size_ = new_queue_size;
00250 }
00251
00252 virtual uint32_t getQueueSize()
00253 {
00254 return queue_size_;
00255 }
00256
00257 private:
00258
00259 void init()
00260 {
00261 message_count_ = 0;
00262 new_transforms_ = false;
00263 successful_transform_count_ = 0;
00264 failed_transform_count_ = 0;
00265 failed_out_the_back_count_ = 0;
00266 transform_message_count_ = 0;
00267 incoming_message_count_ = 0;
00268 dropped_message_count_ = 0;
00269 time_tolerance_ = ros::Duration(0.0);
00270 warned_about_unresolved_name_ = false;
00271 warned_about_empty_frame_id_ = false;
00272
00273 tf_connection_ = tf_.addTransformsChangedListener(boost::bind(&MessageFilterJointState::transformsChanged, this));
00274
00275 max_rate_timer_ = nh_.createTimer(max_rate_, &MessageFilterJointState::maxRateTimerCallback, this);
00276 }
00277
00278 typedef std::list<MEvent> L_Event;
00279
00280 bool testMessage(const MEvent& evt)
00281 {
00282 const MConstPtr& message = evt.getMessage();
00283 std::string callerid = evt.getPublisherName();
00284 std::string frame_id = ros::message_traits::FrameId<M>::value(*message);
00285 ros::Time stamp = ros::message_traits::TimeStamp<M>::value(*message);
00286
00287
00288 if (frame_id.empty()) frame_id = * (target_frames_.begin());
00289
00290 if (frame_id.empty())
00291 {
00292 if (!warned_about_empty_frame_id_)
00293 {
00294 warned_about_empty_frame_id_ = true;
00295 TF_MESSAGEFILTER_WARN("Discarding message from [%s] due to empty frame_id. This message will only print once.", callerid.c_str());
00296 }
00297 signalFailure(evt, filter_failure_reasons::EmptyFrameID);
00298 return true;
00299 }
00300
00301 if (frame_id[0] != '/')
00302 {
00303 std::string unresolved = frame_id;
00304 frame_id = tf::resolve(tf_.getTFPrefix(), frame_id);
00305
00306 if (!warned_about_unresolved_name_)
00307 {
00308 warned_about_unresolved_name_ = true;
00309 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());
00310 }
00311 }
00312
00313
00315 for (std::vector<std::string>::iterator target_it = target_frames_.begin(); target_it != target_frames_.end(); ++target_it)
00316 {
00317 const std::string& target_frame = *target_it;
00318
00319 if (target_frame != frame_id && stamp != ros::Time(0))
00320 {
00321 ros::Time latest_transform_time ;
00322
00323 tf_.getLatestCommonTime(frame_id, target_frame, latest_transform_time, 0) ;
00324 if (stamp + tf_.getCacheLength() < latest_transform_time)
00325 {
00326 ++failed_out_the_back_count_;
00327 ++dropped_message_count_;
00328 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_);
00329
00330 last_out_the_back_stamp_ = stamp;
00331 last_out_the_back_frame_ = frame_id;
00332
00333 signalFailure(evt, filter_failure_reasons::OutTheBack);
00334 return true;
00335 }
00336 }
00337
00338 }
00339
00340 bool ready = !target_frames_.empty();
00341 for (std::vector<std::string>::iterator target_it = target_frames_.begin(); ready && target_it != target_frames_.end(); ++target_it)
00342 {
00343 std::string& target_frame = *target_it;
00344 if (time_tolerance_ != ros::Duration(0.0))
00345 {
00346 ready = ready && (tf_.canTransform(target_frame, frame_id, stamp) &&
00347 tf_.canTransform(target_frame, frame_id, stamp + time_tolerance_) );
00348 }
00349 else
00350 {
00351 ready = ready && tf_.canTransform(target_frame, frame_id, stamp);
00352 }
00353 }
00354
00355 if (ready)
00356 {
00357 TF_MESSAGEFILTER_DEBUG("Message ready in frame %s at time %.3f, count now %d", frame_id.c_str(), stamp.toSec(), message_count_);
00358
00359 ++successful_transform_count_;
00360
00361 signalMessage(evt);
00362 }
00363 else
00364 {
00365 ++failed_transform_count_;
00366 }
00367
00368 return ready;
00369 }
00370
00371 void testMessages()
00372 {
00373 if (!messages_.empty() && getTargetFramesString() == " ")
00374 {
00375 ROS_WARN_NAMED("message_notifier", "MessageFilter [target=%s]: empty target frame", getTargetFramesString().c_str());
00376 }
00377
00378 int i = 0;
00379
00380 L_Event::iterator it = messages_.begin();
00381 for (; it != messages_.end(); ++i)
00382 {
00383 MEvent& evt = *it;
00384
00385 if (testMessage(evt))
00386 {
00387 --message_count_;
00388 it = messages_.erase(it);
00389 }
00390 else
00391 {
00392 ++it;
00393 }
00394 }
00395 }
00396
00397 void maxRateTimerCallback(const ros::TimerEvent&)
00398 {
00399 boost::mutex::scoped_lock list_lock(messages_mutex_);
00400 if (new_transforms_)
00401 {
00402 testMessages();
00403 new_transforms_ = false;
00404 }
00405
00406 checkFailures();
00407 }
00408
00412 void incomingMessage(const ros::MessageEvent<M const>& evt)
00413 {
00414 add(evt);
00415 }
00416
00417 void transformsChanged()
00418 {
00419 new_transforms_ = true;
00420
00421 ++transform_message_count_;
00422 }
00423
00424 void checkFailures()
00425 {
00426 if (next_failure_warning_.isZero())
00427 {
00428 next_failure_warning_ = ros::Time::now() + ros::Duration(15);
00429 }
00430
00431 if (ros::Time::now() >= next_failure_warning_)
00432 {
00433 if (incoming_message_count_ - message_count_ == 0)
00434 {
00435 return;
00436 }
00437
00438 double dropped_pct = (double)dropped_message_count_ / (double)(incoming_message_count_ - message_count_);
00439 if (dropped_pct > 0.95)
00440 {
00441 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);
00442 next_failure_warning_ = ros::Time::now() + ros::Duration(60);
00443
00444 if ((double)failed_out_the_back_count_ / (double)dropped_message_count_ > 0.5)
00445 {
00446 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());
00447 }
00448 }
00449 }
00450 }
00451
00452 void disconnectFailure(const message_filters::Connection& c)
00453 {
00454 boost::mutex::scoped_lock lock(failure_signal_mutex_);
00455 c.getBoostConnection().disconnect();
00456 }
00457
00458 void signalFailure(const MEvent& evt, FilterFailureReason reason)
00459 {
00460 boost::mutex::scoped_lock lock(failure_signal_mutex_);
00461 failure_signal_(evt.getMessage(), reason);
00462 }
00463
00464 Transformer& tf_;
00465 ros::NodeHandle nh_;
00466 ros::Duration max_rate_;
00467 ros::Timer max_rate_timer_;
00468 std::vector<std::string> target_frames_;
00469 std::string target_frames_string_;
00470 boost::mutex target_frames_string_mutex_;
00471 uint32_t queue_size_;
00472
00473 L_Event messages_;
00474 uint32_t message_count_;
00475 boost::mutex messages_mutex_;
00476
00477 bool new_messages_;
00478 volatile bool new_transforms_;
00479
00480 bool warned_about_unresolved_name_;
00481 bool warned_about_empty_frame_id_;
00482
00483 uint64_t successful_transform_count_;
00484 uint64_t failed_transform_count_;
00485 uint64_t failed_out_the_back_count_;
00486 uint64_t transform_message_count_;
00487 uint64_t incoming_message_count_;
00488 uint64_t dropped_message_count_;
00489
00490 ros::Time last_out_the_back_stamp_;
00491 std::string last_out_the_back_frame_;
00492
00493 ros::Time next_failure_warning_;
00494
00495 ros::Duration time_tolerance_;
00496
00497 #ifdef RVIZ_USE_BOOST_SIGNAL_1
00498 boost::signals::connection tf_connection_;
00499 #else
00500 boost::signals2::connection tf_connection_;
00501 #endif
00502 message_filters::Connection message_connection_;
00503
00504 FailureSignal failure_signal_;
00505 boost::mutex failure_signal_mutex_;
00506 };
00507 }
00508
00509 #ifndef Q_MOC_RUN
00510 #include <message_filters/subscriber.h>
00511 #include <tf/message_filter.h>
00512 #endif
00513
00514 #include "rviz/display_context.h"
00515 #include "rviz/frame_manager.h"
00516 #include "rviz/properties/ros_topic_property.h"
00517
00518 namespace rviz
00519 {
00526 class MessageFilterJointStateDisplay: public _RosTopicDisplay
00527 {
00528
00529 public:
00532 typedef MessageFilterJointStateDisplay MFDClass;
00533
00534 MessageFilterJointStateDisplay()
00535 : tf_filter_( NULL )
00536 , messages_received_( 0 )
00537 {
00538 QString message_type = QString::fromStdString( ros::message_traits::datatype<sensor_msgs::JointState>() );
00539 topic_property_->setMessageType( message_type );
00540 topic_property_->setDescription( message_type + " topic to subscribe to." );
00541 }
00542
00543 virtual void onInitialize()
00544 {
00545 tf_filter_ = new tf::MessageFilterJointState( *context_->getTFClient(),
00546 fixed_frame_.toStdString(), 10, update_nh_ );
00547
00548 tf_filter_->connectInput( sub_ );
00549 tf_filter_->registerCallback( boost::bind( &MessageFilterJointStateDisplay::incomingMessage, this, _1 ));
00550 context_->getFrameManager()->registerFilterForTransformStatusCheck( tf_filter_, this );
00551 }
00552
00553 virtual ~MessageFilterJointStateDisplay()
00554 {
00555 unsubscribe();
00556 delete tf_filter_;
00557 }
00558
00559 virtual void reset()
00560 {
00561 Display::reset();
00562 tf_filter_->clear();
00563 messages_received_ = 0;
00564 }
00565
00566 protected:
00567 virtual void updateTopic()
00568 {
00569 unsubscribe();
00570 reset();
00571 subscribe();
00572 context_->queueRender();
00573 }
00574
00575 virtual void subscribe()
00576 {
00577 if( !isEnabled() )
00578 {
00579 return;
00580 }
00581
00582 try
00583 {
00584 sub_.subscribe( update_nh_, topic_property_->getTopicStd(), 10 );
00585 setStatus( StatusProperty::Ok, "Topic", "OK" );
00586 }
00587 catch( ros::Exception& e )
00588 {
00589 setStatus( StatusProperty::Error, "Topic", QString( "Error subscribing: " ) + e.what() );
00590 }
00591 }
00592
00593 virtual void unsubscribe()
00594 {
00595 sub_.unsubscribe();
00596 }
00597
00598 virtual void onEnable()
00599 {
00600 subscribe();
00601 }
00602
00603 virtual void onDisable()
00604 {
00605 unsubscribe();
00606 reset();
00607 }
00608
00609 virtual void fixedFrameChanged()
00610 {
00611 tf_filter_->setTargetFrame( fixed_frame_.toStdString() );
00612 reset();
00613 }
00614
00618 void incomingMessage( const sensor_msgs::JointState::ConstPtr& msg )
00619 {
00620 if( !msg )
00621 {
00622 return;
00623 }
00624
00625 ++messages_received_;
00626 setStatus( StatusProperty::Ok, "Topic", QString::number( messages_received_ ) + " messages received" );
00627
00628 processMessage( msg );
00629 }
00630
00634 virtual void processMessage( const sensor_msgs::JointState::ConstPtr& msg ) = 0;
00635
00636 message_filters::Subscriber<sensor_msgs::JointState> sub_;
00637 tf::MessageFilterJointState* tf_filter_;
00638 uint32_t messages_received_;
00639 };
00640 }
00641
00642 namespace rviz
00643 {
00644 class JointInfo: public QObject {
00645 Q_OBJECT
00646 public:
00647 JointInfo(const std::string name, rviz::Property* parent_category);
00648 ~JointInfo();
00649
00650 void setEffort(double e);
00651 double getEffort();
00652 void setMaxEffort(double m);
00653 double getMaxEffort();
00654 bool getEnabled() const;
00655
00656 ros::Time last_update_;
00657
00658 public Q_SLOTS:
00659 void updateVisibility();
00660
00661 private:
00662 std::string name_;
00663 double effort_, max_effort_;
00664
00665 rviz::Property* category_;
00666 rviz::FloatProperty* effort_property_;
00667 rviz::FloatProperty* max_effort_property_;
00668 };
00669
00670 typedef std::set<JointInfo*> S_JointInfo;
00671 typedef std::vector<std::string> V_string;
00672
00673 class EffortVisual;
00674
00675 class EffortDisplay: public rviz::MessageFilterJointStateDisplay
00676 {
00677 Q_OBJECT
00678 public:
00679
00680
00681 EffortDisplay();
00682 virtual ~EffortDisplay();
00683
00684
00685 virtual void onInitialize();
00686 virtual void reset();
00687
00688 private Q_SLOTS:
00689
00690 void updateColorAndAlpha();
00691 void updateHistoryLength();
00692 void updateRobotDescription();
00693
00694 JointInfo* getJointInfo( const std::string& joint);
00695 JointInfo* createJoint(const std::string &joint);
00696
00697 protected:
00698
00699 virtual void onEnable();
00700 virtual void onDisable();
00701
00702
00703 void load();
00704 void clear();
00705
00706
00707 boost::shared_ptr<urdf::Model> robot_model_;
00708
00709
00710 std::string robot_description_;
00711
00712 private:
00713 void processMessage( const sensor_msgs::JointState::ConstPtr& msg );
00714
00715
00716
00717 boost::circular_buffer<boost::shared_ptr<EffortVisual> > visuals_;
00718
00719 typedef std::map<std::string, JointInfo*> M_JointInfo;
00720 M_JointInfo joints_;
00721
00722
00723 rviz::FloatProperty *alpha_property_,* width_property_,* scale_property_;
00724 rviz::IntProperty *history_length_property_;
00725
00726 rviz::StringProperty *robot_description_property_;
00727 rviz::Property *joints_category_;
00728 rviz::BoolProperty *all_enabled_property_;
00729 };
00730 }
00731
00732 #endif // EFFORT_DISPLAY_H