niklas/rclcpp
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

major refactor of rclcpp to support executors

Browse source
This commit is contained in:
William Woodall 2014-08-25 15:40:40 -07:00
parent 5b5db8914a
commit b6e1a2687a
18 changed files with 1270 additions and 171 deletions
Download patch file Download diff file Expand all files Collapse all files

66
rclcpp/include/rclcpp/Node.h
View file

@ -1,66 +0,0 @@
#ifndef __rclcpp__Node__h__
#define __rclcpp__Node__h__
#include <memory>
#include <string>
#include <vector>
#include "Publisher.h"
#include "Subscriber.h"
#include "ros_middleware_interface/functions.h"
#include "ros_middleware_interface/handles.h"
#include "ros_middleware_interface/get_type_support_handle.h"
namespace rclcpp
{
namespace executor
{
class SingleThreadExecutor;
}
class Node
{
friend class rclcpp::executor::SingleThreadExecutor;
public:
Node()
{
node_handle_ = ::ros_middleware_interface::create_node();
}
template<typename ROSMessage>
Publisher<ROSMessage>* create_publisher(const char * topic_name)
{
const rosidl_generator_cpp::MessageTypeSupportHandle & type_support_handle = ::ros_middleware_interface::get_type_support_handle<ROSMessage>();
ros_middleware_interface::PublisherHandle publisher_handle = ::ros_middleware_interface::create_publisher(node_handle_, type_support_handle, topic_name);
return new Publisher<ROSMessage>(publisher_handle);
}
template<typename ROSMessage>
Subscriber<ROSMessage>* create_subscriber(const char * topic_name, typename Subscriber<ROSMessage>::CallbackType callback)
{
const rosidl_generator_cpp::MessageTypeSupportHandle & type_support_handle = ::ros_middleware_interface::get_type_support_handle<ROSMessage>();
ros_middleware_interface::SubscriberHandle subscriber_handle = ::ros_middleware_interface::create_subscriber(node_handle_, type_support_handle, topic_name);
SubscriberInterface *sub = new Subscriber<ROSMessage>(subscriber_handle, std::string(topic_name), callback);
this->subscribers_.push_back(sub);
return static_cast<Subscriber<ROSMessage> *>(sub);
}
private:
ros_middleware_interface::NodeHandle node_handle_;
std::vector<SubscriberInterface *> subscribers_;
};
rclcpp::Node* create_node()
{
return new rclcpp::Node();
}
} /* namespace rclcpp */
#endif // __rclcpp__Node__h__

30
rclcpp/include/rclcpp/Publisher.h
View file

@ -1,30 +0,0 @@
#ifndef __rclcpp__Publisher__h__
#define __rclcpp__Publisher__h__
#include "ros_middleware_interface/functions.h"
#include "ros_middleware_interface/handles.h"
namespace rclcpp
{
template<typename ROSMessage>
class Publisher
{
public:
Publisher(const ros_middleware_interface::PublisherHandle& publisher_handle)
: publisher_handle_(publisher_handle)
{}
void publish(const ROSMessage& ros_message)
{
::ros_middleware_interface::publish(publisher_handle_, &ros_message);
}
private:
ros_middleware_interface::PublisherHandle publisher_handle_;
};
}
#endif // __rclcpp__Publisher__h__

75
rclcpp/include/rclcpp/Subscriber.h
View file

@ -1,75 +0,0 @@
#ifndef __rclcpp__Subscriber__h__
#define __rclcpp__Subscriber__h__
#include <functional>
#include <string>
#include "ros_middleware_interface/functions.h"
#include "ros_middleware_interface/handles.h"
namespace rclcpp
{
class Node;
namespace executor
{
class SingleThreadExecutor;
}
class SubscriberInterface
{
friend class rclcpp::executor::SingleThreadExecutor;
public:
SubscriberInterface(const ros_middleware_interface::SubscriberHandle &subscriber_handle, std::string topic_name)
: subscriber_handle_(subscriber_handle), topic_name_(topic_name)
{}
private:
virtual void * create_message() = 0;
virtual void delete_message(void * ros_message) = 0;
virtual void handle_message(void * ros_message) = 0;
ros_middleware_interface::SubscriberHandle subscriber_handle_;
std::string topic_name_;
};
template<typename ROSMessage>
class Subscriber : public SubscriberInterface
{
friend class rclcpp::Node;
public:
typedef std::function<void(const ROSMessage *)> CallbackType;
Subscriber(const ros_middleware_interface::SubscriberHandle &subscriber_handle, std::string topic_name, CallbackType callback)
: SubscriberInterface(subscriber_handle, topic_name), callback_(callback)
{}
private:
void * create_message()
{
return new ROSMessage();
}
void delete_message(void * ros_message)
{
ROSMessage* msg = (ROSMessage*)ros_message;
delete msg;
ros_message = 0;
}
void handle_message(void * ros_message)
{
ROSMessage* msg = (ROSMessage*)ros_message;
callback_(msg);
}
CallbackType callback_;
};
}
#endif // __rclcpp__Subscriber__h__

64
rclcpp/include/rclcpp/callback_group.hpp Normal file
View file

@ -0,0 +1,64 @@
/* Copyright 2014 Open Source Robotics Foundation, Inc.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef RCLCPP_RCLCPP_CALLBACK_GROUP_HPP_
#define RCLCPP_RCLCPP_CALLBACK_GROUP_HPP_
#include <string>
#include <vector>
#include <rclcpp/subscription.hpp>
namespace rclcpp
{
// Forward declarations for friend statement in class CallbackGroup
namespace node {class Node;}
namespace executor {class Executor;}
namespace callback_group
{
enum class CallbackGroupType {NonThreadSafe, ThreadSafe};
class CallbackGroup
{
friend class rclcpp::node::Node;
friend class rclcpp::executor::Executor;
public:
RCLCPP_MAKE_SHARED_DEFINITIONS(CallbackGroup);
private:
CallbackGroup(std::string group_name, CallbackGroupType group_type)
: name_(group_name), type_(group_type)
{}
RCLCPP_DISABLE_COPY(CallbackGroup);
void
add_subscription(subscription::SubscriptionBase::SharedPtr &subscription_ptr)
{
subscription_ptrs_.push_back(subscription_ptr);
}
std::string name_;
CallbackGroupType type_;
std::vector<subscription::SubscriptionBase::SharedPtr> subscription_ptrs_;
};
} /* namespace callback_group */
} /* namespace rclcpp */
#endif /* RCLCPP_RCLCPP_CALLBACK_GROUP_HPP_ */

44
rclcpp/include/rclcpp/context.hpp Normal file
View file

@ -0,0 +1,44 @@
/* Copyright 2014 Open Source Robotics Foundation, Inc.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef RCLCPP_RCLCPP_CONTEXT_HPP_
#define RCLCPP_RCLCPP_CONTEXT_HPP_
#include <memory>
#include <rclcpp/macros.hpp>
namespace rclcpp
{
namespace context
{
/* ROS Context */
class Context
{
public:
RCLCPP_MAKE_SHARED_DEFINITIONS(Context);
Context() {}
private:
RCLCPP_DISABLE_COPY(Context);
};
} /* namespace context */
} /* namespace rclcpp */
#endif /* RCLCPP_RCLCPP_CONTEXT_HPP_ */

41
rclcpp/include/rclcpp/contexts/default_context.hpp Normal file
View file

@ -0,0 +1,41 @@
/* Copyright 2014 Open Source Robotics Foundation, Inc.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef RCLCPP_RCLCPP_CONTEXTS_DEFAULT_CONTEXT_HPP_
#define RCLCPP_RCLCPP_CONTEXTS_DEFAULT_CONTEXT_HPP_
#include <rclcpp/context.hpp>
namespace rclcpp
{
namespace contexts
{
namespace default_context
{
class DefaultContext : public rclcpp::context::Context
{
public:
RCLCPP_MAKE_SHARED_DEFINITIONS(DefaultContext);
DefaultContext() {}
};
}
}
}
#endif /* RCLCPP_RCLCPP_CONTEXTS_DEFAULT_CONTEXT_HPP_ */

293
rclcpp/include/rclcpp/executor.hpp Normal file
View file

@ -0,0 +1,293 @@
/* Copyright 2014 Open Source Robotics Foundation, Inc.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef RCLCPP_RCLCPP_EXECUTOR_HPP_
#define RCLCPP_RCLCPP_EXECUTOR_HPP_
#include <iostream>
#include <algorithm>
#include <cassert>
#include <cstdlib>
#include <list>
#include <memory>
#include <vector>
#include <rclcpp/macros.hpp>
#include <rclcpp/node.hpp>
#include <rclcpp/utilities.hpp>
namespace rclcpp
{
namespace executor
{
class Executor
{
public:
RCLCPP_MAKE_SHARED_DEFINITIONS(Executor);
Executor()
{
subscriber_handles_.subscriber_count_ = 0;
subscriber_handles_.subscribers_ = 0;
}
~Executor() {}
void add_node(rclcpp::node::Node::SharedPtr &node_ptr)
{
this->weak_nodes_.push_back(node_ptr);
}
protected:
static void execute_subscription(
rclcpp::subscription::SubscriptionBase::SharedPtr &subscription)
{
std::shared_ptr<void> message = subscription->create_message();
bool taken = ros_middleware_interface::take(
subscription->subscription_handle_,
message.get());
if (taken)
{
subscription->handle_message(message);
}
else
{
std::cout << "[rclcpp::error] take failed for subscription on topic: "
<< subscription->get_topic_name()
<< std::endl;
}
}
void reset_subscriber_handles()
{
if (subscriber_handles_.subscriber_count_ != 0)
{
subscriber_handles_.subscriber_count_ = 0;
std::free(subscriber_handles_.subscribers_);
}
}
void populate_subscriber_handles_with_node(rclcpp::node::Node &node)
{
// Use the number of subscriptions to pre allocate subscriber_handles
subscriber_handles_.subscriber_count_ = node.number_of_subscriptions_;
subscriber_handles_.subscribers_ = static_cast<void **>(
std::malloc(sizeof(void *) * node.number_of_subscriptions_));
// Add subscriptions from groups
size_t handles_index = 0;
for (auto group : node.callback_groups_)
{
for (auto subscription : group->subscription_ptrs_)
{
assert(handles_index < node.number_of_subscriptions_);
subscriber_handles_.subscribers_[handles_index] = \
subscription->subscription_handle_.data_;
handles_index += 1;
}
}
assert(handles_index == node.number_of_subscriptions_);
}
void populate_subscriber_handles()
{
// Calculate the number of subscriptions and create shared_ptrs
size_t number_of_subscriptions = 0;
std::vector<rclcpp::node::Node::SharedPtr> nodes;
bool has_invalid_weak_nodes = false;
for (auto &weak_node : weak_nodes_)
{
auto node = weak_node.lock();
if (!node)
{
has_invalid_weak_nodes = false;
continue;
}
nodes.push_back(node);
number_of_subscriptions += node->number_of_subscriptions_;
}
// Clean up any invalid nodes, if they were detected
if (has_invalid_weak_nodes)
{
weak_nodes_.erase(remove_if(weak_nodes_.begin(), weak_nodes_.end(),
[](std::weak_ptr<rclcpp::node::Node> i)
{
return i.expired();
}));
}
// Use the number of subscriptions to pre allocate subscriber_handles
subscriber_handles_.subscriber_count_ = number_of_subscriptions;
subscriber_handles_.subscribers_ = static_cast<void **>(
std::malloc(sizeof(void *) * number_of_subscriptions));
if (subscriber_handles_.subscribers_ == NULL)
{
// TODO: Use a different error here? maybe std::bad_alloc?
throw std::runtime_error("Could not malloc for subscriber pointers.");
}
// Now fill the subscriber_handles with subscribers from the nodes
size_t handles_index = 0;
for (auto &node : nodes)
{
for (auto group : node->callback_groups_)
{
for (auto subscription : group->subscription_ptrs_)
{
assert(handles_index < number_of_subscriptions);
subscriber_handles_.subscribers_[handles_index] = \
subscription->subscription_handle_.data_;
handles_index += 1;
}
}
}
assert(handles_index == number_of_subscriptions);
}
struct AnyExecutable
{
AnyExecutable() : subscription(0), guard_condition_handle(0) {}
rclcpp::subscription::SubscriptionBase::SharedPtr subscription;
ros_middleware_interface::GuardConditionHandle *guard_condition_handle;
};
std::list<void *> get_ready_subscriber_handles()
{
std::list<void *> ready_subscriber_handles;
for (size_t i = 0; i < subscriber_handles_.subscriber_count_; ++i)
{
void *handle = subscriber_handles_.subscribers_[i];
if (!handle)
{
continue;
}
ready_subscriber_handles.push_back(handle);
}
return ready_subscriber_handles;
}
rclcpp::subscription::SubscriptionBase::SharedPtr get_subscription_by_handle(
void * subscriber_handle)
{
for (auto weak_node : weak_nodes_)
{
auto node = weak_node.lock();
if (!node)
{
continue;
}
for (auto group : node->callback_groups_)
{
for (auto subscription : group->subscription_ptrs_)
{
if (subscription->subscription_handle_.data_ == subscriber_handle)
{
return subscription;
}
}
}
}
return rclcpp::subscription::SubscriptionBase::SharedPtr();
}
void remove_subscriber_handle_from_subscriber_handles_(void *handle)
{
for (size_t i = 0; i < subscriber_handles_.subscriber_count_; ++i)
{
if (handle == subscriber_handles_.subscribers_[i])
{
// TODO: use nullptr here?
subscriber_handles_.subscribers_[i] = NULL;
}
}
}
rclcpp::subscription::SubscriptionBase::SharedPtr get_next_subscription(
std::list<void *> &ready_subscriber_handles)
{
if (ready_subscriber_handles.size() == 0)
{
return rclcpp::subscription::SubscriptionBase::SharedPtr();
}
while (ready_subscriber_handles.size() != 0)
{
void *handle = ready_subscriber_handles.front();
ready_subscriber_handles.pop_front();
remove_subscriber_handle_from_subscriber_handles_(handle);
auto subscription = get_subscription_by_handle(handle);
if (subscription) return subscription;
}
return rclcpp::subscription::SubscriptionBase::SharedPtr();
}
std::shared_ptr<AnyExecutable> get_next_executable(bool nonblocking=false)
{
namespace rmi = ros_middleware_interface;
// Get any ready subscriber handles out of subscriber_handles_.
// TODO: operate directly on subscriber_handles_ or
// store ready_subscriber_handles to prevent rebuilding it every time
auto ready_subscriber_handles = get_ready_subscriber_handles();
// If there are none
if (ready_subscriber_handles.size() == 0)
{
// Repopulate the subscriber handles and wait on them
reset_subscriber_handles();
populate_subscriber_handles();
// TODO: populate the guard handles correctly
rmi::GuardConditionHandles guard_condition_handles;
guard_condition_handles.guard_condition_count_ = 1;
guard_condition_handles.guard_conditions_ = static_cast<void **>(
std::malloc(sizeof(void *) * 1));
guard_condition_handles.guard_conditions_[0] = \
rclcpp::utilities::get_global_sigint_guard_cond().data_;
// Wait on the handles.
rmi::wait(subscriber_handles_, guard_condition_handles, nonblocking);
std::free(guard_condition_handles.guard_conditions_);
ready_subscriber_handles = get_ready_subscriber_handles();
}
// At this point there should be something in either the subscriber handles
// Or in the guard handles.
// TODO: also check the timers here
std::shared_ptr<AnyExecutable> any_exec(new AnyExecutable());
// TODO: actually check the guard handles
any_exec->guard_condition_handle = 0;
// If any_exec.guard_condition_handle is valid, return now before checking
// for valid subscriptions.
if (any_exec->guard_condition_handle)
{
return any_exec;
}
// Check for subscriptions which are ready to be handled
any_exec->subscription = get_next_subscription(ready_subscriber_handles);
// If any_exec.subscription is valid, return now
if (any_exec->subscription)
{
return any_exec;
}
// If there is neither a ready guard condition nor subscription, return an
// empty any_exec anyways
return any_exec;
}
private:
RCLCPP_DISABLE_COPY(Executor);
std::vector<std::weak_ptr<rclcpp::node::Node>> weak_nodes_;
ros_middleware_interface::SubscriberHandles subscriber_handles_;
};
} /* executor */
} /* rclcpp */
#endif /* RCLCPP_RCLCPP_EXECUTOR_HPP_ */

33
rclcpp/include/rclcpp/executors.hpp Normal file
View file

@ -0,0 +1,33 @@
/* Copyright 2014 Open Source Robotics Foundation, Inc.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef RCLCPP_RCLCPP_EXECUTORS_HPP_
#define RCLCPP_RCLCPP_EXECUTORS_HPP_
#include <rclcpp/executors/multi_threaded_executor.hpp>
#include <rclcpp/executors/single_threaded_executor.hpp>
namespace rclcpp
{
namespace executors
{
using rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor;
using rclcpp::executors::single_threaded_executor::SingleThreadedExecutor;
}
}
#endif /* RCLCPP_RCLCPP_EXECUTORS_HPP_ */

68
rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp Normal file
View file

@ -0,0 +1,68 @@
/* Copyright 2014 Open Source Robotics Foundation, Inc.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef RCLCPP_RCLCPP_EXECUTORS_MULTI_THREADED_EXECUTOR_HPP_
#define RCLCPP_RCLCPP_EXECUTORS_MULTI_THREADED_EXECUTOR_HPP_
#include <cassert>
#include <cstdlib>
#include <memory>
#include <vector>
#include <ros_middleware_interface/functions.h>
#include <ros_middleware_interface/handles.h>
#include <rclcpp/executor.hpp>
#include <rclcpp/macros.hpp>
#include <rclcpp/node.hpp>
#include <rclcpp/utilities.hpp>
namespace rclcpp
{
namespace executors
{
namespace multi_threaded_executor
{
class MultiThreadedExecutor : public executor::Executor
{
public:
RCLCPP_MAKE_SHARED_DEFINITIONS(MultiThreadedExecutor);
void add_node(rclcpp::node::Node::SharedPtr &node_ptr)
{
this->weak_nodes_.push_back(node_ptr);
}
void spin()
{
}
void spin_some()
{
}
private:
RCLCPP_DISABLE_COPY(MultiThreadedExecutor);
std::vector<std::weak_ptr<rclcpp::node::Node>> weak_nodes_;
};
} /* namespace multi_threaded_executor */
} /* namespace executors */
} /* namespace rclcpp */
#endif /* RCLCPP_RCLCPP_EXECUTORS_MULTI_THREADED_EXECUTOR_HPP_ */

88
rclcpp/include/rclcpp/executors/single_threaded_executor.hpp Normal file
View file

@ -0,0 +1,88 @@
/* Copyright 2014 Open Source Robotics Foundation, Inc.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef RCLCPP_RCLCPP_EXECUTORS_SINGLE_THREADED_EXECUTOR_HPP_
#define RCLCPP_RCLCPP_EXECUTORS_SINGLE_THREADED_EXECUTOR_HPP_
#include <cassert>
#include <cstdlib>
#include <memory>
#include <vector>
#include <ros_middleware_interface/functions.h>
#include <ros_middleware_interface/handles.h>
#include <rclcpp/executor.hpp>
#include <rclcpp/macros.hpp>
#include <rclcpp/node.hpp>
#include <rclcpp/utilities.hpp>
#include <rclcpp/rate.hpp>
namespace rclcpp
{
namespace executors
{
namespace single_threaded_executor
{
class SingleThreadedExecutor : public executor::Executor
{
public:
RCLCPP_MAKE_SHARED_DEFINITIONS(SingleThreadedExecutor);
SingleThreadedExecutor() {}
~SingleThreadedExecutor() {}
void spin()
{
while (rclcpp::utilities::ok())
{
auto any_exec = get_next_executable();
if (any_exec->subscription)
{
// Do callback
execute_subscription(any_exec->subscription);
}
}
}
void spin_node_some(rclcpp::node::Node &node)
{
reset_subscriber_handles();
populate_subscriber_handles_with_node(node);
// non-blocking = true
auto any_exec = get_next_executable(true);
while (any_exec->subscription)
{
execute_subscription(any_exec->subscription);
// non-blocking = true
any_exec = get_next_executable(true);
}
reset_subscriber_handles();
}
private:
RCLCPP_DISABLE_COPY(SingleThreadedExecutor);
std::vector<std::weak_ptr<rclcpp::node::Node>> weak_nodes_;
};
} /* namespace single_threaded_executor */
} /* namespace executors */
} /* namespace rclcpp */
#endif /* RCLCPP_RCLCPP_EXECUTORS_SINGLE_THREADED_EXECUTOR_HPP_ */

42
rclcpp/include/rclcpp/macros.hpp Normal file
View file

@ -0,0 +1,42 @@
/* Copyright 2014 Open Source Robotics Foundation, Inc.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef RCLCPP_RCLCPP_MACROS_HPP_
#define RCLCPP_RCLCPP_MACROS_HPP_
/* Disables the copy constructor and operator= for the given class.
*
* Use in the private section of the class.
*/
#define RCLCPP_DISABLE_COPY(Class) \
Class(const Class&) = delete; \
Class& operator=(const Class&) = delete;
/* Defines a make_shared static function on the class using perfect forwarding.
*
* Use in the public section of the class.
* Make sure to include <memory> in the header when using this.
*/
#define RCLCPP_MAKE_SHARED_DEFINITIONS(Class) \
typedef std::shared_ptr<Class> SharedPtr; \
\
template<typename...Args> \
static std::shared_ptr<Class> \
make_shared(Args &&...args) \
{ \
return std::make_shared<Class>(std::forward<Args>(args)...); \
}
#endif /* RCLCPP_RCLCPP_MACROS_HPP_ */

102
rclcpp/include/rclcpp/node.hpp Normal file
View file

@ -0,0 +1,102 @@
/* Copyright 2014 Open Source Robotics Foundation, Inc.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef RCLCPP_RCLCPP_NODE_HPP_
#define RCLCPP_RCLCPP_NODE_HPP_
#include <memory>
#include <string>
#include <rclcpp/callback_group.hpp>
#include <rclcpp/context.hpp>
#include <rclcpp/macros.hpp>
#include <rclcpp/publisher.hpp>
#include <rclcpp/subscription.hpp>
namespace rclcpp
{
// Forward declaration for friend statement
namespace executor {class Executor;}
namespace node
{
/* ROS Node Interface.
*
* This is the single point of entry for creating publishers and subscribers.
*/
class Node
{
friend class rclcpp::executor::Executor;
public:
RCLCPP_MAKE_SHARED_DEFINITIONS(Node);
/* Create a node based on the node name. */
Node(std::string node_name);
/* Create a node based on the node name and a rclcpp::context::Context. */
Node(std::string node_name, rclcpp::context::Context::SharedPtr context);
/* Get the name of the node. */
std::string
get_name();
/* Creates a named callback group. */
void
create_callback_group(
std::string group_name,
rclcpp::callback_group::CallbackGroupType callback_group_type);
/* Return callback group if it exists, otherwise evaluates to false. */
rclcpp::callback_group::CallbackGroup::SharedPtr
get_callback_group(std::string group_name);
/* Create and return a Publisher. */
template<typename MessageT> rclcpp::publisher::Publisher::SharedPtr
create_publisher(std::string topic_name, size_t queue_size);
/* Create and return a Subscription. */
template<typename MessageT>
typename rclcpp::subscription::Subscription<MessageT>::SharedPtr
create_subscription(
std::string topic_name,
size_t queue_size,
std::function<void(const std::shared_ptr<MessageT> &)> callback);
private:
RCLCPP_DISABLE_COPY(Node);
std::string name_;
ros_middleware_interface::NodeHandle node_handle_;
rclcpp::context::Context::SharedPtr context_;
rclcpp::callback_group::CallbackGroup::SharedPtr default_callback_group_;
std::vector<rclcpp::callback_group::CallbackGroup::SharedPtr> callback_groups_;
size_t number_of_subscriptions_;
};
} /* namespace node */
} /* namespace rclcpp */
#ifndef RCLCPP_RCLCPP_NODE_IMPL_HPP_
// Template implementations
#include "node_impl.hpp"
#endif
#endif /* RCLCPP_RCLCPP_NODE_HPP_ */

91
rclcpp/include/rclcpp/node_impl.hpp Normal file
View file

@ -0,0 +1,91 @@
/* Copyright 2014 Open Source Robotics Foundation, Inc.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef RCLCPP_RCLCPP_NODE_IMPL_HPP_
#define RCLCPP_RCLCPP_NODE_IMPL_HPP_
#include <memory>
#include <string>
#include <ros_middleware_interface/functions.h>
#include <ros_middleware_interface/handles.h>
#include <ros_middleware_interface/get_type_support_handle.h>
#include <rclcpp/contexts/default_context.hpp>
#ifndef RCLCPP_RCLCPP_NODE_HPP_
#include "node.hpp"
#endif
using namespace rclcpp;
using namespace rclcpp::node;
using rclcpp::contexts::default_context::DefaultContext;
Node::Node(std::string node_name)
: Node(node_name, DefaultContext::make_shared())
{}
Node::Node(std::string node_name, context::Context::SharedPtr context)
: name_(node_name), context_(context), number_of_subscriptions_(0)
{
node_handle_ = ::ros_middleware_interface::create_node();
using rclcpp::callback_group::CallbackGroup;
using rclcpp::callback_group::CallbackGroupType;
default_callback_group_.reset(new CallbackGroup(
"default_group", CallbackGroupType::NonThreadSafe));
callback_groups_.push_back(default_callback_group_);
}
template<typename MessageT> publisher::Publisher::SharedPtr
Node::create_publisher(std::string topic_name, size_t queue_size)
{
namespace rmi = ::ros_middleware_interface;
auto type_support_handle = rmi::get_type_support_handle<MessageT>();
auto publisher_handle = rmi::create_publisher(this->node_handle_,
type_support_handle,
topic_name.c_str());
return publisher::Publisher::make_shared(publisher_handle);
}
template<typename MessageT>
typename subscription::Subscription<MessageT>::SharedPtr
Node::create_subscription(
std::string topic_name,
size_t queue_size,
std::function<void(const std::shared_ptr<MessageT> &)> callback)
{
namespace rmi = ::ros_middleware_interface;
auto &type_support_handle = rmi::get_type_support_handle<MessageT>();
auto subscriber_handle = rmi::create_subscriber(this->node_handle_,
type_support_handle,
topic_name.c_str());
using namespace rclcpp::subscription;
auto sub = Subscription<MessageT>::make_shared(subscriber_handle,
topic_name,
callback);
auto sub_base_ptr = std::dynamic_pointer_cast<SubscriptionBase>(sub);
this->default_callback_group_->add_subscription(sub_base_ptr);
number_of_subscriptions_++;
return sub;
}
#endif /* RCLCPP_RCLCPP_NODE_IMPL_HPP_ */

58
rclcpp/include/rclcpp/publisher.hpp Normal file
View file

@ -0,0 +1,58 @@
/* Copyright 2014 Open Source Robotics Foundation, Inc.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef RCLCPP_RCLCPP_PUBLISHER_HPP_
#define RCLCPP_RCLCPP_PUBLISHER_HPP_
#include <memory>
#include <ros_middleware_interface/functions.h>
#include <ros_middleware_interface/handles.h>
#include <rclcpp/macros.hpp>
namespace rclcpp
{
// Forward declaration for friend statement
namespace node {class Node;}
namespace publisher
{
class Publisher
{
public:
RCLCPP_MAKE_SHARED_DEFINITIONS(Publisher);
Publisher(ros_middleware_interface::PublisherHandle publisher_handle)
: publisher_handle_(publisher_handle)
{}
template<typename MessageT> void
publish(std::shared_ptr<MessageT> &msg)
{
::ros_middleware_interface::publish(publisher_handle_, msg.get());
}
private:
ros_middleware_interface::PublisherHandle publisher_handle_;
};
} /* namespace publisher */
} /* namespace rclcpp */
#endif /* RCLCPP_RCLCPP_PUBLISHER_HPP_ */

115
rclcpp/include/rclcpp/rate.hpp Normal file
View file

@ -0,0 +1,115 @@
/* Copyright 2014 Open Source Robotics Foundation, Inc.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef RCLCPP_RCLCPP_RATE_HPP_
#define RCLCPP_RCLCPP_RATE_HPP_
#include <chrono>
#include <memory>
#include <thread>
#include <rclcpp/macros.hpp>
namespace rclcpp
{
namespace rate
{
class RateBase
{
public:
RCLCPP_MAKE_SHARED_DEFINITIONS(RateBase);
virtual bool sleep() = 0;
virtual bool is_steady() = 0;
virtual void reset() = 0;
};
template<class Clock = std::chrono::high_resolution_clock>
class GenericRate : public RateBase
{
public:
RCLCPP_MAKE_SHARED_DEFINITIONS(GenericRate);
GenericRate(double rate) : rate_(rate), last_interval_(Clock::now())
{
typedef std::chrono::nanoseconds nanoseconds;
auto tmp = std::chrono::duration<double>(1.0f/rate_);
period_ = std::chrono::duration_cast<nanoseconds>(tmp);
}
virtual bool
sleep()
{
// Time coming into sleep
auto now = Clock::now();
// Time of next interval
auto next_interval = last_interval_ + period_;
// Detect backwards time flow
if (now < last_interval_)
{
// Best thing to do is to set the next_interval to now + period
next_interval = now + period_;
}
// Calculate the time to sleep
auto time_to_sleep = next_interval - now;
// Update the interval
last_interval_ += period_;
// If the time_to_sleep is negative or zero, don't sleep
if (time_to_sleep <= std::chrono::seconds(0))
{
// If an entire cycle was missed then reset next interval.
// This might happen if the loop took more than a cycle.
// Or if time jumps forward.
if (now > next_interval + period_)
{
last_interval_ = now + period_;
}
// Either way do not sleep and return false
return false;
}
// Sleep
std::this_thread::sleep_for(time_to_sleep);
return true;
}
virtual bool
is_steady()
{
return Clock::is_steady;
}
virtual void
reset()
{
last_interval_ = Clock::now();
}
private:
RCLCPP_DISABLE_COPY(GenericRate);
double rate_;
std::chrono::nanoseconds period_;
std::chrono::time_point<Clock> last_interval_;
};
typedef GenericRate<std::chrono::system_clock> Rate;
typedef GenericRate<std::chrono::steady_clock> WallRate;
}
}
#endif /* RCLCPP_RCLCPP_RATE_HPP_ */

68
rclcpp/include/rclcpp/rclcpp.hpp Normal file
View file

@ -0,0 +1,68 @@
/* Copyright 2014 Open Source Robotics Foundation, Inc.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef RCLCPP_RCLCPP_RCLCPP_HPP_
#define RCLCPP_RCLCPP_RCLCPP_HPP_
#include <csignal>
#include <memory>
#include <mutex>
#include <rclcpp/node.hpp>
#include <rclcpp/executors.hpp>
#include <rclcpp/rate.hpp>
#include <rclcpp/utilities.hpp>
namespace
{
std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> g_executor(0);
std::mutex g_executor_mutex;
}
namespace rclcpp
{
using rclcpp::node::Node;
using rclcpp::rate::GenericRate;
using rclcpp::rate::WallRate;
using rclcpp::utilities::ok;
using rclcpp::utilities::init;
void spin_some(Node &node)
{
std::lock_guard<std::mutex> lock(g_executor_mutex);
if (!g_executor)
{
// Create an executor
g_executor.reset(new rclcpp::executors::SingleThreadedExecutor());
}
g_executor->spin_node_some(node);
}
void spin_some(Node::SharedPtr &node_ptr)
{
spin_some(*node_ptr);
}
void spin(Node::SharedPtr &node_ptr)
{
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(node_ptr);
executor.spin();
}
} /* namespace rclcpp */
#endif /* RCLCPP_RCLCPP_RCLCPP_HPP_ */

99
rclcpp/include/rclcpp/subscription.hpp Normal file
View file

@ -0,0 +1,99 @@
/* Copyright 2014 Open Source Robotics Foundation, Inc.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef RCLCPP_RCLCPP_SUBSCRIPTION_HPP_
#define RCLCPP_RCLCPP_SUBSCRIPTION_HPP_
#include <functional>
#include <memory>
#include <string>
#include <ros_middleware_interface/functions.h>
#include <ros_middleware_interface/handles.h>
#include <rclcpp/macros.hpp>
namespace rclcpp
{
// Forward declaration is for friend statement in SubscriptionBase
namespace executor {class Executor;}
namespace subscription
{
class SubscriptionBase
{
friend class rclcpp::executor::Executor;
public:
RCLCPP_MAKE_SHARED_DEFINITIONS(SubscriptionBase);
SubscriptionBase(
ros_middleware_interface::SubscriberHandle subscription_handle,
std::string &topic_name)
: subscription_handle_(subscription_handle), topic_name_(topic_name)
{}
std::string get_topic_name()
{
return this->topic_name_;
}
virtual std::shared_ptr<void> create_message() = 0;
virtual void handle_message(std::shared_ptr<void> &message) = 0;
private:
RCLCPP_DISABLE_COPY(SubscriptionBase);
ros_middleware_interface::SubscriberHandle subscription_handle_;
std::string topic_name_;
};
template<typename MessageT>
class Subscription : public SubscriptionBase
{
public:
typedef std::function<void(const std::shared_ptr<MessageT> &)> CallbackType;
RCLCPP_MAKE_SHARED_DEFINITIONS(Subscription);
Subscription(ros_middleware_interface::SubscriberHandle subscription_handle,
std::string &topic_name,
CallbackType callback)
: SubscriptionBase(subscription_handle, topic_name), callback_(callback)
{}
std::shared_ptr<void> create_message()
{
return std::shared_ptr<void>(new MessageT());
}
void handle_message(std::shared_ptr<void> &message)
{
auto typed_message = std::static_pointer_cast<MessageT>(message);
callback_(typed_message);
}
private:
RCLCPP_DISABLE_COPY(Subscription);
CallbackType callback_;
};
} /* namespace subscription */
} /* namespace rclcpp */
#endif /* RCLCPP_RCLCPP_SUBSCRIPTION_HPP_ */

64
rclcpp/include/rclcpp/utilities.hpp Normal file
View file

@ -0,0 +1,64 @@
/* Copyright 2014 Open Source Robotics Foundation, Inc.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef RCLCPP_RCLCPP_UTILITIES_HPP_
#define RCLCPP_RCLCPP_UTILITIES_HPP_
#include <iostream>
#include <csignal>
#include <ros_middleware_interface/functions.h>
#include <ros_middleware_interface/handles.h>
namespace
{
volatile sig_atomic_t g_signal_status = 0;
ros_middleware_interface::GuardConditionHandle g_sigint_guard_cond_handle_;
void signal_handler(int signal_value)
{
std::cout << "signal_handler(" << signal_value << ")" << std::endl;
g_signal_status = signal_value;
ros_middleware_interface::trigger_guard_condition(g_sigint_guard_cond_handle_);
}
}
namespace rclcpp
{
namespace utilities
{
void init(int argc, char *argv[])
{
::g_sigint_guard_cond_handle_ = \
ros_middleware_interface::create_guard_condition();
std::signal(SIGINT, ::signal_handler);
}
bool ok()
{
return ::g_signal_status == 0;
}
ros_middleware_interface::GuardConditionHandle get_global_sigint_guard_cond()
{
return ::g_sigint_guard_cond_handle_;
}
} /* namespace utilities */
} /* namespace rclcpp */
#endif /* RCLCPP_RCLCPP_UTILITIES_HPP_ */