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

code style only

Browse source
This commit is contained in:
Dirk Thomas 2015-04-07 10:57:33 -07:00
parent b4a2822749
commit f80b89a687
18 changed files with 393 additions and 493 deletions
Download patch file Download diff file Expand all files Collapse all files

24
rclcpp/include/rclcpp/callback_group.hpp
View file

@ -28,18 +28,28 @@ namespace rclcpp
{
// Forward declarations for friend statement in class CallbackGroup
namespace node {class Node;}
namespace executor {class Executor;}
namespace node
{
class Node;
} // namespace node
namespace executor
{
class Executor;
} // namespace executor
namespace callback_group
{
enum class CallbackGroupType {MutuallyExclusive, Reentrant};
enum class CallbackGroupType {
MutuallyExclusive,
Reentrant
};
class CallbackGroup
{
friend class rclcpp::node::Node;
friend class rclcpp::executor::Executor;
public:
RCLCPP_MAKE_SHARED_DEFINITIONS(CallbackGroup);
@ -52,25 +62,25 @@ private:
void
add_subscription(
const subscription::SubscriptionBase::SharedPtr &subscription_ptr)
const subscription::SubscriptionBase::SharedPtr & subscription_ptr)
{
subscription_ptrs_.push_back(subscription_ptr);
}
void
add_timer(const timer::TimerBase::SharedPtr &timer_ptr)
add_timer(const timer::TimerBase::SharedPtr & timer_ptr)
{
timer_ptrs_.push_back(timer_ptr);
}
void
add_service(const service::ServiceBase::SharedPtr &service_ptr)
add_service(const service::ServiceBase::SharedPtr & service_ptr)
{
service_ptrs_.push_back(service_ptr);
}
void
add_client(const client::ClientBase::SharedPtr &client_ptr)
add_client(const client::ClientBase::SharedPtr & client_ptr)
{
client_ptrs_.push_back(client_ptr);
}

33
rclcpp/include/rclcpp/client.hpp
View file

@ -29,7 +29,10 @@ namespace rclcpp
{
// Forward declaration for friend statement
namespace executor {class Executor;}
namespace executor
{
class Executor;
} // namespace executor
namespace client
{
@ -37,18 +40,17 @@ namespace client
class ClientBase
{
friend class rclcpp::executor::Executor;
public:
RCLCPP_MAKE_SHARED_DEFINITIONS(ClientBase);
ClientBase(rmw_client_t * client_handle,
const std::string &service_name)
: client_handle_(client_handle), service_name_(service_name)
ClientBase(rmw_client_t * client_handle, const std::string & service_name)
: client_handle_(client_handle), service_name_(service_name)
{}
~ClientBase()
{
if (client_handle_ != nullptr)
{
if (client_handle_ != nullptr) {
rmw_destroy_client(client_handle_);
client_handle_ = nullptr;
}
@ -66,8 +68,8 @@ public:
virtual std::shared_ptr<void> create_response() = 0;
virtual std::shared_ptr<void> create_request_header() = 0;
virtual void handle_response(std::shared_ptr<void> &request_header,
std::shared_ptr<void> &response) = 0;
virtual void handle_response(
std::shared_ptr<void> & request_header, std::shared_ptr<void> & response) = 0;
private:
RCLCPP_DISABLE_COPY(ClientBase);
@ -85,13 +87,12 @@ public:
typedef std::shared_ptr<Promise> SharedPromise;
typedef std::shared_future<typename ServiceT::Response::Ptr> SharedFuture;
typedef std::function<void(SharedFuture)> CallbackType;
typedef std::function<void (SharedFuture)> CallbackType;
RCLCPP_MAKE_SHARED_DEFINITIONS(Client);
Client(rmw_client_t * client_handle,
const std::string& service_name)
: ClientBase(client_handle, service_name)
Client(rmw_client_t * client_handle, const std::string & service_name)
: ClientBase(client_handle, service_name)
{}
std::shared_ptr<void> create_response()
@ -106,7 +107,7 @@ public:
return std::shared_ptr<void>(new rmw_request_id_t);
}
void handle_response(std::shared_ptr<void> &request_header, std::shared_ptr<void> &response)
void handle_response(std::shared_ptr<void> & request_header, std::shared_ptr<void> & response)
{
auto typed_request_header = std::static_pointer_cast<rmw_request_id_t>(request_header);
auto typed_response = std::static_pointer_cast<typename ServiceT::Response>(response);
@ -121,13 +122,13 @@ public:
}
SharedFuture async_send_request(
typename ServiceT::Request::Ptr &request)
typename ServiceT::Request::Ptr & request)
{
return async_send_request(request, [] (SharedFuture f) { });
return async_send_request(request, [](SharedFuture f) {});
}
SharedFuture async_send_request(
typename ServiceT::Request::Ptr &request,
typename ServiceT::Request::Ptr & request,
CallbackType cb)
{
int64_t sequence_number;

6
rclcpp/include/rclcpp/contexts/default_context.hpp
View file

@ -33,8 +33,8 @@ public:
};
}
}
}
} // namespace default_context
} // namespace contexts
} // namespace rclcpp
#endif /* RCLCPP_RCLCPP_CONTEXTS_DEFAULT_CONTEXT_HPP_ */

461
rclcpp/include/rclcpp/executor.hpp
View file

@ -42,8 +42,7 @@ public:
virtual ~Executor()
{
if (interrupt_guard_condition_ != nullptr)
{
if (interrupt_guard_condition_ != nullptr) {
// TODO(wjwwood): Check ret code.
rmw_destroy_guard_condition(interrupt_guard_condition_);
}
@ -52,14 +51,12 @@ public:
virtual void spin() = 0;
virtual void
add_node(rclcpp::node::Node::SharedPtr &node_ptr)
add_node(rclcpp::node::Node::SharedPtr & node_ptr)
{
// Check to ensure node not already added
for (auto &weak_node : weak_nodes_)
{
for (auto & weak_node : weak_nodes_) {
auto node = weak_node.lock();
if (node == node_ptr)
{
if (node == node_ptr) {
// TODO: Use a different error here?
throw std::runtime_error(
"Cannot add node to executor, node already added.");
@ -71,31 +68,30 @@ public:
}
virtual void
remove_node(rclcpp::node::Node::SharedPtr &node_ptr)
remove_node(rclcpp::node::Node::SharedPtr & node_ptr)
{
bool node_removed = false;
weak_nodes_.erase(
std::remove_if(weak_nodes_.begin(), weak_nodes_.end(),
[&](std::weak_ptr<rclcpp::node::Node> &i)
std::remove_if(
weak_nodes_.begin(), weak_nodes_.end(),
[&](std::weak_ptr<rclcpp::node::Node> & i)
{
bool matched = (i.lock() == node_ptr);
node_removed |= matched;
return matched;
}));
// If the node was matched and removed, interrupt waiting
if (node_removed)
{
if (node_removed) {
rmw_trigger_guard_condition(interrupt_guard_condition_);
}
}
void spin_node_some(rclcpp::node::Node::SharedPtr &node)
void spin_node_some(rclcpp::node::Node::SharedPtr & node)
{
this->add_node(node);
// non-blocking = true
std::shared_ptr<AnyExecutable> any_exec;
while ((any_exec = get_next_executable(true)))
{
while ((any_exec = get_next_executable(true))) {
execute_any_executable(any_exec);
}
this->remove_node(node);
@ -116,26 +112,21 @@ protected:
};
void
execute_any_executable(std::shared_ptr<AnyExecutable> &any_exec)
execute_any_executable(std::shared_ptr<AnyExecutable> & any_exec)
{
if (!any_exec)
{
if (!any_exec) {
return;
}
if (any_exec->timer)
{
if (any_exec->timer) {
execute_timer(any_exec->timer);
}
if (any_exec->subscription)
{
if (any_exec->subscription) {
execute_subscription(any_exec->subscription);
}
if (any_exec->service)
{
if (any_exec->service) {
execute_service(any_exec->service);
}
if (any_exec->client)
{
if (any_exec->client) {
execute_client(any_exec->client);
}
// Reset the callback_group, regardless of type
@ -147,17 +138,14 @@ protected:
static void
execute_subscription(
rclcpp::subscription::SubscriptionBase::SharedPtr &subscription)
rclcpp::subscription::SubscriptionBase::SharedPtr & subscription)
{
std::shared_ptr<void> message = subscription->create_message();
bool taken = false;
rmw_take(subscription->subscription_handle_, message.get(), &taken);
if (taken)
{
if (taken) {
subscription->handle_message(message);
}
else
{
} else {
std::cout << "[rclcpp::error] take failed for subscription on topic: "
<< subscription->get_topic_name()
<< std::endl;
@ -166,28 +154,26 @@ protected:
static void
execute_timer(
rclcpp::timer::TimerBase::SharedPtr &timer)
rclcpp::timer::TimerBase::SharedPtr & timer)
{
timer->callback_();
}
static void
execute_service(
rclcpp::service::ServiceBase::SharedPtr &service)
rclcpp::service::ServiceBase::SharedPtr & service)
{
std::shared_ptr<void> request_header = service->create_request_header();
std::shared_ptr<void> request = service->create_request();
bool taken = false;
rmw_take_request(service->service_handle_,
request_header.get(),
request.get(),
&taken);
if (taken)
{
rmw_take_request(
service->service_handle_,
request_header.get(),
request.get(),
&taken);
if (taken) {
service->handle_request(request_header, request);
}
else
{
} else {
std::cout << "[rclcpp::error] take failed for service on service: "
<< service->get_service_name()
<< std::endl;
@ -196,21 +182,19 @@ protected:
static void
execute_client(
rclcpp::client::ClientBase::SharedPtr &client)
rclcpp::client::ClientBase::SharedPtr & client)
{
std::shared_ptr<void> request_header = client->create_request_header();
std::shared_ptr<void> response = client->create_response();
bool taken = false;
rmw_take_response(client->client_handle_,
request_header.get(),
response.get(),
&taken);
if (taken)
{
rmw_take_response(
client->client_handle_,
request_header.get(),
response.get(),
&taken);
if (taken) {
client->handle_response(request_header, response);
}
else
{
} else {
std::cout << "[rclcpp::error] take failed for service on client" << std::endl;
}
}
@ -226,47 +210,39 @@ protected:
std::vector<rclcpp::timer::TimerBase::SharedPtr> timers;
std::vector<rclcpp::service::ServiceBase::SharedPtr> services;
std::vector<rclcpp::client::ClientBase::SharedPtr> clients;
for (auto &weak_node : weak_nodes_)
{
for (auto & weak_node : weak_nodes_) {
auto node = weak_node.lock();
if (!node)
{
if (!node) {
has_invalid_weak_nodes = false;
continue;
}
for (auto &weak_group : node->callback_groups_)
{
for (auto & weak_group : node->callback_groups_) {
auto group = weak_group.lock();
if (!group || group->can_be_taken_from_.load() == false)
{
if (!group || !group->can_be_taken_from_.load()) {
continue;
}
for (auto &subscription : group->subscription_ptrs_)
{
for (auto & subscription : group->subscription_ptrs_) {
subs.push_back(subscription);
}
for (auto &timer : group->timer_ptrs_)
{
for (auto & timer : group->timer_ptrs_) {
timers.push_back(timer);
}
for (auto &service : group->service_ptrs_)
{
for (auto & service : group->service_ptrs_) {
services.push_back(service);
}
for (auto &client : group->client_ptrs_)
{
for (auto & client : group->client_ptrs_) {
clients.push_back(client);
}
}
}
// 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();
}));
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 allocate memory in the handles
unsigned long number_of_subscriptions = subs.size();
@ -275,15 +251,13 @@ protected:
// TODO(wjwwood): Avoid redundant malloc's
subscriber_handles.subscribers = static_cast<void **>(
std::malloc(sizeof(void *) * number_of_subscriptions));
if (subscriber_handles.subscribers == NULL)
{
if (subscriber_handles.subscribers == NULL) {
// TODO(wjwwood): Use a different error here? maybe std::bad_alloc?
throw std::runtime_error("Could not malloc for subscriber pointers.");
}
// Then fill the SubscriberHandles with ready subscriptions
size_t subscriber_handle_index = 0;
for (auto &subscription : subs)
{
for (auto & subscription : subs) {
subscriber_handles.subscribers[subscriber_handle_index] = \
subscription->subscription_handle_->data;
subscriber_handle_index += 1;
@ -296,15 +270,13 @@ protected:
// TODO(esteve): Avoid redundant malloc's
service_handles.services = static_cast<void **>(
std::malloc(sizeof(void *) * number_of_services));
if (service_handles.services == NULL)
{
if (service_handles.services == NULL) {
// TODO(esteve): Use a different error here? maybe std::bad_alloc?
throw std::runtime_error("Could not malloc for service pointers.");
}
// Then fill the ServiceHandles with ready services
size_t service_handle_index = 0;
for (auto &service : services)
{
for (auto & service : services) {
service_handles.services[service_handle_index] = \
service->service_handle_->data;
service_handle_index += 1;
@ -317,15 +289,13 @@ protected:
// TODO: Avoid redundant malloc's
client_handles.clients = static_cast<void **>(
std::malloc(sizeof(void *) * number_of_clients));
if (client_handles.clients == NULL)
{
if (client_handles.clients == NULL) {
// TODO: Use a different error here? maybe std::bad_alloc?
throw std::runtime_error("Could not malloc for client pointers.");
}
// Then fill the ServiceHandles with ready clients
size_t client_handle_index = 0;
for (auto &client : clients)
{
for (auto & client : clients) {
client_handles.clients[client_handle_index] = \
client->client_handle_->data;
client_handle_index += 1;
@ -341,8 +311,7 @@ protected:
// TODO(wjwwood): Avoid redundant malloc's
guard_condition_handles.guard_conditions = static_cast<void **>(
std::malloc(sizeof(void *) * number_of_guard_conds));
if (guard_condition_handles.guard_conditions == NULL)
{
if (guard_condition_handles.guard_conditions == NULL) {
// TODO(wjwwood): Use a different error here? maybe std::bad_alloc?
throw std::runtime_error(
"Could not malloc for guard condition pointers.");
@ -350,28 +319,27 @@ protected:
// Put the global ctrl-c guard condition in
assert(guard_condition_handles.guard_condition_count > 1);
guard_condition_handles.guard_conditions[0] = \
rclcpp::utilities::get_global_sigint_guard_condition()->data;
rclcpp::utilities::get_global_sigint_guard_condition()->data;
// Put the executor's guard condition in
guard_condition_handles.guard_conditions[1] = \
interrupt_guard_condition_->data;
interrupt_guard_condition_->data;
// Then fill the SubscriberHandles with ready subscriptions
size_t guard_cond_handle_index = start_of_timer_guard_conds;
for (auto &timer : timers)
{
for (auto & timer : timers) {
guard_condition_handles.guard_conditions[guard_cond_handle_index] = \
timer->guard_condition_->data;
guard_cond_handle_index += 1;
}
// Now wait on the waitable subscriptions and timers
rmw_wait(&subscriber_handles,
&guard_condition_handles,
&service_handles,
&client_handles,
nonblocking);
rmw_wait(
&subscriber_handles,
&guard_condition_handles,
&service_handles,
&client_handles,
nonblocking);
// If ctrl-c guard condition, return directly
if (guard_condition_handles.guard_conditions[0] != 0)
{
if (guard_condition_handles.guard_conditions[0] != 0) {
// Make sure to free memory
// TODO(wjwwood): Remove theses when the "Avoid redundant malloc's"
// todo has been addressed.
@ -382,38 +350,30 @@ protected:
}
// Add the new work to the class's list of things waiting to be executed
// Starting with the subscribers
for (size_t i = 0; i < number_of_subscriptions; ++i)
{
void *handle = subscriber_handles.subscribers[i];
if (handle)
{
for (size_t i = 0; i < number_of_subscriptions; ++i) {
void * handle = subscriber_handles.subscribers[i];
if (handle) {
subscriber_handles_.push_back(handle);
}
}
// Then the timers, start with start_of_timer_guard_conds
for (size_t i = start_of_timer_guard_conds; i < number_of_guard_conds; ++i)
{
void *handle = guard_condition_handles.guard_conditions[i];
if (handle)
{
for (size_t i = start_of_timer_guard_conds; i < number_of_guard_conds; ++i) {
void * handle = guard_condition_handles.guard_conditions[i];
if (handle) {
guard_condition_handles_.push_back(handle);
}
}
// Then the services
for (size_t i = 0; i < number_of_services; ++i)
{
void *handle = service_handles.services[i];
if (handle)
{
for (size_t i = 0; i < number_of_services; ++i) {
void * handle = service_handles.services[i];
if (handle) {
service_handles_.push_back(handle);
}
}
// Then the clients
for (size_t i = 0; i < number_of_clients; ++i)
{
void *handle = client_handles.clients[i];
if (handle)
{
for (size_t i = 0; i < number_of_clients; ++i) {
void * handle = client_handles.clients[i];
if (handle) {
client_handles_.push_back(handle);
}
}
@ -431,24 +391,18 @@ protected:
rclcpp::subscription::SubscriptionBase::SharedPtr
get_subscription_by_handle(void * subscriber_handle)
{
for (auto weak_node : weak_nodes_)
{
for (auto weak_node : weak_nodes_) {
auto node = weak_node.lock();
if (!node)
{
if (!node) {
continue;
}
for (auto weak_group : node->callback_groups_)
{
for (auto weak_group : node->callback_groups_) {
auto group = weak_group.lock();
if (!group)
{
if (!group) {
continue;
}
for (auto subscription : group->subscription_ptrs_)
{
if (subscription->subscription_handle_->data == subscriber_handle)
{
for (auto subscription : group->subscription_ptrs_) {
if (subscription->subscription_handle_->data == subscriber_handle) {
return subscription;
}
}
@ -460,24 +414,18 @@ protected:
rclcpp::timer::TimerBase::SharedPtr
get_timer_by_handle(void * guard_condition_handle)
{
for (auto weak_node : weak_nodes_)
{
for (auto weak_node : weak_nodes_) {
auto node = weak_node.lock();
if (!node)
{
if (!node) {
continue;
}
for (auto weak_group : node->callback_groups_)
{
for (auto weak_group : node->callback_groups_) {
auto group = weak_group.lock();
if (!group)
{
if (!group) {
continue;
}
for (auto timer : group->timer_ptrs_)
{
if (timer->guard_condition_->data == guard_condition_handle)
{
for (auto timer : group->timer_ptrs_) {
if (timer->guard_condition_->data == guard_condition_handle) {
return timer;
}
}
@ -489,24 +437,18 @@ protected:
rclcpp::service::ServiceBase::SharedPtr
get_service_by_handle(void * service_handle)
{
for (auto weak_node : weak_nodes_)
{
for (auto weak_node : weak_nodes_) {
auto node = weak_node.lock();
if (!node)
{
if (!node) {
continue;
}
for (auto weak_group : node->callback_groups_)
{
for (auto weak_group : node->callback_groups_) {
auto group = weak_group.lock();
if (!group)
{
if (!group) {
continue;
}
for (auto service : group->service_ptrs_)
{
if (service->service_handle_->data == service_handle)
{
for (auto service : group->service_ptrs_) {
if (service->service_handle_->data == service_handle) {
return service;
}
}
@ -518,24 +460,18 @@ protected:
rclcpp::client::ClientBase::SharedPtr
get_client_by_handle(void * client_handle)
{
for (auto weak_node : weak_nodes_)
{
for (auto weak_node : weak_nodes_) {
auto node = weak_node.lock();
if (!node)
{
if (!node) {
continue;
}
for (auto weak_group : node->callback_groups_)
{
for (auto weak_group : node->callback_groups_) {
auto group = weak_group.lock();
if (!group)
{
if (!group) {
continue;
}
for (auto client : group->client_ptrs_)
{
if (client->client_handle_->data == client_handle)
{
for (auto client : group->client_ptrs_) {
if (client->client_handle_->data == client_handle) {
return client;
}
}
@ -546,52 +482,46 @@ protected:
void
remove_subscriber_handle_from_subscriber_handles(void *handle)
remove_subscriber_handle_from_subscriber_handles(void * handle)
{
subscriber_handles_.remove(handle);
}
void
remove_guard_condition_handle_from_guard_condition_handles(void *handle)
remove_guard_condition_handle_from_guard_condition_handles(void * handle)
{
guard_condition_handles_.remove(handle);
}
void
remove_service_handle_from_service_handles(void *handle)
remove_service_handle_from_service_handles(void * handle)
{
service_handles_.remove(handle);
}
void
remove_client_handle_from_client_handles(void *handle)
remove_client_handle_from_client_handles(void * handle)
{
client_handles_.remove(handle);
}
rclcpp::node::Node::SharedPtr
get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr &group)
get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr & group)
{
if (!group)
{
if (!group) {
return rclcpp::node::Node::SharedPtr();
}
for (auto &weak_node : weak_nodes_)
{
for (auto & weak_node : weak_nodes_) {
auto node = weak_node.lock();
if (!node)
{
if (!node) {
continue;
}
for (auto &weak_group : node->callback_groups_)
{
for (auto & weak_group : node->callback_groups_) {
auto callback_group = weak_group.lock();
if (!callback_group)
{
if (!callback_group) {
continue;
}
if (callback_group == group)
{
if (callback_group == group) {
return node;
}
}
@ -601,22 +531,17 @@ protected:
rclcpp::callback_group::CallbackGroup::SharedPtr
get_group_by_timer(
rclcpp::timer::TimerBase::SharedPtr &timer)
rclcpp::timer::TimerBase::SharedPtr & timer)
{
for (auto &weak_node : weak_nodes_)
{
for (auto & weak_node : weak_nodes_) {
auto node = weak_node.lock();
if (!node)
{
if (!node) {
continue;
}
for (auto &weak_group : node->callback_groups_)
{
for (auto & weak_group : node->callback_groups_) {
auto group = weak_group.lock();
for (auto &t : group->timer_ptrs_)
{
if (t == timer)
{
for (auto & t : group->timer_ptrs_) {
if (t == timer) {
return group;
}
}
@ -626,24 +551,20 @@ protected:
}
void
get_next_timer(std::shared_ptr<AnyExecutable> &any_exec)
get_next_timer(std::shared_ptr<AnyExecutable> & any_exec)
{
for (auto handle : guard_condition_handles_)
{
for (auto handle : guard_condition_handles_) {
auto timer = get_timer_by_handle(handle);
if (timer)
{
if (timer) {
// Find the group for this handle and see if it can be serviced
auto group = get_group_by_timer(timer);
if (!group)
{
if (!group) {
// Group was not found, meaning the timer is not valid...
// Remove it from the ready list and continue looking
remove_guard_condition_handle_from_guard_condition_handles(handle);
continue;
}
if (!group->can_be_taken_from_.load())
{
if (!group->can_be_taken_from_.load()) {
// Group is mutually exclusive and is being used, so skip it for now
// Leave it to be checked next time, but continue searching
continue;
@ -662,22 +583,17 @@ protected:
rclcpp::callback_group::CallbackGroup::SharedPtr
get_group_by_subscription(
rclcpp::subscription::SubscriptionBase::SharedPtr &subscription)
rclcpp::subscription::SubscriptionBase::SharedPtr & subscription)
{
for (auto &weak_node : weak_nodes_)
{
for (auto & weak_node : weak_nodes_) {
auto node = weak_node.lock();
if (!node)
{
if (!node) {
continue;
}
for (auto &weak_group : node->callback_groups_)
{
for (auto & weak_group : node->callback_groups_) {
auto group = weak_group.lock();
for (auto &sub : group->subscription_ptrs_)
{
if (sub == subscription)
{
for (auto & sub : group->subscription_ptrs_) {
if (sub == subscription) {
return group;
}
}
@ -687,24 +603,20 @@ protected:
}
void
get_next_subscription(std::shared_ptr<AnyExecutable> &any_exec)
get_next_subscription(std::shared_ptr<AnyExecutable> & any_exec)
{
for (auto handle : subscriber_handles_)
{
for (auto handle : subscriber_handles_) {
auto subscription = get_subscription_by_handle(handle);
if (subscription)
{
if (subscription) {
// Find the group for this handle and see if it can be serviced
auto group = get_group_by_subscription(subscription);
if (!group)
{
if (!group) {
// Group was not found, meaning the subscription is not valid...
// Remove it from the ready list and continue looking
remove_subscriber_handle_from_subscriber_handles(handle);
continue;
}
if (!group->can_be_taken_from_.load())
{
if (!group->can_be_taken_from_.load()) {
// Group is mutually exclusive and is being used, so skip it for now
// Leave it to be checked next time, but continue searching
continue;
@ -723,22 +635,17 @@ protected:
rclcpp::callback_group::CallbackGroup::SharedPtr
get_group_by_service(
rclcpp::service::ServiceBase::SharedPtr &service)
rclcpp::service::ServiceBase::SharedPtr & service)
{
for (auto &weak_node : weak_nodes_)
{
for (auto & weak_node : weak_nodes_) {
auto node = weak_node.lock();
if (!node)
{
if (!node) {
continue;
}
for (auto &weak_group : node->callback_groups_)
{
for (auto & weak_group : node->callback_groups_) {
auto group = weak_group.lock();
for (auto &serv : group->service_ptrs_)
{
if (serv == service)
{
for (auto & serv : group->service_ptrs_) {
if (serv == service) {
return group;
}
}
@ -748,24 +655,20 @@ protected:
}
void
get_next_service(std::shared_ptr<AnyExecutable> &any_exec)
get_next_service(std::shared_ptr<AnyExecutable> & any_exec)
{
for (auto handle : service_handles_)
{
for (auto handle : service_handles_) {
auto service = get_service_by_handle(handle);
if (service)
{
if (service) {
// Find the group for this handle and see if it can be serviced
auto group = get_group_by_service(service);
if (!group)
{
if (!group) {
// Group was not found, meaning the service is not valid...
// Remove it from the ready list and continue looking
remove_service_handle_from_service_handles(handle);
continue;
}
if (!group->can_be_taken_from_.load())
{
if (!group->can_be_taken_from_.load()) {
// Group is mutually exclusive and is being used, so skip it for now
// Leave it to be checked next time, but continue searching
continue;
@ -784,22 +687,17 @@ protected:
rclcpp::callback_group::CallbackGroup::SharedPtr
get_group_by_client(
rclcpp::client::ClientBase::SharedPtr &client)
rclcpp::client::ClientBase::SharedPtr & client)
{
for (auto &weak_node : weak_nodes_)
{
for (auto & weak_node : weak_nodes_) {
auto node = weak_node.lock();
if (!node)
{
if (!node) {
continue;
}
for (auto &weak_group : node->callback_groups_)
{
for (auto & weak_group : node->callback_groups_) {
auto group = weak_group.lock();
for (auto &cli : group->client_ptrs_)
{
if (cli == client)
{
for (auto & cli : group->client_ptrs_) {
if (cli == client) {
return group;
}
}
@ -809,24 +707,20 @@ protected:
}
void
get_next_client(std::shared_ptr<AnyExecutable> &any_exec)
get_next_client(std::shared_ptr<AnyExecutable> & any_exec)
{
for (auto handle : client_handles_)
{
for (auto handle : client_handles_) {
auto client = get_client_by_handle(handle);
if (client)
{
if (client) {
// Find the group for this handle and see if it can be serviced
auto group = get_group_by_client(client);
if (!group)
{
if (!group) {
// Group was not found, meaning the service is not valid...
// Remove it from the ready list and continue looking
remove_client_handle_from_client_handles(handle);
continue;
}
if (!group->can_be_taken_from_.load())
{
if (!group->can_be_taken_from_.load()) {
// Group is mutually exclusive and is being used, so skip it for now
// Leave it to be checked next time, but continue searching
continue;
@ -849,26 +743,22 @@ protected:
std::shared_ptr<AnyExecutable> any_exec(new AnyExecutable());
// Check the timers to see if there are any that are ready, if so return
get_next_timer(any_exec);
if (any_exec->timer)
{
if (any_exec->timer) {
return any_exec;
}
// Check the subscriptions to see if there are any that are ready
get_next_subscription(any_exec);
if (any_exec->subscription)
{
if (any_exec->subscription) {
return any_exec;
}
// Check the services to see if there are any that are ready
get_next_service(any_exec);
if (any_exec->service)
{
if (any_exec->service) {
return any_exec;
}
// Check the clients to see if there are any that are ready
get_next_client(any_exec);
if (any_exec->client)
{
if (any_exec->client) {
return any_exec;
}
// If there is neither a ready timer nor subscription, return a null ptr
@ -877,14 +767,13 @@ protected:
}
std::shared_ptr<AnyExecutable>
get_next_executable(bool nonblocking=false)
get_next_executable(bool nonblocking = false)
{
// Check to see if there are any subscriptions or timers needing service
// TODO(wjwwood): improve run to run efficiency of this function
auto any_exec = get_next_ready_executable();
// If there are none
if (!any_exec)
{
if (!any_exec) {
// Wait for subscriptions or timers to work on
wait_for_work(nonblocking);
// Try again
@ -892,15 +781,13 @@ protected:
}
// At this point any_exec should be valid with either a valid subscription
// or a valid timer, or it should be a null shared_ptr
if (any_exec)
{
if (any_exec) {
// If it is valid, check to see if the group is mutually exclusive or
// not, then mark it accordingly
if (any_exec->callback_group->type_ == \
callback_group::CallbackGroupType::MutuallyExclusive)
{
callback_group::CallbackGroupType::MutuallyExclusive) {
// It should not have been taken otherwise
assert(any_exec->callback_group->can_be_taken_from_.load() == true);
assert(any_exec->callback_group->can_be_taken_from_.load());
// Set to false to indicate something is being run from this group
any_exec->callback_group->can_be_taken_from_.store(false);
}
@ -913,14 +800,14 @@ protected:
private:
RCLCPP_DISABLE_COPY(Executor);
std::vector<std::weak_ptr<rclcpp::node::Node>> weak_nodes_;
typedef std::list<void*> SubscriberHandles;
std::vector<std::weak_ptr<rclcpp::node::Node> > weak_nodes_;
typedef std::list<void *> SubscriberHandles;
SubscriberHandles subscriber_handles_;
typedef std::list<void*> GuardConditionHandles;
typedef std::list<void *> GuardConditionHandles;
GuardConditionHandles guard_condition_handles_;
typedef std::list<void*> ServiceHandles;
typedef std::list<void *> ServiceHandles;
ServiceHandles service_handles_;
typedef std::list<void*> ClientHandles;
typedef std::list<void *> ClientHandles;
ClientHandles client_handles_;
};

4
rclcpp/include/rclcpp/executors.hpp
View file

@ -26,7 +26,7 @@ namespace executors
using rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor;
using rclcpp::executors::single_threaded_executor::SingleThreadedExecutor;
}
}
} // namespace executors
} // namespace rclcpp
#endif /* RCLCPP_RCLCPP_EXECUTORS_HPP_ */

15
rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp
View file

@ -43,8 +43,7 @@ public:
MultiThreadedExecutor()
{
number_of_threads_ = std::thread::hardware_concurrency();
if (number_of_threads_ == 0)
{
if (number_of_threads_ == 0) {
number_of_threads_ = 1;
}
}
@ -58,15 +57,13 @@ public:
{
std::lock_guard<std::mutex> wait_lock(wait_mutex_);
size_t thread_id_ = 1; // Use a _ suffix to avoid shadowing `rclcpp::thread_id`
for (size_t i = number_of_threads_; i > 0; --i)
{
for (size_t i = number_of_threads_; i > 0; --i) {
std::this_thread::sleep_for(std::chrono::milliseconds(100));
auto func = std::bind(&MultiThreadedExecutor::run, this, thread_id_++);
threads.emplace_back(func);
}
}
for (auto &thread : threads)
{
for (auto & thread : threads) {
thread.join();
}
}
@ -81,13 +78,11 @@ private:
void run(size_t this_thread_id)
{
rclcpp::thread_id = this_thread_id;
while (rclcpp::utilities::ok())
{
while (rclcpp::utilities::ok()) {
std::shared_ptr<AnyExecutable> any_exec;
{
std::lock_guard<std::mutex> wait_lock(wait_mutex_);
if (!rclcpp::utilities::ok())
{
if (!rclcpp::utilities::ok()) {
return;
}
any_exec = get_next_executable();

3
rclcpp/include/rclcpp/executors/single_threaded_executor.hpp
View file

@ -46,8 +46,7 @@ public:
void spin()
{
while (rclcpp::utilities::ok())
{
while (rclcpp::utilities::ok()) {
auto any_exec = get_next_executable();
execute_any_executable(any_exec);
}

10
rclcpp/include/rclcpp/macros.hpp
View file

@ -20,8 +20,8 @@
* Use in the private section of the class.
*/
#define RCLCPP_DISABLE_COPY(Class) \
Class(const Class&) = delete; \
Class& operator=(const Class&) = delete;
Class(const Class &) = delete; \
Class & operator=(const Class &) = delete;
/* Defines a make_shared static function on the class using perfect forwarding.
*
@ -31,11 +31,11 @@
#define RCLCPP_MAKE_SHARED_DEFINITIONS(Class) \
typedef std::shared_ptr<Class> SharedPtr; \
\
template<typename...Args> \
template<typename ... Args> \
static std::shared_ptr<Class> \
make_shared(Args &&...args) \
make_shared(Args && ... args) \
{ \
return std::make_shared<Class>(std::forward<Args>(args)...); \
return std::make_shared<Class>(std::forward<Args>(args) ...); \
}
#define RCLCPP_INFO(Args) std::cout << Args << std::endl;

44
rclcpp/include/rclcpp/node.hpp
View file

@ -29,13 +29,19 @@
#include <rclcpp/timer.hpp>
// Forward declaration of ROS middleware class
namespace rmw {struct rmw_node_t;}
namespace rmw
{
struct rmw_node_t;
} // namespace rmw
namespace rclcpp
{
// Forward declaration for friend statement
namespace executor {class Executor;}
namespace executor
{
class Executor;
} // namespace executor
namespace node
{
@ -47,6 +53,7 @@ namespace node
class Node
{
friend class rclcpp::executor::Executor;
public:
RCLCPP_MAKE_SHARED_DEFINITIONS(Node);
@ -64,7 +71,8 @@ public:
create_callback_group(rclcpp::callback_group::CallbackGroupType group_type);
/* Create and return a Publisher. */
template<typename MessageT> rclcpp::publisher::Publisher::SharedPtr
template<typename MessageT>
rclcpp::publisher::Publisher::SharedPtr
create_publisher(std::string topic_name, size_t queue_size);
/* Create and return a Subscription. */
@ -74,46 +82,46 @@ public:
std::string topic_name,
size_t queue_size,
std::function<void(const std::shared_ptr<MessageT> &)> callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group=nullptr);
rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
/* Create a timer. */
rclcpp::timer::WallTimer::SharedPtr
create_wall_timer(
std::chrono::nanoseconds period,
rclcpp::timer::CallbackType callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group=nullptr);
rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
rclcpp::timer::WallTimer::SharedPtr
create_wall_timer(
std::chrono::duration<long double, std::nano> period,
rclcpp::timer::CallbackType callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group=nullptr);
rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
typedef rclcpp::callback_group::CallbackGroup CallbackGroup;
typedef std::weak_ptr<CallbackGroup> CallbackGroupWeakPtr;
typedef std::list<CallbackGroupWeakPtr> CallbackGroupWeakPtrList;
/* Create and return a Client. */
template <typename ServiceT>
template<typename ServiceT>
typename rclcpp::client::Client<ServiceT>::SharedPtr
create_client(
std::string service_name,
rclcpp::callback_group::CallbackGroup::SharedPtr group=nullptr);
rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
/* Create and return a Service. */
template <typename ServiceT>
template<typename ServiceT>
typename rclcpp::service::Service<ServiceT>::SharedPtr
create_service(
std::string service_name,
std::function<void(const std::shared_ptr<typename ServiceT::Request> &,
std::shared_ptr<typename ServiceT::Response>&)> callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group=nullptr);
std::shared_ptr<typename ServiceT::Response> &)> callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
private:
RCLCPP_DISABLE_COPY(Node);
bool
group_in_node(callback_group::CallbackGroup::SharedPtr &group);
group_in_node(callback_group::CallbackGroup::SharedPtr & group);
std::string name_;
@ -135,11 +143,13 @@ private:
} /* namespace rclcpp */
#define RCLCPP_REGISTER_NODE(Class) RMW_EXPORT \
rclcpp::node::Node::SharedPtr \
create_node() \
{ \
return rclcpp::node::Node::SharedPtr(new Class(rclcpp::contexts::default_context::DefaultContext::make_shared())); \
}
rclcpp::node::Node::SharedPtr \
create_node() \
{ \
return rclcpp::node::Node::SharedPtr(new Class( \
rclcpp::contexts::default_context::DefaultContext:: \
make_shared())); \
}
#ifndef RCLCPP_RCLCPP_NODE_IMPL_HPP_
// Template implementations

80
rclcpp/include/rclcpp/node_impl.hpp
View file

@ -39,7 +39,7 @@ Node::Node(std::string node_name)
Node::Node(std::string node_name, context::Context::SharedPtr context)
: name_(node_name), context_(context),
number_of_subscriptions_(0), number_of_timers_(0), number_of_services_(0)
number_of_subscriptions_(0), number_of_timers_(0), number_of_services_(0)
{
node_handle_ = rmw_create_node(name_.c_str());
using rclcpp::callback_group::CallbackGroupType;
@ -71,14 +71,12 @@ Node::create_publisher(std::string topic_name, size_t queue_size)
}
bool
Node::group_in_node(callback_group::CallbackGroup::SharedPtr &group)
Node::group_in_node(callback_group::CallbackGroup::SharedPtr & group)
{
bool group_belongs_to_this_node = false;
for (auto &weak_group : this->callback_groups_)
{
for (auto & weak_group : this->callback_groups_) {
auto cur_group = weak_group.lock();
if (cur_group && cur_group == group)
{
if (cur_group && (cur_group == group)) {
group_belongs_to_this_node = true;
}
}
@ -100,21 +98,18 @@ Node::create_subscription(
using namespace rclcpp::subscription;
auto sub = Subscription<MessageT>::make_shared(subscriber_handle,
topic_name,
callback);
auto sub = Subscription<MessageT>::make_shared(
subscriber_handle,
topic_name,
callback);
auto sub_base_ptr = std::dynamic_pointer_cast<SubscriptionBase>(sub);
if (group)
{
if (!group_in_node(group))
{
if (group) {
if (!group_in_node(group)) {
// TODO: use custom exception
throw std::runtime_error("Cannot create timer, group not in node.");
}
group->add_subscription(sub_base_ptr);
}
else
{
} else {
default_callback_group_->add_subscription(sub_base_ptr);
}
number_of_subscriptions_++;
@ -122,22 +117,19 @@ Node::create_subscription(
}
rclcpp::timer::WallTimer::SharedPtr
Node::create_wall_timer(std::chrono::nanoseconds period,
rclcpp::timer::CallbackType callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group)
Node::create_wall_timer(
std::chrono::nanoseconds period,
rclcpp::timer::CallbackType callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group)
{
auto timer = rclcpp::timer::WallTimer::make_shared(period, callback);
if (group)
{
if (!group_in_node(group))
{
if (group) {
if (!group_in_node(group)) {
// TODO: use custom exception
throw std::runtime_error("Cannot create timer, group not in node.");
}
group->add_timer(timer);
}
else
{
} else {
default_callback_group_->add_timer(timer);
}
number_of_timers_++;
@ -171,21 +163,18 @@ Node::create_client(
using namespace rclcpp::client;
auto cli = Client<ServiceT>::make_shared(client_handle,
service_name);
auto cli = Client<ServiceT>::make_shared(
client_handle,
service_name);
auto cli_base_ptr = std::dynamic_pointer_cast<ClientBase>(cli);
if (group)
{
if (!group_in_node(group))
{
if (group) {
if (!group_in_node(group)) {
// TODO(esteve): use custom exception
throw std::runtime_error("Cannot create client, group not in node.");
}
group->add_client(cli_base_ptr);
}
else
{
} else {
default_callback_group_->add_client(cli_base_ptr);
}
number_of_clients_++;
@ -193,12 +182,12 @@ Node::create_client(
return cli;
}
template <typename ServiceT>
template<typename ServiceT>
typename service::Service<ServiceT>::SharedPtr
Node::create_service(
std::string service_name,
std::function<void(const std::shared_ptr<typename ServiceT::Request> &,
std::shared_ptr<typename ServiceT::Response>&)> callback,
std::shared_ptr<typename ServiceT::Response> &)> callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group)
{
using rosidl_generator_cpp::get_service_type_support_handle;
@ -210,22 +199,19 @@ Node::create_service(
using namespace rclcpp::service;
auto serv = Service<ServiceT>::make_shared(service_handle,
service_name,
callback);
auto serv = Service<ServiceT>::make_shared(
service_handle,
service_name,
callback);
auto serv_base_ptr = std::dynamic_pointer_cast<ServiceBase>(serv);
if (group)
{
if (!group_in_node(group))
{
if (group) {
if (!group_in_node(group)) {
// TODO: use custom exception
throw std::runtime_error("Cannot create service, group not in node.");
}
group->add_service(serv_base_ptr);
}
else
{
} else {
default_callback_group_->add_service(serv_base_ptr);
}
number_of_services_++;

10
rclcpp/include/rclcpp/publisher.hpp
View file

@ -25,7 +25,10 @@ namespace rclcpp
{
// Forward declaration for friend statement
namespace node {class Node;}
namespace node
{
class Node;
} // namespace node
namespace publisher
{
@ -39,8 +42,9 @@ public:
: publisher_handle_(publisher_handle)
{}
template<typename MessageT> void
publish(std::shared_ptr<MessageT> &msg)
template<typename MessageT>
void
publish(std::shared_ptr<MessageT> & msg)
{
rmw_publish(publisher_handle_, msg.get());
}

15
rclcpp/include/rclcpp/rate.hpp
View file

@ -49,7 +49,7 @@ public:
GenericRate(double rate)
: GenericRate<Clock>(
duration_cast<nanoseconds>(duration<double>(1.0/rate)))
duration_cast<nanoseconds>(duration<double>(1.0 / rate)))
{}
GenericRate(std::chrono::nanoseconds period)
: period_(period), last_interval_(Clock::now())
@ -63,8 +63,7 @@ public:
// Time of next interval
auto next_interval = last_interval_ + period_;
// Detect backwards time flow
if (now < last_interval_)
{
if (now < last_interval_) {
// Best thing to do is to set the next_interval to now + period
next_interval = now + period_;
}
@ -73,13 +72,11 @@ public:
// 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 (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_)
{
if (now > next_interval + period_) {
last_interval_ = now + period_;
}
// Either way do not sleep and return false
@ -113,7 +110,7 @@ private:
typedef GenericRate<std::chrono::system_clock> Rate;
typedef GenericRate<std::chrono::steady_clock> WallRate;
}
}
} // namespace rate
} // namespace rclcpp
#endif /* RCLCPP_RCLCPP_RATE_HPP_ */

24
rclcpp/include/rclcpp/rclcpp.hpp
View file

@ -26,26 +26,26 @@
namespace rclcpp
{
const std::chrono::seconds operator "" _s(unsigned long long s)
const std::chrono::seconds operator"" _s(unsigned long long s)
{
return std::chrono::seconds(s);
return std::chrono::seconds(s);
}
const std::chrono::nanoseconds operator "" _s(long double s)
const std::chrono::nanoseconds operator"" _s(long double s)
{
return std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<long double>(s));
return std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<long double>(s));
}
const std::chrono::nanoseconds
operator "" _ns(unsigned long long ns)
operator"" _ns(unsigned long long ns)
{
return std::chrono::nanoseconds(ns);
return std::chrono::nanoseconds(ns);
}
const std::chrono::nanoseconds
operator "" _ns(long double ns)
operator"" _ns(long double ns)
{
return std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<long double, std::nano>(ns));
return std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<long double, std::nano>(ns));
}
using rclcpp::node::Node;
@ -63,13 +63,13 @@ using rclcpp::utilities::shutdown;
using rclcpp::utilities::init;
using rclcpp::utilities::sleep_for;
void spin_some(Node::SharedPtr &node_ptr)
void spin_some(Node::SharedPtr & node_ptr)
{
rclcpp::executors::SingleThreadedExecutor executor;
executor.spin_node_some(node_ptr);
}
void spin(Node::SharedPtr &node_ptr)
void spin(Node::SharedPtr & node_ptr)
{
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(node_ptr);

41
rclcpp/include/rclcpp/service.hpp
View file

@ -28,7 +28,10 @@ namespace rclcpp
{
// Forward declaration for friend statement
namespace executor {class Executor;}
namespace executor
{
class Executor;
} // namespace executor
namespace service
{
@ -36,18 +39,19 @@ namespace service
class ServiceBase
{
friend class rclcpp::executor::Executor;
public:
RCLCPP_MAKE_SHARED_DEFINITIONS(ServiceBase);
ServiceBase(rmw_service_t * service_handle,
const std::string service_name)
: service_handle_(service_handle), service_name_(service_name)
ServiceBase(
rmw_service_t * service_handle,
const std::string service_name)
: service_handle_(service_handle), service_name_(service_name)
{}
~ServiceBase()
{
if (service_handle_ != nullptr)
{
if (service_handle_ != nullptr) {
rmw_destroy_service(service_handle_);
service_handle_ = nullptr;
}
@ -65,8 +69,9 @@ public:
virtual std::shared_ptr<void> create_request() = 0;
virtual std::shared_ptr<void> create_request_header() = 0;
virtual void handle_request(std::shared_ptr<void> &request_header,
std::shared_ptr<void> &request) = 0;
virtual void handle_request(
std::shared_ptr<void> & request_header,
std::shared_ptr<void> & request) = 0;
private:
RCLCPP_DISABLE_COPY(ServiceBase);
@ -81,14 +86,15 @@ class Service : public ServiceBase
{
public:
typedef std::function<
void(const std::shared_ptr<typename ServiceT::Request> &,
std::shared_ptr<typename ServiceT::Response>&)> CallbackType;
void (const std::shared_ptr<typename ServiceT::Request> &,
std::shared_ptr<typename ServiceT::Response> &)> CallbackType;
RCLCPP_MAKE_SHARED_DEFINITIONS(Service);
Service(rmw_service_t * service_handle,
const std::string &service_name,
CallbackType callback)
: ServiceBase(service_handle, service_name), callback_(callback)
Service(
rmw_service_t * service_handle,
const std::string & service_name,
CallbackType callback)
: ServiceBase(service_handle, service_name), callback_(callback)
{}
std::shared_ptr<void> create_request()
@ -103,7 +109,7 @@ public:
return std::shared_ptr<void>(new rmw_request_id_t);
}
void handle_request(std::shared_ptr<void> &request_header, std::shared_ptr<void> &request)
void handle_request(std::shared_ptr<void> & request_header, std::shared_ptr<void> & request)
{
auto typed_request = std::static_pointer_cast<typename ServiceT::Request>(request);
auto typed_request_header = std::static_pointer_cast<rmw_request_id_t>(request_header);
@ -112,8 +118,9 @@ public:
send_response(typed_request_header, response);
}
void send_response(std::shared_ptr<rmw_request_id_t> &req_id,
std::shared_ptr<typename ServiceT::Response> &response)
void send_response(
std::shared_ptr<rmw_request_id_t> & req_id,
std::shared_ptr<typename ServiceT::Response> & response)
{
rmw_send_response(get_service_handle(), req_id.get(), response.get());
}

21
rclcpp/include/rclcpp/subscription.hpp
View file

@ -27,7 +27,10 @@ namespace rclcpp
{
// Forward declaration is for friend statement in SubscriptionBase
namespace executor {class Executor;}
namespace executor
{
class Executor;
} // namespace executor
namespace subscription
{
@ -35,12 +38,13 @@ namespace subscription
class SubscriptionBase
{
friend class rclcpp::executor::Executor;
public:
RCLCPP_MAKE_SHARED_DEFINITIONS(SubscriptionBase);
SubscriptionBase(
rmw_subscription_t * subscription_handle,
std::string &topic_name)
std::string & topic_name)
: subscription_handle_(subscription_handle), topic_name_(topic_name)
{}
@ -50,7 +54,7 @@ public:
}
virtual std::shared_ptr<void> create_message() = 0;
virtual void handle_message(std::shared_ptr<void> &message) = 0;
virtual void handle_message(std::shared_ptr<void> & message) = 0;
private:
RCLCPP_DISABLE_COPY(SubscriptionBase);
@ -64,12 +68,13 @@ template<typename MessageT>
class Subscription : public SubscriptionBase
{
public:
typedef std::function<void(const std::shared_ptr<MessageT> &)> CallbackType;
typedef std::function<void (const std::shared_ptr<MessageT> &)> CallbackType;
RCLCPP_MAKE_SHARED_DEFINITIONS(Subscription);
Subscription(rmw_subscription_t * subscription_handle,
std::string &topic_name,
CallbackType callback)
Subscription(
rmw_subscription_t * subscription_handle,
std::string & topic_name,
CallbackType callback)
: SubscriptionBase(subscription_handle, topic_name), callback_(callback)
{}
@ -78,7 +83,7 @@ public:
return std::shared_ptr<void>(new MessageT());
}
void handle_message(std::shared_ptr<void> &message)
void handle_message(std::shared_ptr<void> & message)
{
auto typed_message = std::static_pointer_cast<MessageT>(message);
callback_(typed_message);

19
rclcpp/include/rclcpp/timer.hpp
View file

@ -30,23 +30,27 @@ namespace rclcpp
{
// Forward declaration is for friend statement in GenericTimer
namespace executor {class Executor;}
namespace executor
{
class Executor;
} // namespace executor
namespace timer
{
typedef std::function<void()> CallbackType;
typedef std::function<void ()> CallbackType;
class TimerBase
{
friend class rclcpp::executor::Executor;
public:
RCLCPP_MAKE_SHARED_DEFINITIONS(TimerBase);
TimerBase(std::chrono::nanoseconds period, CallbackType callback)
: period_(period),
callback_(callback),
canceled_(false)
callback_(callback),
canceled_(false)
{
guard_condition_ = rmw_create_guard_condition();
}
@ -72,6 +76,7 @@ template<class Clock = std::chrono::high_resolution_clock>
class GenericTimer : public TimerBase
{
friend class rclcpp::executor::Executor;
public:
RCLCPP_MAKE_SHARED_DEFINITIONS(GenericTimer);
@ -89,11 +94,9 @@ public:
void
run()
{
while (rclcpp::utilities::ok() && !this->canceled_)
{
while (rclcpp::utilities::ok() && !this->canceled_) {
loop_rate_.sleep();
if (!rclcpp::utilities::ok())
{
if (!rclcpp::utilities::ok()) {
return;
}
rmw_trigger_guard_condition(guard_condition_);

74
rclcpp/include/rclcpp/utilities.hpp
View file

@ -36,55 +36,51 @@
namespace
{
volatile sig_atomic_t g_signal_status = 0;
rmw_guard_condition_t * g_sigint_guard_cond_handle = \
rmw_create_guard_condition();
std::condition_variable g_interrupt_condition_variable;
std::mutex g_interrupt_mutex;
volatile sig_atomic_t g_signal_status = 0;
rmw_guard_condition_t * g_sigint_guard_cond_handle = \
rmw_create_guard_condition();
std::condition_variable g_interrupt_condition_variable;
std::mutex g_interrupt_mutex;
#ifdef HAS_SIGACTION
struct sigaction old_action;
struct sigaction old_action;
#else
void (*old_signal_handler)(int) = 0;
void (* old_signal_handler)(int) = 0;
#endif
void
void
#ifdef HAS_SIGACTION
signal_handler(int signal_value, siginfo_t *siginfo, void *context)
signal_handler(int signal_value, siginfo_t * siginfo, void * context)
#else
signal_handler(int signal_value)
signal_handler(int signal_value)
#endif
{
// TODO(wjwwood): remove
std::cout << "signal_handler(" << signal_value << ")" << std::endl;
{
// TODO(wjwwood): remove
std::cout << "signal_handler(" << signal_value << ")" << std::endl;
#ifdef HAS_SIGACTION
if (old_action.sa_flags & SA_SIGINFO)
{
if (old_action.sa_sigaction != NULL)
{
old_action.sa_sigaction(signal_value, siginfo, context);
}
if (old_action.sa_flags & SA_SIGINFO) {
if (old_action.sa_sigaction != NULL) {
old_action.sa_sigaction(signal_value, siginfo, context);
}
else
{
if (old_action.sa_handler != NULL && // Is set
old_action.sa_handler != SIG_DFL && // Is not default
old_action.sa_handler != SIG_IGN) // Is not ignored
{
old_action.sa_handler(signal_value);
}
} else {
if (
old_action.sa_handler != NULL && // Is set
old_action.sa_handler != SIG_DFL && // Is not default
old_action.sa_handler != SIG_IGN // Is not ignored
) {
old_action.sa_handler(signal_value);
}
#else
if (old_signal_handler)
{
old_signal_handler(signal_value);
}
#endif
g_signal_status = signal_value;
rmw_trigger_guard_condition(g_sigint_guard_cond_handle);
g_interrupt_condition_variable.notify_all();
}
#else
if (old_signal_handler) {
old_signal_handler(signal_value);
}
#endif
g_signal_status = signal_value;
rmw_trigger_guard_condition(g_sigint_guard_cond_handle);
g_interrupt_condition_variable.notify_all();
}
} // namespace
namespace rclcpp
{
@ -95,7 +91,7 @@ namespace utilities
{
void
init(int argc, char *argv[])
init(int argc, char * argv[])
{
// TODO(wjwwood): Handle rmw_ret_t's value.
rmw_init();
@ -114,7 +110,7 @@ init(int argc, char *argv[])
{
throw std::runtime_error(
std::string("Failed to set SIGINT signal handler: (" +
std::to_string(errno) + ")") +
std::to_string(errno) + ")") +
// TODO(wjwwood): use strerror_r on POSIX and strerror_s on Windows.
std::strerror(errno));
}
@ -142,7 +138,7 @@ get_global_sigint_guard_condition()
}
bool
sleep_for(const std::chrono::nanoseconds& nanoseconds)
sleep_for(const std::chrono::nanoseconds & nanoseconds)
{
// TODO: determine if posix's nanosleep(2) is more efficient here
std::unique_lock<std::mutex> lock(::g_interrupt_mutex);

2
rclcpp/src/node_main.cpp
View file

@ -17,7 +17,7 @@
// This forward declaration is implemented by the RCLCPP_REGISTER_NODE macro
RMW_IMPORT rclcpp::Node::SharedPtr create_node();
int main(int argc, char **argv)
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
rclcpp::executors::SingleThreadedExecutor executor;