p36850791
/
kinetic-devel
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge from Node removal branch, removing all Node references from the ROS stack

This commit is contained in:
Josh Faust 2009-09-17 21:21:47 +00:00
parent 29b9b0332f
commit 930433fe8f
76 changed files with 806 additions and 4263 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

112
core/roscpp/include/ros/common.h
View File

@ -49,8 +49,6 @@
namespace ros
{
class Node;
extern Node *g_node;
/**
* \deprecated Use ros::package::getPath() in the roslib package instead
@ -74,116 +72,6 @@ public:
: buf(buf), num_bytes(num_bytes) { }
};
class AbstractFunctor
{
public:
virtual void call() = 0;
virtual ~AbstractFunctor() { }
virtual bool operator==(const AbstractFunctor &rhs) { return equals(&rhs); }
private:
virtual bool equals(const AbstractFunctor *) = 0;
};
template<class T>
class MethodFunctor : public AbstractFunctor
{
public:
MethodFunctor(T *_obj, void (T::*_mp)())
: mp_(_mp)
, mp_data_(NULL)
, obj_(_obj)
, user_data_(NULL)
{ }
MethodFunctor(T *_obj, void (T::*_mp_data)(void *), void *_user_data_)
: mp_(NULL)
, mp_data_(_mp_data)
, obj_(_obj)
, user_data_(_user_data_)
{ }
virtual void call()
{
if (mp_)
{
(*obj_.*mp_)();
}
else if (mp_data_)
{
(*obj_.*mp_data_)(user_data_);
}
else
{
ROS_BREAK();
(void)0;
}
}
private:
void (T::*mp_)();
void (T::*mp_data_)(void *);
T* obj_;
void* user_data_;
virtual bool equals(const AbstractFunctor *_rhs)
{
const MethodFunctor<T> *rhs = dynamic_cast<const MethodFunctor<T> *>(_rhs);
if (!rhs)
return false;
return rhs->mp_ == mp_ && rhs->mp_data_ == mp_data_ &&
rhs->obj_ == obj_ && rhs->user_data_ == user_data_;
}
};
class FunctionFunctor : public AbstractFunctor
{
public:
FunctionFunctor(void (*_fp)())
: fp_(_fp)
, fp_data_(NULL)
, user_data_(NULL)
{ }
FunctionFunctor(void (*_fp_data)(void *), void *_user_data_)
: fp_(NULL)
, fp_data_(_fp_data)
, user_data_(_user_data_)
{ }
virtual void call()
{
if (fp_)
{
(*fp_)();
}
else if (fp_data_)
{
(*fp_data_)(user_data_);
}
else
{
ROS_BREAK();
(void)0;
}
}
private:
void (*fp_)();
void (*fp_data_)(void *);
void* user_data_;
virtual bool equals(const AbstractFunctor *_rhs)
{
const FunctionFunctor* rhs = dynamic_cast<const FunctionFunctor*>(_rhs);
if (!rhs)
{
return false;
}
return rhs->fp_ == fp_ && rhs->fp_data_ == fp_data_ && rhs->user_data_ == user_data_;
}
};
}
#endif

15
core/roscpp/include/ros/init.h
View File

@ -98,17 +98,6 @@ void init(const M_string& remappings, const std::string& name, uint32_t options
*/
void init(const VP_string& remapping_args, const std::string& name, uint32_t options = 0);
/**
* \deprecated Use one of the init() functions that takes a name, and use the NodeHandle API
*
*/
ROSCPP_DEPRECATED void init(int &argc, char **argv);
/**
* \deprecated Use one of the init() functions that takes a name, and use the NodeHandle API
*/
ROSCPP_DEPRECATED void init(const VP_string& remapping_args);
/**
* \brief Returns whether or not ros::init() has been called
*/
@ -155,8 +144,7 @@ void spinOnce();
/** \brief Check whether it's time to exit.
*
* This method checks the value of Node::ok(), to see whether it's yet time
* to exit. ok() is false once ros::shutdown() has been called
* ok() becomes false once ros::shutdown() has been called and is finished
*
* \return true if we're still OK, false if it's time to exit
*/
@ -177,6 +165,7 @@ void shutdown();
void requestShutdown();
void start();
bool isStarted();
/**
* \brief Returns a pointer to the global default callback queue.

6
core/roscpp/include/ros/message.h
View File

@ -32,7 +32,6 @@
#include "ros/assert.h"
#include <string>
#include <string.h>
#include <boost/thread/mutex.hpp>
#include <boost/shared_ptr.hpp>
#define ROSCPP_MESSAGE_HAS_DEFINITION
@ -44,7 +43,6 @@ typedef std::map<std::string, std::string> M_string;
class Message
{
friend class Node;
public:
typedef boost::shared_ptr<Message> Ptr;
typedef boost::shared_ptr<Message const> ConstPtr;
@ -63,13 +61,9 @@ public:
virtual uint32_t serializationLength() const = 0;
virtual uint8_t *serialize(uint8_t *write_ptr, uint32_t seq) const = 0;
virtual uint8_t *deserialize(uint8_t *read_ptr) = 0;
ROSCPP_DEPRECATED void lock() { __mutex.lock(); }
ROSCPP_DEPRECATED void unlock() { __mutex.unlock(); }
uint32_t __serialized_length;
boost::shared_ptr<M_string> __connection_header;
private:
boost::mutex __mutex;
};
typedef boost::shared_ptr<Message> MessagePtr;

1378
core/roscpp/include/ros/node.h
View File

File diff suppressed because it is too large Load Diff

16
core/roscpp/include/ros/node_handle.h
View File

@ -53,7 +53,6 @@
namespace ros
{
class Node;
class NodeHandleBackingCollection;
/**
@ -85,8 +84,8 @@ public:
/**
* \brief Constructor
*
* When a NodeHandle is constructed, it checks to see if the global node state has already been started. If so, it increments the reference count
* on the global Node. If not, it starts the node and sets the reference count to 1.
* When a NodeHandle is constructed, it checks to see if the global node state has already been started. If so, it increments a global reference count.
* If not, it starts the node with ros::start() and sets the reference count to 1.
*
* \param ns Namespace for this NodeHandle. This acts in addition to any namespace assigned to this ROS node.
* eg. If the node's namespace is "/a" and the namespace passed in here is "b", all topics/services/parameters
@ -130,7 +129,7 @@ NodeHandle child(parent.getNamespace() + "/" + ns, remappings);
/**
* \brief Destructor
*
* When a NodeHandle is destroyed, it decrements the global Node's reference count by 1, and if the reference count is now 0, shuts down the node.
* When a NodeHandle is destroyed, it decrements a global reference count by 1, and if the reference count is now 0, shuts down the node.
*/
~NodeHandle();
@ -1038,20 +1037,11 @@ if (handle)
*/
ROSCPP_DEPRECATED void getSubscribedTopics(V_string& topics) const;
/**
* \deprecated Node is going away...
*/
ROSCPP_DEPRECATED Node* getNode() const;
/**
* \brief deprecated in favor of ros::this_node::getName()
*/
ROSCPP_DEPRECATED const std::string& getName() const;
/**
* \deprecated in favor of ros::names::getRemappings()
*/
ROSCPP_DEPRECATED static const V_string& getParsedArgs();
/** \brief Get the hostname where the master runs.
*

2
core/roscpp/include/ros/ros.h
View File

@ -37,6 +37,8 @@
#include "ros/time.h"
#include "ros/rate.h"
#include "ros/console.h"
#include "ros/assert.h"
#include "ros/common.h"
#include "ros/types.h"

1
core/roscpp/include/ros/rosout_appender.h
View File

@ -45,7 +45,6 @@
namespace ros
{
class Node;
class Publication;
typedef boost::shared_ptr<Publication> PublicationPtr;
typedef boost::weak_ptr<Publication> PublicationWPtr;

2
core/roscpp/include/ros/service_manager.h
View File

@ -114,7 +114,7 @@ public:
*/
bool unadvertiseService(const std::string& serv_name);
bool advertiseService(const AdvertiseServiceOptions& ops, int32_t thread_pool_size);
bool advertiseService(const AdvertiseServiceOptions& ops);
void start();
void shutdown();

31
core/roscpp/include/ros/service_publication.h
View File

@ -62,7 +62,7 @@ class ServicePublication : public boost::enable_shared_from_this<ServicePublicat
{
public:
ServicePublication(const std::string& name, const std::string &md5sum, const std::string& data_type, const std::string& request_data_type,
const std::string& response_data_type, const ServiceMessageHelperPtr& helper, int thread_pool_size, CallbackQueueInterface* queue,
const std::string& response_data_type, const ServiceMessageHelperPtr& helper, CallbackQueueInterface* queue,
const VoidPtr& tracked_object);
~ServicePublication();
@ -96,49 +96,20 @@ public:
const std::string& getName() { return name_; }
private:
/**
* \brief Queues up a request to be serviced by one of our worker threads
*/
void enqueueRequest(boost::shared_array<uint8_t> buf, size_t num_bytes, const ServiceClientLinkPtr& link);
/**
* \brief Deserializes a request and calls a callback on it, notifying link when it's done
*/
void callCallback(boost::shared_array<uint8_t> buf, size_t num_bytes, const ServiceClientLinkPtr& link);
void dropAllConnections();
void threadFunc();
std::string name_;
std::string md5sum_;
std::string data_type_;
std::string request_data_type_;
std::string response_data_type_;
ServiceMessageHelperPtr helper_;
boost::mutex callback_mutex_;
int32_t thread_pool_size_;
V_ServiceClientLink client_links_;
boost::mutex client_links_mutex_;
bool dropped_;
boost::thread_group thread_group_;
boost::condition_variable new_request_;
typedef std::vector<boost::thread*> V_threadpointer;
V_threadpointer threads_;
struct RequestInfo
{
boost::shared_array<uint8_t> buf_;
size_t num_bytes_;
ServiceClientLinkPtr link_;
};
typedef boost::shared_ptr<RequestInfo> RequestInfoPtr;
typedef std::queue<RequestInfoPtr> Q_RequestInfo;
Q_RequestInfo request_queue_;
boost::mutex request_queue_mutex_;
CallbackQueueInterface* callback_queue_;
bool has_tracked_object_;
VoidWPtr tracked_object_;

51
core/roscpp/include/ros/subscription.h
View File

@ -60,7 +60,7 @@ typedef boost::shared_ptr<SubscriptionQueue> SubscriptionQueuePtr;
class Subscription : public boost::enable_shared_from_this<Subscription>
{
public:
Subscription(const std::string &name, const std::string& md5sum, const std::string& datatype, bool threaded, int max_queue, const TransportHints& transport_hints);
Subscription(const std::string &name, const std::string& md5sum, const std::string& datatype, const TransportHints& transport_hints);
virtual ~Subscription();
/**
@ -86,16 +86,6 @@ public:
* \brief Returns whether this Subscription has been dropped or not
*/
bool isDropped() { return dropped_; }
/**
* \brief Adds a Functor/message to our list of callbacks/messages. Used for multiple subscriptions to the
* same topic
*/
bool addFunctorMessagePair(AbstractFunctor* cb, Message* m);
/**
* \brief Remove a Functor/message from our list of callbacks/messages.Used for multiple subscriptions to the
* same topic
*/
void removeFunctorMessagePair(AbstractFunctor* cb);
XmlRpc::XmlRpcValue getStats();
void getInfo(XmlRpc::XmlRpcValue& info);
@ -116,18 +106,12 @@ public:
const std::string datatype();
const std::string md5sum();
/**
* \brief Returns true if we update the message pointed to by _msg
*/
bool updatesMessage(const void *_msg);
/**
* \brief Removes a subscriber from our list
*/
void removePublisherLink(const PublisherLinkPtr& pub_link);
const std::string& getName() { return name_; }
int getMaxQueue() { return max_queue_; }
void setMaxQueue(int max_queue);
uint32_t getNumCallbacks() { return callbacks_.size(); }
// We'll keep a list of these objects, representing in-progress XMLRPC
@ -196,13 +180,8 @@ private:
void dropAllConnections();
void subscriptionThreadFunc();
struct CallbackInfo
{
AbstractFunctor* callback_;
Message* message_;
CallbackQueueInterface* callback_queue_;
// Only used if callback_queue_ is non-NULL (NodeHandle API)
@ -228,34 +207,6 @@ private:
S_PendingConnection pending_connections_;
boost::mutex pending_connections_mutex_;
// If threaded is true, then we're running a separate thread (identified
// by callback_thread_) that pulls message from inbox, and invokes the
// callback on each one.
//
// Otherwise, the callback is invoked in place when the message is
// received, and none of this machinery is used.
bool threaded_;
int max_queue_;
boost::thread callback_thread_;
struct MessageInfo
{
MessageInfo()
{}
MessageInfo(const SerializedMessage& m, const boost::shared_ptr<M_string>& connection_header)
: serialized_message_(m)
, connection_header_(connection_header)
{}
SerializedMessage serialized_message_;
boost::shared_ptr<M_string> connection_header_;
};
std::queue<MessageInfo> inbox_;
boost::mutex inbox_mutex_;
boost::condition_variable inbox_cond_;
bool queue_full_;
typedef std::vector<PublisherLinkPtr> V_PublisherLink;
V_PublisherLink publisher_links_;
boost::mutex publisher_links_mutex_;

27
core/roscpp/include/ros/topic_manager.h
View File

@ -67,15 +67,12 @@ public:
void start();
void shutdown();
bool subscribe(const SubscribeOptions& ops, Message* m, AbstractFunctor *cb);
bool unsubscribe(const std::string &topic, AbstractFunctor *afp);
bool advertise(const AdvertiseOptions& ops, bool allow_multiple = false);
bool unadvertise(const std::string &topic, const SubscriberCallbacksPtr& callbacks = SubscriberCallbacksPtr());
bool unsubscribe(const std::string &_topic);
bool unsubscribe(const Message& _msg);
bool subscribe(const SubscribeOptions& ops);
bool unsubscribe(const std::string &_topic, const SubscriptionMessageHelperPtr& helper);
bool advertise(const AdvertiseOptions& ops, const SubscriberCallbacksPtr& callbacks);
bool unadvertise(const std::string &topic, const SubscriberCallbacksPtr& callbacks);
/** @brief Get the list of topics advertised by this node
*
* @param[out] topics The advertised topics
@ -88,15 +85,6 @@ public:
*/
void getSubscribedTopics(V_string& topics);
/** if it finds a pre-existing subscription to the same topic and of the
* same message type, it appends the Functor to the callback vector for
* that subscription. otherwise, it returns false, indicating that a new
* subscription needs to be created.
*/
bool addSubCallback(const std::string &_topic, Message* m, AbstractFunctor *fp,
const SubscriptionMessageHelperPtr& helper, CallbackQueueInterface* callback_queue, int max_queue,
const VoidPtr& tracked_object);
/** @brief Lookup an advertised topic.
*
* This method iterates over advertised_topics, looking for one with name
@ -140,6 +128,13 @@ public:
void publish(const PublicationPtr& p, const Message& m);
private:
/** if it finds a pre-existing subscription to the same topic and of the
* same message type, it appends the Functor to the callback vector for
* that subscription. otherwise, it returns false, indicating that a new
* subscription needs to be created.
*/
bool addSubCallback(const SubscribeOptions& ops);
/** @brief Request a topic
*
* Negotiate a subscriber connection on a topic.

1
core/roscpp/src/libros/CMakeLists.txt
View File

@ -42,7 +42,6 @@ rospack_add_library(ros
service_client_link.cpp
service.cpp
rosout_appender.cpp
node.cpp
callback_queue.cpp
spinner.cpp
node_handle.cpp

67
core/roscpp/src/libros/init.cpp
View File

@ -32,15 +32,8 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
// TEMP to remove warnings during build while things internally still use deprecated APIs
#include "ros/macros.h"
#undef ROSCPP_DEPRECATED
#define ROSCPP_DEPRECATED
// END TEMP
#include "ros/init.h"
#include "ros/names.h"
#include "ros/node.h"
#include "ros/xmlrpc_manager.h"
#include "ros/poll_manager.h"
#include "ros/connection_manager.h"
@ -58,6 +51,9 @@
#include <ros/console.h>
#include <ros/time.h>
#include <roslib/Time.h>
#include <signal.h>
namespace ros
{
@ -228,6 +224,11 @@ void internalCallbackQueueThreadFunc()
}
}
bool isStarted()
{
return g_started;
}
void start()
{
boost::mutex::scoped_lock lock(g_start_mutex);
@ -268,7 +269,7 @@ void start()
ros::AdvertiseServiceOptions ops;
ops.init<roscpp::GetLoggers>(names::resolve("~get_loggers"), getLoggers);
ops.callback_queue = getInternalCallbackQueue().get();
ServiceManager::instance()->advertiseService(ops, 0);
ServiceManager::instance()->advertiseService(ops);
}
if (!g_shutting_down)
@ -277,7 +278,7 @@ void start()
ros::AdvertiseServiceOptions ops;
ops.init<roscpp::SetLoggerLevel>(names::resolve("~set_logger_level"), setLoggerLevel);
ops.callback_queue = getInternalCallbackQueue().get();
ServiceManager::instance()->advertiseService(ops, 0);
ServiceManager::instance()->advertiseService(ops);
}
if (!g_shutting_down)
@ -295,7 +296,7 @@ void start()
ros::SubscribeOptions ops;
ops.init<roslib::Time>("/time", 1, timeCallback);
ops.callback_queue = getInternalCallbackQueue().get();
TopicManager::instance()->subscribe(ops, 0, 0);
TopicManager::instance()->subscribe(ops);
}
g_internal_queue_thread = boost::thread(internalCallbackQueueThreadFunc);
@ -326,7 +327,6 @@ void init(const M_string& remappings, const std::string& name, uint32_t options)
file_log::init(remappings);
g_initialized = true;
Node::s_initialized_ = true;
}
}
@ -439,49 +439,4 @@ void shutdown()
Time::shutdown();
}
/////////////////////////////////////////////////////////////////////////////////////////////////
// Deprecated init functions
/////////////////////////////////////////////////////////////////////////////////////////////////
void init(int& _argc, char** _argv)
{
Node::s_args_.clear();
int full_argc = _argc;
// now, move the remapping argv's to the end, and decrement argc as needed
for (int i = 0; i < _argc; )
{
std::string arg = _argv[i];
size_t pos = arg.find(":=");
if (pos != std::string::npos)
{
std::string local_name = arg.substr(0, pos);
std::string external_name = arg.substr(pos + 2);
Node::s_remappings_.push_back(std::make_pair(local_name, external_name));
Node::s_args_.push_back(_argv[i]);
// shuffle everybody down and stuff this guy at the end of argv
char *tmp = _argv[i];
for (int j = i; j < full_argc - 1; j++)
_argv[j] = _argv[j+1];
_argv[_argc-1] = tmp;
_argc--;
}
else
{
i++; // move on, since we didn't shuffle anybody here to replace it
}
}
init(Node::s_remappings_);
Node::s_initialized_ = true;
}
void init(const VP_string& remappings)
{
Node::s_remappings_ = remappings;
Node::s_initialized_ = true;
}
}

419
core/roscpp/src/libros/node.cpp
View File

@ -1,419 +0,0 @@
/*
* 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 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.
*/
#include <cstdarg>
#include <sstream>
#include <cerrno>
#include <algorithm>
#include <sys/poll.h>
#include <arpa/inet.h>
#include <netdb.h>
#include <fcntl.h>
#include <cstdlib>
#include <cstdio>
#include <cstring>
// TEMP to remove warnings during build while things internally still use deprecated APIs
#include "ros/macros.h"
#undef ROSCPP_DEPRECATED
#define ROSCPP_DEPRECATED
// END TEMP
#include "ros/node.h"
#include "ros/subscription.h"
#include "ros/header.h"
#include "ros/time.h"
#include "ros/connection.h"
#include "ros/publication.h"
#include "ros/subscriber_link.h"
#include "ros/service_publication.h"
#include "ros/service_client_link.h"
#include "ros/service_server_link.h"
#include "ros/transport/transport_tcp.h"
#include "ros/transport/transport_udp.h"
#include "ros/callback_queue.h"
#include "ros/xmlrpc_manager.h"
#include "ros/master.h"
#include "ros/names.h"
#include "ros/param.h"
#include "ros/network.h"
#include "ros/this_node.h"
#include "ros/init.h"
#include "ros/file_log.h"
#include "ros/connection_manager.h"
#include "ros/poll_manager.h"
#include "ros/topic_manager.h"
#include "ros/service_manager.h"
#include <boost/bind.hpp>
#include <boost/thread/thread_time.hpp>
#include <boost/lexical_cast.hpp>
#include <config.h>
#ifdef HAVE_IFADDRS_H
#include <ifaddrs.h>
#endif
using namespace std;
using namespace XmlRpc;
namespace ros
{
Node* g_node = NULL;
bool ros::Node::s_initialized_ = false;
std::string ros::Node::s_name_;
uint32_t ros::Node::s_flags_ = 0;
ros::V_string ros::Node::s_args_;
ros::VP_string ros::Node::s_remappings_;
boost::mutex ros::Node::s_refcount_mutex_;
uint32_t ros::Node::s_refcount_ = 0;
bool ros::Node::s_created_by_handle_ = true;
Node *Node::instance()
{
return g_node;
}
///////////////////////////////////////////////////////
///////////////////////////////////////////////////////
Node::Node(string _name, uint32_t options, int32_t master_retry_timeout)
{
M_string remappings;
VP_string::iterator it = s_remappings_.begin();
VP_string::iterator end = s_remappings_.end();
for (; it != end; ++it)
{
remappings[it->first] = it->second;
}
ros::init(remappings, _name, options);
master::setRetryTimeout(ros::WallDuration(master_retry_timeout / 1000.));
init(options, master_retry_timeout);
}
Node::Node()
{
init(s_flags_, -1);
}
Node::~Node()
{
ROS_DEBUG( "entering ros::Node destructor");
shutdown(); // if we haven't disconnected already, do so
// Set the instance pointer to NULL, to allow applications that create
// and destroy the node multiple times in one run.
g_node = NULL;
}
void Node::init(uint32_t options, int32_t master_retry_timeout)
{
g_node = this;
shutting_down_ = false;
master_retry_timeout_ = master_retry_timeout;
if (!s_initialized_)
{
ROS_ERROR("You didn't call ros::init(...) before you tried to call a node constructor!");
ROS_BREAK();
}
ros::start();
}
bool Node::ok() const
{
return ros::ok();
}
void Node::shutdown()
{
shutting_down_ = true;
ros::shutdown();
}
const std::string& Node::getLogFilePath()
{
return file_log::getLogFilename();
}
std::string Node::getUnreliableServerIp() const
{
return network::getHost();
}
const std::string& Node::getName() const
{
return this_node::getName();
}
const std::string& Node::getNamespace() const
{
return this_node::getNamespace();
}
void Node::requestShutdown()
{
ros::requestShutdown();
}
std::string Node::cleanName(const std::string& name)
{
return names::clean(name);
}
std::string Node::resolveName(const std::string& name, bool remap)
{
return names::resolve(name, remap);
}
void Node::getAdvertisedTopics(V_string& topics)
{
TopicManager::instance()->getAdvertisedTopics(topics);
}
void Node::getSubscribedTopics(V_string& topics)
{
TopicManager::instance()->getSubscribedTopics(topics);
}
// this function has the subscription code that doesn't need to be templated.
bool Node::subscribe(const SubscribeOptions& ops, Message* m, AbstractFunctor *cb)
{
SubscribeOptions copy = ops;
copy.topic = resolveName(ops.topic);
return TopicManager::instance()->subscribe(copy, m, cb);
}
void Node::setMasterRetryTimeout(int32_t milliseconds)
{
master_retry_timeout_ = milliseconds;
}
// this function handles all the stuff that doesn't need to be templated
bool Node::advertise(const AdvertiseOptions& ops,
bool allow_multiple)
{
AdvertiseOptions copy = ops;
copy.topic = resolveName(ops.topic);
return TopicManager::instance()->advertise(copy, allow_multiple);
}
bool Node::unadvertise(const std::string &topic, const SubscriberCallbacksPtr& callbacks)
{
return TopicManager::instance()->unadvertise(resolveName(topic), callbacks);
}
bool Node::advertiseService(const AdvertiseServiceOptions& ops, int32_t thread_pool_size)
{
AdvertiseServiceOptions copy = ops;
copy.service = resolveName(ops.service);
return ServiceManager::instance()->advertiseService(copy, thread_pool_size);
}
bool Node::unadvertiseService(const string &_serv_name)
{
return ServiceManager::instance()->unadvertiseService(resolveName(_serv_name));
}
bool Node::advertise(const string &_topic,
const Message& msgref, size_t max_queue)
{
AdvertiseOptions ops(resolveName(_topic),
max_queue,
msgref.__getMD5Sum(),
msgref.__getDataType(),
msgref.__getMessageDefinition());
return advertise(ops);
}
void Node::spin()
{
while (ros::ok())
{
WallDuration(0.01).sleep();
}
}
bool Node::getPublishedTopics(VP_string* topics)
{
XmlRpcValue args, result, payload;
args[0] = this_node::getName();
args[1] = ""; //TODO: Fix this
if (!master::execute("getPublishedTopics", args, result, payload, true))
{
return false;
}
topics->clear();
for (int i = 0; i < payload.size(); i++)
{
topics->push_back(pair<string, string>(string(payload[i][0]), string(payload[i][1])));
}
return true;
}
bool Node::getNodes(V_string& nodes)
{
return master::getNodes(nodes);
}
bool Node::splitURI(const string &uri, string &host, int &_port)
{
uint32_t port = 0;
bool b = network::splitURI(uri, host, port);
_port = port;
return b;
}
bool Node::unsubscribe(const std::string &_topic)
{
return TopicManager::instance()->unsubscribe(resolveName(_topic));
}
bool Node::unsubscribe(const Message& _msg)
{
return TopicManager::instance()->unsubscribe(_msg);
}
bool Node::unsubscribe(const std::string &_topic, const SubscriptionMessageHelperPtr& helper)
{
return TopicManager::instance()->unsubscribe(resolveName(_topic), helper);
}
bool Node::unsubscribe(const std::string &topic, AbstractFunctor *afp)
{
return TopicManager::instance()->unsubscribe(resolveName(topic), afp);
}
size_t Node::numSubscribers(const std::string &_topic)
{
return TopicManager::instance()->getNumSubscribers(resolveName(_topic));
}
bool Node::checkMaster()
{
return master::check();
}
void Node::publish(const std::string& topic, const Message& msg)
{
TopicManager::instance()->publish(resolveName(topic), msg);
}
void Node::setParam(const std::string &key, const XmlRpc::XmlRpcValue &v)
{
return param::set(resolveName(key), v);
}
void Node::setParam(const std::string &key, const std::string &s)
{
return param::set(resolveName(key), s);
}
void Node::setParam(const std::string &key, const char* s)
{
return param::set(resolveName(key), s);
}
void Node::setParam(const std::string &key, double d)
{
return param::set(resolveName(key), d);
}
void Node::setParam(const std::string &key, int i)
{
return param::set(resolveName(key), i);
}
void Node::setParam(const std::string &key, bool b)
{
return param::set(resolveName(key), b);
}
bool Node::hasParam(const std::string &key)
{
return param::has(resolveName(key));
}
bool Node::deleteParam(const std::string &key)
{
return param::del(resolveName(key));
}
bool Node::getParam(const std::string &key, XmlRpc::XmlRpcValue &v, bool use_cache)
{
return param::get(resolveName(key), v, use_cache);
}
bool Node::getParam(const std::string &key, std::string &s, bool use_cache)
{
return param::get(resolveName(key), s, use_cache);
}
bool Node::getParam(const std::string &key, double &d, bool use_cache)
{
return param::get(resolveName(key), d, use_cache);
}
bool Node::getParam(const std::string &key, int &i, bool use_cache)
{
return param::get(resolveName(key), i, use_cache);
}
bool Node::getParam(const std::string &key, bool &b, bool use_cache)
{
return param::get(resolveName(key), b, use_cache);
}
size_t Node::numSubscriptions()
{
return TopicManager::instance()->getNumSubscriptions();
}
std::string Node::getIP()
{
return network::getHost();
}
} // namespace ros

63
core/roscpp/src/libros/node_handle.cpp
View File

@ -25,14 +25,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
// TEMP to remove warnings during build while things internally still use deprecated APIs
#include "ros/macros.h"
#undef ROSCPP_DEPRECATED
#define ROSCPP_DEPRECATED
// END TEMP
#include "ros/node_handle.h"
#include "ros/node.h"
#include "ros/this_node.h"
#include "ros/service.h"
#include "ros/callback_queue.h"
@ -55,6 +48,9 @@
namespace ros
{
boost::mutex g_nh_refcount_mutex_;
int32_t g_nh_refcount_ = 0;
class NodeHandleBackingCollection
{
public:
@ -143,46 +139,27 @@ void NodeHandle::construct()
namespace_ = names::resolve(namespace_);
ok_ = true;
boost::mutex::scoped_lock lock(ros::Node::s_refcount_mutex_);
boost::mutex::scoped_lock lock(g_nh_refcount_mutex_);
if (ros::Node::s_refcount_ == 0)
if (g_nh_refcount_ == 0)
{
if (Node::instance())
{
ros::Node::s_created_by_handle_ = false;
ROS_WARN("NodeHandle API is being used inside an application started with the old Node API. "
"Automatically starting a ros::spin() thread. Please switch this application to use the NodeHandle API.");
// start a thread that automatically calls spin() for us
boost::thread t(spinThread);
t.detach();
}
else
{
ros::Node::s_created_by_handle_ = true;
new ros::Node();
ros::start();
}
ros::start();
}
++ros::Node::s_refcount_;
++g_nh_refcount_;
}
void NodeHandle::destruct()
{
delete collection_;
boost::mutex::scoped_lock lock(ros::Node::s_refcount_mutex_);
boost::mutex::scoped_lock lock(g_nh_refcount_mutex_);
--ros::Node::s_refcount_;
--g_nh_refcount_;
if (ros::Node::s_refcount_ == 0)
if (g_nh_refcount_ == 0)
{
if (ros::Node::s_created_by_handle_)
{
ros::shutdown();
delete Node::instance();
}
ros::shutdown();
}
}
@ -206,11 +183,6 @@ void NodeHandle::setCallbackQueue(CallbackQueueInterface* queue)
callback_queue_ = queue;
}
ros::Node* NodeHandle::getNode() const
{
return ros::Node::instance();
}
std::string NodeHandle::remapName(const std::string& name) const
{
// First search any remappings that were passed in specifically for this NodeHandle
@ -253,7 +225,7 @@ std::string NodeHandle::resolveName(const std::string& name, bool remap) const
Publisher NodeHandle::advertise(AdvertiseOptions& ops)
{
SubscriberCallbacksPtr callbacks(new SubscriberCallbacks(ops.connect_cb, ops.disconnect_cb));
SubscriberCallbacksPtr callbacks(new SubscriberCallbacks(ops.connect_cb, ops.disconnect_cb, ops.tracked_object));
ops.topic = resolveName(ops.topic);
if (ops.callback_queue == 0)
@ -268,7 +240,7 @@ Publisher NodeHandle::advertise(AdvertiseOptions& ops)
}
}
if (TopicManager::instance()->advertise(ops, true))
if (TopicManager::instance()->advertise(ops, callbacks))
{
Publisher pub(ops.topic, *this, callbacks);
@ -298,7 +270,7 @@ Subscriber NodeHandle::subscribe(SubscribeOptions& ops)
}
}
if (TopicManager::instance()->subscribe(ops, 0, 0))
if (TopicManager::instance()->subscribe(ops))
{
Subscriber sub(ops.topic, *this, ops.helper);
@ -328,7 +300,7 @@ ServiceServer NodeHandle::advertiseService(AdvertiseServiceOptions& ops)
}
}
if (ServiceManager::instance()->advertiseService(ops, 0))
if (ServiceManager::instance()->advertiseService(ops))
{
ServiceServer srv(ops.service, *this);
@ -582,11 +554,6 @@ const std::string& NodeHandle::getName() const
return this_node::getName();
}
const V_string& NodeHandle::getParsedArgs()
{
return ros::Node::getParsedArgs();
}
const std::string& NodeHandle::getMasterHost() const
{
return ros::master::getHost();

1
core/roscpp/src/libros/publication.cpp
View File

@ -28,7 +28,6 @@
#include "ros/publication.h"
#include "ros/subscriber_link.h"
#include "ros/connection.h"
#include "ros/node.h"
#include "ros/callback_queue_interface.h"
#include "ros/single_subscriber_publisher.h"

1
core/roscpp/src/libros/publisher_link.cpp
View File

@ -36,7 +36,6 @@
#include "ros/subscription.h"
#include "ros/header.h"
#include "ros/connection.h"
#include "ros/node.h"
#include "ros/transport/transport.h"
#include "ros/this_node.h"
#include "ros/connection_manager.h"

3
core/roscpp/src/libros/rosout_appender.cpp
View File

@ -51,7 +51,8 @@ ROSOutAppender::ROSOutAppender()
AdvertiseOptions ops;
ops.init<roslib::Log>(names::resolve("/rosout"), 0);
ops.latch = true;
TopicManager::instance()->advertise(ops);
SubscriberCallbacksPtr cbs(new SubscriberCallbacks);
TopicManager::instance()->advertise(ops, cbs);
}
ROSOutAppender::~ROSOutAppender()

4
core/roscpp/src/libros/service_manager.cpp
View File

@ -130,7 +130,7 @@ void ServiceManager::shutdown()
}
bool ServiceManager::advertiseService(const AdvertiseServiceOptions& ops, int32_t thread_pool_size)
bool ServiceManager::advertiseService(const AdvertiseServiceOptions& ops)
{
boost::mutex::scoped_lock shutdown_lock(shutting_down_mutex_);
if (shutting_down_)
@ -147,7 +147,7 @@ bool ServiceManager::advertiseService(const AdvertiseServiceOptions& ops, int32_
return false;
}
ServicePublicationPtr pub(new ServicePublication(ops.service, ops.md5sum, ops.datatype, ops.req_datatype, ops.res_datatype, ops.helper, thread_pool_size, ops.callback_queue, ops.tracked_object));
ServicePublicationPtr pub(new ServicePublication(ops.service, ops.md5sum, ops.datatype, ops.req_datatype, ops.res_datatype, ops.helper, ops.callback_queue, ops.tracked_object));
service_publications_.push_back(pub);
}

146
core/roscpp/src/libros/service_publication.cpp
View File

@ -35,7 +35,6 @@
#include "ros/service_publication.h"
#include "ros/service_client_link.h"
#include "ros/connection.h"
#include "ros/node.h"
#include "ros/callback_queue_interface.h"
#include <boost/bind.hpp>
@ -44,7 +43,7 @@ namespace ros
{
ServicePublication::ServicePublication(const std::string& name, const std::string &md5sum, const std::string& data_type, const std::string& request_data_type,
const std::string& response_data_type, const ServiceMessageHelperPtr& helper, int thread_pool_size, CallbackQueueInterface* callback_queue,
const std::string& response_data_type, const ServiceMessageHelperPtr& helper, CallbackQueueInterface* callback_queue,
const VoidPtr& tracked_object)
: name_(name)
, md5sum_(md5sum)
@ -52,7 +51,6 @@ ServicePublication::ServicePublication(const std::string& name, const std::strin
, request_data_type_(request_data_type)
, response_data_type_(response_data_type)
, helper_(helper)
, thread_pool_size_(thread_pool_size)
, dropped_(false)
, callback_queue_(callback_queue)
, has_tracked_object_(false)
@ -62,11 +60,6 @@ ServicePublication::ServicePublication(const std::string& name, const std::strin
{
has_tracked_object_ = true;
}
for (int i = 0; i < thread_pool_size; ++i)
{
threads_.push_back(thread_group_.create_thread(boost::bind(&ServicePublication::threadFunc, this)));
}
}
ServicePublication::~ServicePublication()
@ -79,31 +72,11 @@ void ServicePublication::drop()
// grab a lock here, to ensure that no subscription callback will
// be invoked after we return
{
boost::mutex::scoped_lock lock(request_queue_mutex_);
boost::mutex::scoped_lock lock(client_links_mutex_);
dropped_ = true;
}
new_request_.notify_all();
dropAllConnections();
bool found_self = false;
V_threadpointer::iterator it = threads_.begin();
V_threadpointer::iterator end = threads_.end();
for (; it != end; ++it)
{
boost::thread* thread = *it;
if (thread->get_id() == boost::this_thread::get_id())
{
found_self = true;
break;
}
}
if (!found_self)
{
thread_group_.join_all();
}
}
class ServiceCallback : public CallbackInterface
@ -162,75 +135,15 @@ private:
void ServicePublication::processRequest(boost::shared_array<uint8_t> buf, size_t num_bytes, const ServiceClientLinkPtr& link)
{
if (callback_queue_)
{
CallbackInterfacePtr cb(new ServiceCallback(helper_, buf, num_bytes, link, has_tracked_object_, tracked_object_));
callback_queue_->addCallback(cb);
}
else
{
if (thread_pool_size_ != 0)
{
enqueueRequest(buf, num_bytes, link);
}
else
{
callCallback(buf, num_bytes, link);
}
}
}
void ServicePublication::callCallback(boost::shared_array<uint8_t> buf, size_t num_bytes, const ServiceClientLinkPtr& link)
{
MessagePtr req = helper_->createRequest();
MessagePtr resp = helper_->createResponse();
req->__connection_header = link->getConnection()->getHeader().getValues();
req->__serialized_length = num_bytes;
req->deserialize(buf.get());
bool ok = helper_->call(req, resp);
if (!ok)
{
resp.reset();
}
link->processResponse(ok, resp);
}
void ServicePublication::enqueueRequest(boost::shared_array<uint8_t> buf, size_t num_bytes, const ServiceClientLinkPtr& link)
{
RequestInfoPtr info(new RequestInfo);
info->buf_ = buf;
info->num_bytes_ = num_bytes;
info->link_ = link;
{
boost::mutex::scoped_lock lock(request_queue_mutex_);
request_queue_.push(info);
}
new_request_.notify_one();
CallbackInterfacePtr cb(new ServiceCallback(helper_, buf, num_bytes, link, has_tracked_object_, tracked_object_));
callback_queue_->addCallback(cb);
}
void ServicePublication::addServiceClientLink(const ServiceClientLinkPtr& link)
{
{
boost::mutex::scoped_lock lock(client_links_mutex_);
boost::mutex::scoped_lock lock(client_links_mutex_);
client_links_.push_back(link);
if (thread_pool_size_ == -1)
{
if (thread_group_.size() < client_links_.size())
{
threads_.push_back(thread_group_.create_thread(boost::bind(&ServicePublication::threadFunc, this)));
ROS_ASSERT(thread_group_.size() == client_links_.size());
}
}
}
client_links_.push_back(link);
}
void ServicePublication::removeServiceClientLink(const ServiceClientLinkPtr& link)
@ -263,51 +176,4 @@ void ServicePublication::dropAllConnections()
}
}
void ServicePublication::threadFunc()
{
disableAllSignalsInThisThread();
ServicePublicationPtr self;
while (!dropped_)
{
RequestInfoPtr info;
{
boost::mutex::scoped_lock lock(request_queue_mutex_);
while (!dropped_ && request_queue_.empty())
{
new_request_.wait(lock);
}
if (dropped_)
{
break;
}
if (request_queue_.empty())
{
continue;
}
info = request_queue_.front();
request_queue_.pop();
// Keep a shared pointer to ourselves so we don't get deleted while in a callback
// Fixes the case of unadvertising from within a callback
if (!self)
{
self = shared_from_this();
}
}
if (!dropped_)
{
ROS_ASSERT(info);
callCallback(info->buf_, info->num_bytes_, info->link_);
}
}
}
} // namespace ros

1
core/roscpp/src/libros/subscriber_link.cpp
View File

@ -29,7 +29,6 @@
#include "ros/publication.h"
#include "ros/header.h"
#include "ros/connection.h"
#include "ros/node.h"
#include "ros/transport/transport.h"
#include "ros/this_node.h"
#include "ros/connection_manager.h"

243
core/roscpp/src/libros/subscription.cpp
View File

@ -39,13 +39,7 @@
#include <sys/poll.h> // for POLLOUT
#include <cerrno>
#include <cstring>
// TEMP to remove warnings during build while things internally still use deprecated APIs
#include "ros/macros.h"
#undef ROSCPP_DEPRECATED
#define ROSCPP_DEPRECATED
// END TEMP
#include "ros/node.h"
#include "ros/common.h"
#include "ros/subscription.h"
#include "ros/publisher_link.h"
@ -65,31 +59,18 @@ using XmlRpc::XmlRpcValue;
namespace ros
{
Subscription::Subscription(const std::string &name, const std::string& md5sum, const std::string& datatype, bool threaded, int max_queue, const TransportHints& transport_hints)
Subscription::Subscription(const std::string &name, const std::string& md5sum, const std::string& datatype, const TransportHints& transport_hints)
: name_(name)
, md5sum_(md5sum)
, datatype_(datatype)
, dropped_(false)
, shutting_down_(false)
, threaded_(threaded)
, max_queue_(max_queue)
, queue_full_(false)
, transport_hints_(transport_hints)
{
if(threaded_)
{
callback_thread_ = boost::thread(boost::bind(&Subscription::subscriptionThreadFunc, this));
}
}
Subscription::~Subscription()
{
for (V_CallbackInfo::iterator cb = callbacks_.begin();
cb != callbacks_.end(); ++cb)
{
delete (*cb)->callback_;
}
pending_connections_.clear();
callbacks_.clear();
}
@ -102,34 +83,6 @@ void Subscription::shutdown()
}
drop();
// Set the callback thread free
if (threaded_)
{
if(callback_thread_.get_id() != boost::this_thread::get_id())
{
// Grab the callback lock, to ensure that we wait until the callback,
// which might be in progress, returns before we join the thread
boost::mutex::scoped_lock lock(callbacks_mutex_);
// We signal the condition, in case the callback thread is waiting on it
inbox_cond_.notify_all();
callback_thread_.join();
// Empty the inbox queue. No locking because the callback thread has already
// been joined
while(!inbox_.empty())
{
inbox_.pop();
}
}
else
{
inbox_cond_.notify_all();
}
}
}
XmlRpcValue Subscription::getStats()
@ -553,123 +506,12 @@ bool Subscription::handleMessage(const boost::shared_array<uint8_t>& buf, size_t
{
bool dropped = false;
if(threaded_)
{
SerializedMessage m(buf, num_bytes);
{
boost::mutex::scoped_lock lock(inbox_mutex_);
if((max_queue_ > 0) &&
(inbox_.size() >= (unsigned int)max_queue_))
{
inbox_.pop();
if (!queue_full_)
{
ROS_DEBUG("Incoming queue full for topic \"%s\". "
"Discarding oldest message\n",
name_.c_str());
}
queue_full_ = true;
dropped = true;
}
else
{
queue_full_ = false;
}
inbox_.push(MessageInfo(m, connection_header));
}
inbox_cond_.notify_all();
}
else
{
boost::mutex::scoped_lock lock(callbacks_mutex_);
invokeCallback(buf, num_bytes, connection_header);
}
boost::mutex::scoped_lock lock(callbacks_mutex_);
invokeCallback(buf, num_bytes, connection_header);
return dropped;
}
void Subscription::subscriptionThreadFunc()
{
disableAllSignalsInThisThread();
SubscriptionPtr self;
// service the incoming message queue, invoking callbacks
while(!dropped_ && !shutting_down_)
{
MessageInfo m;
{
boost::mutex::scoped_lock lock(inbox_mutex_);
while(inbox_.empty() && !dropped_ && !shutting_down_)
{
inbox_cond_.wait(lock);
}
if (dropped_ || shutting_down_)
{
break;
}
if (inbox_.size() == 0)
{
ROS_INFO("incoming queue sem was posted; nothing there.");
continue;
}
m = inbox_.front();
inbox_.pop();
}
{
boost::mutex::scoped_lock lock(callbacks_mutex_);
if (!dropped_)
{
// Keep a shared pointer to ourselves so we don't get deleted while in a callback
// Fixes the case of unsubscribing from within a callback
if (!self)
{
self = shared_from_this();
}
invokeCallback(m.serialized_message_.buf, m.serialized_message_.num_bytes, m.connection_header_);
}
}
}
}
bool Subscription::addFunctorMessagePair(AbstractFunctor* cb, Message* m)
{
ROS_ASSERT(m);
if (m->__getMD5Sum() != md5sum())
{
return false;
}
{
boost::mutex::scoped_lock lock(callbacks_mutex_);
CallbackInfoPtr info(new CallbackInfo);
info->callback_ = cb;
info->message_ = m;
info->callback_queue_ = 0;
callbacks_.push_back(info);
}
return true;
}
class SubscriptionCallback : public CallbackInterface
{
public:
@ -707,8 +549,6 @@ bool Subscription::addCallback(const SubscriptionMessageHelperPtr& helper, Callb
boost::mutex::scoped_lock lock(callbacks_mutex_);
CallbackInfoPtr info(new CallbackInfo);
info->callback_ = 0;
info->message_ = 0;
info->helper_ = helper;
info->callback_queue_ = queue;
info->subscription_queue_.reset(new SubscriptionQueue(name_, queue_size));
@ -751,73 +591,20 @@ void Subscription::invokeCallback(const boost::shared_array<uint8_t>& buffer, si
{
const CallbackInfoPtr& info = *cb;
if (info->callback_queue_)
ROS_ASSERT(info->callback_queue_);
if (!deserializer)
{
if (!deserializer)
{
deserializer.reset(new MessageDeserializer(info->helper_, buffer, num_bytes, connection_header));
}
uint64_t id = info->subscription_queue_->push(info->helper_, deserializer, info->has_tracked_object_, info->tracked_object_);
//ROS_DEBUG_STREAM("Subscription::invokeCallback for " << (*connection_header)["callerid"] << ", pushed buffer " << (uint64_t*)buffer.get() << " seq " << *(uint32_t*)buffer.get());
SubscriptionCallbackPtr cb(new SubscriptionCallback(info->subscription_queue_, id));
info->callback_queue_->addCallback(cb);
deserializer.reset(new MessageDeserializer(info->helper_, buffer, num_bytes, connection_header));
}
else
{
info->message_->lock();
info->message_->__serialized_length = num_bytes;
info->message_->__connection_header = connection_header;
info->message_->deserialize(buffer.get());
info->callback_->call();
info->message_->unlock();
}
uint64_t id = info->subscription_queue_->push(info->helper_, deserializer, info->has_tracked_object_, info->tracked_object_);
//ROS_DEBUG_STREAM("Subscription::invokeCallback for " << (*connection_header)["callerid"] << ", pushed buffer " << (uint64_t*)buffer.get() << " seq " << *(uint32_t*)buffer.get());
SubscriptionCallbackPtr cb(new SubscriptionCallback(info->subscription_queue_, id));
info->callback_queue_->addCallback(cb);
}
}
void Subscription::removeFunctorMessagePair(AbstractFunctor* cb)
{
typedef std::vector<int> V_int;
V_int to_delete;
boost::mutex::scoped_lock cbs_lock(callbacks_mutex_);
for (V_CallbackInfo::iterator it = callbacks_.begin();
it != callbacks_.end(); ++it)
{
if ((*it)->callback_ && *(*it)->callback_ == *cb)
{
delete (*it)->callback_;
to_delete.push_back(it - callbacks_.begin());
}
}
V_int::iterator it = to_delete.begin();
V_int::iterator end = to_delete.end();
for (; it != end; ++it)
{
callbacks_.erase(callbacks_.begin() + *it);
}
}
bool Subscription::updatesMessage(const void* _msg)
{
bool found = false;
boost::mutex::scoped_lock lock(callbacks_mutex_);
for (V_CallbackInfo::iterator it = callbacks_.begin();
!found && it != callbacks_.end(); ++it)
{
if ((*it)->message_ == _msg)
{
found = true;
break;
}
}
return found;
}
void Subscription::removePublisherLink(const PublisherLinkPtr& pub_link)
{
boost::mutex::scoped_lock lock(publisher_links_mutex_);
@ -839,12 +626,4 @@ const std::string Subscription::md5sum()
return md5sum_;
}
void Subscription::setMaxQueue(int max_queue)
{
{
boost::mutex::scoped_lock lock(inbox_mutex_);
this->max_queue_ = max_queue;
}
}
}

253
core/roscpp/src/libros/topic_manager.cpp
View File

@ -209,9 +209,7 @@ PublicationPtr TopicManager::lookupPublication(const std::string& topic)
return lookupPublicationWithoutLock(topic);
}
bool TopicManager::addSubCallback(const std::string &_topic, Message* m, AbstractFunctor *fp,
const SubscriptionMessageHelperPtr& helper, CallbackQueueInterface* queue, int max_queue,
const VoidPtr& tracked_object)
bool TopicManager::addSubCallback(const SubscribeOptions& ops)
{
// spin through the subscriptions and see if we find a match. if so, use it.
bool found = false;
@ -230,23 +228,12 @@ bool TopicManager::addSubCallback(const std::string &_topic, Message* m, Abstrac
s != subscriptions_.end() && !found; ++s)
{
sub = *s;
if (!sub->isDropped() && sub->getName() == _topic)
if (!sub->isDropped() && sub->getName() == ops.topic)
{
if (helper)
if (sub->md5sum() == ops.helper->getMD5Sum())
{
if (sub->md5sum() == helper->getMD5Sum())
{
found = true;
break;
}
}
else
{
if (sub->md5sum() == m->__getMD5Sum())
{
found = true;
break;
}
found = true;
break;
}
}
}
@ -254,29 +241,7 @@ bool TopicManager::addSubCallback(const std::string &_topic, Message* m, Abstrac
if (found)
{
bool added = false;
if (helper)
{
added = sub->addCallback(helper, queue, max_queue, tracked_object);
}
else
{
added = sub->addFunctorMessagePair(fp, m);
}
if (added)
{
if (!queue)
{
if ((sub->getMaxQueue() != 0 && sub->getMaxQueue() < max_queue) || (max_queue == 0 && sub->getMaxQueue() != 0))
{
ROS_WARN("Changing subscription '%s' max_queue size from %d to %d\n", sub->getName().c_str(), sub->getMaxQueue(), max_queue);
sub->setMaxQueue(max_queue);
}
}
}
else
if (!sub->addCallback(ops.helper, ops.callback_queue, ops.queue_size, ops.tracked_object))
{
return false;
}
@ -286,9 +251,9 @@ bool TopicManager::addSubCallback(const std::string &_topic, Message* m, Abstrac
}
// this function has the subscription code that doesn't need to be templated.
bool TopicManager::subscribe(const SubscribeOptions& ops, Message* m, AbstractFunctor *cb)
bool TopicManager::subscribe(const SubscribeOptions& ops)
{
if (addSubCallback(ops.topic, m, cb, ops.helper, ops.callback_queue, ops.queue_size, ops.tracked_object))
if (addSubCallback(ops))
{
return true;
}
@ -298,32 +263,15 @@ bool TopicManager::subscribe(const SubscribeOptions& ops, Message* m, AbstractFu
if (isShuttingDown())
{
delete cb;
return false;
}
}
std::string md5sum, datatype;
if (ops.helper)
{
md5sum = ops.helper->getMD5Sum();
datatype = ops.helper->getDataType();
}
else
{
md5sum = m->__getMD5Sum();
datatype = m->__getDataType();
}
std::string md5sum = ops.helper->getMD5Sum();
std::string datatype = ops.helper->getDataType();
SubscriptionPtr s(new Subscription(ops.topic, md5sum, datatype, ops.callback_queue ? false : true, ops.queue_size, ops.transport_hints));
if (ops.helper)
{
s->addCallback(ops.helper, ops.callback_queue, ops.queue_size, ops.tracked_object);
}
else
{
s->addFunctorMessagePair(cb, m);
}
SubscriptionPtr s(new Subscription(ops.topic, md5sum, datatype, ops.transport_hints));
s->addCallback(ops.helper, ops.callback_queue, ops.queue_size, ops.tracked_object);
if (!registerSubscriber(s, ops.datatype))
{
@ -340,8 +288,7 @@ bool TopicManager::subscribe(const SubscribeOptions& ops, Message* m, AbstractFu
return true;
}
bool TopicManager::advertise(const AdvertiseOptions& ops,
bool allow_multiple)
bool TopicManager::advertise(const AdvertiseOptions& ops, const SubscriberCallbacksPtr& callbacks)
{
if (ops.datatype == "*")
{
@ -367,44 +314,23 @@ bool TopicManager::advertise(const AdvertiseOptions& ops,
pub.reset();
}
SubscriberCallbacksPtr callbacks;
if (ops.connect_cb || ops.disconnect_cb)
{
callbacks.reset(new SubscriberCallbacks(ops.connect_cb, ops.disconnect_cb, ops.tracked_object));
}
if (pub)
{
if (pub->getMD5Sum() != ops.md5sum)
{
ROS_ERROR("Tried to advertise on topic [%s] with md5sum [%s] and datatype[%s], but the topic is already advertised as md5sum [%s] and datatype[%s]",
ROS_ERROR("Tried to advertise on topic [%s] with md5sum [%s] and datatype [%s], but the topic is already advertised as md5sum [%s] and datatype [%s]",
ops.topic.c_str(), ops.md5sum.c_str(), ops.datatype.c_str(), pub->getMD5Sum().c_str(), pub->getDataType().c_str());
return false;
}
if (allow_multiple)
{
pub->incrementRefcount();
if (callbacks)
{
pub->addCallbacks(callbacks);
}
pub->incrementRefcount();
pub->addCallbacks(callbacks);
return true;
}
else
{
ROS_DEBUG("Topic [%s] is already advertised", ops.topic.c_str());
return false;
}
return true;
}
pub = PublicationPtr(new Publication(ops.topic, ops.datatype, ops.md5sum, ops.message_definition, ops.queue_size, ops.callback_queue, ops.latch));
if (callbacks)
{
pub->addCallbacks(callbacks);
}
pub->addCallbacks(callbacks);
advertised_topics_.push_back(pub);
}
@ -467,13 +393,9 @@ bool TopicManager::unadvertise(const std::string &topic, const SubscriberCallbac
{
PublicationPtr pub = *i;
pub->decrementRefcount();
pub->removeCallbacks(callbacks);
if (callbacks)
{
pub->removeCallbacks(callbacks);
}
if (!callbacks || pub->getRefcount() == 0)
if (pub->getRefcount() == 0)
{
unregisterPublisher(pub->getName());
pub->drop();
@ -760,141 +682,6 @@ PublicationPtr TopicManager::lookupPublicationWithoutLock(const string &topic)
return t;
}
bool TopicManager::unsubscribe(const std::string &topic)
{
SubscriptionPtr sub;
{
boost::mutex::scoped_lock lock(subs_mutex_);
if (isShuttingDown())
{
return false;
}
if (!unregisterSubscriber(topic))
{
ROS_ERROR("couldn't unregister subscriber for topic [%s]", topic.c_str());
}
for (L_Subscription::iterator it = subscriptions_.begin();
it != subscriptions_.end() && !sub; ++it)
{
if ((*it)->getName() == topic)
{
sub = *it;
subscriptions_.erase(it);
break;
}
}
}
if(!sub)
{
ROS_ERROR( "couldn't find the subscription object in unsubscribe(%s)",
topic.c_str());
return false;
}
else
{
sub->shutdown();
return true;
}
}
bool TopicManager::unsubscribe(const Message& _msg)
{
SubscriptionPtr sub;
{
boost::mutex::scoped_lock lock(subs_mutex_);
if (isShuttingDown())
{
return false;
}
// dig around to see who is responsible for updating this message
for (L_Subscription::iterator it = subscriptions_.begin();
it != subscriptions_.end() && !sub; ++it)
{
if ((*it)->updatesMessage(&_msg))
{
sub = (*it);
subscriptions_.erase(it);
break;
}
}
if (!sub)
{
ROS_ERROR("Couldn't find subscriber in unsubscribe(Message)");
return false;
}
if (!unregisterSubscriber(sub->getName()))
{
ROS_ERROR("Couldn't unregister subscriber for topic [%s]", sub->getName().c_str());
}
}
sub->shutdown();
return true;
}
bool TopicManager::unsubscribe(const std::string &topic, AbstractFunctor *afp)
{
SubscriptionPtr sub;
L_Subscription::iterator it;
{
boost::mutex::scoped_lock lock(subs_mutex_);
if (isShuttingDown())
{
return false;
}
for (it = subscriptions_.begin();
it != subscriptions_.end() && !sub; ++it)
{
if ((*it)->getName() == topic)
{
sub = *it;
break;
}
}
}
if (!sub)
{
return false;
}
sub->removeFunctorMessagePair(afp);
if (sub->getNumCallbacks() == 0)
{
// nobody is left. blow away the subscription.
{
boost::mutex::scoped_lock lock(subs_mutex_);
subscriptions_.erase(it);
if (!unregisterSubscriber(topic))
{
ROS_ERROR("Couldn't unregister subscriber for topic [%s]", topic.c_str());
}
}
sub->shutdown();
return true;
}
return true;
}
bool TopicManager::unsubscribe(const std::string &topic, const SubscriptionMessageHelperPtr& helper)
{
SubscriptionPtr sub;

3
core/roscpp/src/libros/xmlrpc_manager.cpp
View File

@ -26,9 +26,10 @@
*/
#include "ros/xmlrpc_manager.h"
#include "ros/node.h"
#include "ros/network.h"
#include "ros/param.h"
#include "ros/assert.h"
#include "ros/common.h"
using namespace XmlRpc;

38
test/test_roscpp/src/CMakeLists.txt
View File

@ -1,11 +1,19 @@
rospack_add_executable(handles handles.cpp)
rospack_add_gtest_build_flags(handles)
target_link_libraries(handles ros)
rospack_add_executable(timer_callbacks timer_callbacks.cpp)
rospack_add_gtest_build_flags(timer_callbacks)
target_link_libraries(timer_callbacks ros)
rospack_add_executable(latching_publisher latching_publisher)
rospack_add_gtest_build_flags(latching_publisher)
target_link_libraries(latching_publisher ros)
rospack_add_executable(publish_n_fast publish_n_fast.cpp)
rospack_declare_test(publish_n_fast)
target_link_libraries(publish_n_fast ros)
rospack_add_executable(old_publish_n_fast old_publish_n_fast.cpp)
rospack_declare_test(old_publish_n_fast)
target_link_libraries(old_publish_n_fast ros)
rospack_add_executable(subscribe_self subscribe_self.cpp)
rospack_add_gtest_build_flags(subscribe_self)
target_link_libraries(subscribe_self ros)
@ -30,10 +38,6 @@ rospack_add_executable(subscribe_n_fast subscribe_n_fast.cpp)
rospack_add_gtest_build_flags(subscribe_n_fast)
target_link_libraries(subscribe_n_fast ros)
rospack_add_executable(old_subscribe_n_fast old_subscribe_n_fast.cpp)
rospack_add_gtest_build_flags(old_subscribe_n_fast)
target_link_libraries(old_subscribe_n_fast ros)
rospack_add_executable(subscribe_empty subscribe_empty.cpp)
rospack_add_gtest_build_flags(subscribe_empty)
target_link_libraries(subscribe_empty ros)
@ -92,7 +96,7 @@ rospack_declare_test(service_call_repeatedly)
target_link_libraries(service_call_repeatedly ros)
# Repeatedly call ros::init() and ros::fini()
# Repeatedly call ros::init()
rospack_add_executable(multiple_init_fini multiple_init_fini.cpp)
rospack_add_gtest_build_flags(multiple_init_fini)
target_link_libraries(multiple_init_fini ros)
@ -150,23 +154,7 @@ rospack_add_executable(multiple_subscriptions multiple_subscriptions.cpp)
rospack_add_gtest_build_flags(multiple_subscriptions)
target_link_libraries(multiple_subscriptions ros)
rospack_add_executable(multiple_subscriptions_standalone multiple_subscriptions_standalone.cpp)
rospack_declare_test(multiple_subscriptions_standalone)
target_link_libraries(multiple_subscriptions_standalone ros)
rospack_add_executable(check_master check_master.cpp)
rospack_add_gtest_build_flags(check_master)
target_link_libraries(check_master ros)
rospack_add_executable(handles handles.cpp)
rospack_add_gtest_build_flags(handles)
target_link_libraries(handles ros)
rospack_add_executable(timer_callbacks timer_callbacks.cpp)
rospack_add_gtest_build_flags(timer_callbacks)
target_link_libraries(timer_callbacks ros)
rospack_add_executable(latching_publisher latching_publisher)
rospack_add_gtest_build_flags(latching_publisher)
target_link_libraries(latching_publisher ros)

17
test/test_roscpp/src/check_master.cpp
View File

@ -38,16 +38,12 @@
#include <time.h>
#include <stdlib.h>
#include "ros/master.h"
#include "ros/this_node.h"
#include "ros/node.h"
#include "ros/ros.h"
#include "XmlRpc.h"
using namespace XmlRpc;
ros::Node* n;
TEST(CheckMaster, checkMaster)
{
ASSERT_TRUE(ros::master::check());
@ -66,13 +62,8 @@ int
main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "check_master");
ros::NodeHandle nh;
ros::init(argc, argv);
n = new ros::Node("checkMaster");
int ret = RUN_ALL_TESTS();
delete n;
return ret;
return RUN_ALL_TESTS();
}

64
test/test_roscpp/src/get_master_information.cpp
View File

@ -43,50 +43,50 @@
#include <time.h>
#include <stdlib.h>
#include "ros/node.h"
#include "ros/ros.h"
#include "test_roscpp/TestEmpty.h"
ros::Node* g_node;
const char* g_node_name = "test_node";
TEST(masterInfo, getPublishedTopics)
{
typedef std::set<std::string> S_string;
S_string advertised_topics_;
advertised_topics_.insert( "/test_topic_1" );
advertised_topics_.insert( "/test_topic_2" );
advertised_topics_.insert( "/test_topic_3" );
advertised_topics_.insert( "/test_topic_4" );
advertised_topics_.insert( "/test_topic_5" );
advertised_topics_.insert( "/test_topic_6" );
advertised_topics_.insert( "/test_topic_7" );
advertised_topics_.insert( "/test_topic_8" );
ros::NodeHandle nh;
S_string::iterator adv_it = advertised_topics_.begin();
S_string::iterator adv_end = advertised_topics_.end();
typedef std::set<std::string> S_string;
S_string advertised_topics;
advertised_topics.insert( "/test_topic_1" );
advertised_topics.insert( "/test_topic_2" );
advertised_topics.insert( "/test_topic_3" );
advertised_topics.insert( "/test_topic_4" );
advertised_topics.insert( "/test_topic_5" );
advertised_topics.insert( "/test_topic_6" );
advertised_topics.insert( "/test_topic_7" );
advertised_topics.insert( "/test_topic_8" );
std::vector<ros::Publisher> pubs;
S_string::iterator adv_it = advertised_topics.begin();
S_string::iterator adv_end = advertised_topics.end();
for ( ; adv_it != adv_end; ++adv_it )
{
const std::string& topic = *adv_it;
g_node->advertise<test_roscpp::TestEmpty>( topic, 0 );
pubs.push_back(nh.advertise<test_roscpp::TestEmpty>( topic, 0 ));
}
typedef std::vector<std::pair<std::string, std::string> > V_TopicAndType;
V_TopicAndType master_topics;
g_node->getPublishedTopics( &master_topics );
ros::master::V_TopicInfo master_topics;
ros::master::getTopics(master_topics);
adv_it = advertised_topics_.begin();
adv_end = advertised_topics_.end();
adv_it = advertised_topics.begin();
adv_end = advertised_topics.end();
for ( ; adv_it != adv_end; ++adv_it )
{
const std::string& topic = *adv_it;
bool found = false;
V_TopicAndType::iterator master_it = master_topics.begin();
V_TopicAndType::iterator master_end = master_topics.end();
ros::master::V_TopicInfo::iterator master_it = master_topics.begin();
ros::master::V_TopicInfo::iterator master_end = master_topics.end();
for ( ; master_it != master_end; ++master_it )
{
const std::string& master_topic = master_it->first;
if ( topic == master_topic )
const ros::master::TopicInfo& info = *master_it;
if ( topic == info.name )
{
found = true;
break;
@ -102,14 +102,8 @@ int
main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init( argc, argv );
ros::init( argc, argv, "get_master_information" );
ros::NodeHandle nh;
g_node = new ros::Node( g_node_name );
int ret = RUN_ALL_TESTS();
delete g_node;
return ret;
return RUN_ALL_TESTS();
}

65
test/test_roscpp/src/handles.cpp
View File

@ -43,48 +43,49 @@
#include <stdlib.h>
#include "ros/ros.h"
#include "ros/node.h"
#include "ros/callback_queue.h"
#include <test_roscpp/TestArray.h>
#include <test_roscpp/TestStringString.h>
#include <boost/thread.hpp>
using namespace ros;
using namespace test_roscpp;
TEST(RoscppHandles, nodeHandleConstructionDestruction)
{
{
ASSERT_FALSE(ros::Node::instance());
ASSERT_FALSE(ros::isStarted());
ros::NodeHandle n1;
ASSERT_TRUE(ros::Node::instance());
ASSERT_TRUE(ros::isStarted());
{
ros::NodeHandle n2;
ASSERT_TRUE(ros::Node::instance());
ASSERT_TRUE(ros::isStarted());
{
ros::NodeHandle n3(n2);
ASSERT_TRUE(ros::Node::instance());
ASSERT_TRUE(ros::isStarted());
{
ros::NodeHandle n4 = n3;
ASSERT_TRUE(ros::Node::instance());
ASSERT_TRUE(ros::isStarted());
}
}
}
ASSERT_TRUE(ros::Node::instance());
ASSERT_TRUE(ros::isStarted());
}
ASSERT_FALSE(ros::Node::instance());
ASSERT_FALSE(ros::isStarted());
{
ros::NodeHandle n;
ASSERT_TRUE(ros::Node::instance());
ASSERT_TRUE(ros::isStarted());
}
ASSERT_FALSE(ros::Node::instance());
ASSERT_FALSE(ros::isStarted());
}
int32_t g_recv_count = 0;
@ -185,24 +186,24 @@ TEST(RoscppHandles, subscriberCopy)
ASSERT_TRUE(sub3 == sub2);
V_string topics;
n.getSubscribedTopics(topics);
this_node::getSubscribedTopics(topics);
ASSERT_TRUE(std::find(topics.begin(), topics.end(), "/test") != topics.end());
}
ASSERT_TRUE(sub2 == sub1);
V_string topics;
n.getSubscribedTopics(topics);
this_node::getSubscribedTopics(topics);
ASSERT_TRUE(std::find(topics.begin(), topics.end(), "/test") != topics.end());
}
V_string topics;
n.getSubscribedTopics(topics);
this_node::getSubscribedTopics(topics);
ASSERT_TRUE(std::find(topics.begin(), topics.end(), "/test") != topics.end());
}
V_string topics;
n.getSubscribedTopics(topics);
this_node::getSubscribedTopics(topics);
ASSERT_TRUE(std::find(topics.begin(), topics.end(), "/test") == topics.end());
}
@ -224,24 +225,24 @@ TEST(RoscppHandles, publisherCopy)
ASSERT_TRUE(pub3 == pub2);
V_string topics;
n.getAdvertisedTopics(topics);
this_node::getAdvertisedTopics(topics);
ASSERT_TRUE(std::find(topics.begin(), topics.end(), "/test") != topics.end());
}
ASSERT_TRUE(pub2 == pub1);
V_string topics;
n.getAdvertisedTopics(topics);
this_node::getAdvertisedTopics(topics);
ASSERT_TRUE(std::find(topics.begin(), topics.end(), "/test") != topics.end());
}
V_string topics;
n.getAdvertisedTopics(topics);
this_node::getAdvertisedTopics(topics);
ASSERT_TRUE(std::find(topics.begin(), topics.end(), "/test") != topics.end());
}
V_string topics;
n.getAdvertisedTopics(topics);
this_node::getAdvertisedTopics(topics);
ASSERT_TRUE(std::find(topics.begin(), topics.end(), "/test") == topics.end());
}
@ -260,17 +261,17 @@ TEST(RoscppHandles, publisherMultiple)
ASSERT_TRUE(pub1 != pub2);
V_string topics;
n.getAdvertisedTopics(topics);
this_node::getAdvertisedTopics(topics);
ASSERT_TRUE(std::find(topics.begin(), topics.end(), "/test") != topics.end());
}
V_string topics;
n.getAdvertisedTopics(topics);
this_node::getAdvertisedTopics(topics);
ASSERT_TRUE(std::find(topics.begin(), topics.end(), "/test") != topics.end());
}
V_string topics;
n.getAdvertisedTopics(topics);
this_node::getAdvertisedTopics(topics);
ASSERT_TRUE(std::find(topics.begin(), topics.end(), "/test") == topics.end());
}
@ -515,28 +516,6 @@ TEST(RoscppHandles, nodeHandleShutdown)
ASSERT_FALSE(srv);
}
TEST(RoscppHandles, deprecatedAPIAutoSpin)
{
new ros::Node("test");
{
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("/test", 0, subscriberCallback);
ros::Publisher pub = n.advertise<test_roscpp::TestArray>("/test", 0);
g_recv_count = 0;
test_roscpp::TestArray msg;
while (g_recv_count == 0)
{
pub.publish(msg);
ros::Duration d(0.01);
d.sleep();
}
}
delete ros::Node::instance();
}
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);

68
test/test_roscpp/src/inspection.cpp
View File

@ -37,12 +37,12 @@
#include <gtest/gtest.h>
#include "ros/node.h"
#include <ros/ros.h>
#include <ros/names.h>
#include <test_roscpp/TestArray.h>
#include <test_roscpp/TestStringInt.h>
#include <test_roscpp/TestEmpty.h>
ros::Node* g_node;
const char* g_node_name = "inspection_test";
int g_argc;
@ -50,37 +50,37 @@ char* g_argv[8];
TEST(Inspection, getAdvertisedTopics)
{
ros::NodeHandle nh;
std::vector<std::string> topics;
g_node->getAdvertisedTopics(topics);
ros::this_node::getAdvertisedTopics(topics);
// Note that it's 1, not 0, because the rosout appender has already snuck
// in and advertised.
ASSERT_EQ((int)topics.size(),1);
ASSERT_EQ(topics[0], "/rosout");
ASSERT_TRUE(g_node->advertise<test_roscpp::TestArray>("topic",1));
ASSERT_TRUE(g_node->advertise<test_roscpp::TestStringInt>("ns/topic",1));
ASSERT_TRUE(g_node->advertise<test_roscpp::TestEmpty>("/global/topic",1));
{
ros::Publisher pub1 = nh.advertise<test_roscpp::TestArray>("topic",1);
ros::Publisher pub2 = nh.advertise<test_roscpp::TestArray>("ns/topic",1);
ros::Publisher pub3 = nh.advertise<test_roscpp::TestArray>("/global/topic",1);
topics.clear();
ros::this_node::getAdvertisedTopics(topics);
// Note that it's 4, not 3, because the rosout appender has already snuck
// in and advertised.
ASSERT_EQ((int)topics.size(),4);
// The following tests assume strict ordering of the topics, which is not
// guaranteed by the API.
ASSERT_EQ(topics[0], "/rosout");
ASSERT_EQ(topics[1], "/topic");
ASSERT_EQ(topics[2], "/ns/topic");
ASSERT_EQ(topics[3], "/global/topic");
}
topics.clear();
g_node->getAdvertisedTopics(topics);
// Note that it's 4, not 3, because the rosout appender has already snuck
// in and advertised.
ASSERT_EQ((int)topics.size(),4);
// The following tests assume strict ordering of the topics, which is not
// guaranteed by the API.
ASSERT_EQ(topics[0], "/rosout");
ASSERT_EQ(topics[1], "/topic");
ASSERT_EQ(topics[2], "/ns/topic");
ASSERT_EQ(topics[3], "/global/topic");
ASSERT_TRUE(g_node->unadvertise("topic"));
ASSERT_TRUE(g_node->unadvertise("ns/topic"));
ASSERT_TRUE(g_node->unadvertise("/global/topic"));
topics.clear();
g_node->getAdvertisedTopics(topics);
ros::this_node::getAdvertisedTopics(topics);
// Note that it's 1, not 0, because the rosout appender has already snuck
// in and advertised.
ASSERT_EQ((int)topics.size(),1);
@ -90,11 +90,11 @@ TEST(Inspection, getAdvertisedTopics)
TEST(Inspection, commandLineParsing)
{
ASSERT_EQ(g_argc, 5);
const ros::V_string& args = ros::Node::getParsedArgs();
ros::M_string remappings = ros::names::getRemappings();
ASSERT_STREQ(args[0].c_str(), "foo:=bar");
ASSERT_STREQ(args[1].c_str(), "baz:=bang");
ASSERT_STREQ(args[2].c_str(), "bomb:=barn");
ASSERT_STREQ(remappings["foo"].c_str(), "bar");
ASSERT_STREQ(remappings["baz"].c_str(), "bang");
ASSERT_STREQ(remappings["bomb"].c_str(), "barn");
}
int
@ -113,14 +113,8 @@ main(int argc, char** argv)
g_argv[6] = strdup("bomb:=barn");
g_argv[7] = strdup("--bangbang");
ros::init( g_argc, g_argv );
ros::init( g_argc, g_argv, "inspection" );
ros::NodeHandle nh;
g_node = new ros::Node( g_node_name );
int ret = RUN_ALL_TESTS();
delete g_node;
return ret;
return RUN_ALL_TESTS();
}

20
test/test_roscpp/src/log.cpp
View File

@ -42,12 +42,10 @@
#include <time.h>
#include <stdlib.h>
#include "ros/node.h"
#include <ros/ros.h>
#include <ros/file_log.h>
#include <test_roscpp/TestArray.h>
ros::Node* g_node;
const char* g_node_name = "logTest";
TEST(roscpp, logToFile)
{
const std::string log_string = "Testing 1 2 3";
@ -60,7 +58,7 @@ TEST(roscpp, logToFile)
logger->setLevel(old_level);
// Open the log file, read, and try to find the log string
std::string log_file = g_node->getLogFilePath();
std::string log_file = ros::file_log::getLogFilename();
std::ifstream ifs( log_file.c_str() );
ASSERT_TRUE( ifs.is_open() );
@ -88,14 +86,8 @@ main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init( argc, argv );
ros::init( argc, argv, "log" );
ros::NodeHandle nh;
g_node = new ros::Node( g_node_name );
int ret = RUN_ALL_TESTS();
delete g_node;
return ret;
return RUN_ALL_TESTS();
}

32
test/test_roscpp/src/multiple_init_fini.cpp
View File

@ -40,24 +40,17 @@
#include <time.h>
#include <stdlib.h>
#include "ros/node.h"
#include "ros/ros.h"
#include <test_roscpp/TestArray.h>
int g_argc;
char** g_argv;
class FakeObject
void callback(const test_roscpp::TestArrayConstPtr& msg)
{
public:
void function()
{
}
}
};
FakeObject g_fake_object;
TEST(RosCPP, multipleInitAndFini)
TEST(roscpp, multipleInitAndFini)
{
int try_count = 10;
if ( g_argc > 1 )
@ -67,19 +60,16 @@ TEST(RosCPP, multipleInitAndFini)
for ( int i = 0; i < try_count; ++i )
{
ros::init( g_argc, g_argv );
ros::init( g_argc, g_argv, "multiple_init_fini" );
ros::NodeHandle nh;
ros::Node* node = new ros::Node( "multipleInitAndFini" );
test_roscpp::TestArray msg;
ASSERT_TRUE( node->subscribe( "test", msg, &FakeObject::function, &g_fake_object, 1 ) );
ASSERT_TRUE( node->advertise<test_roscpp::TestArray>( "test2", 1 ) );
node->shutdown();
delete node;
ros::Subscriber sub = nh.subscribe("test", 1, callback);
ASSERT_TRUE(sub);
ros::Publisher pub = nh.advertise<test_roscpp::TestArray>( "test2", 1 );
ASSERT_TRUE(pub);
ros::shutdown();
}
}

102
test/test_roscpp/src/multiple_subscriptions.cpp
View File

@ -35,7 +35,7 @@
#include <gtest/gtest.h>
#include <time.h>
#include <stdlib.h>
#include "ros/node.h"
#include "ros/ros.h"
#include <test_roscpp/TestArray.h>
int g_argc;
@ -44,17 +44,19 @@ char** g_argv;
class MultiSub : public testing::Test
{
public:
ros::Node *n;
test_roscpp::TestArray msg0, msg1, msg2, msg3, msg_dummy;
ros::NodeHandle nh_;
bool got_it[4], should_have_it[4];
ros::Subscriber subs_[4];
ros::Subscriber verify_sub_;
ros::Subscriber reset_sub_;
bool test_ready;
int n_test;
void cb0() { if (!test_ready) return; got_it[0] = true; }
void cb1() { if (!test_ready) return; got_it[1] = true; }
void cb2() { if (!test_ready) return; got_it[2] = true; }
void cb3() { if (!test_ready) return; got_it[3] = true; }
void cb_verify()
void cb0(const test_roscpp::TestArrayConstPtr&) { if (!test_ready) return; got_it[0] = true; }
void cb1(const test_roscpp::TestArrayConstPtr&) { if (!test_ready) return; got_it[1] = true; }
void cb2(const test_roscpp::TestArrayConstPtr&) { if (!test_ready) return; got_it[2] = true; }
void cb3(const test_roscpp::TestArrayConstPtr&) { if (!test_ready) return; got_it[3] = true; }
void cb_verify(const test_roscpp::TestArrayConstPtr&)
{
if (!test_ready)
return;
@ -66,75 +68,47 @@ class MultiSub : public testing::Test
(should_have_it[3] ? got_it[3] : true)));
*/
}
void cb_reset()
void cb_reset(const test_roscpp::TestArrayConstPtr&)
{
got_it[0] = got_it[1] = got_it[2] = got_it[3] = false; test_ready = true;
}
protected:
MultiSub() {}
void SetUp()
{
ros::init(g_argc, g_argv);
n = new ros::Node("subscriber");
}
void TearDown()
{
delete n;
}
bool sub(int cb_num)
{
printf("subscribing %d\n", cb_num);
switch(cb_num)
ROS_INFO("Subscribing %d", cb_num);
boost::function<void(const test_roscpp::TestArrayConstPtr&)> funcs[4] =
{
case 0: return n->subscribe("test_roscpp/pubsub_test", msg0,
&MultiSub::cb0, this, 10);
case 1: return n->subscribe("test_roscpp/pubsub_test", msg1,
&MultiSub::cb1, this, 10);
case 2: return n->subscribe("test_roscpp/pubsub_test", msg2,
&MultiSub::cb2, this, 10);
case 3: return n->subscribe("test_roscpp/pubsub_test", msg3,
&MultiSub::cb3, this, 10);
default: return false;
}
boost::bind(&MultiSub::cb0, this, _1),
boost::bind(&MultiSub::cb1, this, _1),
boost::bind(&MultiSub::cb2, this, _1),
boost::bind(&MultiSub::cb3, this, _1),
};
subs_[cb_num] = nh_.subscribe("test_roscpp/pubsub_test", 10, funcs[cb_num]);
return subs_[cb_num];
}
bool sub_wrappers()
{
printf("sub_wrappers\n");
bool ok = true;
ok &= n->subscribe("test_roscpp/pubsub_test", msg_dummy,
&MultiSub::cb_verify, this, 10);
ok &= n->subscribe("test_roscpp/pubsub_test", msg_dummy,
&MultiSub::cb_reset, this, 10);
return ok;
ROS_INFO("sub_wrappers");
verify_sub_ = nh_.subscribe("test_roscpp/pubsub_test", 10, &MultiSub::cb_verify, this);
reset_sub_ = nh_.subscribe("test_roscpp/pubsub_test", 10, &MultiSub::cb_reset, this);
return verify_sub_ && reset_sub_;
}
bool unsub(int cb_num)
{
printf("unsubscribing %d\n", cb_num);
switch(cb_num)
{
case 0: return n->unsubscribe("test_roscpp/pubsub_test",
&MultiSub::cb0, this);
case 1: return n->unsubscribe("test_roscpp/pubsub_test",
&MultiSub::cb1, this);
case 2: return n->unsubscribe("test_roscpp/pubsub_test",
&MultiSub::cb2, this);
case 3: return n->unsubscribe("test_roscpp/pubsub_test",
&MultiSub::cb3, this);
default: return false;
}
ROS_INFO("unsubscribing %d", cb_num);
subs_[cb_num].shutdown();
return true;
}
bool unsub_wrappers()
{
printf("unsub wrappers\n");
bool ok = true;
ok &= n->unsubscribe("test_roscpp/pubsub_test",
&MultiSub::cb_verify, this);
ok &= n->unsubscribe("test_roscpp/pubsub_test",
&MultiSub::cb_reset, this);
return ok;
ROS_INFO("unsub wrappers");
verify_sub_.shutdown();
reset_sub_.shutdown();
return true;
}
};
@ -147,7 +121,7 @@ TEST_F(MultiSub, pubSubNFast)
for (int j = 0; j < 4; j++)
should_have_it[j] = (i & (1 << j) ? true : false);
printf(" testing: %d, %d, %d, %d\n",
ROS_INFO(" testing: %d, %d, %d, %d\n",
should_have_it[0],
should_have_it[1],
should_have_it[2],
@ -164,9 +138,10 @@ TEST_F(MultiSub, pubSubNFast)
{
static int count = 0;
if (count++ % 10 == 0)
printf("%d/100 tests completed...\n", n_test);
ROS_INFO("%d/100 tests completed...\n", n_test);
ros::Duration(0, 10000000).sleep();
ros::spinOnce();
ros::Duration(0.01).sleep();
}
for (int j = 0; j < 4; j++)
@ -180,6 +155,7 @@ TEST_F(MultiSub, pubSubNFast)
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "multiple_subscriptions");
g_argc = argc;
g_argv = argv;
return RUN_ALL_TESTS();

187
test/test_roscpp/src/multiple_subscriptions_standalone.cpp
View File

@ -1,187 +0,0 @@
/*
* Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
*
* 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 names of Stanford University or 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: Morgan Quigley */
/*
* Subscribe to a topic multiple times
*/
#include <string>
#include <time.h>
#include <stdlib.h>
#include <cstdio>
#include "ros/node.h"
#include <test_roscpp/TestArray.h>
int g_argc;
char** g_argv;
class MultiSub
{
public:
ros::Node *n;
test_roscpp::TestArray msg0, msg1, msg2, msg3, msg_dummy;
bool got_it[4], should_have_it[4];
bool test_ready;
int n_test;
void cb0() { if (!test_ready) return; got_it[0] = true; }
void cb1() { if (!test_ready) return; got_it[1] = true; }
void cb2() { if (!test_ready) return; got_it[2] = true; }
void cb3() { if (!test_ready) return; got_it[3] = true; }
void cb_verify()
{
if (!test_ready)
return;
n_test++;
/*
ASSERT_TRUE(((should_have_it[0] ? got_it[0] : true) &&
(should_have_it[1] ? got_it[1] : true) &&
(should_have_it[2] ? got_it[2] : true) &&
(should_have_it[3] ? got_it[3] : true)));
*/
}
void cb_reset()
{
got_it[0] = got_it[1] = got_it[2] = got_it[3] = false; test_ready = true;
}
MultiSub() {}
void SetUp()
{
ros::init(g_argc, g_argv);
n = new ros::Node("subscriber");
}
void TearDown()
{
delete n;
}
bool sub(int cb_num)
{
printf("subscribing %d\n", cb_num);
switch(cb_num)
{
case 0: return n->subscribe("test_roscpp/pubsub_test", msg0,
&MultiSub::cb0, this, 10);
case 1: return n->subscribe("test_roscpp/pubsub_test", msg1,
&MultiSub::cb1, this, 10);
case 2: return n->subscribe("test_roscpp/pubsub_test", msg2,
&MultiSub::cb2, this, 10);
case 3: return n->subscribe("test_roscpp/pubsub_test", msg3,
&MultiSub::cb3, this, 10);
default: return false;
}
}
bool sub_wrappers()
{
printf("sub wrappers\n");
bool ok = true;
ok &= n->subscribe("test_roscpp/pubsub_test", msg_dummy,
&MultiSub::cb_verify, this, 10);
ok &= n->subscribe("test_roscpp/pubsub_test", msg_dummy,
&MultiSub::cb_reset, this, 10);
return ok;
}
bool unsub(int cb_num)
{
printf("unsubscribing %d\n", cb_num);
switch(cb_num)
{
case 0: return n->unsubscribe("test_roscpp/pubsub_test",
&MultiSub::cb0, this);
case 1: return n->unsubscribe("test_roscpp/pubsub_test",
&MultiSub::cb1, this);
case 2: return n->unsubscribe("test_roscpp/pubsub_test",
&MultiSub::cb2, this);
case 3: return n->unsubscribe("test_roscpp/pubsub_test",
&MultiSub::cb3, this);
default: return false;
}
}
bool unsub_wrappers()
{
printf("unsub wrappers\n");
bool ok = true;
ok &= n->unsubscribe("test_roscpp/pubsub_test", &MultiSub::cb_verify, this);
ok &= n->unsubscribe("test_roscpp/pubsub_test", &MultiSub::cb_reset, this);
return ok;
}
void run()
{
test_ready = false;
for (int i = 0; i < 0x10; i++)
{
for (int j = 0; j < 4; j++)
should_have_it[j] = (i & (1 << j) ? true : false);
printf(" testing: %d, %d, %d, %d\n",
should_have_it[0],
should_have_it[1],
should_have_it[2],
should_have_it[3]);
for (int j = 0; j < 4; j++)
if (should_have_it[j])
sub(j);
sub_wrappers();
ros::Time t_start = ros::Time::now();
n_test = 0;
while (n_test < 100 &&
ros::Time::now() - t_start < ros::Duration(2.0))
{
static int count = 0;
if (count++ % 10 == 0)
printf("%d/100 tests completed...\n", n_test);
ros::Duration(0, 10000000).sleep();
}
for (int j = 0; j < 4; j++)
if (should_have_it[j])
unsub(j);
unsub_wrappers();
}
}
};
int main(int argc, char** argv)
{
// testing::InitGoogleTest(&argc, argv);
g_argc = argc;
g_argv = argv;
MultiSub multi_sub;
multi_sub.SetUp();
multi_sub.run();
multi_sub.TearDown();
return 0;
}

34
test/test_roscpp/src/name_remapping.cpp
View File

@ -42,44 +42,36 @@
#include <time.h>
#include <stdlib.h>
#include "ros/node.h"
ros::Node* g_node;
const char* g_node_name = "test_node";
const char* g_expected_name = "/name_remapped";
const char* g_parameter = "test";
#include "ros/ros.h"
#include <ros/param.h>
#include <ros/names.h>
TEST(roscpp, parameterRemapping)
{
std::string param;
ASSERT_TRUE(g_node->getParam(g_parameter, param));
ASSERT_TRUE(ros::param::get("test", param));
ASSERT_STREQ(ros::names::remap("test").c_str(), "test_remap");
}
TEST(roscpp, nodeNameRemapping)
{
std::string node_name = g_node->getName();
ASSERT_STREQ(node_name.c_str(), g_expected_name);
std::string node_name = ros::this_node::getName();
ASSERT_STREQ(node_name.c_str(), "/name_remapped");
}
TEST(roscpp, cleanName)
{
ASSERT_STREQ(g_node->cleanName("////asdf///").c_str(), "/asdf");
ASSERT_STREQ(g_node->cleanName("////asdf///jioweioj").c_str(), "/asdf/jioweioj");
ASSERT_STREQ(g_node->cleanName("////asdf///jioweioj/").c_str(), "/asdf/jioweioj");
ASSERT_STREQ(ros::names::clean("////asdf///").c_str(), "/asdf");
ASSERT_STREQ(ros::names::clean("////asdf///jioweioj").c_str(), "/asdf/jioweioj");
ASSERT_STREQ(ros::names::clean("////asdf///jioweioj/").c_str(), "/asdf/jioweioj");
}
int
main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init( argc, argv );
ros::init( argc, argv, "name_remapping" );
ros::NodeHandle nh;
g_node = new ros::Node( g_node_name );
int ret = RUN_ALL_TESTS();
delete g_node;
return ret;
return RUN_ALL_TESTS();
}

28
test/test_roscpp/src/namespaces.cpp
View File

@ -42,26 +42,23 @@
#include <time.h>
#include <stdlib.h>
#include "ros/node.h"
#include <ros/ros.h>
ros::Node* g_node;
const char* g_node_name = "test_node";
#include <ros/param.h>
TEST(namespaces, param)
{
std::string param;
ASSERT_TRUE( g_node->getParam( "parent", param ) );
ASSERT_TRUE( ros::param::get( "parent", param ) );
}
TEST(namespaces, localParam)
{
std::string param;
ASSERT_TRUE( g_node->getParam( "~/local", param ) );
ASSERT_TRUE( ros::param::get( "~/local", param ) );
ros::NodeHandle n;
ros::NodeHandle n("~");
std::string param2;
n.param<std::string>("~local", param2, param);
n.param<std::string>("local", param2, param);
ASSERT_STREQ(param2.c_str(), param.c_str());
ASSERT_STREQ(param2.c_str(), "test");
}
@ -69,27 +66,20 @@ TEST(namespaces, localParam)
TEST(namespaces, globalParam)
{
std::string param;
ASSERT_TRUE( g_node->getParam( "/global", param ) );
ASSERT_TRUE( ros::param::get( "/global", param ) );
}
TEST(namespaces, otherNamespaceParam)
{
std::string param;
ASSERT_TRUE( g_node->getParam( "/other_namespace/param", param ) );
ASSERT_TRUE( ros::param::get( "/other_namespace/param", param ) );
}
int
main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init( argc, argv );
ros::init( argc, argv, "namespaces" );
g_node = new ros::Node( g_node_name );
int ret = RUN_ALL_TESTS();
delete g_node;
return ret;
return RUN_ALL_TESTS();
}

100
test/test_roscpp/src/old_publish_n_fast.cpp
View File

@ -1,100 +0,0 @@
/*
* 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 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: Brian Gerkey */
/*
* Publish a message N times, back to back
*/
#include <string>
#include <cstdio>
#include <time.h>
#include <stdlib.h>
#include "ros/node.h"
#include <test_roscpp/TestArray.h>
class Publisher : public ros::Node
{
public:
int msg_count, min_sz, max_sz;
Publisher(std::string n, int _msg_count, int _min_sz,
int _max_sz, uint32_t options) :
ros::Node(n, options),
msg_count(_msg_count),
min_sz(_min_sz),
max_sz(_max_sz) {}
void sub_cb(const ros::SingleSubscriberPublisher&)
{
test_roscpp::TestArray msg;
for(int i=0; i<msg_count; i++)
{
msg.counter=i;
int j = min_sz + (int) ((max_sz-min_sz) * (rand() / (RAND_MAX + 1.0)));
msg.set_float_arr_size(j);
printf("published message %d (%d bytes)\n",
msg.counter, msg.serializationLength());
publish("test_roscpp/pubsub_test", msg);
}
}
};
#define USAGE "USAGE: publish_n_fast <count> <min_sz> <max_sz>"
int
main(int argc, char** argv)
{
ros::init(argc, argv);
if(argc != 4)
{
puts(USAGE);
exit(-1);
}
int msg_count = atoi(argv[1]);
int min_sz = atoi(argv[2]);
int max_sz = atoi(argv[3]);
Publisher* p;
p = new Publisher("publisher",msg_count,min_sz,max_sz,0);
test_roscpp::TestArray msg;
p->advertise("test_roscpp/pubsub_test", msg, &Publisher::sub_cb, msg_count);
srand(time(NULL));
p->spin();
delete p;
}

125
test/test_roscpp/src/old_subscribe_n_fast.cpp
View File

@ -1,125 +0,0 @@
/*
* 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 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: Brian Gerkey */
/*
* Subscribe to a topic, expecting to get a single message.
*/
#include <string>
#include <gtest/gtest.h>
#include <time.h>
#include <stdlib.h>
#include "ros/node.h"
#include <test_roscpp/TestArray.h>
int g_argc;
char** g_argv;
class PubSub : public testing::Test
{
public:
// A node is needed to make a service call
ros::Node* n;
test_roscpp::TestArray msg;
bool success;
bool failure;
int msg_count;
int msg_i;
ros::Duration dt;
void MsgCallback()
{
if(failure || success)
return;
printf("received message %d\n", msg.counter);
msg_i++;
if(msg_i != msg.counter)
{
failure = true;
puts("failed");
}
if(msg_i == (msg_count-1))
{
success = true;
puts("success");
}
}
protected:
PubSub() {}
void SetUp()
{
ros::init(g_argc, g_argv);
success = false;
failure = false;
msg_i = -1;
ASSERT_TRUE(g_argc == 3);
n = new ros::Node("subscriber");
msg_count = atoi(g_argv[1]);
dt.fromSec(atof(g_argv[2]));
}
void TearDown()
{
delete n;
}
};
TEST_F(PubSub, pubSubNFast)
{
ASSERT_TRUE(n->subscribe("test_roscpp/pubsub_test", msg, &PubSub::MsgCallback,
(PubSub*)this, msg_count));
ros::Time t1(ros::Time::now()+dt);
while(ros::Time::now() < t1 && !success)
{
ros::WallDuration(0.01).sleep();
}
if(success)
SUCCEED();
else
FAIL();
}
int
main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
g_argc = argc;
g_argv = argv;
return RUN_ALL_TESTS();
}

19
test/test_roscpp/src/param_update_test.cpp
View File

@ -32,37 +32,34 @@
* POSSIBILITY OF SUCH DAMAGE.
********************************************************************/
#include "ros/node.h"
#include "ros/ros.h"
int main(int argc, char** argv)
{
ros::init(argc, argv);
ros::Node n("param_update_test");
ros::init(argc, argv, "param_update_test");
ros::NodeHandle nh;
while (n.ok())
while (ros::ok())
{
ROS_INFO("getting parameters...");
int i = -1;
if (n.getParam("test", i, true))
if (nh.getParam("test", i, true))
{
ROS_INFO("test=%d", i);
}
if (n.getParam("test2", i, true))
if (nh.getParam("test2", i, true))
{
ROS_INFO("test2=%d", i);
}
if (n.getParam("test3", i, true))
if (nh.getParam("test3", i, true))
{
ROS_INFO("test3=%d", i);
}
if (n.ok())
{
usleep(4000000);
}
ros::Duration(0.1).sleep();
}
return 0;

71
test/test_roscpp/src/params.cpp
View File

@ -43,115 +43,113 @@
#include <stdlib.h>
#include "ros/ros.h"
#include "ros/node.h"
#include <ros/param.h>
ros::Node* g_node;
const char* g_node_name = "test_node";
TEST(params, allParamTypes)
{
std::string string_param;
EXPECT_TRUE( g_node->getParam( "string", string_param ) );
EXPECT_TRUE( ros::param::get( "string", string_param ) );
EXPECT_TRUE( string_param == "test" );
int int_param = 0;
EXPECT_TRUE( g_node->getParam( "int", int_param ) );
EXPECT_TRUE( ros::param::get( "int", int_param ) );
EXPECT_TRUE( int_param == 10 );
double double_param = 0.0;
EXPECT_TRUE( g_node->getParam( "double", double_param ) );
EXPECT_TRUE( ros::param::get( "double", double_param ) );
EXPECT_DOUBLE_EQ( double_param, 10.5 );
bool bool_param = true;
EXPECT_TRUE( g_node->getParam( "bool", bool_param ) );
EXPECT_TRUE( ros::param::get( "bool", bool_param ) );
EXPECT_FALSE( bool_param );
}
TEST(params, setThenGetString)
{
g_node->setParam( "test_set_param", std::string("asdf") );
ros::param::set( "test_set_param", std::string("asdf") );
std::string param;
ASSERT_TRUE( g_node->getParam( "test_set_param", param ) );
ASSERT_TRUE( ros::param::get( "test_set_param", param ) );
ASSERT_STREQ( "asdf", param.c_str() );
}
TEST(params, setThenGetStringCached)
{
std::string param;
ASSERT_FALSE( g_node->getParam( "test_set_param_setThenGetStringCached", param, true ) );
ASSERT_FALSE( ros::param::get( "test_set_param_setThenGetStringCached", param, true ) );
g_node->setParam( "test_set_param_setThenGetStringCached", std::string("asdf") );
ros::param::set( "test_set_param_setThenGetStringCached", std::string("asdf") );
ASSERT_TRUE( g_node->getParam( "test_set_param_setThenGetStringCached", param, true ) );
ASSERT_TRUE( ros::param::get( "test_set_param_setThenGetStringCached", param, true ) );
ASSERT_STREQ( "asdf", param.c_str() );
}
TEST(params, setThenGetCString)
{
g_node->setParam( "test_set_param", "asdf" );
ros::param::set( "test_set_param", "asdf" );
std::string param;
ASSERT_TRUE( g_node->getParam( "test_set_param", param ) );
ASSERT_TRUE( ros::param::get( "test_set_param", param ) );
ASSERT_STREQ( "asdf", param.c_str() );
}
TEST(params, setThenGetInt)
{
g_node->setParam( "test_set_param", 42);
ros::param::set( "test_set_param", 42);
int param;
ASSERT_TRUE( g_node->getParam( "test_set_param", param ) );
ASSERT_TRUE( ros::param::get( "test_set_param", param ) );
ASSERT_EQ( 42, param );
}
TEST(params, unknownParam)
{
std::string param;
ASSERT_FALSE( g_node->getParam( "this_param_really_should_not_exist", param ) );
ASSERT_FALSE( ros::param::get( "this_param_really_should_not_exist", param ) );
}
TEST(params, deleteParam)
{
g_node->setParam( "test_delete_param", "asdf" );
g_node->deleteParam( "test_delete_param" );
ros::param::set( "test_delete_param", "asdf" );
ros::param::del( "test_delete_param" );
std::string param;
ASSERT_FALSE( g_node->getParam( "test_delete_param", param ) );
ASSERT_FALSE( ros::param::get( "test_delete_param", param ) );
}
TEST(params, hasParam)
{
ASSERT_TRUE( g_node->hasParam( "string" ) );
ASSERT_TRUE( ros::param::has( "string" ) );
}
TEST(params, setIntDoubleGetInt)
{
g_node->setParam("test_set_int_as_double", 1);
g_node->setParam("test_set_int_as_double", 3.0f);
ros::param::set("test_set_int_as_double", 1);
ros::param::set("test_set_int_as_double", 3.0f);
int i = -1;
ASSERT_TRUE(g_node->getParam("test_set_int_as_double", i));
ASSERT_TRUE(ros::param::get("test_set_int_as_double", i));
ASSERT_EQ(3, i);
double d = 0.0f;
ASSERT_TRUE(g_node->getParam("test_set_int_as_double", d));
ASSERT_TRUE(ros::param::get("test_set_int_as_double", d));
ASSERT_EQ(3.0, d);
}
TEST(params, getIntAsDouble)
{
g_node->setParam("int_param", 1);
ros::param::set("int_param", 1);
double d = 0.0;
ASSERT_TRUE(g_node->getParam("int_param", d));
ASSERT_TRUE(ros::param::get("int_param", d));
ASSERT_EQ(1.0, d);
}
TEST(params, getDoubleAsInt)
{
g_node->setParam("double_param", 2.3);
ros::param::set("double_param", 2.3);
int i = -1;
ASSERT_TRUE(g_node->getParam("double_param", i));
ASSERT_TRUE(ros::param::get("double_param", i));
ASSERT_EQ(2, i);
g_node->setParam("double_param", 3.8);
ros::param::set("double_param", 3.8);
i = -1;
ASSERT_TRUE(g_node->getParam("double_param", i));
ASSERT_TRUE(ros::param::get("double_param", i));
ASSERT_EQ(4, i);
}
@ -192,14 +190,7 @@ int
main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init( argc, argv );
ros::init( argc, argv, "params" );
g_node = new ros::Node( g_node_name );
int ret = RUN_ALL_TESTS();
delete g_node;
return ret;
return RUN_ALL_TESTS();
}

72
test/test_roscpp/src/pub_sub.cpp
View File

@ -38,37 +38,29 @@
#include <time.h>
#include <stdlib.h>
#include "ros/node.h"
#include "ros/ros.h"
#include <test_roscpp/TestArray.h>
class Publisher : public ros::Node
{
public:
Publisher(std::string n, uint32_t options) :
ros::Node(n, options), connected(false) {}
void sub_cb(const ros::SingleSubscriberPublisher&)
{
// It would be nice to publish here, but that doesn't seem to work
// (causes the program to hang).
connected = true;
}
void cb()
{
inmsg.counter++;
publish("test_roscpp/pubsub_test", inmsg);
}
int32_t g_array_size = 1;
bool connected;
test_roscpp::TestArray outmsg;
test_roscpp::TestArray inmsg;
};
void messageCallback(const test_roscpp::TestArrayConstPtr& msg, ros::Publisher pub)
{
test_roscpp::TestArray copy = *msg;
copy.counter++;
while (ros::ok() && pub.getNumSubscribers() == 0)
{
ros::Duration(0.01).sleep();
}
pub.publish(copy);
}
#define USAGE "USAGE: publish_n_fast <sz>"
int
main(int argc, char** argv)
int main(int argc, char** argv)
{
ros::init(argc, argv);
ros::init(argc, argv, "pub_sub");
if(argc != 2)
{
@ -76,34 +68,12 @@ main(int argc, char** argv)
exit(-1);
}
Publisher* p;
p = new Publisher("publisher",0);
g_array_size = atoi(argv[1]);
int sz = atoi(argv[1]);
ros::NodeHandle nh;
ros::Publisher pub = nh.advertise<test_roscpp::TestArray>("test_roscpp/pubsub_test", 1);
ros::Subscriber sub = nh.subscribe<test_roscpp::TestArray>("test_roscpp/subpub_test", 1, boost::bind(messageCallback, _1, pub));
p->subscribe("test_roscpp/subpub_test", p->inmsg, &Publisher::cb, 1);
p->advertise("test_roscpp/pubsub_test", p->outmsg, &Publisher::sub_cb, 1);
bool published = false;
// Loop until Ctrl-C is given (by rostest)
while(p->ok())
{
ros::WallDuration(0.01).sleep();
// Did we get a connection (and is this the first time)?
if(p->connected && !published)
{
p->outmsg.counter=0;
p->outmsg.set_float_arr_size(sz);
p->publish("test_roscpp/pubsub_test", p->outmsg);
published = true;
// Don't quit here, because the message might not be out of our
// outbox yet. Instead rely on rostest to shut us down.
}
}
delete p;
ros::spin();
}

21
test/test_roscpp/src/publish_constantly.cpp
View File

@ -31,30 +31,23 @@
#include <time.h>
#include "ros/node.h"
#include "ros/ros.h"
#include "test_roscpp/TestArray.h"
class Publisher : public ros::Node
{
public:
Publisher() : ros::Node("publisher")
{
advertise<test_roscpp::TestArray>("test_roscpp/pubsub_test", 100);
}
};
int main(int argc, char **argv)
{
ros::init(argc, argv, "publish_constantly");
ros::NodeHandle nh;
ros::Publisher pub = nh.advertise<test_roscpp::TestArray>("test_roscpp/pubsub_test", 100);
test_roscpp::TestArray msg;
msg.set_float_arr_size(100);
ros::init(argc, argv);
Publisher p;
ros::WallDuration d(0.01);
while(p.ok())
while(ros::ok())
{
d.sleep();
p.publish("test_roscpp/pubsub_test", msg);
pub.publish(msg);
}
return 0;

50
test/test_roscpp/src/publish_empty.cpp
View File

@ -38,32 +38,25 @@
#include <time.h>
#include <stdlib.h>
#include "ros/node.h"
#include "ros/ros.h"
#include <test_roscpp/TestEmpty.h>
class Publisher : public ros::Node
int32_t g_msg_count = 0;
void subscriberCallback(const ros::SingleSubscriberPublisher&, ros::Publisher& pub)
{
public:
Publisher(std::string n, int _msg_count, uint32_t options) :
ros::Node(n, options), msg_count(_msg_count) {}
void sub_cb(const ros::SingleSubscriberPublisher&)
{
test_roscpp::TestEmpty msg;
for(int i=0; i<msg_count; i++)
{
publish("test_roscpp/pubsub_test", msg);
printf("published message %d\n", i);
}
}
int msg_count;
};
test_roscpp::TestEmpty msg;
for(int i = 0; i < g_msg_count; i++)
{
pub.publish(msg);
ROS_INFO("published message %d", i);
}
}
#define USAGE "USAGE: publish_n_fast <count>"
#define USAGE "USAGE: publish_empty <count>"
int
main(int argc, char** argv)
int main(int argc, char** argv)
{
ros::init(argc, argv);
ros::init(argc, argv, "publish_onsub");
if(argc != 2)
{
@ -71,18 +64,11 @@ main(int argc, char** argv)
exit(-1);
}
int msg_count = atoi(argv[1]);
g_msg_count = atoi(argv[1]);
Publisher* p;
p = new Publisher("publisher",msg_count,0);
ros::NodeHandle nh;
ros::Publisher pub;
pub = nh.advertise<test_roscpp::TestEmpty>("test_roscpp/pubsub_test", g_msg_count, boost::bind(subscriberCallback, _1, boost::ref(pub)));
test_roscpp::TestEmpty msg;
p->advertise("test_roscpp/pubsub_test", msg, &Publisher::sub_cb, msg_count);
// Loop until Ctrl-C is given (by rostest)
p->spin();
delete p;
ros::spin();
}

6
test/test_roscpp/src/publish_n_fast.cpp
View File

@ -43,7 +43,7 @@
#define USAGE "USAGE: publish_n_fast <count> <min_size> <max_size>"
void connect_cb(const ros::SingleSubscriberPublisher &pub, int msg_count, int min_size, int max_size)
void connectCallback(const ros::SingleSubscriberPublisher &pub, int msg_count, int min_size, int max_size)
{
test_roscpp::TestArray msg;
for(int i = 0; i < msg_count; i++)
@ -51,7 +51,7 @@ void connect_cb(const ros::SingleSubscriberPublisher &pub, int msg_count, int mi
msg.counter = i;
int j = min_size + (int) ((max_size - min_size) * (rand() / (RAND_MAX + 1.0)));
msg.set_float_arr_size(j);
printf("published message %d (%d bytes)\n",
ROS_INFO("published message %d (%d bytes)\n",
msg.counter, msg.serializationLength());
pub.publish(msg);
}
@ -73,7 +73,7 @@ main(int argc, char** argv)
int min_size = atoi(argv[2]);
int max_size = atoi(argv[3]);
ros::Publisher pub_ = n.advertise<test_roscpp::TestArray>("test_roscpp/pubsub_test", msg_count, boost::bind(&connect_cb, _1, msg_count, min_size, max_size));
ros::Publisher pub_ = n.advertise<test_roscpp::TestArray>("test_roscpp/pubsub_test", msg_count, boost::bind(&connectCallback, _1, msg_count, min_size, max_size));
ros::spin();
return 0;

51
test/test_roscpp/src/publish_onsub.cpp
View File

@ -27,10 +27,10 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
/* Author: Rob Wheeler */
/* Author: Brian Gerkey */
/*
* Publish (only to subscriber) an empty message N times, back to back
* Publish an empty message N times, back to back
*/
#include <string>
@ -38,30 +38,25 @@
#include <time.h>
#include <stdlib.h>
#include "ros/node.h"
#include "ros/publisher.h"
#include "ros/ros.h"
#include <test_roscpp/TestEmpty.h>
class Publisher : public ros::Node
int32_t g_msg_count = 0;
void subscriberCallback(const ros::SingleSubscriberPublisher& pub)
{
public:
Publisher(std::string n, int _msg_count, uint32_t options) :
ros::Node(n, options), msg_count(_msg_count) {}
void sub_cb(const ros::SingleSubscriberPublisher& pub)
{
test_roscpp::TestEmpty msg;
for(int i=0; i<msg_count; i++)
pub.publish(msg);
}
int msg_count;
};
test_roscpp::TestEmpty msg;
for(int i = 0; i < g_msg_count; i++)
{
pub.publish(msg);
ROS_INFO("published message %d", i);
}
}
#define USAGE "USAGE: publish_onsub <count>"
#define USAGE "USAGE: publish_empty <count>"
int
main(int argc, char** argv)
int main(int argc, char** argv)
{
ros::init(argc, argv);
ros::init(argc, argv, "publish_onsub");
if(argc != 2)
{
@ -69,18 +64,10 @@ main(int argc, char** argv)
exit(-1);
}
int msg_count = atoi(argv[1]);
g_msg_count = atoi(argv[1]);
Publisher* p;
p = new Publisher("publisher",msg_count,0);
ros::NodeHandle nh;
ros::Publisher pub = nh.advertise<test_roscpp::TestEmpty>("test_roscpp/pubsub_test", g_msg_count, boost::bind(subscriberCallback, _1));
test_roscpp::TestEmpty msg;
p->advertise("test_roscpp/pubsub_test", msg, &Publisher::sub_cb, msg_count);
// Loop until Ctrl-C is given (by rostest)
p->spin();
delete p;
ros::spin();
}

97
test/test_roscpp/src/publish_unadvertise.cpp
View File

@ -36,89 +36,85 @@
#include <time.h>
#include <stdlib.h>
#include "ros/node.h"
#include "ros/ros.h"
#include <test_roscpp/TestArray.h>
static int g_argc;
static char** g_argv;
static bool failure;
static bool advertised;
// An auxilliary class created to hold subscriber callback, because there's
// no way to advertise() with such a callback from outside a ros::Node
class Dummy : public ros::Node
{
public:
test_roscpp::TestArray msg;
Dummy(std::string name, uint32_t options) : ros::Node(name,options) {}
void sub_cb(const ros::SingleSubscriberPublisher&)
{
puts("sub_cb invoked");
if(!advertised)
failure = true;
}
bool adv()
{ return advertise("test_roscpp/pubsub_test", msg, &Dummy::sub_cb, 1); }
bool unadv()
{ return unadvertise("test_roscpp/pubsub_test"); }
};
bool failure;
bool advertised;
class Pub : public testing::Test
{
public:
Dummy* n;
ros::Duration dt;
public:
ros::NodeHandle nh_;
ros::Publisher pub_;
protected:
Pub() {}
void SetUp()
void subscriberCallback(const ros::SingleSubscriberPublisher&)
{
ROS_INFO("subscriberCallback invoked");
if(!advertised)
{
ros::init(g_argc, g_argv);
advertised = false;
failure = false;
ASSERT_TRUE(g_argc == 1);
n = new Dummy("publisher",0);
ROS_INFO("but not advertised");
failure = true;
}
void TearDown()
{
}
delete n;
}
bool adv()
{
pub_ = nh_.advertise<test_roscpp::TestArray>("test_roscpp/pubsub_test", 1, boost::bind(&Pub::subscriberCallback, this, _1));
return pub_;
}
void unadv()
{
pub_.shutdown();
}
protected:
Pub() {}
void SetUp()
{
advertised = false;
failure = false;
ASSERT_TRUE(g_argc == 1);
}
void TearDown()
{
}
};
TEST_F(Pub, pubUnadvertise)
{
advertised = true;
puts("advertising");
ASSERT_TRUE(n->adv());
ASSERT_FALSE(n->adv());
ROS_INFO("advertising");
ASSERT_TRUE(adv());
ros::Time t1(ros::Time::now()+ros::Duration(2.0));
while(ros::Time::now() < t1 && !failure)
{
ros::WallDuration(0.01).sleep();
ros::spinOnce();
}
ASSERT_TRUE(n->unadv());
ASSERT_FALSE(n->unadv());
puts("unadvertised");
unadv();
ROS_INFO("unadvertised");
advertised = false;
ros::Time t2(ros::Time::now()+ros::Duration(2.0));
while(ros::Time::now() < t2 && !failure)
{
ros::WallDuration(0.01).sleep();
ros::spinOnce();
}
advertised = true;
ASSERT_TRUE(n->adv());
ASSERT_TRUE(n->unadv());
ASSERT_TRUE(n->adv());
ASSERT_TRUE(adv());
unadv();
ASSERT_TRUE(adv());
if(failure)
FAIL();
@ -129,6 +125,7 @@ TEST_F(Pub, pubUnadvertise)
int
main(int argc, char** argv)
{
ros::init(argc, argv, "publish_unadvertise");
testing::InitGoogleTest(&argc, argv);
g_argc = argc;
g_argv = argv;

74
test/test_roscpp/src/real_time_test.cpp
View File

@ -42,70 +42,42 @@
#include <gtest/gtest.h>
#include <time.h>
#include <stdlib.h>
#include "ros/node.h"
#include "ros/ros.h"
#include <roslib/Time.h>
int g_argc;
char** g_argv;
class TimeReader : public ros::Node
{
public:
ros::Time getTime()
{
return ros::Time::now();
}
void setTime(ros::Time t)
{
roslib::Time message;
message.rostime = t;
publish("time", message);
}
void subCB(const ros::SingleSubscriberPublisher &)
{
has_subscriber_ = true;
}
TimeReader(std::string name) : ros::Node(name)
{
has_subscriber_ = false;
roslib::Time msg;
advertise("time", msg, &TimeReader::subCB, 1);
while (!has_subscriber_) {
struct timespec sleep_time = {0, 10000000};
nanosleep(&sleep_time,NULL);
}
}
bool has_subscriber_;
};
class RosTimeTest : public testing::Test
{
public:
TimeReader *n;
public:
void setTime(ros::Time t)
{
roslib::Time message;
message.rostime = t;
pub_.publish(message);
}
protected:
RosTimeTest()
protected:
RosTimeTest()
{
pub_ = nh_.advertise<roslib::Time>("/time", 1);
while (pub_.getNumSubscribers() == 0)
{
ros::Duration(0.01).sleep();
}
void SetUp()
{
ros::init(g_argc, g_argv);
n = new TimeReader("timereader");
}
void TearDown()
{
}
delete n;
}
ros::NodeHandle nh_;
ros::Publisher pub_;
};
TEST_F(RosTimeTest, RealTimeTest)
{
//Get the start time.
ros::Time start = n->getTime();
ros::Time start = ros::Time::now();
//Checks to see if the time is larger than a thousand seconds
//this is a good indication that we are getting the system time.
@ -113,7 +85,7 @@ TEST_F(RosTimeTest, RealTimeTest)
//Wait a second
ros::Duration wait(1, 0); wait.sleep();
ros::Time end = n->getTime();
ros::Time end = ros::Time::now();
ros::Duration d = end - start;
//After waiting one second, see if we really waited on second.
@ -121,13 +93,14 @@ TEST_F(RosTimeTest, RealTimeTest)
ASSERT_GT(d.toSec(), 0.9);
//Publish a rostime of 42.
n->setTime(ros::Time(42, 0));
setTime(ros::Time(42, 0));
//Wait half a second to get the message.
boost::this_thread::sleep(boost::posix_time::milliseconds(500));
ros::WallDuration(0.5).sleep();
ros::spinOnce();
//Make sure that it is really set
ASSERT_EQ(n->getTime().toSec(), 42.0);
ASSERT_EQ(ros::Time::now().toSec(), 42.0);
SUCCEED();
@ -137,6 +110,7 @@ TEST_F(RosTimeTest, RealTimeTest)
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "real_time_test");
g_argc = argc;
g_argv = argv;
return RUN_ALL_TESTS();

117
test/test_roscpp/src/service_adv.cpp
View File

@ -33,100 +33,57 @@
* Advertise a service
*/
#include "ros/node.h"
#include "ros/ros.h"
#include <test_roscpp/TestStringString.h>
ros::Node* g_node = NULL;
class Dummy
{
public:
bool srvCallback(test_roscpp::TestStringString::Request &req,
bool caseFlip(test_roscpp::TestStringString::Request &req,
test_roscpp::TestStringString::Response &res)
{
// copy over the request and overwrite the letters with their case-flip
res.str = req.str;
for (size_t i = 0; i < res.str.length(); i++)
{
char c = res.str[i];
if (islower(c))
c = toupper(c);
else if (isupper(c))
c = tolower(c);
res.str[i] = c;
}
return true;
}
};
class Dummy2
{
public:
bool srvCallback(test_roscpp::TestStringString::Request &req,
// copy over the request and overwrite the letters with their case-flip
res.str = req.str;
for (size_t i = 0; i < res.str.length(); i++)
{
char c = res.str[i];
if (islower(c))
c = toupper(c);
else if (isupper(c))
c = tolower(c);
res.str[i] = c;
}
return true;
}
bool caseFlipLongRunning(test_roscpp::TestStringString::Request &req,
test_roscpp::TestStringString::Response &res)
{
// copy over the request and overwrite the letters with their case-flip
res.str = req.str;
for (size_t i = 0; i < res.str.length(); i++)
{
char c = res.str[i];
if (islower(c))
c = toupper(c);
else if (isupper(c))
c = tolower(c);
res.str[i] = c;
}
struct timespec sl = {2, 0};
nanosleep(&sl,NULL);
return true;
}
};
class Dummy3
{
public:
bool srvCallback(test_roscpp::TestStringString::Request &req,
test_roscpp::TestStringString::Response &res)
{
// copy over the request and overwrite the letters with their case-flip
res.str = req.str;
for (size_t i = 0; i < res.str.length(); i++)
{
char c = res.str[i];
if (islower(c))
c = toupper(c);
else if (isupper(c))
c = tolower(c);
res.str[i] = c;
}
caseFlip(req, res);
g_node->unadvertiseService("service_adv_unadv_in_callback");
ros::Duration(2).sleep();
return true;
}
struct timespec sl = {2, 0};
nanosleep(&sl,NULL);
return true;
}
};
bool caseFlipUnadvertise(test_roscpp::TestStringString::Request &req,
test_roscpp::TestStringString::Response &res, ros::ServiceServer& srv)
{
caseFlip(req, res);
srv.shutdown();
ros::Duration(2).sleep();
return true;
}
int
main(int argc, char** argv)
{
ros::init(argc, argv);
g_node = new ros::Node("advertiser" );
ros::init(argc, argv, "service_adv");
ros::NodeHandle nh;
Dummy d;
Dummy2 d2;
Dummy3 d3;
g_node->advertiseService("service_adv", &Dummy::srvCallback, &d, -1);
g_node->advertiseService("service_adv_long", &Dummy2::srvCallback, &d2);
g_node->advertiseService("service_adv_unadv_in_callback", &Dummy3::srvCallback, &d3);
g_node->spin();
delete g_node;
ros::ServiceServer srv1, srv2, srv3;
srv1 = nh.advertiseService("service_adv", caseFlip);
srv2 = nh.advertiseService("service_adv_long", caseFlipLongRunning);
srv3 = nh.advertiseService<test_roscpp::TestStringString::Request, test_roscpp::TestStringString::Response>("service_adv_unadv_in_callback", boost::bind(caseFlipUnadvertise, _1, _2, boost::ref(srv3)));
ros::spin();
}

44
test/test_roscpp/src/service_adv_a.cpp
View File

@ -33,7 +33,7 @@
* Advertise a service, then run another app that advertises another service
*/
#include "ros/node.h"
#include "ros/ros.h"
#include <test_roscpp/TestStringString.h>
#include <unistd.h>
@ -43,34 +43,20 @@
#include <sys/wait.h>
#include <cstdlib>
class Dummy
bool srvCallback(test_roscpp::TestStringString::Request &req,
test_roscpp::TestStringString::Response &res)
{
public:
bool srvCallback(test_roscpp::TestStringString::Request &req,
test_roscpp::TestStringString::Response &res)
{
res.str = "A";
return true;
}
};
ros::Node* g_n;
int
main(int argc, char** argv)
{
ros::init(argc, argv);
g_n = new ros::Node("advertiser_a");
Dummy d;
g_n->advertiseService("service_adv", &Dummy::srvCallback, &d);
g_n->spin();
delete g_n;
res.str = "A";
return true;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "service_adv_a");
ros::NodeHandle nh;
ros::ServiceServer srv = nh.advertiseService("service_adv", srvCallback);
ros::spin();
}

40
test/test_roscpp/src/service_adv_multiple.cpp
View File

@ -37,44 +37,32 @@
#include <gtest/gtest.h>
#include "ros/node.h"
#include "ros/ros.h"
#include "ros/service.h"
#include <test_roscpp/TestStringString.h>
class Dummy
bool srvCallback(test_roscpp::TestStringString::Request &req,
test_roscpp::TestStringString::Response &res)
{
public:
bool srvCallback(test_roscpp::TestStringString::Request &req,
test_roscpp::TestStringString::Response &res)
{
return true;
}
};
ros::Node* g_node;
const char* g_node_name = "serviceAdvertiseMultipleTest";
return true;
}
TEST(SrvCall, advertiseMultiple)
{
Dummy d;
ASSERT_TRUE( g_node->advertiseService("service_adv", &Dummy::srvCallback, &d) );
ASSERT_FALSE( g_node->advertiseService("service_adv", &Dummy::srvCallback, &d) );
ros::NodeHandle nh;
ros::ServiceServer srv = nh.advertiseService("service_adv", srvCallback);
ASSERT_TRUE(srv);
ros::ServiceServer srv2 = nh.advertiseService("service_adv", srvCallback);
ASSERT_FALSE(srv2);
}
int
main(int argc, char** argv)
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "service_adv_multiple");
ros::NodeHandle nh;
ros::init(argc, argv);
g_node = new ros::Node( g_node_name );
int ret = RUN_ALL_TESTS();
delete g_node;
return ret;
return RUN_ALL_TESTS();
}

66
test/test_roscpp/src/service_adv_unadv.cpp
View File

@ -35,7 +35,7 @@
#include <gtest/gtest.h>
#include "ros/node.h"
#include "ros/ros.h"
#include <test_roscpp/TestStringString.h>
@ -45,86 +45,71 @@ static char** g_argv;
class ServiceAdvertiser : public testing::Test
{
public:
ros::Node* n;
bool advertised;
bool failure;
ros::NodeHandle nh_;
ros::ServiceServer srv_;
bool advertised_;
bool failure_;
bool srvCallback(test_roscpp::TestStringString::Request &req,
test_roscpp::TestStringString::Response &res)
{
puts("in callback");
if(!advertised)
ROS_INFO("in callback");
if(!advertised_)
{
puts("but not advertised!");
failure = true;
ROS_INFO("but not advertised!");
failure_ = true;
}
// Can't do this for now; unadvertise() in service callback and in
// other thread causes race conditions
/*
else
{
unadv();
advertised = false;
}
*/
return true;
}
protected:
ServiceAdvertiser() {}
void SetUp()
{
ros::init(g_argc, g_argv);
failure = false;
advertised = false;
failure_ = false;
advertised_ = false;
ASSERT_TRUE(g_argc == 1);
n = new ros::Node("advertiser" );
}
void TearDown()
{
delete n;
}
bool adv()
{
puts("advertising");
bool ret = n->advertiseService("service_adv",
&ServiceAdvertiser::srvCallback, this);
puts("advertised");
return ret;
ROS_INFO("advertising");
srv_ = nh_.advertiseService("service_adv", &ServiceAdvertiser::srvCallback, this);
ROS_INFO("advertised");
return srv_;
}
bool unadv()
{
puts("unadvertising");
bool ret = n->unadvertiseService("service_adv");
puts("unadvertised");
return ret;
ROS_INFO("unadvertising");
srv_.shutdown();
ROS_INFO("unadvertised");
return true;
}
};
TEST_F(ServiceAdvertiser, advUnadv)
{
advertised = true;
advertised_ = true;
ASSERT_TRUE(adv());
for(int i=0;i<100;i++)
{
if(advertised)
if(advertised_)
{
ASSERT_TRUE(unadv());
advertised = false;
advertised_ = false;
}
else
{
advertised = true;
advertised_ = true;
ASSERT_TRUE(adv());
}
ros::WallDuration(0.01).sleep();
}
if(failure)
if(failure_)
FAIL();
else
SUCCEED();
@ -133,6 +118,7 @@ TEST_F(ServiceAdvertiser, advUnadv)
int
main(int argc, char** argv)
{
ros::init(argc, argv, "service_adv_unadv");
testing::InitGoogleTest(&argc, argv);
g_argc = argc;
g_argv = argv;

15
test/test_roscpp/src/service_call.cpp
View File

@ -37,7 +37,7 @@
#include <gtest/gtest.h>
#include "ros/node.h"
#include "ros/ros.h"
#include "ros/time.h"
#include "ros/service.h"
#include "ros/connection.h"
@ -107,7 +107,8 @@ TEST(SrvCall, callSrvHandle)
std::map<std::string, std::string> header;
header["test1"] = "testing 1";
header["test2"] = "testing 2";
ros::ServiceClient handle = ros::service::createClient<test_roscpp::TestStringString>("service_adv", false, header);
ros::NodeHandle nh;
ros::ServiceClient handle = nh.serviceClient<test_roscpp::TestStringString>("service_adv", false, header);
ros::Time start = ros::Time::now();
@ -135,7 +136,8 @@ TEST(SrvCall, callSrvPersistentHandle)
std::map<std::string, std::string> header;
header["test1"] = "testing 1";
header["test2"] = "testing 2";
ros::ServiceClient handle = ros::service::createClient<test_roscpp::TestStringString>("service_adv", true, header);
ros::NodeHandle nh;
ros::ServiceClient handle = nh.serviceClient<test_roscpp::TestStringString>("service_adv", true, header);
ros::Time start = ros::Time::now();
@ -187,7 +189,8 @@ TEST(SrvCall, handleValid)
std::map<std::string, std::string> header;
header["test1"] = "testing 1";
header["test2"] = "testing 2";
ros::ServiceClient handle = ros::service::createClient<test_roscpp::TestStringString>("service_adv", true, header);
ros::NodeHandle nh;
ros::ServiceClient handle = nh.serviceClient<test_roscpp::TestStringString>("service_adv", true, header);
ASSERT_TRUE(handle.call(req, res));
ASSERT_TRUE(handle.isValid());
handle.shutdown();
@ -206,8 +209,8 @@ main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv);
ros::Node n("caller");
ros::init(argc, argv, "service_call");
ros::NodeHandle nh;
int ret = RUN_ALL_TESTS();

19
test/test_roscpp/src/service_call_expect_b.cpp
View File

@ -37,22 +37,21 @@
#include <gtest/gtest.h>
#include "ros/node.h"
#include "ros/ros.h"
#include "ros/service.h"
#include <test_roscpp/TestStringString.h>
ros::Node* g_n;
TEST(SrvCall, callSrv)
{
test_roscpp::TestStringString::Request req;
test_roscpp::TestStringString::Response res;
req.str = "nothing";
ros::NodeHandle nh;
int param;
while(!g_n->getParam("advertisers_ready", param))
usleep(100000);
while(!nh.getParam("advertisers_ready", param))
ros::Duration(0.01).sleep();
bool call_result = ros::service::call("service_adv", req, res);
ASSERT_TRUE(call_result);
@ -64,14 +63,10 @@ main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv);
g_n = new ros::Node("caller");
ros::init(argc, argv, "service_call_expect_b");
ros::NodeHandle nh;
int ret = RUN_ALL_TESTS();
delete g_n;
return ret;
return RUN_ALL_TESTS();
}

11
test/test_roscpp/src/service_call_repeatedly.cpp
View File

@ -35,24 +35,23 @@
#include <time.h>
#include "ros/node.h"
#include "ros/ros.h"
#include "ros/service.h"
#include <test_roscpp/TestStringString.h>
int
main(int argc, char** argv)
{
ros::init(argc, argv);
ros::Node n("caller");
ros::init(argc, argv, "service_call_repeatedly");
ros::NodeHandle nh;
test_roscpp::TestStringString::Request req;
test_roscpp::TestStringString::Response res;
struct timespec sleep_time = {0, 10000000};
while(n.ok())
while(ros::ok())
{
ros::service::call("service_adv", req, res);
nanosleep(&sleep_time,NULL);
ros::Duration(0.01).sleep();
}

42
test/test_roscpp/src/service_wait_a_adv_b.cpp
View File

@ -33,34 +33,24 @@
* Advertise a service
*/
#include "ros/node.h"
#include "ros/ros.h"
#include <test_roscpp/TestStringString.h>
class Dummy
bool srvCallback(test_roscpp::TestStringString::Request &req,
test_roscpp::TestStringString::Response &res)
{
public:
bool srvCallback(test_roscpp::TestStringString::Request &req,
test_roscpp::TestStringString::Response &res)
{
res.str = "B";
return true;
}
};
int
main(int argc, char** argv)
{
ros::init(argc, argv);
ros::Node n("advertiser_b");
Dummy d;
ros::service::waitForService("service_adv");
n.advertiseService("service_adv", &Dummy::srvCallback, &d);
n.setParam("advertisers_ready", 1);
n.spin();
res.str = "B";
return true;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "service_wait_a_adv_b");
ros::NodeHandle nh;
ros::service::waitForService("service_adv");
ros::ServiceServer srv = nh.advertiseService("service_adv", srvCallback);
nh.setParam("advertisers_ready", 1);
ros::spin();
}

73
test/test_roscpp/src/sim_time_test.cpp
View File

@ -42,88 +42,61 @@
#include <gtest/gtest.h>
#include <time.h>
#include <stdlib.h>
#include "ros/node.h"
#include "ros/ros.h"
#include <roslib/Time.h>
int g_argc;
char** g_argv;
class TimeReader : public ros::Node
{
public:
ros::Time getTime()
{
return ros::Time::now();
}
void setTime(ros::Time t)
{
roslib::Time message;
message.rostime = t;
publish("time", message);
}
void subCB(const ros::SingleSubscriberPublisher &)
{
has_subscriber_ = true;
}
TimeReader(std::string name) : ros::Node(name)
{
has_subscriber_ = false;
roslib::Time msg;
advertise("time", msg, &TimeReader::subCB, 1);
while (!has_subscriber_) {
struct timespec sleep_time = {0, 10000000};
nanosleep(&sleep_time,NULL);
}
}
bool has_subscriber_;
};
class RosTimeTest : public testing::Test
{
public:
TimeReader *n;
public:
void setTime(ros::Time t)
{
roslib::Time message;
message.rostime = t;
pub_.publish(message);
}
protected:
RosTimeTest()
protected:
RosTimeTest()
{
pub_ = nh_.advertise<roslib::Time>("/time", 1);
while (pub_.getNumSubscribers() == 0)
{
ros::Duration(0.01).sleep();
}
void SetUp()
{
ros::init(g_argc, g_argv);
n = new TimeReader("timereader");
}
void TearDown()
{
}
delete n;
}
ros::NodeHandle nh_;
ros::Publisher pub_;
};
TEST_F(RosTimeTest, SimTimeTest)
{
//Get the start time.
ros::Time start = n->getTime();
ros::Time start = ros::Time::now();
//The start time should be zero before a message is published.
ASSERT_EQ(0.0, start.toSec());
ASSERT_TRUE(start.isZero());
//Publish a rostime of 42.
n->setTime(ros::Time(42, 0));
setTime(ros::Time(42, 0));
//Wait half a second to get the message.
boost::this_thread::sleep(boost::posix_time::milliseconds(500));
ros::WallDuration(0.5).sleep();
//Make sure that it is really set
ASSERT_EQ(42.0, n->getTime().toSec());
ASSERT_EQ(42.0, ros::Time::now().toSec());
}
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "sim_time_test");
g_argc = argc;
g_argv = argv;
return RUN_ALL_TESTS();

53
test/test_roscpp/src/sub_pub.cpp
View File

@ -40,62 +40,63 @@
#include <time.h>
#include <stdlib.h>
#include "ros/node.h"
#include "ros/ros.h"
#include <ros/callback_queue.h>
#include <test_roscpp/TestArray.h>
int g_msg_count;
ros::Duration g_dt;
uint32_t g_options;
class PubSub : public testing::Test, public ros::Node
class SubPub : public testing::Test
{
public:
test_roscpp::TestArray msg;
bool success;
bool failure;
int msg_i;
bool connected;
ros::Publisher pub_;
void MsgCallback()
void messageCallback(const test_roscpp::TestArrayConstPtr& msg)
{
if(failure || success)
return;
//printf("received message %d\n", msg.counter);
if (msg_i == -1)
{
msg_i = msg->counter - 1;
}
msg_i++;
if(msg_i != msg.counter)
ROS_INFO("msg_i=%d, counter=%d", msg_i, msg->counter);
if(msg_i != msg->counter)
{
failure = true;
puts("failed");
ROS_INFO("failed");
}
else if(msg_i == (g_msg_count-1))
{
success = true;
puts("success");
ROS_INFO("success");
}
else
{
while (!connected)
{
ros::Duration t(0.01);
t.sleep();
}
publish("test_roscpp/subpub_test", msg);
pub_.publish(msg);
}
}
void sub_cb(const ros::SingleSubscriberPublisher&)
void subscriberCallback(const ros::SingleSubscriberPublisher&)
{
connected = true;
test_roscpp::TestArray msg;
msg.counter = 0;
pub_.publish(msg);
}
protected:
PubSub() : ros::Node("subscriber") {}
void SetUp()
{
success = false;
failure = false;
connected = false;
msg_i = -1;
}
@ -105,16 +106,18 @@ class PubSub : public testing::Test, public ros::Node
}
};
TEST_F(PubSub, pubSubNFast)
TEST_F(SubPub, pubSubNFast)
{
ASSERT_TRUE(subscribe("test_roscpp/pubsub_test", msg, &PubSub::MsgCallback,
(PubSub*)this, 1));
ASSERT_TRUE(advertise("test_roscpp/subpub_test", msg, &PubSub::sub_cb, 1));
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe("test_roscpp/pubsub_test", 0, &SubPub::messageCallback, (SubPub*)this);
ASSERT_TRUE(sub);
pub_ = nh.advertise<test_roscpp::TestArray>("test_roscpp/subpub_test", 0, boost::bind(&SubPub::subscriberCallback, this, _1));
ASSERT_TRUE(pub_);
ros::Time t1(ros::Time::now()+g_dt);
while(ros::Time::now() < t1 && !success && !failure)
{
ros::WallDuration(0.01).sleep();
ros::getGlobalCallbackQueue()->callAvailable();
}
if(success)
@ -129,7 +132,7 @@ int
main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv);
ros::init(argc, argv, "sub_pub");
if(argc != 3)
{
@ -139,5 +142,7 @@ main(int argc, char** argv)
g_msg_count = atoi(argv[1]);
g_dt.fromSec(atof(argv[2]));
ros::NodeHandle nh;
return RUN_ALL_TESTS();
}

23
test/test_roscpp/src/subscribe_empty.cpp
View File

@ -30,7 +30,7 @@
/* Author: Brian Gerkey */
/*
* Subscribe to a topic, expecting to get a single message.
* Subscribe to a topic, expecting to get a number of messages based on the first command line argument
*/
#include <string>
@ -40,7 +40,7 @@
#include <time.h>
#include <stdlib.h>
#include "ros/node.h"
#include "ros/ros.h"
#include <test_roscpp/TestEmpty.h>
int g_argc;
@ -49,8 +49,6 @@ char** g_argv;
class PubSub : public testing::Test
{
public:
// A node is needed to make a service call
ros::Node* n;
test_roscpp::TestEmpty msg;
bool success;
bool failure;
@ -58,7 +56,7 @@ class PubSub : public testing::Test
int msg_i;
ros::Duration dt;
void MsgCallback()
void messageCallback(const test_roscpp::TestEmptyConstPtr& msg)
{
if(failure || success)
return;
@ -76,32 +74,29 @@ class PubSub : public testing::Test
PubSub() {}
void SetUp()
{
ros::init(g_argc, g_argv);
success = false;
failure = false;
msg_i = -1;
ASSERT_TRUE(g_argc == 3);
n = new ros::Node("subscriber");
msg_count = atoi(g_argv[1]);
dt.fromSec(atof(g_argv[2]));
}
void TearDown()
{
delete n;
}
};
TEST_F(PubSub, pubSubNFast)
{
ASSERT_TRUE(n->subscribe("test_roscpp/pubsub_test", msg, &PubSub::MsgCallback,
(PubSub*)this, msg_count));
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe("test_roscpp/pubsub_test", msg_count, &PubSub::messageCallback, (PubSub*)this);
ros::Time t1(ros::Time::now()+dt);
while(ros::Time::now() < t1 && !success)
{
ros::WallDuration(0.01).sleep();
ros::spinOnce();
}
if(success)
@ -110,9 +105,11 @@ TEST_F(PubSub, pubSubNFast)
FAIL();
}
int
main(int argc, char** argv)
int main(int argc, char** argv)
{
ros::init(argc, argv, "subscribe_empty");
ros::NodeHandle nh;
testing::InitGoogleTest(&argc, argv);
g_argc = argc;
g_argv = argv;

53
test/test_roscpp/src/subscribe_resubscribe.cpp
View File

@ -40,7 +40,7 @@
#include <time.h>
#include <stdlib.h>
#include "ros/node.h"
#include "ros/ros.h"
#include <test_roscpp/TestArray.h>
int g_argc;
@ -49,30 +49,28 @@ char** g_argv;
class PubSub : public testing::Test
{
public:
ros::Node* n;
test_roscpp::TestArray msg;
bool success;
bool failure;
int msg_count;
int msg_i;
ros::Duration dt;
void MsgCallback()
void messageCallback(const test_roscpp::TestArrayConstPtr& msg)
{
if(failure || success)
return;
printf("received message %d\n", msg.counter);
ROS_INFO("received message %d", msg->counter);
msg_i++;
if(msg_i != msg.counter)
if(msg_i != msg->counter)
{
failure = true;
puts("failed");
ROS_INFO("failed");
}
if(msg_i == (msg_count-1))
{
success = true;
puts("success");
ROS_INFO("success");
}
}
@ -80,51 +78,53 @@ class PubSub : public testing::Test
PubSub() {}
void SetUp()
{
ros::init(g_argc, g_argv);
success = false;
failure = false;
msg_i = -1;
ASSERT_TRUE(g_argc == 3);
n = new ros::Node("subscriber");
msg_count = atoi(g_argv[1]);
dt.fromSec(atof(g_argv[2]));
}
void TearDown()
{
delete n;
}
};
TEST_F(PubSub, pubSubNFast)
{
ASSERT_TRUE(n->subscribe("test_roscpp/pubsub_test", msg, &PubSub::MsgCallback,
(PubSub*)this, 0));
ros::Time t1(ros::Time::now()+dt);
ros::NodeHandle nh;
while(ros::Time::now() < t1 && !success)
{
ros::WallDuration(0.01).sleep();
ros::Subscriber sub = nh.subscribe("test_roscpp/pubsub_test", 0, &PubSub::messageCallback, (PubSub*)this);
ASSERT_TRUE(sub);
ros::Time t1(ros::Time::now()+dt);
while(ros::Time::now() < t1 && !success)
{
ros::WallDuration(0.01).sleep();
ros::spinOnce();
}
}
if(!success)
FAIL();
else
{
ASSERT_TRUE(n->unsubscribe("test_roscpp/pubsub_test"));
success = false;
failure = false;
msg_i = -1;
ASSERT_TRUE(n->subscribe("test_roscpp/pubsub_test2", msg, &PubSub::MsgCallback,
(PubSub*)this, 0));
ros::Time t1(ros::Time::now()+dt);
while(ros::Time::now() < t1 && !success)
{
ros::WallDuration(0.01).sleep();
ros::Subscriber sub = nh.subscribe("test_roscpp/pubsub_test2", 0, &PubSub::messageCallback, (PubSub*)this);
ASSERT_TRUE(sub);
ros::Time t1(ros::Time::now()+dt);
while(ros::Time::now() < t1 && !success)
{
ros::WallDuration(0.01).sleep();
ros::spinOnce();
}
}
if(success)
@ -137,6 +137,9 @@ TEST_F(PubSub, pubSubNFast)
int
main(int argc, char** argv)
{
ros::init(argc, argv, "subscribe_resubscribe");
ros::NodeHandle nh;
testing::InitGoogleTest(&argc, argv);
g_argc = argc;
g_argv = argv;

151
test/test_roscpp/src/subscribe_self.cpp
View File

@ -39,106 +39,96 @@
#include <stdlib.h>
#include "ros/node.h"
#include "ros/ros.h"
#include <test_roscpp/TestArray.h>
int g_msg_count;
ros::Duration g_dt;
uint32_t g_options = 0;
bool g_success = false;
bool g_failure = false;
int32_t g_msg_i = -1;
class SelfSubscribe : public testing::Test, public ros::Node
void subscriberCallback(const ros::SingleSubscriberPublisher&, const ros::Publisher& pub)
{
public:
test_roscpp::TestArray msg;
bool success;
bool failure;
int msg_i;
test_roscpp::TestArray outmsg;
for(int i=0;i<g_msg_count;i++)
{
outmsg.counter = i;
pub.publish(outmsg);
ROS_INFO("published %d", i);
}
}
void MsgCallback()
{
printf("received message %d\n", msg.counter);
if(failure || success)
return;
msg_i++;
if(msg_i != msg.counter)
{
failure = true;
puts("failed");
}
else if(msg_i == (g_msg_count-1))
{
success = true;
puts("success");
}
}
void sub_cb(const ros::SingleSubscriberPublisher&)
{
test_roscpp::TestArray outmsg;
outmsg.set_float_arr_size(100);
for(int i=0;i<g_msg_count;i++)
{
outmsg.counter = i;
publish("test_roscpp/pubsub_test", outmsg);
printf("published %d\n", i);
}
}
protected:
SelfSubscribe() : ros::Node("subscriber", g_options ) {}
void SetUp()
{
}
void TearDown()
{
}
};
TEST_F(SelfSubscribe, advSub)
void messageCallback(const test_roscpp::TestArrayConstPtr& msg)
{
ROS_INFO("received message %d", msg->counter);
if(g_failure || g_success)
return;
g_msg_i++;
if(g_msg_i != msg->counter)
{
g_failure = true;
ROS_INFO("failed");
}
else if(g_msg_i == (g_msg_count-1))
{
g_success = true;
ROS_INFO("success");
}
}
TEST(SelfSubscribe, advSub)
{
ros::NodeHandle nh;
ros::Duration d;
d.fromNSec(10000000);
success = false;
failure = false;
msg_i = -1;
g_success = false;
g_failure = false;
g_msg_i = -1;
test_roscpp::TestArray tmp;
ASSERT_TRUE(advertise("test_roscpp/pubsub_test", tmp, &SelfSubscribe::sub_cb, g_msg_count));
ASSERT_TRUE(subscribe("test_roscpp/pubsub_test", msg, &SelfSubscribe::MsgCallback, g_msg_count));
ros::Time t1(ros::Time::now()+g_dt);
while(ros::Time::now() < t1 && !success && !failure)
{
d.sleep();
ros::Publisher pub;
pub = nh.advertise<test_roscpp::TestArray>("test_roscpp/pubsub_test", g_msg_count, boost::bind(subscriberCallback, _1, boost::ref(pub)));
ASSERT_TRUE(pub);
ros::Subscriber sub = nh.subscribe("test_roscpp/pubsub_test", g_msg_count, messageCallback);
ASSERT_TRUE(sub);
ros::Time t1(ros::Time::now()+g_dt);
while(ros::Time::now() < t1 && !g_success && !g_failure)
{
d.sleep();
ros::spinOnce();
}
}
ASSERT_TRUE(unsubscribe("test_roscpp/pubsub_test"));
ASSERT_TRUE(unadvertise("test_roscpp/pubsub_test"));
ASSERT_TRUE(g_success);
ASSERT_FALSE(g_failure);
if(success)
SUCCEED();
else
FAIL();
// Now try the other order
success = false;
failure = false;
msg_i = -1;
g_success = false;
g_failure = false;
g_msg_i = -1;
ASSERT_TRUE(subscribe("test_roscpp/pubsub_test", msg, &SelfSubscribe::MsgCallback, g_msg_count));
ASSERT_TRUE(advertise("test_roscpp/pubsub_test", tmp, &SelfSubscribe::sub_cb, g_msg_count));
t1 = ros::Time(ros::Time::now()+g_dt);
while(ros::Time::now() < t1 && !success && !failure)
{
d.sleep();
ros::Subscriber sub = nh.subscribe("test_roscpp/pubsub_test", g_msg_count, messageCallback);
ASSERT_TRUE(sub);
ros::Publisher pub;
pub = nh.advertise<test_roscpp::TestArray>("test_roscpp/pubsub_test", g_msg_count, boost::bind(subscriberCallback, _1, boost::ref(pub)));
ASSERT_TRUE(pub);
ros::Time t1(ros::Time::now()+g_dt);
while(ros::Time::now() < t1 && !g_success && !g_failure)
{
d.sleep();
ros::spinOnce();
}
}
if(success)
SUCCEED();
else
FAIL();
ASSERT_TRUE(g_success);
ASSERT_FALSE(g_failure);
}
#define USAGE "USAGE: sub_pub <count> <time>"
@ -147,15 +137,18 @@ int
main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv);
ros::init(argc, argv, "subscribe_self");
if(argc != 3)
{
puts(USAGE);
return -1;
}
g_msg_count = atoi(argv[1]);
g_dt.fromSec(atof(argv[2]));
ros::NodeHandle nh;
return RUN_ALL_TESTS();
}

106
test/test_roscpp/src/subscribe_unsubscribe.cpp
View File

@ -40,96 +40,107 @@
#include <time.h>
#include <stdlib.h>
#include "ros/node.h"
#include "ros/ros.h"
#include <test_roscpp/TestArray.h>
#include <boost/thread/barrier.hpp>
#include <boost/thread.hpp>
int g_argc;
char** g_argv;
ros::Node* g_n;
class Subscriptions : public testing::Test
{
public:
test_roscpp::TestArray msg;
bool subscribed;
public:
ros::NodeHandle nh_;
ros::Subscriber sub_;
void MsgCallback()
void messageCallback(const test_roscpp::TestArrayConstPtr& msg)
{
ROS_INFO("in callback");
if(!sub_)
{
puts("in callback");
if(!subscribed)
{
puts("but not subscribed!");
FAIL();
}
ROS_INFO("but not subscribed!");
FAIL();
}
}
void AutoUnsubscribeCallback()
{
puts("in autounsub callback");
g_n->unsubscribe("test_roscpp/pubsub_test");
void autoUnsubscribeCallback(const test_roscpp::TestArrayConstPtr& msg)
{
ROS_INFO("in autounsub callback");
sub_.shutdown();
}
subscribed = false;
}
protected:
Subscriptions() {}
protected:
Subscriptions() {}
};
TEST_F(Subscriptions, subUnsub)
{
subscribed = false;
sub_.shutdown();
for(int i=0;i<100;i++)
{
if(!subscribed)
if(!sub_)
{
subscribed = true;
ASSERT_TRUE(g_n->subscribe("test_roscpp/pubsub_test", msg, &Subscriptions::MsgCallback, (Subscriptions*)this, 1));
sub_ = nh_.subscribe("test_roscpp/pubsub_test", 0, &Subscriptions::messageCallback, (Subscriptions*)this);
ASSERT_TRUE(sub_);
}
else
{
ASSERT_TRUE(g_n->unsubscribe("test_roscpp/pubsub_test"));
subscribed = false;
sub_.shutdown();
ASSERT_FALSE(sub_);
}
ros::WallDuration(0.01).sleep();
ros::spinOnce();
}
}
TEST_F(Subscriptions, unsubInCallback)
{
ASSERT_TRUE(g_n->subscribe("test_roscpp/pubsub_test", msg, &Subscriptions::AutoUnsubscribeCallback, (Subscriptions*)this, 1));
subscribed = true;
sub_ = nh_.subscribe("test_roscpp/pubsub_test", 0, &Subscriptions::autoUnsubscribeCallback, (Subscriptions*)this);
while (subscribed && g_n->ok())
while (sub_ && ros::ok())
{
ros::WallDuration(0.01).sleep();
ros::spinOnce();
}
}
boost::barrier* g_barrier = NULL;
void unsubscribeAfterBarrierWait()
void spinThread(bool volatile* cont)
{
g_barrier->wait();
while (*cont)
{
ros::spinOnce();
ros::Duration(0.001).sleep();
}
}
g_n->unsubscribe("test_roscpp/pubsub_test");
void unsubscribeAfterBarrierWait(boost::barrier* barrier, ros::Subscriber& sub)
{
barrier->wait();
sub.shutdown();
}
TEST_F(Subscriptions, unsubInCallbackAndOtherThread)
{
boost::barrier barrier(2);
for (int i = 0; i < 100; ++i)
{
g_barrier = new boost::barrier(2);
ros::Subscriber sub;
sub_ = nh_.subscribe<test_roscpp::TestArray>("test_roscpp/pubsub_test", 1, boost::bind(unsubscribeAfterBarrierWait, &barrier, boost::ref(sub)));
sub = sub_;
ASSERT_TRUE(g_n->subscribe("test_roscpp/pubsub_test", msg, unsubscribeAfterBarrierWait, 1));
g_barrier->wait();
bool cont = true;
boost::thread t(spinThread, &cont);
g_n->unsubscribe("test_roscpp/pubsub_test");
barrier.wait();
delete g_barrier;
sub_.shutdown();
cont = false;
t.join();
}
}
@ -141,7 +152,7 @@ main(int argc, char** argv)
g_argc = argc;
g_argv = argv;
ros::init(g_argc, g_argv);
ros::init(g_argc, g_argv, "subscribe_unsubscribe");
if (g_argc != 1)
{
@ -149,12 +160,5 @@ main(int argc, char** argv)
return -1;
}
g_n = new ros::Node("subscriber");
int ret = RUN_ALL_TESTS();
delete g_n;
return ret;
return RUN_ALL_TESTS();
}

29
test/test_roscpp/src/subscribe_unsubscribe_repeatedly.cpp
View File

@ -29,32 +29,23 @@
#include <time.h>
#include "ros/node.h"
#include "ros/ros.h"
#include <test_roscpp/TestArray.h>
class Subscriber : public ros::Node
void callback(const test_roscpp::TestArrayConstPtr& msg)
{
public:
test_roscpp::TestArray msg;
Subscriber() : ros::Node("subscriber")
{
}
void cb()
{
}
};
}
int main(int argc, char **argv)
{
ros::init(argc, argv);
Subscriber s;
struct timespec sleep_time = {0, 100000000};
while(s.ok())
ros::init(argc, argv, "subscribe_unsubscribe_repeatedly");
ros::NodeHandle nh;
ros::Duration sleep_time(0, 100000000);
while(ros::ok())
{
s.subscribe("test_roscpp/pubsub_test", s.msg, &Subscriber::cb, 1);
nanosleep(&sleep_time,NULL);
s.unsubscribe("test_roscpp/pubsub_test");
nanosleep(&sleep_time,NULL);
sleep_time.sleep();
ros::Subscriber sub = nh.subscribe("test_roscpp/pubsub_test", 1, callback);
sleep_time.sleep();
}
return 0;

1
test/test_roscpp/src/timer_callbacks.cpp
View File

@ -43,7 +43,6 @@
#include <stdlib.h>
#include "ros/ros.h"
#include "ros/node.h"
#include "ros/callback_queue.h"
#include <test_roscpp/TestArray.h>
#include <test_roscpp/TestStringString.h>

4
test/test_roscpp/test/CMakeLists.txt
View File

@ -3,12 +3,10 @@ rospack_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/sim_time_test.xml)
# Publish one message
rospack_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/pubsub_once.xml)
rospack_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/old_pubsub_once.xml)
# Publish a bunch of messages back to back
rospack_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/pubsub_n_fast.xml)
rospack_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/pubsub_n_fast_udp.xml)
rospack_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/old_pubsub_n_fast.xml)
# Publish a bunch of empty messages
rospack_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/pubsub_empty.xml)
@ -18,12 +16,10 @@ rospack_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/pub_onsub.xml)
# Publish a bunch of large messages back to back
rospack_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/pubsub_n_fast_large_message.xml)
rospack_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/old_pubsub_n_fast_large_message.xml)
# Subscribe, listen, unsubscribe, re-subscribe to a different topic, listen
# again
rospack_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/pubsub_resub_once.xml)
rospack_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/old_pubsub_resub_once.xml)
# Subscribe and unsubscribe repeatedly, ensuring that callbacks don't get
# called when not subscribed.

6
test/test_roscpp/test/old_pubsub_n_fast.xml
View File

@ -1,6 +0,0 @@
<launch>
<node pkg="test_roscpp" type="old_publish_n_fast" args="10000 1 1"/>
<test test-name="old_pubsub_n_fast" pkg="test_roscpp"
type="old_subscribe_n_fast" args="10000 5.0"/>
</launch>

6
test/test_roscpp/test/old_pubsub_n_fast_large_message.xml
View File

@ -1,6 +0,0 @@
<launch>
<node pkg="test_roscpp" type="old_publish_n_fast" args="50 100000 1000000"/>
<test test-name="old_pubsub_n_fast_large_message" pkg="test_roscpp"
type="old_subscribe_n_fast" args="50 10.0"/>
</launch>

4
test/test_roscpp/test/old_pubsub_once.xml
View File

@ -1,4 +0,0 @@
<launch>
<node pkg="test_roscpp" type="old_publish_n_fast" args="1 1 1"/>
<test test-name="old_pubsub_once" pkg="test_roscpp" type="old_subscribe_n_fast" args="1 1.0"/>
</launch>

5
test/test_roscpp/test/old_pubsub_resub_once.xml
View File

@ -1,5 +0,0 @@
<launch>
<node pkg="test_roscpp" type="old_publish_n_fast" args="1 1 1"/>
<node pkg="test_roscpp" type="old_publish_n_fast" args="1 1 1 __name:=publisher2 test_roscpp/pubsub_test:=test_roscpp/pubsub_test2"/>
<test test-name="old_pubsub_once" pkg="test_roscpp" type="subscribe_resubscribe" args="1 1.0"/>
</launch>

2
tools/rosrecord/include/rosrecord/Player.h
View File

@ -35,7 +35,7 @@
#ifndef ROSRECORDPLAYER_H
#define ROSRECORDPLAYER_H
#include "ros/node.h"
#include "ros/ros.h"
#include "ros/header.h"
#include "ros/time.h"
#include "rosrecord/AnyMsg.h"

2
tools/rosrecord/include/rosrecord/rosplay.h
View File

@ -37,7 +37,7 @@
#include <time.h>
#include <sys/stat.h>
#include "ros/node.h"
#include "ros/ros.h"
#include "ros/time.h"
#include <string>

2
tools/rxtools/src/rxconsole/rxconsole.cpp
View File

@ -31,7 +31,7 @@
#include "rxtools/rosout_panel.h"
#include "ros/node.h"
#include "ros/ros.h"
#ifdef __WXMAC__
#include <ApplicationServices/ApplicationServices.h>

2
tools/rxtools/src/rxtools/logger_level_panel.cpp
View File

@ -29,7 +29,7 @@
#include "logger_level_panel.h"
#include <ros/node.h>
#include <ros/ros.h>
#include <roscpp/GetLoggers.h>
#include <roscpp/SetLoggerLevel.h>

2
tools/rxtools/src/rxtools/rosout_panel.cpp
View File

@ -40,7 +40,7 @@
#include <wx/aui/auibook.h>
#include <wx/richtext/richtextctrl.h>
#include <ros/node.h>
#include <ros/ros.h>
#include <sstream>
#include <algorithm>

1
tools/topic_tools/src/drop.cpp
View File

@ -29,7 +29,6 @@
#include <cstdlib>
#include <cstdio>
#include "ros/node.h"
#include "topic_tools/shape_shifter.h"
using std::string;

1
tools/topic_tools/src/mux.cpp
View File

@ -30,7 +30,6 @@
#include <cstdio>
#include "ros/node.h"
#include "std_msgs/String.h"
#include "topic_tools/MuxSelect.h"
#include "topic_tools/shape_shifter.h"

1
tools/topic_tools/src/relay.cpp
View File

@ -32,7 +32,6 @@
#include <cstdio>
#include "ros/node.h"
#include "topic_tools/shape_shifter.h"
using std::string;

1
tools/topic_tools/src/throttle.cpp
View File

@ -35,7 +35,6 @@
#include <cstdio>
#include <cstdlib>
#include <deque>
#include "ros/node.h"
#include "topic_tools/shape_shifter.h"
using std::string;