|
From: <jfa...@us...> - 2009-07-17 20:54:01
|
Revision: 19136
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19136&view=rev
Author: jfaustwg
Date: 2009-07-17 20:53:58 +0000 (Fri, 17 Jul 2009)
Log Message:
-----------
Updated message_filters according to API review: connect() -> registerCallback(), connectTo() -> connectInputs()
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/dense_laser_msg_filter.h
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/joint_pv_msg_filter.h
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/laser_scan_tagger.h
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/joint_extractor_display.cpp
pkg/trunk/sandbox/message_filters/include/message_filters/cache.h
pkg/trunk/sandbox/message_filters/include/message_filters/subscriber.h
pkg/trunk/sandbox/message_filters/include/message_filters/time_sequencer.h
pkg/trunk/sandbox/message_filters/include/message_filters/time_synchronizer.h
pkg/trunk/sandbox/message_filters/test/time_sequencer_unittest.cpp
pkg/trunk/sandbox/message_filters/test/time_synchronizer_unittest.cpp
pkg/trunk/stacks/geometry/tf/include/tf/message_filter.h
pkg/trunk/stacks/geometry/tf/test/test_message_filter.cpp
pkg/trunk/stacks/nav/nav_view/src/nav_view/nav_view_panel.cpp
pkg/trunk/stacks/visualization/rviz/src/rviz/displays/camera_display.cpp
pkg/trunk/stacks/visualization/rviz/src/rviz/displays/collision_map_display.cpp
pkg/trunk/stacks/visualization/rviz/src/rviz/displays/laser_scan_display.cpp
pkg/trunk/stacks/visualization/rviz/src/rviz/displays/marker_display.cpp
pkg/trunk/stacks/visualization/rviz/src/rviz/displays/point_cloud_display.cpp
pkg/trunk/stacks/visualization/rviz/src/rviz/displays/poly_line_2d_display.cpp
pkg/trunk/stacks/visualization/rviz/src/rviz/displays/polygonal_map_display.cpp
pkg/trunk/stacks/visualization/rviz/src/rviz/displays/robot_base2d_pose_display.cpp
Added Paths:
-----------
pkg/trunk/sandbox/message_filters/include/message_filters/simple_filter.h
Modified: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/dense_laser_msg_filter.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/dense_laser_msg_filter.h 2009-07-17 20:49:52 UTC (rev 19135)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/dense_laser_msg_filter.h 2009-07-17 20:53:58 UTC (rev 19136)
@@ -43,6 +43,7 @@
#include "dense_laser_assembler/laser_scan_tagger.h"
#include "message_filters/cache.h"
#include "message_filters/connection.h"
+#include "message_filters/simple_filter.h"
// Messages
@@ -68,13 +69,11 @@
/**
* \brief Listens to LaserScan and MechanismState messages, and builds DenseLaserSnapshots
*/
-class DenseLaserMsgFilter
+class DenseLaserMsgFilter : public message_filters::SimpleFilter<TaggedLaserScan<JointPVArray> >
{
public:
typedef boost::shared_ptr<const TaggedLaserScan<JointPVArray> > MConstPtr ;
- typedef boost::function<void(const MConstPtr&)> Callback;
- typedef boost::signal<void(const MConstPtr&)> Signal;
//! \brief Not yet implemented
template<class A, class B>
@@ -125,21 +124,15 @@
// Configure the joint_pv_filter and associated cache
joint_pv_filter_.setJointNames(joint_names_) ;
joint_cache_.setCacheSize(mech_state_cache_size) ;
- joint_cache_.connectTo(joint_pv_filter_) ;
+ joint_cache_.connectInput(joint_pv_filter_) ;
// Set up the laser tagger and associated cache
laser_tagger_.setMaxQueueSize(laser_queue_size) ;
laser_tagger_.subscribeTagCache(joint_cache_) ;
tagged_laser_cache_.setCacheSize(laser_cache_size) ;
- tagged_laser_cache_.connectTo(laser_tagger_) ;
+ tagged_laser_cache_.connectInput(laser_tagger_) ;
}
- /**
- * \brief Used to connect this MessageFilter to a callback
- * \param callback Function to be called whenever a new TaggedLaserScan message is available
- */
- message_filters::Connection connect(const Callback& callback) ;
-
//! \brief Not yet implemented
void processLaserScan(const sensor_msgs::LaserScanConstPtr& msg) ;
@@ -183,7 +176,7 @@
template<class B>
void subscribeMechState(B& b)
{
- joint_pv_filter_.subscribe(b) ;
+ joint_pv_filter_.connectInput(b) ;
}
//! Extracts positions data for a subset of joints in MechanismState
Modified: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/joint_pv_msg_filter.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/joint_pv_msg_filter.h 2009-07-17 20:49:52 UTC (rev 19135)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/joint_pv_msg_filter.h 2009-07-17 20:53:58 UTC (rev 19136)
@@ -46,6 +46,7 @@
#include <boost/signals.hpp>
#include <boost/bind.hpp>
#include <message_filters/connection.h>
+#include <message_filters/simple_filter.h>
// Messages
#include "dense_laser_assembler/JointPVArray.h"
@@ -58,12 +59,9 @@
* Streams an array of joint positions and velocities from MechanismState,
* given a mapping from joint_names to array indices
*/
-class JointPVMsgFilter
+class JointPVMsgFilter : public message_filters::SimpleFilter<JointPVArray>
{
public:
- typedef boost::function<void(const JointPVArrayConstPtr&)> Callback;
- typedef boost::signal<void(const JointPVArrayConstPtr&)> Signal;
-
/**
* \brief Subscribe to another MessageFilter at construction time
* \param a The parent message filter
@@ -92,23 +90,12 @@
* \param a The MsgFilter that this MsgFilter should get data from
*/
template<class A>
- void subscribe(A& a)
+ void connectInput(A& a)
{
- boost::mutex::scoped_lock lock(signal_mutex_);
- incoming_connection_ = a.connect(boost::bind(&JointPVMsgFilter::processMechState, this, _1));
+ incoming_connection_ = a.registerCallback(boost::bind(&JointPVMsgFilter::processMechState, this, _1));
}
/**
- * \brief Called by user wants they want to connect this MessageFilter to their own code
- * \param callback Function to be called whenever a JointPVArray message is available
- */
- message_filters::Connection connect(const Callback& callback)
- {
- boost::mutex::scoped_lock lock(signal_mutex_);
- return message_filters::Connection(boost::bind(&JointPVMsgFilter::disconnect, this, _1), signal_.connect(callback));
- }
-
- /**
* \brief Define the mapping from MechanismState to JointPVArray
*/
void setJointNames(std::vector<std::string> joint_names)
@@ -152,23 +139,15 @@
}
// Call all connected callbacks
- signal_(joint_array_ptr) ;
+ signalMessage(joint_array_ptr) ;
}
protected:
- void disconnect(const message_filters::Connection& c)
- {
- boost::mutex::scoped_lock lock(signal_mutex_);
- signal_.disconnect(c.getBoostConnection());
- }
-
boost::mutex joint_mapping_mutex_ ;
std::vector<std::string> joint_names_ ;
// Filter Connection Stuff
message_filters::Connection incoming_connection_;
- Signal signal_;
- boost::mutex signal_mutex_;
} ;
}
Modified: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/laser_scan_tagger.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/laser_scan_tagger.h 2009-07-17 20:49:52 UTC (rev 19135)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/laser_scan_tagger.h 2009-07-17 20:53:58 UTC (rev 19136)
@@ -45,6 +45,8 @@
#include "boost/shared_ptr.hpp"
#include "dense_laser_assembler/tagged_laser_scan.h"
+#include <message_filters/simple_filter.h>
+
namespace dense_laser_assembler
{
@@ -53,14 +55,12 @@
* whenever there is a tag msg that occurs before and after the scan.
*/
template <class T>
-class LaserScanTagger
+class LaserScanTagger : public message_filters::SimpleFilter<TaggedLaserScan<T> >
{
public:
typedef boost::shared_ptr<const T> TConstPtr ;
typedef boost::shared_ptr<const TaggedLaserScan<T> > MConstPtr ;
- typedef boost::function<void(const MConstPtr&)> Callback;
- typedef boost::signal<void(const MConstPtr&)> Signal;
@@ -92,15 +92,13 @@
template<class A>
void subscribeLaserScan(A& a)
{
- boost::mutex::scoped_lock lock(signal_mutex_);
- incoming_laser_scan_connection_ = a.connect(boost::bind(&LaserScanTagger<T>::processLaserScan, this, _1));
+ incoming_laser_scan_connection_ = a.registerCallback(boost::bind(&LaserScanTagger<T>::processLaserScan, this, _1));
}
void subscribeTagCache(message_filters::Cache<T>& tag_cache)
{
- boost::mutex::scoped_lock lock(signal_mutex_);
tag_cache_ = &tag_cache ;
- incoming_tag_connection_ = tag_cache.connect(boost::bind(&LaserScanTagger<T>::processTag, this, _1));
+ incoming_tag_connection_ = tag_cache.registerCallback(boost::bind(&LaserScanTagger<T>::processTag, this, _1));
}
void setMaxQueueSize(unsigned int max_queue_size)
@@ -136,16 +134,6 @@
}
/**
- * \brief Connect this message filter's output to some callback
- * \param callback Function to call after we've tagged a LaserScan
- */
- message_filters::Connection connect(const Callback& callback)
- {
- boost::mutex::scoped_lock lock(signal_mutex_);
- return message_filters::Connection(boost::bind(&LaserScanTagger::disconnect, this, _1), signal_.connect(callback));
- }
-
- /**
* Triggers the module to try to service the queue
*/
void update()
@@ -206,28 +194,20 @@
queue_.pop_front() ;
queue_mutex_.unlock() ;
- boost::mutex::scoped_lock lock(signal_mutex_);
- signal_(tagged_scan) ;
+ signalMessage(tagged_scan) ;
}
}
}
private:
- void disconnect(const message_filters::Connection& c)
- {
- boost::mutex::scoped_lock lock(signal_mutex_);
- signal_.disconnect(c.getBoostConnection());
- }
std::deque<sensor_msgs::LaserScanConstPtr> queue_ ; //!< Incoming queue of laser scans
boost::mutex queue_mutex_ ; //!< Mutex for laser scan queue
unsigned int max_queue_size_ ; //!< Max # of laser scans to queue up for processing
message_filters::Cache<T>* tag_cache_ ; //!< Cache of the tags that we need to merge with laser data
- Signal signal_;
message_filters::Connection incoming_laser_scan_connection_;
message_filters::Connection incoming_tag_connection_;
- boost::mutex signal_mutex_;
} ;
}
Modified: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/joint_extractor_display.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/joint_extractor_display.cpp 2009-07-17 20:49:52 UTC (rev 19135)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/joint_extractor_display.cpp 2009-07-17 20:53:58 UTC (rev 19136)
@@ -85,7 +85,7 @@
JointPVMsgFilter joint_extractor(joint_names) ;
//JointPVDiagMsgFilter joint_extractor(diagnostic, joint_names) ;
- message_filters::Connection con = joint_extractor.connect( boost::bind(&display_joints, joint_names, _1) ) ;
+ joint_extractor.registerCallback( boost::bind(&display_joints, joint_names, _1) ) ;
ros::Subscriber sub = nh.subscribe("mechanism_state", 1, &JointPVMsgFilter::processMechState, &joint_extractor) ;
@@ -94,7 +94,5 @@
ros::spin() ;
- con.disconnect() ;
-
return 0 ;
}
Modified: pkg/trunk/sandbox/message_filters/include/message_filters/cache.h
===================================================================
--- pkg/trunk/sandbox/message_filters/include/message_filters/cache.h 2009-07-17 20:49:52 UTC (rev 19135)
+++ pkg/trunk/sandbox/message_filters/include/message_filters/cache.h 2009-07-17 20:53:58 UTC (rev 19136)
@@ -39,10 +39,10 @@
#include "boost/thread.hpp"
#include "boost/shared_ptr.hpp"
#include <boost/signals.hpp>
-#include <boost/noncopyable.hpp>
#include "ros/time.h"
#include "connection.h"
+#include "simple_filter.h"
namespace message_filters
{
@@ -63,18 +63,16 @@
\endverbatim
*/
template<class M>
-class Cache : public boost::noncopyable
+class Cache : public SimpleFilter<M>
{
public:
- typedef boost::shared_ptr<M const> MConstPtr ;
- typedef boost::function<void(const MConstPtr&)> Callback;
- typedef boost::signal<void(const MConstPtr&)> Signal;
+ typedef boost::shared_ptr<M const> MConstPtr;
template<class F>
Cache(F& f, unsigned int cache_size = 1)
{
setCacheSize(cache_size) ;
- connectTo(f) ;
+ connectInput(f) ;
}
/**
@@ -87,9 +85,9 @@
}
template<class F>
- void connectTo(F& f)
+ void connectInput(F& f)
{
- incoming_connection_ = f.connect(boost::bind(&Cache::add, this, _1));
+ incoming_connection_ = f.registerCallback(boost::bind(&Cache::add, this, _1));
}
~Cache()
@@ -112,12 +110,6 @@
cache_size_ = cache_size ;
}
- Connection connect(const Callback& callback)
- {
- boost::mutex::scoped_lock lock(signal_mutex_);
- return Connection(boost::bind(&Cache::disconnect, this, _1), signal_.connect(callback));
- }
-
/**
* Add the most recent message to the cache, and pop off any elements that are too old.
* This method is registered with a data provider when connectTo is called.
@@ -146,11 +138,7 @@
}
- {
- boost::mutex::scoped_lock lock(signal_mutex_);
- // Sequentially call each registered call
- signal_(msg);
- }
+ signalMessage(msg);
}
/**
@@ -293,19 +281,11 @@
}
private:
- void disconnect(const Connection& c)
- {
- boost::mutex::scoped_lock lock(signal_mutex_);
- c.getBoostConnection().disconnect();
- }
-
boost::mutex cache_lock_ ; //!< Lock for cache_
std::deque<MConstPtr > cache_ ; //!< Cache for the messages
unsigned int cache_size_ ; //!< Maximum number of elements allowed in the cache.
Connection incoming_connection_;
- Signal signal_;
- boost::mutex signal_mutex_;
};
}
Added: pkg/trunk/sandbox/message_filters/include/message_filters/simple_filter.h
===================================================================
--- pkg/trunk/sandbox/message_filters/include/message_filters/simple_filter.h (rev 0)
+++ pkg/trunk/sandbox/message_filters/include/message_filters/simple_filter.h 2009-07-17 20:53:58 UTC (rev 19136)
@@ -0,0 +1,82 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, 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 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 MESSAGE_FILTERS_SIMPLE_FILTER_H
+#define MESSAGE_FILTERS_SIMPLE_FILTER_H
+
+#include <boost/noncopyable.hpp>
+
+#include <ros/ros.h>
+
+#include "connection.h"
+
+namespace message_filters
+{
+
+template<class M>
+class SimpleFilter : public boost::noncopyable
+{
+public:
+ typedef boost::shared_ptr<M const> MConstPtr;
+ typedef boost::function<void(const MConstPtr&)> Callback;
+ typedef boost::signal<void(const MConstPtr&)> Signal;
+
+ Connection registerCallback(const Callback& callback)
+ {
+ boost::mutex::scoped_lock lock(signal_mutex_);
+ return Connection(boost::bind(&SimpleFilter::disconnect, this, _1), signal_.connect(callback));
+ }
+
+protected:
+ void signalMessage(const MConstPtr& msg)
+ {
+ boost::mutex::scoped_lock lock(signal_mutex_);
+ signal_(msg);
+ }
+
+private:
+ void disconnect(const Connection& c)
+ {
+ boost::mutex::scoped_lock lock(signal_mutex_);
+ c.getBoostConnection().disconnect();
+ }
+
+ Signal signal_;
+ boost::mutex signal_mutex_;
+};
+
+}
+
+#endif
+
Modified: pkg/trunk/sandbox/message_filters/include/message_filters/subscriber.h
===================================================================
--- pkg/trunk/sandbox/message_filters/include/message_filters/subscriber.h 2009-07-17 20:49:52 UTC (rev 19135)
+++ pkg/trunk/sandbox/message_filters/include/message_filters/subscriber.h 2009-07-17 20:53:58 UTC (rev 19136)
@@ -39,9 +39,9 @@
#include <boost/signals.hpp>
#include <boost/thread/mutex.hpp>
-#include <boost/noncopyable.hpp>
#include "connection.h"
+#include "simple_filter.h"
namespace message_filters
{
@@ -66,12 +66,10 @@
\endverbatim
*/
template<class M>
-class Subscriber : public boost::noncopyable
+class Subscriber : public SimpleFilter<M>
{
public:
typedef boost::shared_ptr<M const> MConstPtr;
- typedef boost::function<void(const MConstPtr&)> Callback;
- typedef boost::signal<void(const MConstPtr&)> Signal;
/**
* \brief Constructor
@@ -134,34 +132,14 @@
sub_.shutdown();
}
- /**
- * \brief Connect a callback to this filter. That callback will be called whenever new data arrives.
- *
- * \param callback A function of the same form as the message callbacks used in roscpp
- * \return A connection object that allows you to disconnect a single connection from this subscriber
- */
- Connection connect(const Callback& callback)
- {
- boost::mutex::scoped_lock lock(signal_mutex_);
- return Connection(boost::bind(&Subscriber::disconnect, this, _1), signal_.connect(callback));
- }
-
private:
- void disconnect(const Connection& c)
- {
- boost::mutex::scoped_lock lock(signal_mutex_);
- c.getBoostConnection().disconnect();
- }
void cb(const MConstPtr& m)
{
- boost::mutex::scoped_lock lock(signal_mutex_);
- signal_(m);
+ signalMessage(m);
}
ros::Subscriber sub_;
- Signal signal_;
- boost::mutex signal_mutex_;
};
}
Modified: pkg/trunk/sandbox/message_filters/include/message_filters/time_sequencer.h
===================================================================
--- pkg/trunk/sandbox/message_filters/include/message_filters/time_sequencer.h 2009-07-17 20:49:52 UTC (rev 19135)
+++ pkg/trunk/sandbox/message_filters/include/message_filters/time_sequencer.h 2009-07-17 20:53:58 UTC (rev 19136)
@@ -35,11 +35,10 @@
#ifndef MESSAGE_FILTERS_TIME_SEQUENCER_H
#define MESSAGE_FILTERS_TIME_SEQUENCER_H
-#include <boost/noncopyable.hpp>
-
#include <ros/ros.h>
#include "connection.h"
+#include "simple_filter.h"
namespace message_filters
{
@@ -73,12 +72,10 @@
*
*/
template<class M>
-class TimeSequencer : public boost::noncopyable
+class TimeSequencer : public SimpleFilter<M>
{
public:
- typedef boost::shared_ptr<M const> MConstPtr ;
- typedef boost::function<void(const MConstPtr&)> Callback;
- typedef boost::signal<void(const MConstPtr&)> Signal;
+ typedef boost::shared_ptr<M const> MConstPtr;
/**
* \brief Constructor
@@ -96,13 +93,13 @@
, nh_(nh)
{
init();
- connectTo(f);
+ connectInput(f);
}
/**
* \brief Constructor
*
- * This version of the constructor does not take a filter immediately. You can connect to a filter later with the connectTo() function
+ * This version of the constructor does not take a filter immediately. You can connect to a filter later with the connectInput() function
*
* \param delay The minimum time to hold a message before passing it through.
* \param update_rate The rate at which to check for messages which have passed "delay"
@@ -122,7 +119,7 @@
* \brief Connect this filter's input to another filter's output.
*/
template<class F>
- void connectTo(F& f)
+ void connectInput(F& f)
{
incoming_connection_.disconnect();
incoming_connection_ = f.connect(boost::bind(&TimeSequencer::cb, this, _1));
@@ -134,12 +131,6 @@
incoming_connection_.disconnect();
}
- Connection connect(const Callback& callback)
- {
- boost::mutex::scoped_lock lock(signal_mutex_);
- return Connection(boost::bind(&TimeSequencer::disconnect, this, _1), signal_.connect(callback));
- }
-
void add(const MConstPtr& msg)
{
boost::mutex::scoped_lock lock(messages_mutex_);
@@ -173,12 +164,6 @@
add(msg);
}
- void disconnect(const Connection& c)
- {
- boost::mutex::scoped_lock lock(signal_mutex_);
- c.getBoostConnection().disconnect();
- }
-
void dispatch()
{
V_Message to_call;
@@ -203,13 +188,11 @@
}
{
- boost::mutex::scoped_lock lock(signal_mutex_);
-
typename V_Message::iterator it = to_call.begin();
typename V_Message::iterator end = to_call.end();
for (; it != end; ++it)
{
- signal_(*it);
+ signalMessage(*it);
}
}
}
@@ -234,8 +217,6 @@
ros::Timer update_timer_;
Connection incoming_connection_;
- Signal signal_;
- boost::mutex signal_mutex_;
S_Message messages_;
Modified: pkg/trunk/sandbox/message_filters/include/message_filters/time_synchronizer.h
===================================================================
--- pkg/trunk/sandbox/message_filters/include/message_filters/time_synchronizer.h 2009-07-17 20:49:52 UTC (rev 19135)
+++ pkg/trunk/sandbox/message_filters/include/message_filters/time_synchronizer.h 2009-07-17 20:53:58 UTC (rev 19136)
@@ -66,7 +66,7 @@
public:
typedef boost::shared_ptr<M const> MConstPtr;
typedef boost::function<void(const MConstPtr&)> Callback;
- Connection connect(const Callback& cb)
+ Connection registerCallback(const Callback& cb)
{
return Connection();
}
@@ -97,7 +97,7 @@
* Example usage would be:
\verbatim
TimeSynchronizer<sensor_msgs::CamInfo, sensor_msgs::Image, sensor_msgs::Image> sync(caminfo_sub, limage_sub, rimage_sub, 3);
-sync.connect(callback);
+sync.registerCallback(callback);
\endverbatim
* The callback is then of the form:
@@ -128,7 +128,7 @@
: queue_size_(queue_size)
{
determineRealTypeCount();
- connectTo(f0, f1);
+ connectInput(f0, f1);
}
template<class F0, class F1, class F2>
@@ -136,7 +136,7 @@
: queue_size_(queue_size)
{
determineRealTypeCount();
- connectTo(f0, f1, f2);
+ connectInput(f0, f1, f2);
}
template<class F0, class F1, class F2, class F3>
@@ -144,7 +144,7 @@
: queue_size_(queue_size)
{
determineRealTypeCount();
- connectTo(f0, f1, f2, f3);
+ connectInput(f0, f1, f2, f3);
}
template<class F0, class F1, class F2, class F3, class F4>
@@ -152,7 +152,7 @@
: queue_size_(queue_size)
{
determineRealTypeCount();
- connectTo(f0, f1, f2, f3, f4);
+ connectInput(f0, f1, f2, f3, f4);
}
template<class F0, class F1, class F2, class F3, class F4, class F5>
@@ -160,7 +160,7 @@
: queue_size_(queue_size)
{
determineRealTypeCount();
- connectTo(f0, f1, f2, f3, f4, f5);
+ connectInput(f0, f1, f2, f3, f4, f5);
}
template<class F0, class F1, class F2, class F3, class F4, class F5, class F6>
@@ -168,7 +168,7 @@
: queue_size_(queue_size)
{
determineRealTypeCount();
- connectTo(f0, f1, f2, f3, f4, f5, f6);
+ connectInput(f0, f1, f2, f3, f4, f5, f6);
}
template<class F0, class F1, class F2, class F3, class F4, class F5, class F6, class F7>
@@ -176,7 +176,7 @@
: queue_size_(queue_size)
{
determineRealTypeCount();
- connectTo(f0, f1, f2, f3, f4, f5, f6, f7);
+ connectInput(f0, f1, f2, f3, f4, f5, f6, f7);
}
template<class F0, class F1, class F2, class F3, class F4, class F5, class F6, class F7, class F8>
@@ -184,7 +184,7 @@
: queue_size_(queue_size)
{
determineRealTypeCount();
- connectTo(f0, f1, f2, f3, f4, f5, f6, f7, f8);
+ connectInput(f0, f1, f2, f3, f4, f5, f6, f7, f8);
}
TimeSynchronizer(uint32_t queue_size)
@@ -199,72 +199,72 @@
}
template<class F0, class F1>
- void connectTo(F0& f0, F1& f1)
+ void connectInput(F0& f0, F1& f1)
{
NullFilter<M2> f2;
- connectTo(f0, f1, f2);
+ connectInput(f0, f1, f2);
}
template<class F0, class F1, class F2>
- void connectTo(F0& f0, F1& f1, F2& f2)
+ void connectInput(F0& f0, F1& f1, F2& f2)
{
NullFilter<M3> f3;
- connectTo(f0, f1, f2, f3);
+ connectInput(f0, f1, f2, f3);
}
template<class F0, class F1, class F2, class F3>
- void connectTo(F0& f0, F1& f1, F2& f2, F3& f3)
+ void connectInput(F0& f0, F1& f1, F2& f2, F3& f3)
{
NullFilter<M4> f4;
- connectTo(f0, f1, f2, f3, f4);
+ connectInput(f0, f1, f2, f3, f4);
}
template<class F0, class F1, class F2, class F3, class F4>
- void connectTo(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4)
+ void connectInput(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4)
{
NullFilter<M5> f5;
- connectTo(f0, f1, f2, f3, f4, f5);
+ connectInput(f0, f1, f2, f3, f4, f5);
}
template<class F0, class F1, class F2, class F3, class F4, class F5>
- void connectTo(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5)
+ void connectInput(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5)
{
NullFilter<M6> f6;
- connectTo(f0, f1, f2, f3, f4, f5, f6);
+ connectInput(f0, f1, f2, f3, f4, f5, f6);
}
template<class F0, class F1, class F2, class F3, class F4, class F5, class F6>
- void connectTo(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6)
+ void connectInput(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6)
{
NullFilter<M7> f7;
- connectTo(f0, f1, f2, f3, f4, f5, f6, f7);
+ connectInput(f0, f1, f2, f3, f4, f5, f6, f7);
}
template<class F0, class F1, class F2, class F3, class F4, class F5, class F6, class F7>
- void connectTo(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6, F7& f7)
+ void connectInput(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6, F7& f7)
{
NullFilter<M8> f8;
- connectTo(f0, f1, f2, f3, f4, f5, f6, f7, f8);
+ connectInput(f0, f1, f2, f3, f4, f5, f6, f7, f8);
}
template<class F0, class F1, class F2, class F3, class F4, class F5, class F6, class F7, class F8>
- void connectTo(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6, F7& f7, F8& f8)
+ void connectInput(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6, F7& f7, F8& f8)
{
disconnectAll();
- input_connections_[0] = f0.connect(boost::bind(&TimeSynchronizer::cb0, this, _1));
- input_connections_[1] = f1.connect(boost::bind(&TimeSynchronizer::cb1, this, _1));
- input_connections_[2] = f2.connect(boost::bind(&TimeSynchronizer::cb2, this, _1));
- input_connections_[3] = f3.connect(boost::bind(&TimeSynchronizer::cb3, this, _1));
- input_connections_[4] = f4.connect(boost::bind(&TimeSynchronizer::cb4, this, _1));
- input_connections_[5] = f5.connect(boost::bind(&TimeSynchronizer::cb5, this, _1));
- input_connections_[6] = f6.connect(boost::bind(&TimeSynchronizer::cb6, this, _1));
- input_connections_[7] = f7.connect(boost::bind(&TimeSynchronizer::cb7, this, _1));
- input_connections_[8] = f8.connect(boost::bind(&TimeSynchronizer::cb8, this, _1));
+ input_connections_[0] = f0.registerCallback(boost::bind(&TimeSynchronizer::cb0, this, _1));
+ input_connections_[1] = f1.registerCallback(boost::bind(&TimeSynchronizer::cb1, this, _1));
+ input_connections_[2] = f2.registerCallback(boost::bind(&TimeSynchronizer::cb2, this, _1));
+ input_connections_[3] = f3.registerCallback(boost::bind(&TimeSynchronizer::cb3, this, _1));
+ input_connections_[4] = f4.registerCallback(boost::bind(&TimeSynchronizer::cb4, this, _1));
+ input_connections_[5] = f5.registerCallback(boost::bind(&TimeSynchronizer::cb5, this, _1));
+ input_connections_[6] = f6.registerCallback(boost::bind(&TimeSynchronizer::cb6, this, _1));
+ input_connections_[7] = f7.registerCallback(boost::bind(&TimeSynchronizer::cb7, this, _1));
+ input_connections_[8] = f8.registerCallback(boost::bind(&TimeSynchronizer::cb8, this, _1));
}
template<class C>
- Connection connect(const C& callback)
+ Connection registerCallback(const C& callback)
{
boost::mutex::scoped_lock lock(signal_mutex_);
return Connection(boost::bind(&TimeSynchronizer::disconnect, this, _1), signal_.connect(boost::bind(callback, _1, _2, _3, _4, _5, _6, _7, _8, _9)));
Modified: pkg/trunk/sandbox/message_filters/test/time_sequencer_unittest.cpp
===================================================================
--- pkg/trunk/sandbox/message_filters/test/time_sequencer_unittest.cpp 2009-07-17 20:49:52 UTC (rev 19135)
+++ pkg/trunk/sandbox/message_filters/test/time_sequencer_unittest.cpp 2009-07-17 20:53:58 UTC (rev 19136)
@@ -72,7 +72,7 @@
{
TimeSequencer<Msg> seq(ros::Duration(1.0), ros::Duration(0.01), 10);
Helper h;
- seq.connect(boost::bind(&Helper::cb, &h, _1));
+ seq.registerCallback(boost::bind(&Helper::cb, &h, _1));
MsgPtr msg(new Msg);
msg->header.stamp = ros::Time::now();
seq.add(msg);
Modified: pkg/trunk/sandbox/message_filters/test/time_synchronizer_unittest.cpp
===================================================================
--- pkg/trunk/sandbox/message_filters/test/time_synchronizer_unittest.cpp 2009-07-17 20:49:52 UTC (rev 19135)
+++ pkg/trunk/sandbox/message_filters/test/time_synchronizer_unittest.cpp 2009-07-17 20:53:58 UTC (rev 19136)
@@ -120,7 +120,7 @@
{
TimeSynchronizer<Msg, Msg> sync(1);
Helper h;
- sync.connect(boost::bind(&Helper::cb, &h));
+ sync.registerCallback(boost::bind(&Helper::cb, &h));
MsgPtr m(new Msg);
m->header.stamp = ros::Time::now();
@@ -134,7 +134,7 @@
{
TimeSynchronizer<Msg, Msg, Msg> sync(1);
Helper h;
- sync.connect(boost::bind(&Helper::cb, &h));
+ sync.registerCallback(boost::bind(&Helper::cb, &h));
MsgPtr m(new Msg);
m->header.stamp = ros::Time::now();
@@ -150,7 +150,7 @@
{
TimeSynchronizer<Msg, Msg, Msg, Msg> sync(1);
Helper h;
- sync.connect(boost::bind(&Helper::cb, &h));
+ sync.registerCallback(boost::bind(&Helper::cb, &h));
MsgPtr m(new Msg);
m->header.stamp = ros::Time::now();
@@ -168,7 +168,7 @@
{
TimeSynchronizer<Msg, Msg, Msg, Msg, Msg> sync(1);
Helper h;
- sync.connect(boost::bind(&Helper::cb, &h));
+ sync.registerCallback(boost::bind(&Helper::cb, &h));
MsgPtr m(new Msg);
m->header.stamp = ros::Time::now();
@@ -188,7 +188,7 @@
{
TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg> sync(1);
Helper h;
- sync.connect(boost::bind(&Helper::cb, &h));
+ sync.registerCallback(boost::bind(&Helper::cb, &h));
MsgPtr m(new Msg);
m->header.stamp = ros::Time::now();
@@ -210,7 +210,7 @@
{
TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg, Msg> sync(1);
Helper h;
- sync.connect(boost::bind(&Helper::cb, &h));
+ sync.registerCallback(boost::bind(&Helper::cb, &h));
MsgPtr m(new Msg);
m->header.stamp = ros::Time::now();
@@ -234,7 +234,7 @@
{
TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg, Msg, Msg> sync(1);
Helper h;
- sync.connect(boost::bind(&Helper::cb, &h));
+ sync.registerCallback(boost::bind(&Helper::cb, &h));
MsgPtr m(new Msg);
m->header.stamp = ros::Time::now();
@@ -260,7 +260,7 @@
{
TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg, Msg, Msg, Msg> sync(1);
Helper h;
- sync.connect(boost::bind(&Helper::cb, &h));
+ sync.registerCallback(boost::bind(&Helper::cb, &h));
MsgPtr m(new Msg);
m->header.stamp = ros::Time::now();
@@ -292,7 +292,7 @@
{
TimeSynchronizer<Msg, Msg, Msg> sync(2);
Helper h;
- sync.connect(boost::bind(&Helper::cb, &h));
+ sync.registerCallback(boost::bind(&Helper::cb, &h));
MsgPtr m(new Msg);
m->header.stamp = ros::Time();
@@ -313,7 +313,7 @@
{
TimeSynchronizer<Msg, Msg, Msg> sync(1);
Helper h;
- sync.connect(boost::bind(&Helper::cb, &h));
+ sync.registerCallback(boost::bind(&Helper::cb, &h));
MsgPtr m(new Msg);
m->header.stamp = ros::Time();
Modified: pkg/trunk/stacks/geometry/tf/include/tf/message_filter.h
===================================================================
--- pkg/trunk/stacks/geometry/tf/include/tf/message_filter.h 2009-07-17 20:49:52 UTC (rev 19135)
+++ pkg/trunk/stacks/geometry/tf/include/tf/message_filter.h 2009-07-17 20:53:58 UTC (rev 19136)
@@ -50,6 +50,7 @@
#include <ros/callback_queue.h>
#include <message_filters/connection.h>
+#include <message_filters/simple_filter.h>
#define TF_MESSAGEFILTER_DEBUG(fmt, ...) \
ROS_DEBUG_NAMED("message_filter", "MessageFilter [target=%s]: "fmt, getTargetFramesString().c_str(), __VA_ARGS__)
@@ -78,14 +79,10 @@
* MessageFilter is templated on a message type.
*/
template<class M>
-class MessageFilter : public MessageFilterBase
+class MessageFilter : public MessageFilterBase, public message_filters::SimpleFilter<M>
{
public:
typedef boost::shared_ptr<M const> MConstPtr;
- typedef boost::function<void (const MConstPtr&)> Callback;
- typedef boost::signal<void (const MConstPtr&)> Signal;
- typedef boost::shared_ptr<Signal> SignalPtr;
- typedef boost::weak_ptr<Signal> SignalWPtr;
/**
* \brief Constructor
@@ -132,17 +129,17 @@
setTargetFrame(target_frame);
- connectTo(f);
+ connectInput(f);
}
/**
* \brief Connect this filter's input to another filter's output. If this filter is already connected, disconnects first.
*/
template<class F>
- void connectTo(F& f)
+ void connectInput(F& f)
{
message_connection_.disconnect();
- message_connection_ = f.connect(boost::bind(&MessageFilter::incomingMessage, this, _1));
+ message_connection_ = f.registerCallback(boost::bind(&MessageFilter::incomingMessage, this, _1));
}
/**
@@ -246,15 +243,6 @@
++incoming_message_count_;
}
- /**
- * \brief Connect a callback to the output of this filter
- */
- message_filters::Connection connect(const Callback& callback)
- {
- boost::mutex::scoped_lock lock(signal_mutex_);
- return message_filters::Connection(boost::bind(&MessageFilter::disconnect, this, _1), signal_.connect(callback));
- }
-
private:
void init()
@@ -279,12 +267,6 @@
typedef std::list<MConstPtr> L_Message;
- void signal(const MConstPtr& message)
- {
- boost::mutex::scoped_lock lock(signal_mutex_);
- signal_(message);
- }
-
bool testMessage(const MConstPtr& message)
{
//Throw out messages which are too old
@@ -334,7 +316,7 @@
++successful_transform_count_;
- signal(message);
+ signalMessage(message);
}
else
{
@@ -435,12 +417,6 @@
}
}
- void disconnect(const message_filters::Connection& c)
- {
- boost::mutex::scoped_lock lock(signal_mutex_);
- signal_.disconnect(c.getBoostConnection());
- }
-
Transformer& tf_; ///< The Transformer used to determine if transformation data is available
ros::NodeHandle nh_; ///< The node used to subscribe to the topic
ros::Duration min_rate_;
@@ -452,9 +428,6 @@
boost::mutex target_frames_string_mutex_;
uint32_t queue_size_; ///< The maximum number of messages we queue up
- Signal signal_;
- boost::mutex signal_mutex_;
-
L_Message messages_; ///< The message list
uint32_t message_count_; ///< The number of messages in the list. Used because messages_.size() has linear cost
boost::mutex messages_mutex_; ///< The mutex used for locking message list operations
Modified: pkg/trunk/stacks/geometry/tf/test/test_message_filter.cpp
===================================================================
--- pkg/trunk/stacks/geometry/tf/test/test_message_filter.cpp 2009-07-17 20:49:52 UTC (rev 19135)
+++ pkg/trunk/stacks/geometry/tf/test/test_message_filter.cpp 2009-07-17 20:53:58 UTC (rev 19136)
@@ -68,7 +68,7 @@
tf::TransformListener tf_client;
Notification n(1);
MessageFilter<robot_msgs::PointStamped> filter(tf_client, "frame1", 1);
- filter.connect(boost::bind(&Notification::notify, &n, _1));
+ filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
robot_msgs::PointStampedPtr msg(new robot_msgs::PointStamped);
msg->header.stamp = ros::Time::now();
@@ -83,7 +83,7 @@
tf::TransformListener tf_client;
Notification n(1);
MessageFilter<robot_msgs::PointStamped> filter(tf_client, "frame1", 1);
- filter.connect(boost::bind(&Notification::notify, &n, _1));
+ filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
robot_msgs::PointStampedPtr msg(new robot_msgs::PointStamped);
msg->header.stamp = ros::Time::now();
@@ -98,7 +98,7 @@
tf::TransformListener tf_client;
Notification n(1);
MessageFilter<robot_msgs::PointStamped> filter(tf_client, "frame1", 1);
- filter.connect(boost::bind(&Notification::notify, &n, _1));
+ filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
ros::Time stamp = ros::Time::now();
tf::Stamped<tf::Transform> transform(btTransform(btQuaternion(0,0,0), btVector3(1,2,3)), stamp, "frame1", "frame2");
@@ -118,7 +118,7 @@
tf::TransformListener tf_client;
Notification n(1);
MessageFilter<robot_msgs::PointStamped> filter(tf_client, "frame1", 1);
- filter.connect(boost::bind(&Notification::notify, &n, _1));
+ filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
ros::Time stamp = ros::Time::now();
@@ -141,7 +141,7 @@
tf::TransformListener tf_client;
Notification n(10);
MessageFilter<robot_msgs::PointStamped> filter(tf_client, "frame1", 10);
- filter.connect(boost::bind(&Notification::notify, &n, _1));
+ filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
ros::Time stamp = ros::Time::now();
@@ -167,7 +167,7 @@
tf::TransformListener tf_client;
Notification n(1);
MessageFilter<robot_msgs::PointStamped> filter(tf_client, "frame1", 1);
- filter.connect(boost::bind(&Notification::notify, &n, _1));
+ filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
filter.setTargetFrame("frame1000");
ros::Time stamp = ros::Time::now();
@@ -189,7 +189,7 @@
tf::TransformListener tf_client;
Notification n(1);
MessageFilter<robot_msgs::PointStamped> filter(tf_client, "", 1);
- filter.connect(boost::bind(&Notification::notify, &n, _1));
+ filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
std::vector<std::string> target_frames;
target_frames.push_back("frame1");
@@ -221,7 +221,7 @@
tf::TransformListener tf_client;
Notification n(1);
MessageFilter<robot_msgs::PointStamped> filter(tf_client, "frame1", 1);
- filter.connect(boost::bind(&Notification::notify, &n, _1));
+ filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
filter.setTolerance(offset);
ros::Time stamp = ros::Time::now();
@@ -253,7 +253,7 @@
tf::TransformListener tf_client;
Notification n(1);
MessageFilter<robot_msgs::PointStamped> filter(tf_client, "frame1", 1, ros::NodeHandle(), ros::Duration(1.0), ros::Duration());
- filter.connect(boost::bind(&Notification::notify, &n, _1));
+ filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
ros::Time stamp = ros::Time::now();
tf::Stamped<tf::Transform> transform(btTransform(btQuaternion(0,0,0), btVector3(1,2,3)), stamp, "frame1", "frame2");
Modified: pkg/trunk/stacks/nav/nav_view/src/nav_view/nav_view_panel.cpp
===================================================================
--- pkg/trunk/stacks/nav/nav_view/src/nav_view/nav_view_panel.cpp 2009-07-17 20:49:52 UTC (rev 19135)
+++ pkg/trunk/stacks/nav/nav_view/src/nav_view/nav_view_panel.cpp 2009-07-17 20:53:58 UTC (rev 19136)
@@ -111,12 +111,12 @@
raw_obs_filter_ = PolylineFilterPtr(new PolylineFilter(raw_obs_sub_, *tf_client_, global_frame_id_, 2));
gui_laser_filter_ = PolylineFilterPtr(new PolylineFilter(gui_laser_sub_, *tf_client_, global_frame_id_, 2));
- gui_path_filter_->connect(boost::bind(&NavViewPanel::incomingGuiPath, this, _1));
- local_path_filter_->connect(boost::bind(&NavViewPanel::incomingLocalPath, this, _1));
- robot_footprint_filter_->connect(boost::bind(&NavViewPanel::incomingRobotFootprint, this, _1));
- inflated_obs_filter_->connect(boost::bind(&NavViewPanel::incomingInflatedObstacles, this, _1));
- raw_obs_filter_->connect(boost::bind(&NavViewPanel::incomingRawObstacles, this, _1));
- gui_laser_filter_->connect(boost::bind(&NavViewPanel::incomingGuiLaser, this, _1));
+ gui_path_filter_->registerCallback(boost::bind(&NavViewPanel::incomingGuiPath, this, _1));
+ local_path_filter_->registerCallback(boost::bind(&NavViewPanel::incomingLocalPath, this, _1));
+ robot_footprint_filter_->registerCallback(boost::bind(&NavViewPanel::incomingRobotFootprint, this, _1));
+ inflated_obs_filter_->registerCallback(boost::bind(&NavViewPanel::incomingInflatedObstacles, this, _1));
+ raw_obs_filter_->registerCallback(boost::bind(&NavViewPanel::incomingRawObstacles, this, _1));
+ gui_laser_filter_->registerCallback(boost::bind(&NavViewPanel::incomingGuiLaser, this, _1));
render_panel_->Connect( wxEVT_LEFT_DOWN, wxMouseEventHandler( NavViewPanel::onRenderWindowMouseEvents ), NULL, this );
render_panel_->Connect( wxEVT_MIDDLE_DOWN, wxMouseEventHandler( NavViewPanel::onRenderWindowMouseEvents ), NULL, this );
Modified: pkg/trunk/stacks/visualization/rviz/src/rviz/displays/camera_display.cpp
===================================================================
--- pkg/trunk/stacks/visualization/rviz/src/rviz/displays/camera_display.cpp 2009-07-17 20:49:52 UTC (rev 19135)
+++ pkg/trunk/stacks/visualization/rviz/src/rviz/displays/camera_display.cpp 2009-07-17 20:53:58 UTC (rev 19136)
@@ -188,8 +188,8 @@
camera_->setNearClipDistance( 0.1f );
}
- caminfo_tf_filter_.connectTo(caminfo_sub_);
- caminfo_tf_filter_.connect(boost::bind(&CameraDisplay::caminfoCallback, this, _1));
+ caminfo_tf_filter_.connectInput(caminfo_sub_);
+ caminfo_tf_filter_.registerCallback(boost::bind(&CameraDisplay::caminfoCallback, this, _1));
}
CameraDisplay::~CameraDisplay()
Modified: pkg/trunk/stacks/visualization/rviz/src/rviz/displays/collision_map_display.cpp
===================================================================
--- pkg/trunk/stacks/visualization/rviz/src/rviz/displays/collision_map_display.cpp 2009-07-17 20:49:52 UTC (rev 19135)
+++ pkg/trunk/stacks/visualization/rviz/src/rviz/displays/collision_map_display.cpp 2009-07-17 20:53:58 UTC (rev 19136)
@@ -70,8 +70,8 @@
setPointSize(0.05f);
scene_node_->attachObject(cloud_);
- tf_filter_.connectTo(sub_);
- tf_filter_.connect(boost::bind(&CollisionMapDisplay::incomingMessage, this, _1));
+ tf_filter_.connectInput(sub_);
+ tf_filter_.registerCallback(boost::bind(&CollisionMapDisplay::incomingMessage, this, _1));
}
CollisionMapDisplay::~CollisionMapDisplay()
Modified: pkg/trunk/stacks/visualization/rviz/src/rviz/displays/laser_scan_display.cpp
===================================================================
--- pkg/trunk/stacks/visualization/rviz/src/rviz/displays/laser_scan_display.cpp 2009-07-17 20:49:52 UTC (rev 19135)
+++ pkg/trunk/stacks/visualization/rviz/src/rviz/displays/laser_scan_display.cpp 2009-07-17 20:53:58 UTC (rev 19136)
@@ -52,8 +52,8 @@
{
projector_ = new laser_scan::LaserProjection();
- tf_filter_.connectTo(sub_);
- tf_filter_.connect(boost::bind(&LaserScanDisplay::incomingScanCallback, this, _1));
+ tf_filter_.connectInput(sub_);
+ tf_filter_.registerCallback(boost::bind(&LaserScanDisplay::incomingScanCallback, this, _1));
}
LaserScanDisplay::~LaserScanDisplay()
Modified: pkg/trunk/stacks/visualization/rviz/src/rviz/displays/marker_display.cpp
===================================================================
--- pkg/trunk/stacks/visualization/rviz/src/rviz/displays/marker_display.cpp 2009-07-17 20:49:52 UTC (rev 19135)
+++ pkg/trunk/stacks/visualization/rviz/src/rviz/displays/marker_display.cpp 2009-07-17 20:53:58 UTC (rev 19136)
@@ -64,8 +64,8 @@
{
scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
- tf_filter_.connectTo(sub_);
- tf_filter_.connect(boost::bind(&MarkerDisplay::incomingMarker, this, _1));
+ tf_filter_.connectInput(sub_);
+ tf_filter_.registerCallback(boost::bind(&MarkerDisplay::incomingMarker, this, _1));
}
MarkerDisplay::~MarkerDisplay()
Modified: pkg/trunk/stacks/visualization/rviz/src/rviz/displays/point_cloud_display.cpp
===================================================================
--- pkg/trunk/stacks/visualization/rviz/src/rviz/displays/point_cloud_display.cpp 2009-07-17 20:49:52 UTC (rev 19135)
+++ pkg/trunk/stacks/visualization/rviz/src/rviz/displays/point_cloud_display.cpp 2009-07-17 20:53:58 UTC (rev 19136)
@@ -49,8 +49,8 @@
: PointCloudBase( name, manager )
, tf_filter_(*manager->getThreadedTFClient(), "", 10, threaded_nh_)
{
- tf_filter_.connectTo(sub_);
- tf_filter_.connect(boost::bind(&PointCloudDisplay::incomingCloudCallback, this, _1));
+ tf_filter_.connectInput(sub_);
+ tf_filter_.registerCallback(boost::bind(&PointCloudDisplay::incomingCloudCallback, this, _1));
}
PointCloudDisplay::~PointCloudDisplay()
Modified: pkg/trunk/stacks/visualization/rviz/src/rviz/displays/poly_line_2d_display.cpp
===================================================================
--- pkg/trunk/stacks/visualization/rviz/src/rviz/displays/poly_line_2d_display.cpp 2009-07-17 20:49:52 UTC (rev 19135)
+++ pkg/trunk/stacks/visualization/rviz/src/rviz/displays/poly_line_2d_display.cpp 2009-07-17 20:53:58 UTC (rev 19136)
@@ -75,8 +75,8 @@
setPointSize( 0.05f );
setZPosition( 0.0f );
- tf_filter_.connectTo(sub_);
- tf_filter_.connect(boost::bind(&PolyLine2DDisplay::incomingMessage, this, _1));
+ tf_filter_.connectInput(sub_);
+ tf_filter_.registerCallback(boost::bind(&PolyLine2DDisplay::incomingMessage, this, _1));
}
PolyLine2DDisplay::~PolyLine2DDisplay()
Modified: pkg/trunk/stacks/visualization/rviz/src/rviz/displays/polygonal_map_display.cpp
===================================================================
--- pkg/trunk/stacks/visualization/rviz/src/rviz/displays/polygonal_map_display.cpp 2009-07-17 20:49:52 UTC (rev 19135)
+++ pkg/trunk/stacks/visualization/rviz/src/rviz/displays/polygonal_map_display.cpp 2009-07-17 20:53:58 UTC (rev 19136)
@@ -76,8 +76,8 @@
setPointSize (0.02f);
setLineWidth(0.02f);
- tf_filter_.connectTo(sub_);
- tf_filter_.connect(boost::bind(&PolygonalMapDisplay::incomingMessage, this, _1));
+ tf_filter_.connectInput(sub_);
+ tf_filter_.registerCallback(boost::bind(&PolygonalMapDisplay::incomingMessage, this, _1));
}
PolygonalMapDisplay::~PolygonalMapDisplay()
Modified: pkg/trunk/stacks/visualization/rviz/src/rviz/displays/robot_base2d_pose_display.cpp
===================================================================
--- pkg/trunk/stacks/visualization/rviz/src/rviz/displays/robot_base2d_pose_display.cpp 2009-07-17 20:49:52 UTC (rev 19135)
+++ pkg/trunk/stacks/visualization/rviz/src/rviz/displays/robot_base2d_pose_display.cpp 2009-07-17 20:53:58 UTC (rev 19136)
@@ -55,8 +55,8 @@
{
scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
- tf_filter_.connectTo(sub_);
- tf_filter_.connect(boost::bind(&RobotBase2DPoseDisplay::incomingMessage, this, _1));
+ tf_filter_.connectInput(sub_);
+ tf_filter_.registerCallback(boost::bind(&RobotBase2DPoseDisplay::incomingMessage, this, _1));
}
RobotBase2DPoseDisplay::~RobotBase2DPoseDisplay()
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|