Skip to content

Commit

Permalink
migrate MessageFilterDisplay to tf2
Browse files Browse the repository at this point in the history
  • Loading branch information
wjwwood committed May 9, 2018
1 parent 19e81d2 commit 0f227e5
Showing 1 changed file with 9 additions and 6 deletions.
15 changes: 9 additions & 6 deletions src/rviz/message_filter_display.h
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@
#include <OgreSceneNode.h>

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

#include "rviz/display_context.h"
Expand Down Expand Up @@ -72,10 +72,10 @@ protected Q_SLOTS:
BoolProperty* unreliable_property_;
};

/** @brief Display subclass using a tf::MessageFilter, templated on the ROS message type.
/** @brief Display subclass using a tf2_ros::MessageFilter, templated on the ROS message type.
*
* This class brings together some common things used in many Display
* types. It has a tf::MessageFilter to filter incoming messages, and
* types. It has a tf2_ros::MessageFilter to filter incoming messages, and
* it handles subscribing and unsubscribing when the display is
* enabled or disabled. It also has an Ogre::SceneNode which */
template<class MessageType>
Expand All @@ -98,8 +98,11 @@ class MessageFilterDisplay: public _RosTopicDisplay

virtual void onInitialize()
{
tf_filter_ = new tf::MessageFilter<MessageType>( *context_->getTFClient(),
fixed_frame_.toStdString(), 10, update_nh_ );
tf_filter_ = new tf2_ros::MessageFilter<MessageType>(
*context_->getTF2BufferPtr(),
fixed_frame_.toStdString(),
10,
update_nh_);

tf_filter_->connectInput( sub_ );
tf_filter_->registerCallback( boost::bind( &MessageFilterDisplay<MessageType>::incomingMessage, this, _1 ));
Expand Down Expand Up @@ -201,7 +204,7 @@ class MessageFilterDisplay: public _RosTopicDisplay
virtual void processMessage( const typename MessageType::ConstPtr& msg ) = 0;

message_filters::Subscriber<MessageType> sub_;
tf::MessageFilter<MessageType>* tf_filter_;
tf2_ros::MessageFilter<MessageType>* tf_filter_;
uint32_t messages_received_;
};

Expand Down

0 comments on commit 0f227e5

Please sign in to comment.