Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[6552] pkg/trunk
|
From: <jfa...@us...> - 2008-11-11 23:16:56
|
Revision: 6552
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6552&view=rev
Author: jfaustwg
Date: 2008-11-11 23:16:51 +0000 (Tue, 11 Nov 2008)
Log Message:
-----------
Update trunk from user branch: tf::MessageNotifier
r11791@iad (orig r6401): jfaustwg | 2008-11-07 13:03:45 -0800
Creating personal branch
r11938@iad (orig r6524): jfaustwg | 2008-11-11 10:19:39 -0800
Adding tf::MessageNotifier for automatic buffering of messages until transformation data is available
r11962@iad (orig r6546): jfaustwg | 2008-11-11 14:51:19 -0800
* Remove all use of the message list from the message callback, instead pushing things into a queue and having the worker thread do a fast swap() operation on it to avoid lock contension.
* Fix tests to remove the race conditions it's possible to remove
Modified Paths:
--------------
pkg/trunk/util/tf/CMakeLists.txt
pkg/trunk/util/tf/manifest.xml
pkg/trunk/util/tf/test/testBroadcaster.cpp
Added Paths:
-----------
pkg/trunk/util/tf/include/tf/message_notifier.h
pkg/trunk/util/tf/src/message_notifier.cpp
pkg/trunk/util/tf/test/test_notifier.cpp
pkg/trunk/util/tf/test/test_notifier.xml
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
+ 920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:6549
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
Modified: pkg/trunk/util/tf/CMakeLists.txt
===================================================================
--- pkg/trunk/util/tf/CMakeLists.txt 2008-11-11 23:15:04 UTC (rev 6551)
+++ pkg/trunk/util/tf/CMakeLists.txt 2008-11-11 23:16:51 UTC (rev 6552)
@@ -4,8 +4,13 @@
genmsg()
-rospack_add_library(tf src/tf.cpp src/transform_listener.cpp src/cache.cpp)
+find_package(Boost 0 REQUIRED COMPONENTS thread)
+include_directories(${Boost_INCLUDE_DIRS})
+link_directories(${Boost_LIBRARY_DIRS})
+rospack_add_library(tf src/tf.cpp src/transform_listener.cpp src/cache.cpp src/message_notifier.cpp)
+target_link_libraries(tf ${Boost_LIBRARIES})
+
rospack_add_executable(simpletest simpletest.cpp)
target_link_libraries(simpletest tf)
#rospack_add_executable(testtf src/test/testtf.cc)
@@ -27,7 +32,6 @@
rospack_add_gtest(bullet_unittest test/bullet_unittest.cpp)
target_link_libraries(bullet_unittest tf)
-
rospack_add_executable(btTest test/quaternion.cpp)
rospack_add_executable(testListener test/testListener.cpp)
target_link_libraries(testListener tf)
@@ -36,3 +40,8 @@
target_link_libraries(testBroadcaster tf)
rospack_add_executable(transform_sender src/transform_sender.cpp)
+rospack_add_executable(test_notifier test/test_notifier.cpp)
+rospack_is_test_harness(test_notifier)
+rospack_add_gtest_build_flags(test_notifier)
+target_link_libraries(test_notifier tf ${Boost_LIBRARIES})
+rospack_add_rostest(test/test_notifier.xml)
Added: pkg/trunk/util/tf/include/tf/message_notifier.h
===================================================================
--- pkg/trunk/util/tf/include/tf/message_notifier.h (rev 0)
+++ pkg/trunk/util/tf/include/tf/message_notifier.h 2008-11-11 23:16:51 UTC (rev 6552)
@@ -0,0 +1,439 @@
+/*
+ * 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, Inc. 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.
+ */
+
+/** \author Josh Faust */
+
+#include <ros/node.h>
+#include <tf/tf.h>
+#include <tf/tfMessage.h>
+
+#include <string>
+#include <list>
+#include <vector>
+#include <boost/function.hpp>
+#include <boost/bind.hpp>
+#include <boost/shared_ptr.hpp>
+#include <boost/thread.hpp>
+
+/// \todo remove backward compatability
+#include "rosTF/TransformArray.h"
+
+namespace tf
+{
+
+/**
+ * \brief For internal use only.
+ *
+ * In general, allocating memory inside a library's header using new/delete is a good way to cause difficult to find bugs.
+ * This provides a way for MessageNotifier to allocate from within the tf library, rather than within the calling library.
+ */
+void* notifierAllocate(uint32_t size);
+/**
+ * \brief For internal use only.
+ *
+ * In general, allocating memory inside a library's header using new/delete is a good way to cause difficult to find bugs.
+ * This provides a way for MessageNotifier to allocate from within the tf library, rather than within the calling library.
+ */
+void notifierDeallocate(void* p);
+
+class Transformer;
+
+/**
+ * \class MessageNotifier
+ * \brief Queues messages that include a Header until there is transform data available for the time the timestamp on the message.
+ *
+ * For use instead of extrapolation, MessageNotifier provides a way of waiting until it is possible to transform a message
+ * before getting a callback notifying that the message is available.
+ *
+ * \section callback THE CALLBACK
+ * The callback takes one argument, which is a boost shared_ptr to a message. MessageNotifier provides a MessagePtr typedef,
+ * so the signature of the callback is:
+\verbatim
+void funcName(const MessageNotifier<Message>::MessagePtr& message);
+\endverbatim
+ *
+ * A bare function can be passed directly as the callback. Methods must be passed using boost::bind. For example
+\verbatim
+boost::bind(&MyObject::funcName, this, _1);
+\endverbatim
+ *
+ * The message is \b not locked when your callback is invoked.
+ *
+ * \section threading THREADING
+ * MessageNotifier spins up a single thread to call your callback from, so that it's possible to do a lot of work in your callback
+ * without blocking the rest of the application.
+ *
+ * \section linking_boost LINKING BOOST
+ * MessageNotifier uses boost::thread. Since MessageNotifier is entirely implemented in a header (because it is templated),
+ * you must link against boost thread. An example of doing so in CMake:
+\verbatim
+find_package(Boost 0 REQUIRED COMPONENTS thread)
+include_directories(${Boost_INCLUDE_DIRS})
+link_directories(${Boost_LIBRARY_DIRS})
+target_link_libraries(tf ${Boost_LIBRARIES})
+\endverbatim
+ */
+template<class Message>
+class MessageNotifier
+{
+public:
+ typedef boost::shared_ptr<Message> MessagePtr;
+ typedef boost::function<void(const MessagePtr&)> Callback;
+
+ /**
+ * \brief Constructor
+ * \param tf The Transformer to use for checking if the transform data is available
+ * \param node The ros::node to subscribe on
+ * \param callback The function to call when a message is ready to be transformed
+ * \param topic The topic to listen on
+ * \param target_frame The frame we need to be able to transform to before a message is ready
+ * \param queue_size The number of messages to keep around waiting for transform data. This is passed directly to ros::node::subscribe.
+ * \note A queue size of 0 means infinite, which is dangerous
+ */
+ MessageNotifier(Transformer* tf, ros::node* node, Callback callback, const std::string& topic, const std::string& target_frame, uint32_t queue_size)
+ : tf_(tf)
+ , node_(node)
+ , callback_(callback)
+ , target_frame_(target_frame)
+ , queue_size_(queue_size)
+ , message_count_(0)
+ , destructing_(false)
+ {
+ setTopic(topic);
+
+ node_->subscribe("/tf_message", transforms_message_, &MessageNotifier::incomingTFMessage, this, 1);
+ node_->subscribe("/TransformArray", old_transforms_message_, &MessageNotifier::incomingOldTFMessage, this, 1);
+
+ thread_handle_ = new boost::thread(boost::bind(&MessageNotifier::workerThread, this));
+ }
+
+ /**
+ * \brief Destructor
+ */
+ ~MessageNotifier()
+ {
+ unsubscribeFromMessage();
+
+ node_->unsubscribe("/tf_message", &MessageNotifier::incomingTFMessage, this);
+ node_->unsubscribe("/TransformArray", &MessageNotifier::incomingOldTFMessage, this);
+
+ // Tell the worker thread that we're destructing
+ destructing_ = true;
+ new_data_.notify_all();
+
+ // Wait for the worker thread to exit
+ thread_handle_->join();
+
+ delete thread_handle_;
+ }
+
+ /**
+ * \brief Set the frame you need to be able to transform to before getting a message callback
+ */
+ void setTargetFrame(const std::string& target_frame)
+ {
+ boost::mutex::scoped_lock lock(queue_mutex_);
+
+ target_frame_ = target_frame;
+ new_data_.notify_all();
+ }
+
+ /**
+ * \brief Set the topic to listen on
+ */
+ void setTopic(const std::string& topic)
+ {
+ unsubscribeFromMessage();
+
+ topic_ = topic;
+
+ subscribeToMessage();
+ }
+
+ /**
+ * \brief Clear any messages currently in the queue
+ * \note Note that if callbacks are currently being invoked, this does *not*
+ */
+ void clear()
+ {
+ boost::mutex::scoped_lock notify_lock(notify_mutex_);
+ boost::mutex::scoped_lock list_lock(queue_mutex_);
+
+ messages_.clear();
+ }
+
+private:
+
+ typedef std::vector<MessagePtr> V_Message;
+ typedef std::list<MessagePtr> L_Message;
+
+ /**
+ * \brief Gather any messages ready to be transformed
+ * \param to_notify Filled with the messages ready to be transformed, in the order they were received
+ * \note Assumes the message list is already locked
+ */
+ void gatherReadyMessages(V_Message& to_notify)
+ {
+ to_notify.reserve(message_count_);
+
+ int i = 0;
+
+ typename L_Message::iterator it = messages_.begin();
+ typename L_Message::iterator end = messages_.end();
+ for (; it != end; ++i)
+ {
+ MessagePtr& message = *it;
+
+ /// \todo Once the canTransform function is implemented, don't do things this way
+ try
+ {
+ // Attempt to transform from the source frame to the target frame at the message's timestamp
+ Stamped<Vector3> v(Vector3(0.0, 0.0, 0.0), message->header.stamp, message->header.frame_id);
+ // If the transform fails an exception will be thrown, so the push_back statement will not be hit
+ tf_->transformVector(target_frame_, v, v);
+
+ // If we get here the transform succeeded, so push the message onto the notify list, and erase it from or message list
+ to_notify.push_back(message);
+
+ it = messages_.erase(it);
+ --message_count_;
+
+ //printf("Message %d ready, count now %d\n", i, message_count_);
+ }
+ catch(TransformException& e)
+ {
+ //printf("message %d: Transforms unavailable\n", i);
+ // If the transform failed, move on to the next message
+ ++it;
+ }
+ }
+ }
+
+ /**
+ * \brief Calls the notification callback on each message in the passed vector
+ * \param to_notify The list of messages to call the callback on
+ */
+ void notify(V_Message& to_notify)
+ {
+ boost::mutex::scoped_lock lock(notify_mutex_);
+
+ typename V_Message::iterator it = to_notify.begin();
+ typename V_Message::iterator end = to_notify.end();
+ for (; it != end; ++it)
+ {
+ callback_(*it);
+ }
+ }
+
+ /**
+ * \brief Adds messages into the message list, removing old messages if necessary
+ */
+ void processNewMessages(V_Message& messages)
+ {
+ typename V_Message::iterator it = messages.begin();
+ typename V_Message::iterator end = messages.end();
+ for (; it != end; ++it)
+ {
+ MessagePtr& message = *it;
+
+ // If this message is about to push us past our queue size, erase the oldest message
+ if (message_count_ + 1 > queue_size_)
+ {
+ messages_.pop_front();
+ --message_count_;
+
+ //printf("Removed old message, count now %d\n", message_count_);
+ }
+
+ // Add the message to our list
+ messages_.push_back(message);
+ ++message_count_;
+
+ //printf("Added message, count now %d\n", message_count_);
+ }
+ }
+
+ /**
+ * \brief Entry point into the worker thread that does all our actual work, including calling the notification callback
+ */
+ void workerThread()
+ {
+ V_Message to_notify;
+ while (!destructing_)
+ {
+ V_Message local_queue;
+
+ {
+ //printf("workerThread: locking\n");
+ boost::mutex::scoped_lock lock(queue_mutex_);
+
+ // Wait for new data to be available
+ if (message_count_ == 0 && !new_transforms_ && !new_messages_ && !destructing_)
+ {
+ //printf("workerThread: waiting\n");
+ new_data_.wait(lock);
+ }
+
+ // If we're destructing, break out of the loop
+ if (destructing_)
+ {
+ break;
+ }
+
+ local_queue.swap(new_message_queue_);
+
+ new_messages_ = false;
+ }
+
+ processNewMessages(local_queue);
+
+ // Outside the lock, gather and notify that the messages are ready
+ gatherReadyMessages(to_notify);
+
+ new_transforms_ = false;
+
+ notify(to_notify);
+ to_notify.clear();
+ }
+ }
+
+ /**
+ * \brief Subscribe to the message topic
+ */
+ void subscribeToMessage()
+ {
+ if (!topic_.empty())
+ {
+ node_->subscribe(topic_, message_, &MessageNotifier::incomingMessage, this, queue_size_);
+ }
+ }
+
+ /**
+ * \brief Unsubscribe from the message topic
+ */
+ void unsubscribeFromMessage()
+ {
+ if (!topic_.empty())
+ {
+ node_->unsubscribe(topic_, &MessageNotifier::incomingMessage, this);
+ }
+ }
+
+ /**
+ * \class MessageDeleter
+ * \brief Since we allocate with our own special functions, we need to also delete using them. This provides a deletion interface for the boost::shared_ptr
+ */
+ class MessageDeleter
+ {
+ public:
+ void operator()(Message* m)
+ {
+ m->~Message();
+ notifierDeallocate(m);
+ }
+ };
+
+ /**
+ * \brief Callback that happens when we receive a message on the message topic
+ */
+ void incomingMessage()
+ {
+ //printf("incoming message\n");
+ // Allocate our new message and placement-new it
+ Message* mem = (Message*)notifierAllocate(sizeof(Message));
+ new(mem) Message();
+
+ // Create a boost::shared_ptr from the message, with our custom deleter
+ MessagePtr message(mem, MessageDeleter());
+ // Copy the message
+ *message = message_;
+
+ {
+ boost::mutex::scoped_lock lock(queue_mutex_);
+
+ new_message_queue_.push_back(message);
+
+ //printf("Added message, count now %d\n", message_count_);
+
+ new_messages_ = true;
+
+ // Notify the worker thread that there is new data available
+ new_data_.notify_all();
+ }
+ }
+
+ /**
+ * \brief Callback that happens when we receive a message on the TF message topic
+ */
+ void incomingTFMessage()
+ {
+ // Notify the worker thread that there is new data available
+ new_data_.notify_all();
+ new_transforms_ = true;
+ }
+
+ /**
+ * \brief Callback that happens when we receive a message on the old TF TransformArray message topic
+ */
+ void incomingOldTFMessage()
+ {
+ //printf("incoming transforms\n");
+ // Notify the worker thread that there is new data available
+ new_data_.notify_all();
+ new_transforms_ = true;
+ }
+
+ Transformer* tf_; ///< The Transformer used to determine if transformation data is available
+ ros::node* node_; ///< The node used to subscribe to the topic
+ Callback callback_; ///< The callback to call when a message is ready
+ std::string target_frame_; ///< The frame we need to be able to transform to before a message is ready
+ std::string topic_; ///< The topic to listen on
+ uint32_t queue_size_; ///< The maximum number of messages we queue up
+
+ L_Message messages_; ///< The message list
+ uint32_t message_count_; ///< The number of messages in the list. Used because messages_.size() has linear cost
+
+ Message message_; ///< The incoming message
+
+ tfMessage transforms_message_; ///< The incoming TF transforms message
+ rosTF::TransformArray old_transforms_message_; ///< The incoming old TF (rosTF) TransformArray message
+
+ bool destructing_; ///< Used to notify the worker thread that it needs to shutdown
+ boost::thread* thread_handle_; ///< Thread handle for the worker thread
+ boost::condition new_data_; ///< Condition variable used for waking the worker thread
+ bool new_messages_; ///< Used to skip waiting on new_data_ if new messages have come in while calling back
+ volatile bool new_transforms_; ///< Used to skip waiting on new_data_ if new transforms have come in while calling back or transforming data
+ V_Message new_message_queue_; ///< Queues messages to later be processed by the worker thread
+ boost::mutex queue_mutex_; ///< The mutex used for locking message queue operations
+
+ boost::mutex notify_mutex_; ///< Mutex used for locking the notification process
+};
+
+
+} // namespace tf
Modified: pkg/trunk/util/tf/manifest.xml
===================================================================
--- pkg/trunk/util/tf/manifest.xml 2008-11-11 23:15:04 UTC (rev 6551)
+++ pkg/trunk/util/tf/manifest.xml 2008-11-11 23:16:51 UTC (rev 6552)
@@ -16,6 +16,9 @@
<depend package="bullet"/>
<depend package="laser_scan_utils"/>
<depend package="rosTF"/> <!-- For backwards compatibility only Fixme remove /-->
+
+<sysdepend os="ubuntu" version="8.04-hardy" package="libboost-dev"/>
+<sysdepend os="ubuntu" version="8.04-hardy" package="libboost-thread-dev"/>
<export>
<cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -ltf"/>
</export>
Added: pkg/trunk/util/tf/src/message_notifier.cpp
===================================================================
--- pkg/trunk/util/tf/src/message_notifier.cpp (rev 0)
+++ pkg/trunk/util/tf/src/message_notifier.cpp 2008-11-11 23:16:51 UTC (rev 6552)
@@ -0,0 +1,47 @@
+/*
+ * 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, Inc. 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.
+ */
+
+/** \author Josh Faust */
+
+#include "tf/message_notifier.h"
+
+namespace tf
+{
+
+void* notifierAllocate(uint32_t size)
+{
+ return malloc(size);
+}
+
+void notifierDeallocate(void* p)
+{
+ free(p);
+}
+
+} // namespace tf
Modified: pkg/trunk/util/tf/test/testBroadcaster.cpp
===================================================================
--- pkg/trunk/util/tf/test/testBroadcaster.cpp 2008-11-11 23:15:04 UTC (rev 6551)
+++ pkg/trunk/util/tf/test/testBroadcaster.cpp 2008-11-11 23:16:51 UTC (rev 6552)
@@ -1,10 +1,10 @@
/*
* 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
@@ -13,7 +13,7 @@
* * Neither the name of the Willow Garage, Inc. 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
@@ -73,7 +73,7 @@
//Construct/initialize the server
testBroadcaster myTestBroadcaster;
-
+
while(myTestBroadcaster.ok())
{
//Send some data
Added: pkg/trunk/util/tf/test/test_notifier.cpp
===================================================================
--- pkg/trunk/util/tf/test/test_notifier.cpp (rev 0)
+++ pkg/trunk/util/tf/test/test_notifier.cpp 2008-11-11 23:16:51 UTC (rev 6552)
@@ -0,0 +1,381 @@
+/*
+ * 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, Inc. 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.
+ */
+
+/** \author Josh Faust */
+
+#include <tf/message_notifier.h>
+#include <tf/transform_listener.h>
+#include <tf/transform_broadcaster.h>
+#include <std_msgs/PositionStamped.h>
+#include <boost/bind.hpp>
+
+#include <gtest/gtest.h>
+
+using namespace tf;
+
+ros::node* g_node = NULL;
+TransformListener* g_tf = NULL;
+TransformBroadcaster* g_broadcaster = NULL;
+
+class Notification
+{
+public:
+ Notification(int expected_count)
+ : count_(0)
+ , expected_count_(expected_count)
+ {
+ boost::detail::thread::lock_ops<boost::timed_mutex>::lock(mutex_);
+ }
+
+ ~Notification()
+ {
+ if (count_ < expected_count_)
+ {
+ boost::detail::thread::lock_ops<boost::timed_mutex>::unlock(mutex_);
+ }
+ }
+
+ void notify(const MessageNotifier<std_msgs::PositionStamped>::MessagePtr& message)
+ {
+ ++count_;
+
+ //printf("Notify: %d\n", count_);
+
+ if (count_ == expected_count_)
+ {
+ boost::detail::thread::lock_ops<boost::timed_mutex>::unlock(mutex_);
+ }
+ }
+
+ int count_;
+ int expected_count_;
+
+ boost::timed_mutex mutex_;
+};
+
+template<class T>
+class Counter
+{
+public:
+ Counter(const std::string& topic, int expected_count)
+ : count_(0)
+ , expected_count_(expected_count)
+ , topic_(topic)
+ {
+ boost::detail::thread::lock_ops<boost::timed_mutex>::lock(mutex_);
+
+ g_node->subscribe(topic_, message_, &Counter::callback, this, 0);
+ }
+
+ ~Counter()
+ {
+ g_node->unsubscribe(topic_, &Counter::callback, this);
+
+ if (count_ < expected_count_)
+ {
+ boost::detail::thread::lock_ops<boost::timed_mutex>::unlock(mutex_);
+ }
+ }
+
+ void callback()
+ {
+ ++count_;
+
+ //printf("Counter: %d\n", count_);
+
+ if (count_ == expected_count_)
+ {
+ boost::detail::thread::lock_ops<boost::timed_mutex>::unlock(mutex_);
+ }
+ }
+
+ T message_;
+
+ int count_;
+ int expected_count_;
+ std::string topic_;
+
+ boost::timed_mutex mutex_;
+};
+
+TEST(MessageNotifier, noTransforms)
+{
+ Notification n(1);
+ MessageNotifier<std_msgs::PositionStamped>* notifier = new MessageNotifier<std_msgs::PositionStamped>(g_tf, g_node, boost::bind(&Notification::notify, &n, _1), "test_message", "frame1", 1);
+ std::auto_ptr<MessageNotifier<std_msgs::PositionStamped> > notifier_ptr(notifier);
+
+ Counter<std_msgs::PositionStamped> c("test_message", 1);
+
+ ros::Duration(0.2).sleep();
+
+ std_msgs::PositionStamped msg;
+ msg.header.stamp = ros::Time::now();
+ msg.header.frame_id = "frame2";
+ g_node->publish("test_message", msg);
+
+ ros::Duration(0.2).sleep();
+
+ EXPECT_EQ(0, n.count_);
+}
+
+TEST(MessageNotifier, preexistingTransforms)
+{
+ Notification n(1);
+ Counter<rosTF::TransformArray> c("TransformArray", 1); /// \todo Switch this to tf_message once rosTF goes away completely
+ MessageNotifier<std_msgs::PositionStamped>* notifier = new MessageNotifier<std_msgs::PositionStamped>(g_tf, g_node, boost::bind(&Notification::notify, &n, _1), "test_message", "frame1", 1);
+ std::auto_ptr<MessageNotifier<std_msgs::PositionStamped> > notifier_ptr(notifier);
+
+ ros::Duration(0.2).sleep();
+
+ ros::Time stamp = ros::Time::now();
+
+ g_broadcaster->sendTransform(btTransform(btQuaternion(0,0,0), btVector3(1,2,3)), stamp, "frame1", "frame2");
+
+ {
+ boost::xtime xt;
+ boost::xtime_get(&xt, boost::TIME_UTC);
+ xt.sec += 10;
+
+ boost::timed_mutex::scoped_timed_lock lock(c.mutex_, xt);
+
+ EXPECT_EQ(true, lock.locked());
+ }
+
+ std_msgs::PositionStamped msg;
+ msg.header.stamp = stamp;
+ msg.header.frame_id = "frame2";
+ g_node->publish("test_message", msg);
+
+ {
+ boost::xtime xt;
+ boost::xtime_get(&xt, boost::TIME_UTC);
+ xt.sec += 10;
+
+ boost::timed_mutex::scoped_timed_lock lock(n.mutex_, xt);
+
+ EXPECT_EQ(true, lock.locked());
+ }
+
+ EXPECT_EQ(1, n.count_);
+}
+
+TEST(MessageNotifier, postTransforms)
+{
+ Notification n(1);
+ MessageNotifier<std_msgs::PositionStamped>* notifier = new MessageNotifier<std_msgs::PositionStamped>(g_tf, g_node, boost::bind(&Notification::notify, &n, _1), "test_message", "frame3", 1);
+ std::auto_ptr<MessageNotifier<std_msgs::PositionStamped> > notifier_ptr(notifier);
+
+ Counter<std_msgs::PositionStamped> c("test_message", 1);
+
+ ros::Duration(0.2).sleep();
+
+ ros::Time stamp = ros::Time::now();
+
+ std_msgs::PositionStamped msg;
+ msg.header.stamp = stamp;
+ msg.header.frame_id = "frame4";
+ g_node->publish("test_message", msg);
+
+ {
+ boost::xtime xt;
+ boost::xtime_get(&xt, boost::TIME_UTC);
+ xt.sec += 10;
+
+ boost::timed_mutex::scoped_timed_lock lock(c.mutex_, xt);
+
+ EXPECT_EQ(true, lock.locked());
+ }
+
+ g_broadcaster->sendTransform(btTransform(btQuaternion(0,0,0), btVector3(1,2,3)), stamp, "frame3", "frame4");
+
+ {
+ boost::xtime xt;
+ boost::xtime_get(&xt, boost::TIME_UTC);
+ xt.sec += 10;
+
+ boost::timed_mutex::scoped_timed_lock lock(n.mutex_, xt);
+
+ EXPECT_EQ(true, lock.locked());
+ }
+
+ EXPECT_EQ(1, n.count_);
+}
+
+TEST(MessageNotifier, queueSize)
+{
+ Notification n(10);
+ MessageNotifier<std_msgs::PositionStamped>* notifier = new MessageNotifier<std_msgs::PositionStamped>(g_tf, g_node, boost::bind(&Notification::notify, &n, _1), "test_message", "frame5", 10);
+ std::auto_ptr<MessageNotifier<std_msgs::PositionStamped> > notifier_ptr(notifier);
+
+ Counter<std_msgs::PositionStamped> c("test_message", 20);
+
+ ros::Duration(0.2).sleep();
+
+ ros::Time stamp = ros::Time::now();
+
+ for (int i = 0; i < 20; ++i)
+ {
+ std_msgs::PositionStamped msg;
+ msg.header.stamp = stamp;
+ msg.header.frame_id = "frame6";
+ g_node->publish("test_message", msg);
+
+ ros::Duration(0.01).sleep();
+ }
+
+ {
+ boost::xtime xt;
+ boost::xtime_get(&xt, boost::TIME_UTC);
+ xt.sec += 10;
+
+ boost::timed_mutex::scoped_timed_lock lock(c.mutex_, xt);
+
+ EXPECT_EQ(true, lock.locked());
+ }
+
+ g_broadcaster->sendTransform(btTransform(btQuaternion(0,0,0), btVector3(1,2,3)), stamp, "frame5", "frame6");
+
+ {
+ boost::xtime xt;
+ boost::xtime_get(&xt, boost::TIME_UTC);
+ xt.sec += 10;
+
+ boost::timed_mutex::scoped_timed_lock lock(n.mutex_, xt);
+
+ EXPECT_EQ(true, lock.locked());
+ }
+
+ EXPECT_EQ(10, n.count_);
+}
+
+TEST(MessageNotifier, setTopic)
+{
+ Notification n(1);
+ Counter<rosTF::TransformArray> c("TransformArray", 1); /// \todo Switch this to tf_message once rosTF goes away completely
+ MessageNotifier<std_msgs::PositionStamped>* notifier = new MessageNotifier<std_msgs::PositionStamped>(g_tf, g_node, boost::bind(&Notification::notify, &n, _1), "test_message", "frame7", 1);
+ std::auto_ptr<MessageNotifier<std_msgs::PositionStamped> > notifier_ptr(notifier);
+ notifier->setTopic("test_message2");
+
+ ros::Duration(0.2).sleep();
+
+ ros::Time stamp = ros::Time::now();
+
+ g_broadcaster->sendTransform(btTransform(btQuaternion(0,0,0), btVector3(1,2,3)), stamp, "frame7", "frame8");
+
+ {
+ boost::xtime xt;
+ boost::xtime_get(&xt, boost::TIME_UTC);
+ xt.sec += 10;
+
+ boost::timed_mutex::scoped_timed_lock lock(c.mutex_, xt);
+
+ EXPECT_EQ(true, lock.locked());
+ }
+
+ std_msgs::PositionStamped msg;
+ msg.header.stamp = stamp;
+ msg.header.frame_id = "frame8";
+ g_node->publish("test_message2", msg);
+
+ {
+ boost::xtime xt;
+ boost::xtime_get(&xt, boost::TIME_UTC);
+ xt.sec += 10;
+
+ boost::timed_mutex::scoped_timed_lock lock(n.mutex_, xt);
+
+ EXPECT_EQ(true, lock.locked());
+ }
+
+ EXPECT_EQ(1, n.count_);
+}
+
+TEST(MessageNotifier, setTargetFrame)
+{
+ Notification n(1);
+ Counter<rosTF::TransformArray> c("TransformArray", 1); /// \todo Switch this to tf_message once rosTF goes away completely
+ MessageNotifier<std_msgs::PositionStamped>* notifier = new MessageNotifier<std_msgs::PositionStamped>(g_tf, g_node, boost::bind(&Notification::notify, &n, _1), "test_message", "frame9", 1);
+ std::auto_ptr<MessageNotifier<std_msgs::PositionStamped> > notifier_ptr(notifier);
+ notifier->setTargetFrame("frame1000");
+
+ ros::Duration(0.2).sleep();
+
+ ros::Time stamp = ros::Time::now();
+
+ g_broadcaster->sendTransform(btTransform(btQuaternion(0,0,0), btVector3(1,2,3)), stamp, "frame1000", "frame10");
+
+ {
+ boost::xtime xt;
+ boost::xtime_get(&xt, boost::TIME_UTC);
+ xt.sec += 10;
+
+ boost::timed_mutex::scoped_timed_lock lock(c.mutex_, xt);
+
+ EXPECT_EQ(true, lock.locked());
+ }
+
+ std_msgs::PositionStamped msg;
+ msg.header.stamp = stamp;
+ msg.header.frame_id = "frame10";
+ g_node->publish("test_message", msg);
+
+ {
+ boost::xtime xt;
+ boost::xtime_get(&xt, boost::TIME_UTC);
+ xt.sec += 10;
+
+ boost::timed_mutex::scoped_timed_lock lock(n.mutex_, xt);
+
+ EXPECT_EQ(true, lock.locked());
+ }
+
+ EXPECT_EQ(1, n.count_);
+}
+
+int main(int argc, char** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ ros::init(argc, argv);
+ g_node = new ros::node("test_notifier");
+ g_node->advertise<std_msgs::PositionStamped>("test_message", 0);
+ g_node->advertise<std_msgs::PositionStamped>("test_message2", 0);
+
+ g_tf = new TransformListener(*g_node);
+ g_broadcaster = new TransformBroadcaster(*g_node);
+
+ int ret = RUN_ALL_TESTS();
+
+ delete g_broadcaster;
+ delete g_tf;
+
+ ros::fini();
+ delete g_node;
+
+ return ret;
+}
Added: pkg/trunk/util/tf/test/test_notifier.xml
===================================================================
--- pkg/trunk/util/tf/test/test_notifier.xml (rev 0)
+++ pkg/trunk/util/tf/test/test_notifier.xml 2008-11-11 23:16:51 UTC (rev 6552)
@@ -0,0 +1,3 @@
+<launch>
+ <test test-name="test_notifier" pkg="tf" type="test_notifier"/>
+</launch>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
