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

update code after splitting into headers and cpp files

Browse source
This commit is contained in:
William Woodall 2015-11-05 18:55:52 -08:00
parent f531b02928
commit 0bbe9b2099
61 changed files with 2979 additions and 5756 deletions
Download patch file Download diff file Expand all files Collapse all files

93
rclcpp/CMakeLists.txt
View file

@ -4,31 +4,95 @@ project(rclcpp)
find_package(ament_cmake REQUIRED)
find_package(rcl_interfaces REQUIRED)
find_package(rmw REQUIRED)
find_package(rosidl_generator_cpp REQUIRED)
if(NOT WIN32)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall -Wextra")
endif()
include_directories(include)
set(${PROJECT_NAME}_SRCS
src/rclcpp/any_executable.cpp
src/rclcpp/callback_group.cpp
src/rclcpp/client.cpp
src/rclcpp/context.cpp
src/rclcpp/contexts/default_context.cpp
src/rclcpp/executor.cpp
src/rclcpp/executors.cpp
src/rclcpp/executors/multi_threaded_executor.cpp
src/rclcpp/executors/single_threaded_executor.cpp
src/rclcpp/intra_process_manager.cpp
src/rclcpp/intra_process_manager_state.cpp
src/rclcpp/memory_strategies.cpp
src/rclcpp/memory_strategy.cpp
src/rclcpp/parameter.cpp
src/rclcpp/parameter_client.cpp
src/rclcpp/parameter_service.cpp
src/rclcpp/publisher.cpp
src/rclcpp/node.cpp
src/rclcpp/service.cpp
src/rclcpp/subscription.cpp
src/rclcpp/timer.cpp
src/rclcpp/utilities.cpp
)
if(WIN32)
list(APPEND ${PROJECT_NAME}_SRCS src/rclcpp/windows_helper.cpp)
endif()
add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_SRCS})
ament_target_dependencies(
${PROJECT_NAME}
"rcl_interfaces"
"rmw"
"rosidl_generator_cpp"
)
# Causes the visibility macros to use dllexport rather than dllimport,
# which is appropriate when building the dll but not consuming it.
# It also avoids using certain parts of the code that the consuming code must
# use, but which the library cannot consume itself.
target_compile_definitions(${PROJECT_NAME} PRIVATE "RCLCPP_BUILDING_LIBRARY")
if(APPLE)
# Since the rmw_* symbols are unresolved at the time of building librclcpp,
# tell the linker on OS X to dynamically look them up at runtime.
set_target_properties(${PROJECT_NAME} PROPERTIES LINK_FLAGS "-undefined dynamic_lookup")
endif()
ament_export_dependencies(rmw)
ament_export_dependencies(rcl_interfaces)
ament_export_dependencies(rmw)
ament_export_dependencies(rosidl_generator_cpp)
ament_export_include_directories(include)
ament_export_libraries(${PROJECT_NAME})
if(AMENT_ENABLE_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
find_package(rmw REQUIRED)
if(NOT WIN32)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall -Wextra")
endif()
include_directories(include)
ament_add_gtest(test_function_traits test/test_function_traits.cpp)
if(TARGET test_function_traits)
target_include_directories(test_function_traits PUBLIC
${rcl_interfaces_INCLUDE_DIRS}
${rmw_INCLUDE_DIRS}
${rosidl_generator_cpp_INCLUDE_DIRS}
)
endif()
ament_add_gtest(test_mapped_ring_buffer test/test_mapped_ring_buffer.cpp)
if(TARGET test_mapped_ring_buffer)
target_include_directories(test_mapped_ring_buffer PUBLIC
${rcl_interfaces_INCLUDE_DIRS}
${rmw_INCLUDE_DIRS}
${rosidl_generator_cpp_INCLUDE_DIRS}
)
endif()
ament_add_gtest(test_intra_process_manager test/test_intra_process_manager.cpp)
if(TARGET test_intra_process_manager)
target_include_directories(test_intra_process_manager PUBLIC
"${rcl_interfaces_INCLUDE_DIRS}"
"${rmw_INCLUDE_DIRS}")
${rcl_interfaces_INCLUDE_DIRS}
${rmw_INCLUDE_DIRS}
${rosidl_generator_cpp_INCLUDE_DIRS}
)
endif()
endif()
@ -45,3 +109,10 @@ install(
DIRECTORY src/
DESTINATION src/rclcpp
)
install(
TARGETS ${PROJECT_NAME}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)

8
rclcpp/include/rclcpp/any_executable.hpp
View file

@ -19,6 +19,7 @@
#include <rclcpp/macros.hpp>
#include <rclcpp/node.hpp>
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
@ -28,9 +29,10 @@ namespace executor
struct AnyExecutable
{
RCLCPP_SMART_PTR_DEFINITIONS(AnyExecutable);
AnyExecutable()
: subscription(0), timer(0), callback_group(0), node(0)
{}
RCLCPP_PUBLIC
AnyExecutable();
// Only one of the following pointers will be set.
rclcpp::subscription::SubscriptionBase::SharedPtr subscription;
rclcpp::subscription::SubscriptionBase::SharedPtr subscription_intra_process;

2
rclcpp/include/rclcpp/any_service_callback.hpp
View file

@ -22,6 +22,8 @@
#include <type_traits>
#include <rmw/types.h>
#include "rclcpp/function_traits.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{

1
rclcpp/include/rclcpp/any_subscription_callback.hpp
View file

@ -23,6 +23,7 @@
#include "rclcpp/allocator/allocator_common.hpp"
#include "rclcpp/function_traits.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{

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

@ -23,6 +23,7 @@
#include <rclcpp/timer.hpp>
#include <rclcpp/service.hpp>
#include <rclcpp/client.hpp>
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
@ -49,77 +50,57 @@ class CallbackGroup
public:
RCLCPP_SMART_PTR_DEFINITIONS(CallbackGroup);
CallbackGroup(CallbackGroupType group_type)
: type_(group_type), can_be_taken_from_(true)
{}
RCLCPP_PUBLIC
explicit CallbackGroup(CallbackGroupType group_type);
const std::vector<subscription::SubscriptionBase::WeakPtr> &
get_subscription_ptrs() const
{
return subscription_ptrs_;
}
RCLCPP_PUBLIC
const std::vector<rclcpp::subscription::SubscriptionBase::WeakPtr> &
get_subscription_ptrs() const;
const std::vector<timer::TimerBase::WeakPtr> &
get_timer_ptrs() const
{
return timer_ptrs_;
}
RCLCPP_PUBLIC
const std::vector<rclcpp::timer::TimerBase::WeakPtr> &
get_timer_ptrs() const;
const std::vector<service::ServiceBase::SharedPtr> &
get_service_ptrs() const
{
return service_ptrs_;
}
RCLCPP_PUBLIC
const std::vector<rclcpp::service::ServiceBase::SharedPtr> &
get_service_ptrs() const;
const std::vector<client::ClientBase::SharedPtr> &
get_client_ptrs() const
{
return client_ptrs_;
}
RCLCPP_PUBLIC
const std::vector<rclcpp::client::ClientBase::SharedPtr> &
get_client_ptrs() const;
std::atomic_bool & can_be_taken_from()
{
return can_be_taken_from_;
}
RCLCPP_PUBLIC
std::atomic_bool &
can_be_taken_from();
const CallbackGroupType & type() const
{
return type_;
}
RCLCPP_PUBLIC
const CallbackGroupType &
type() const;
private:
RCLCPP_DISABLE_COPY(CallbackGroup);
RCLCPP_PUBLIC
void
add_subscription(
const subscription::SubscriptionBase::SharedPtr subscription_ptr)
{
subscription_ptrs_.push_back(subscription_ptr);
}
add_subscription(const rclcpp::subscription::SubscriptionBase::SharedPtr subscription_ptr);
RCLCPP_PUBLIC
void
add_timer(const timer::TimerBase::SharedPtr timer_ptr)
{
timer_ptrs_.push_back(timer_ptr);
}
add_timer(const rclcpp::timer::TimerBase::SharedPtr timer_ptr);
RCLCPP_PUBLIC
void
add_service(const service::ServiceBase::SharedPtr service_ptr)
{
service_ptrs_.push_back(service_ptr);
}
add_service(const rclcpp::service::ServiceBase::SharedPtr service_ptr);
RCLCPP_PUBLIC
void
add_client(const client::ClientBase::SharedPtr client_ptr)
{
client_ptrs_.push_back(client_ptr);
}
add_client(const rclcpp::client::ClientBase::SharedPtr client_ptr);
CallbackGroupType type_;
std::vector<subscription::SubscriptionBase::WeakPtr> subscription_ptrs_;
std::vector<timer::TimerBase::WeakPtr> timer_ptrs_;
std::vector<service::ServiceBase::SharedPtr> service_ptrs_;
std::vector<client::ClientBase::SharedPtr> client_ptrs_;
std::vector<rclcpp::subscription::SubscriptionBase::WeakPtr> subscription_ptrs_;
std::vector<rclcpp::timer::TimerBase::WeakPtr> timer_ptrs_;
std::vector<rclcpp::service::ServiceBase::SharedPtr> service_ptrs_;
std::vector<rclcpp::client::ClientBase::SharedPtr> client_ptrs_;
std::atomic_bool can_be_taken_from_;
};

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

@ -27,6 +27,7 @@
#include <rclcpp/macros.hpp>
#include <rclcpp/utilities.hpp>
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
@ -40,32 +41,22 @@ class ClientBase
public:
RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientBase);
RCLCPP_PUBLIC
ClientBase(
std::shared_ptr<rmw_node_t> node_handle,
rmw_client_t * client_handle,
const std::string & service_name)
: node_handle_(node_handle), client_handle_(client_handle), service_name_(service_name)
{}
const std::string & service_name);
virtual ~ClientBase()
{
if (client_handle_) {
if (rmw_destroy_client(client_handle_) != RMW_RET_OK) {
fprintf(stderr,
"Error in destruction of rmw client handle: %s\n", rmw_get_error_string_safe());
}
}
}
RCLCPP_PUBLIC
virtual ~ClientBase();
const std::string & get_service_name() const
{
return this->service_name_;
}
RCLCPP_PUBLIC
const std::string &
get_service_name() const;
const rmw_client_t * get_client_handle() const
{
return this->client_handle_;
}
RCLCPP_PUBLIC
const rmw_client_t *
get_client_handle() const;
virtual std::shared_ptr<void> create_response() = 0;
virtual std::shared_ptr<void> create_request_header() = 0;

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

@ -26,6 +26,8 @@
#include <unordered_map>
#include <rmw/rmw.h>
#include "rclcpp/macros.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
@ -38,7 +40,7 @@ class Context
public:
RCLCPP_SMART_PTR_DEFINITIONS(Context);
Context() {}
Context();
template<typename SubContext, typename ... Args>
std::shared_ptr<SubContext>

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

@ -16,6 +16,7 @@
#define RCLCPP_RCLCPP_CONTEXTS_DEFAULT_CONTEXT_HPP_
#include <rclcpp/context.hpp>
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
@ -29,16 +30,13 @@ class DefaultContext : public rclcpp::context::Context
public:
RCLCPP_SMART_PTR_DEFINITIONS(DefaultContext);
DefaultContext() {}
RCLCPP_PUBLIC
DefaultContext();
};
RCLCPP_PUBLIC
DefaultContext::SharedPtr
get_global_default_context()
{
static DefaultContext::SharedPtr default_context = DefaultContext::make_shared();
return default_context;
}
get_global_default_context();
} // namespace default_context
} // namespace contexts

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

@ -23,14 +23,13 @@
#include <memory>
#include <vector>
#include "rcl_interfaces/msg/intra_process_message.hpp"
#include "rclcpp/any_executable.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/memory_strategies.hpp"
#include "rclcpp/memory_strategy.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/utilities.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
@ -54,29 +53,18 @@ public:
/// Default constructor.
// \param[in] ms The memory strategy to be used with this executor.
explicit Executor(memory_strategy::MemoryStrategy::SharedPtr ms =
memory_strategies::create_default_strategy())
: interrupt_guard_condition_(rmw_create_guard_condition()),
memory_strategy_(ms)
{
}
RCLCPP_PUBLIC
explicit Executor(
memory_strategy::MemoryStrategy::SharedPtr ms = memory_strategies::create_default_strategy());
/// Default destructor.
virtual ~Executor()
{
// Try to deallocate the interrupt guard condition.
if (interrupt_guard_condition_ != nullptr) {
rmw_ret_t status = rmw_destroy_guard_condition(interrupt_guard_condition_);
if (status != RMW_RET_OK) {
fprintf(stderr,
"[rclcpp::error] failed to destroy guard condition: %s\n", rmw_get_error_string_safe());
}
}
}
RCLCPP_PUBLIC
virtual ~Executor();
/// Do work periodically as it becomes available to us. Blocking call, may block indefinitely.
// It is up to the implementation of Executor to implement spin.
virtual void spin() = 0;
virtual void
spin() = 0;
/// Add a node to the executor.
/**
@ -86,26 +74,9 @@ public:
* the executor is blocked at the rmw layer while waiting for work and it is notified that a new
* node was added, it will wake up.
*/
RCLCPP_PUBLIC
virtual void
add_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify = true)
{
// Check to ensure node not already added
for (auto & weak_node : weak_nodes_) {
auto node = weak_node.lock();
if (node == node_ptr) {
// TODO(jacquelinekay): Use a different error here?
throw std::runtime_error("Cannot add node to executor, node already added.");
}
}
weak_nodes_.push_back(node_ptr);
if (notify) {
// Interrupt waiting to handle new node
rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_);
if (status != RMW_RET_OK) {
throw std::runtime_error(rmw_get_error_string_safe());
}
}
}
add_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify = true);
/// Remove a node from the executor.
/**
@ -114,29 +85,9 @@ public:
* This is useful if the last node was removed from the executor while the executor was blocked
* waiting for work in another thread, because otherwise the executor would never be notified.
*/
RCLCPP_PUBLIC
virtual void
remove_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify = true)
{
bool node_removed = false;
weak_nodes_.erase(
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 (notify) {
// If the node was matched and removed, interrupt waiting
if (node_removed) {
rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_);
if (status != RMW_RET_OK) {
throw std::runtime_error(rmw_get_error_string_safe());
}
}
}
}
remove_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify = true);
/// Add a node to executor, execute the next available unit of work, and remove the node.
/**
@ -146,28 +97,23 @@ public:
* function to be non-blocking.
*/
template<typename T = std::milli>
void spin_node_once(rclcpp::node::Node::SharedPtr node,
void
spin_node_once(rclcpp::node::Node::SharedPtr node,
std::chrono::duration<int64_t, T> timeout = std::chrono::duration<int64_t, T>(-1))
{
this->add_node(node, false);
// non-blocking = true
auto any_exec = get_next_executable(timeout);
if (any_exec) {
execute_any_executable(any_exec);
}
this->remove_node(node, false);
return spin_node_once_nanoseconds(
node,
std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
);
}
/// Add a node, complete all immediately available work, and remove the node.
/**
* \param[in] node Shared pointer to the node to add.
*/
void spin_node_some(rclcpp::node::Node::SharedPtr node)
{
this->add_node(node, false);
spin_some();
this->remove_node(node, false);
}
RCLCPP_PUBLIC
void
spin_node_some(rclcpp::node::Node::SharedPtr node);
/// Complete all available queued work without blocking.
/**
@ -176,14 +122,9 @@ public:
* Adding subscriptions, timers, services, etc. with blocking callbacks will cause this function
* to block (which may have unintended consequences).
*/
virtual void spin_some()
{
while (AnyExecutable::SharedPtr any_exec =
get_next_executable(std::chrono::milliseconds::zero()))
{
execute_any_executable(any_exec);
}
}
RCLCPP_PUBLIC
virtual void
spin_some();
/// Support dynamic switching of the memory strategy.
/**
@ -191,411 +132,72 @@ public:
* unintended consequences.
* \param[in] memory_strategy Shared pointer to the memory strategy to set.
*/
RCLCPP_PUBLIC
void
set_memory_strategy(memory_strategy::MemoryStrategy::SharedPtr memory_strategy)
{
if (memory_strategy == nullptr) {
throw std::runtime_error("Received NULL memory strategy in executor.");
}
memory_strategy_ = memory_strategy;
}
set_memory_strategy(memory_strategy::MemoryStrategy::SharedPtr memory_strategy);
protected:
RCLCPP_PUBLIC
void
spin_node_once_nanoseconds(rclcpp::node::Node::SharedPtr node, std::chrono::nanoseconds timeout);
/// Find the next available executable and do the work associated with it.
/** \param[in] any_exec Union structure that can hold any executable type (timer, subscription,
* service, client).
*/
RCLCPP_PUBLIC
void
execute_any_executable(AnyExecutable::SharedPtr any_exec)
{
if (!any_exec) {
return;
}
if (any_exec->timer) {
execute_timer(any_exec->timer);
}
if (any_exec->subscription) {
execute_subscription(any_exec->subscription);
}
if (any_exec->subscription_intra_process) {
execute_intra_process_subscription(any_exec->subscription_intra_process);
}
if (any_exec->service) {
execute_service(any_exec->service);
}
if (any_exec->client) {
execute_client(any_exec->client);
}
// Reset the callback_group, regardless of type
any_exec->callback_group->can_be_taken_from().store(true);
// Wake the wait, because it may need to be recalculated or work that
// was previously blocked is now available.
rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_);
if (status != RMW_RET_OK) {
throw std::runtime_error(rmw_get_error_string_safe());
}
}
execute_any_executable(AnyExecutable::SharedPtr any_exec);
RCLCPP_PUBLIC
static void
execute_subscription(
rclcpp::subscription::SubscriptionBase::SharedPtr subscription)
{
std::shared_ptr<void> message = subscription->create_message();
bool taken = false;
rmw_message_info_t message_info;
auto ret =
rmw_take_with_info(subscription->get_subscription_handle(),
message.get(), &taken, &message_info);
if (ret == RMW_RET_OK) {
if (taken) {
message_info.from_intra_process = false;
subscription->handle_message(message, message_info);
}
} else {
fprintf(stderr,
"[rclcpp::error] take failed for subscription on topic '%s': %s\n",
subscription->get_topic_name().c_str(), rmw_get_error_string_safe());
}
subscription->return_message(message);
}
rclcpp::subscription::SubscriptionBase::SharedPtr subscription);
RCLCPP_PUBLIC
static void
execute_intra_process_subscription(
rclcpp::subscription::SubscriptionBase::SharedPtr subscription)
{
rcl_interfaces::msg::IntraProcessMessage ipm;
bool taken = false;
rmw_message_info_t message_info;
rmw_ret_t status = rmw_take_with_info(
subscription->get_intra_process_subscription_handle(),
&ipm,
&taken,
&message_info);
if (status == RMW_RET_OK) {
if (taken) {
message_info.from_intra_process = true;
subscription->handle_intra_process_message(ipm, message_info);
}
} else {
fprintf(stderr,
"[rclcpp::error] take failed for intra process subscription on topic '%s': %s\n",
subscription->get_topic_name().c_str(), rmw_get_error_string_safe());
}
}
rclcpp::subscription::SubscriptionBase::SharedPtr subscription);
RCLCPP_PUBLIC
static void
execute_timer(
rclcpp::timer::TimerBase::SharedPtr timer)
{
timer->execute_callback();
}
execute_timer(rclcpp::timer::TimerBase::SharedPtr timer);
RCLCPP_PUBLIC
static void
execute_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_ret_t status = rmw_take_request(
service->get_service_handle(),
request_header.get(),
request.get(),
&taken);
if (status == RMW_RET_OK) {
if (taken) {
service->handle_request(request_header, request);
}
} else {
fprintf(stderr,
"[rclcpp::error] take request failed for server of service '%s': %s\n",
service->get_service_name().c_str(), rmw_get_error_string_safe());
}
}
execute_service(rclcpp::service::ServiceBase::SharedPtr service);
RCLCPP_PUBLIC
static void
execute_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_ret_t status = rmw_take_response(
client->get_client_handle(),
request_header.get(),
response.get(),
&taken);
if (status == RMW_RET_OK) {
if (taken) {
client->handle_response(request_header, response);
}
} else {
fprintf(stderr,
"[rclcpp::error] take response failed for client of service '%s': %s\n",
client->get_service_name().c_str(), rmw_get_error_string_safe());
}
}
execute_client(rclcpp::client::ClientBase::SharedPtr client);
/*** Populate class storage from stored weak node pointers and wait. ***/
template<typename T = std::milli>
RCLCPP_PUBLIC
void
wait_for_work(std::chrono::duration<int64_t, T> timeout = std::chrono::duration<int64_t, T>(-1))
{
memory_strategy_->clear_active_entities();
wait_for_work(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
// Collect the subscriptions and timers to be waited on
bool has_invalid_weak_nodes = memory_strategy_->collect_entities(weak_nodes_);
// 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 allocate memory in the handles
rmw_subscriptions_t subscriber_handles;
subscriber_handles.subscriber_count =
memory_strategy_->fill_subscriber_handles(subscriber_handles.subscribers);
rmw_services_t service_handles;
service_handles.service_count =
memory_strategy_->fill_service_handles(service_handles.services);
rmw_clients_t client_handles;
client_handles.client_count =
memory_strategy_->fill_client_handles(client_handles.clients);
// The number of guard conditions is fixed at 2: 1 for the ctrl-c guard cond,
// and one for the executor's guard cond (interrupt_guard_condition_)
size_t number_of_guard_conds = 2;
rmw_guard_conditions_t guard_condition_handles;
guard_condition_handles.guard_condition_count = number_of_guard_conds;
guard_condition_handles.guard_conditions = static_cast<void **>(guard_cond_handles_.data());
if (guard_condition_handles.guard_conditions == NULL &&
number_of_guard_conds > 0)
{
// TODO(wjwwood): Use a different error here? maybe std::bad_alloc?
throw std::runtime_error("Could not malloc for guard condition pointers.");
}
// 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;
// Put the executor's guard condition in
guard_condition_handles.guard_conditions[1] = \
interrupt_guard_condition_->data;
rmw_time_t * wait_timeout = NULL;
rmw_time_t rmw_timeout;
auto next_timer_duration = get_earliest_timer();
// If the next timer timeout must preempt the requested timeout
// or if the requested timeout blocks forever, and there exists a valid timer,
// replace the requested timeout with the next timeout.
bool has_valid_timer = next_timer_duration >= std::chrono::nanoseconds::zero();
if ((next_timer_duration < timeout ||
timeout < std::chrono::duration<int64_t, T>::zero()) && has_valid_timer)
{
rmw_timeout.sec =
std::chrono::duration_cast<std::chrono::seconds>(next_timer_duration).count();
rmw_timeout.nsec = next_timer_duration.count() % (1000 * 1000 * 1000);
wait_timeout = &rmw_timeout;
} else if (timeout >= std::chrono::duration<int64_t, T>::zero()) {
// Convert timeout representation to rmw_time
rmw_timeout.sec = std::chrono::duration_cast<std::chrono::seconds>(timeout).count();
rmw_timeout.nsec = std::chrono::duration_cast<std::chrono::nanoseconds>(timeout).count() %
(1000 * 1000 * 1000);
wait_timeout = &rmw_timeout;
}
// Now wait on the waitable subscriptions and timers
rmw_ret_t status = rmw_wait(
&subscriber_handles,
&guard_condition_handles,
&service_handles,
&client_handles,
wait_timeout);
if (status != RMW_RET_OK && status != RMW_RET_TIMEOUT) {
throw std::runtime_error(rmw_get_error_string_safe());
}
// If ctrl-c guard condition, return directly
if (guard_condition_handles.guard_conditions[0] != 0) {
// Make sure to free or clean memory
memory_strategy_->clear_handles();
return;
}
memory_strategy_->revalidate_handles();
}
/******************************/
RCLCPP_PUBLIC
rclcpp::node::Node::SharedPtr
get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group)
{
if (!group) {
return rclcpp::node::Node::SharedPtr();
}
for (auto & weak_node : weak_nodes_) {
auto node = weak_node.lock();
if (!node) {
continue;
}
for (auto & weak_group : node->get_callback_groups()) {
auto callback_group = weak_group.lock();
if (callback_group == group) {
return node;
}
}
}
return rclcpp::node::Node::SharedPtr();
}
get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group);
RCLCPP_PUBLIC
rclcpp::callback_group::CallbackGroup::SharedPtr
get_group_by_timer(
rclcpp::timer::TimerBase::SharedPtr timer)
{
for (auto & weak_node : weak_nodes_) {
auto node = weak_node.lock();
if (!node) {
continue;
}
for (auto & weak_group : node->get_callback_groups()) {
auto group = weak_group.lock();
if (!group) {
continue;
}
for (auto & weak_timer : group->get_timer_ptrs()) {
auto t = weak_timer.lock();
if (t == timer) {
return group;
}
}
}
}
return rclcpp::callback_group::CallbackGroup::SharedPtr();
}
get_group_by_timer(rclcpp::timer::TimerBase::SharedPtr timer);
RCLCPP_PUBLIC
void
get_next_timer(AnyExecutable::SharedPtr any_exec)
{
for (auto & weak_node : weak_nodes_) {
auto node = weak_node.lock();
if (!node) {
continue;
}
for (auto & weak_group : node->get_callback_groups()) {
auto group = weak_group.lock();
if (!group || !group->can_be_taken_from().load()) {
continue;
}
for (auto & timer_ref : group->get_timer_ptrs()) {
auto timer = timer_ref.lock();
if (timer && timer->check_and_trigger()) {
any_exec->timer = timer;
any_exec->callback_group = group;
node = get_node_by_group(group);
return;
}
}
}
}
}
get_next_timer(AnyExecutable::SharedPtr any_exec);
RCLCPP_PUBLIC
std::chrono::nanoseconds
get_earliest_timer()
{
std::chrono::nanoseconds latest = std::chrono::nanoseconds::max();
bool timers_empty = true;
for (auto & weak_node : weak_nodes_) {
auto node = weak_node.lock();
if (!node) {
continue;
}
for (auto & weak_group : node->get_callback_groups()) {
auto group = weak_group.lock();
if (!group || !group->can_be_taken_from().load()) {
continue;
}
for (auto & timer_ref : group->get_timer_ptrs()) {
timers_empty = false;
// Check the expected trigger time
auto timer = timer_ref.lock();
if (timer && timer->time_until_trigger() < latest) {
latest = timer->time_until_trigger();
}
}
}
}
if (timers_empty) {
return std::chrono::nanoseconds(-1);
}
return latest;
}
get_earliest_timer();
RCLCPP_PUBLIC
AnyExecutable::SharedPtr
get_next_ready_executable()
{
auto any_exec = memory_strategy_->instantiate_next_executable();
// Check the timers to see if there are any that are ready, if so return
get_next_timer(any_exec);
if (any_exec->timer) {
return any_exec;
}
// Check the subscriptions to see if there are any that are ready
memory_strategy_->get_next_subscription(any_exec, weak_nodes_);
if (any_exec->subscription || any_exec->subscription_intra_process) {
return any_exec;
}
// Check the services to see if there are any that are ready
memory_strategy_->get_next_service(any_exec, weak_nodes_);
if (any_exec->service) {
return any_exec;
}
// Check the clients to see if there are any that are ready
memory_strategy_->get_next_client(any_exec, weak_nodes_);
if (any_exec->client) {
return any_exec;
}
// If there is no ready executable, return a null ptr
return nullptr;
}
get_next_ready_executable();
template<typename T = std::milli>
RCLCPP_PUBLIC
AnyExecutable::SharedPtr
get_next_executable(std::chrono::duration<int64_t, T> timeout = std::chrono::duration<int64_t,
T>(-1))
{
// 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) {
// Wait for subscriptions or timers to work on
wait_for_work(timeout);
// Try again
any_exec = get_next_ready_executable();
}
// 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 it is valid, check to see if the group is mutually exclusive or
// not, then mark it accordingly
if (any_exec->callback_group && any_exec->callback_group->type() == \
callback_group::CallbackGroupType::MutuallyExclusive)
{
// It should not have been taken otherwise
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);
}
}
return any_exec;
}
get_next_executable(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
/// Guard condition for signaling the rmw layer to wake up for special events.
rmw_guard_condition_t * interrupt_guard_condition_;

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

@ -20,9 +20,23 @@
#include <rclcpp/executors/single_threaded_executor.hpp>
#include <rclcpp/node.hpp>
#include <rclcpp/utilities.hpp>
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
/// Create a default single-threaded executor and execute any immediately available work.
// \param[in] node_ptr Shared pointer to the node to spin.
RCLCPP_PUBLIC
void
spin_some(node::Node::SharedPtr node_ptr);
/// Create a default single-threaded executor and spin the specified node.
// \param[in] node_ptr Shared pointer to the node to spin.
RCLCPP_PUBLIC
void
spin(node::Node::SharedPtr node_ptr);
namespace executors
{
@ -37,8 +51,7 @@ using rclcpp::executors::single_threaded_executor::SingleThreadedExecutor;
*/
enum FutureReturnCode {SUCCESS, INTERRUPTED, TIMEOUT};
/// Spin (blocking) until the future is complete, until the function times out (if applicable),
/// or until rclcpp is interrupted.
/// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted.
/**
* \param[in] executor The executor which will spin the node.
* \param[in] node_ptr The node to spin.
@ -55,7 +68,7 @@ spin_node_until_future_complete(
std::shared_future<ResponseT> & future,
std::chrono::duration<int64_t, TimeT> timeout = std::chrono::duration<int64_t, TimeT>(-1))
{
// TODO(wjwwood): does not work recursively right, can't call spin_node_until_future_complete
// TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete
// inside a callback executed by an executor.
// Check the future before entering the while loop.
@ -82,6 +95,17 @@ spin_node_until_future_complete(
}
} // namespace executors
template<typename FutureT, typename TimeT = std::milli>
rclcpp::executors::FutureReturnCode
spin_until_future_complete(
node::Node::SharedPtr node_ptr, std::shared_future<FutureT> & future,
std::chrono::duration<int64_t, TimeT> timeout = std::chrono::duration<int64_t, TimeT>(-1))
{
rclcpp::executors::SingleThreadedExecutor executor;
return executors::spin_node_until_future_complete<FutureT>(executor, node_ptr, future, timeout);
}
} // namespace rclcpp
#endif /* RCLCPP_RCLCPP_EXECUTORS_HPP_ */

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

@ -27,6 +27,8 @@
#include "rclcpp/macros.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/utilities.hpp"
#include "rclcpp/memory_strategies.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
@ -40,63 +42,32 @@ class MultiThreadedExecutor : public executor::Executor
public:
RCLCPP_SMART_PTR_DEFINITIONS(MultiThreadedExecutor);
MultiThreadedExecutor(memory_strategy::MemoryStrategy::SharedPtr ms =
memory_strategies::create_default_strategy())
: executor::Executor(ms)
{
number_of_threads_ = std::thread::hardware_concurrency();
if (number_of_threads_ == 0) {
number_of_threads_ = 1;
}
}
RCLCPP_PUBLIC
MultiThreadedExecutor(
memory_strategy::MemoryStrategy::SharedPtr ms = memory_strategies::create_default_strategy());
virtual ~MultiThreadedExecutor() {}
RCLCPP_PUBLIC
virtual ~MultiThreadedExecutor();
RCLCPP_PUBLIC
void
spin()
{
std::vector<std::thread> threads;
{
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) {
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) {
thread.join();
}
}
spin();
RCLCPP_PUBLIC
size_t
get_number_of_threads()
{
return number_of_threads_;
}
get_number_of_threads();
protected:
RCLCPP_PUBLIC
void
run(size_t this_thread_number);
private:
void run(size_t this_thread_id)
{
rclcpp::thread_id = this_thread_id;
while (rclcpp::utilities::ok()) {
executor::AnyExecutable::SharedPtr any_exec;
{
std::lock_guard<std::mutex> wait_lock(wait_mutex_);
if (!rclcpp::utilities::ok()) {
return;
}
any_exec = get_next_executable();
}
execute_any_executable(any_exec);
}
}
RCLCPP_DISABLE_COPY(MultiThreadedExecutor);
std::mutex wait_mutex_;
size_t number_of_threads_;
std::unordered_map<std::thread::id, size_t> thread_number_by_thread_id_;
};
} // namespace multi_threaded_executor

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

@ -28,6 +28,7 @@
#include "rclcpp/node.hpp"
#include "rclcpp/utilities.hpp"
#include "rclcpp/rate.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
@ -44,23 +45,20 @@ public:
RCLCPP_SMART_PTR_DEFINITIONS(SingleThreadedExecutor);
/// Default constructor. See the default constructor for Executor.
SingleThreadedExecutor(memory_strategy::MemoryStrategy::SharedPtr ms =
memory_strategies::create_default_strategy())
: executor::Executor(ms) {}
RCLCPP_PUBLIC
SingleThreadedExecutor(
memory_strategy::MemoryStrategy::SharedPtr ms = memory_strategies::create_default_strategy());
/// Default destrcutor.
virtual ~SingleThreadedExecutor() {}
RCLCPP_PUBLIC
virtual ~SingleThreadedExecutor();
/// Single-threaded implementation of spin.
// This function will block until work comes in, execute it, and keep blocking.
// It will only be interrupt by a CTRL-C (managed by the global signal handler).
void spin()
{
while (rclcpp::utilities::ok()) {
auto any_exec = get_next_executable();
execute_any_executable(any_exec);
}
}
RCLCPP_PUBLIC
void
spin();
private:
RCLCPP_DISABLE_COPY(SingleThreadedExecutor);

64
rclcpp/include/rclcpp/intra_process_manager.hpp
View file

@ -32,6 +32,7 @@
#include "rclcpp/macros.hpp"
#include "rclcpp/publisher.hpp"
#include "rclcpp/subscription.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
@ -124,12 +125,12 @@ private:
public:
RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessManager);
RCLCPP_PUBLIC
explicit IntraProcessManager(
IntraProcessManagerStateBase::SharedPtr state = create_default_state()
)
: state_(state)
{
}
IntraProcessManagerStateBase::SharedPtr state = create_default_state());
RCLCPP_PUBLIC
virtual ~IntraProcessManager();
/// Register a subscription with the manager, returns subscriptions unique id.
/* In addition to generating a unique intra process id for the subscription,
@ -143,24 +144,18 @@ public:
* \param subscription the Subscription to register.
* \return an unsigned 64-bit integer which is the subscription's unique id.
*/
RCLCPP_PUBLIC
uint64_t
add_subscription(subscription::SubscriptionBase::SharedPtr subscription)
{
auto id = IntraProcessManager::get_next_unique_id();
state_->add_subscription(id, subscription);
return id;
}
add_subscription(subscription::SubscriptionBase::SharedPtr subscription);
/// Unregister a subscription using the subscription's unique id.
/* This method does not allocate memory.
*
* \param intra_process_subscription_id id of the subscription to remove.
*/
RCLCPP_PUBLIC
void
remove_subscription(uint64_t intra_process_subscription_id)
{
state_->remove_subscription(intra_process_subscription_id);
}
remove_subscription(uint64_t intra_process_subscription_id);
/// Register a publisher with the manager, returns the publisher unique id.
/* In addition to generating and returning a unique id for the publisher,
@ -203,11 +198,9 @@ public:
*
* \param intra_process_publisher_id id of the publisher to remove.
*/
RCLCPP_PUBLIC
void
remove_publisher(uint64_t intra_process_publisher_id)
{
state_->remove_publisher(intra_process_publisher_id);
}
remove_publisher(uint64_t intra_process_publisher_id);
/// Store a message in the manager, and return the message sequence number.
/* The given message is stored in internal storage using the given publisher
@ -336,41 +329,18 @@ public:
}
/// Return true if the given rmw_gid_t matches any stored Publishers.
RCLCPP_PUBLIC
bool
matches_any_publishers(const rmw_gid_t * id) const
{
return state_->matches_any_publishers(id);
}
matches_any_publishers(const rmw_gid_t * id) const;
private:
static uint64_t get_next_unique_id()
{
auto next_id = next_unique_id_.fetch_add(1, std::memory_order_relaxed);
// Check for rollover (we started at 1).
if (0 == next_id) {
// This puts a technical limit on the number of times you can add a publisher or subscriber.
// But even if you could add (and remove) them at 1 kHz (very optimistic rate)
// it would still be a very long time before you could exhaust the pool of id's:
// 2^64 / 1000 times per sec / 60 sec / 60 min / 24 hours / 365 days = 584,942,417 years
// So around 585 million years. Even at 1 GHz, it would take 585 years.
// I think it's safe to avoid trying to handle overflow.
// If we roll over then it's most likely a bug.
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
throw std::overflow_error(
"exhausted the unique id's for publishers and subscribers in this process "
"(congratulations your computer is either extremely fast or extremely old)");
// *INDENT-ON*
}
return next_id;
}
static std::atomic<uint64_t> next_unique_id_;
RCLCPP_PUBLIC
static uint64_t
get_next_unique_id();
IntraProcessManagerStateBase::SharedPtr state_;
};
std::atomic<uint64_t> IntraProcessManager::next_unique_id_ {1};
} // namespace intra_process_manager
} // namespace rclcpp

20
rclcpp/include/rclcpp/intra_process_manager_state.hpp
View file

@ -30,6 +30,8 @@
#include <string>
#include <utility>
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
namespace intra_process_manager
@ -40,6 +42,9 @@ class IntraProcessManagerStateBase
public:
RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(IntraProcessManagerStateBase);
IntraProcessManagerStateBase() = default;
~IntraProcessManagerStateBase() = default;
virtual void
add_subscription(uint64_t id, subscription::SubscriptionBase::SharedPtr subscription) = 0;
@ -70,12 +75,18 @@ public:
virtual bool
matches_any_publishers(const rmw_gid_t * id) const = 0;
private:
RCLCPP_DISABLE_COPY(IntraProcessManagerStateBase);
};
template<typename Allocator = std::allocator<void>>
class IntraProcessManagerState : public IntraProcessManagerStateBase
{
public:
IntraProcessManagerState() = default;
~IntraProcessManagerState() = default;
void
add_subscription(uint64_t id, subscription::SubscriptionBase::SharedPtr subscription)
{
@ -222,6 +233,8 @@ public:
}
private:
RCLCPP_DISABLE_COPY(IntraProcessManagerState);
template<typename T>
using RebindAlloc = typename std::allocator_traits<Allocator>::template rebind_alloc<T>;
@ -259,10 +272,9 @@ private:
PublisherMap publishers_;
};
static IntraProcessManagerStateBase::SharedPtr create_default_state()
{
return std::make_shared<IntraProcessManagerState<>>();
}
RCLCPP_PUBLIC
IntraProcessManagerStateBase::SharedPtr
create_default_state();
} // namespace intra_process_manager
} // namespace rclcpp

3
rclcpp/include/rclcpp/mapped_ring_buffer.hpp
View file

@ -23,13 +23,14 @@
#include "rclcpp/allocator/allocator_common.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
namespace mapped_ring_buffer
{
class MappedRingBufferBase
class RCLCPP_PUBLIC MappedRingBufferBase
{
public:
RCLCPP_SMART_PTR_DEFINITIONS(MappedRingBufferBase);

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

@ -17,18 +17,16 @@
#include <rclcpp/memory_strategy.hpp>
#include <rclcpp/strategies/allocator_memory_strategy.hpp>
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
namespace memory_strategies
{
using rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy;
static memory_strategy::MemoryStrategy::SharedPtr create_default_strategy()
{
return std::make_shared<AllocatorMemoryStrategy<>>();
}
RCLCPP_PUBLIC
memory_strategy::MemoryStrategy::SharedPtr
create_default_strategy();
} // namespace memory_strategies
} // namespace rclcpp

177
rclcpp/include/rclcpp/memory_strategy.hpp
View file

@ -21,6 +21,7 @@
#include "rclcpp/any_executable.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
@ -33,11 +34,11 @@ namespace memory_strategy
* the rmw implementation after the executor waits for work, based on the number of entities that
* come through.
*/
class MemoryStrategy
class RCLCPP_PUBLIC MemoryStrategy
{
public:
RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(MemoryStrategy);
using WeakNodeVector = std::vector<std::weak_ptr<node::Node>>;
using WeakNodeVector = std::vector<std::weak_ptr<rclcpp::node::Node>>;
// return the new number of subscribers
virtual size_t fill_subscriber_handles(void ** & ptr) = 0;
@ -56,200 +57,50 @@ public:
/// Provide a newly initialized AnyExecutable object.
// \return Shared pointer to the fresh executable.
virtual executor::AnyExecutable::SharedPtr instantiate_next_executable() = 0;
virtual rclcpp::executor::AnyExecutable::SharedPtr instantiate_next_executable() = 0;
virtual void
get_next_subscription(executor::AnyExecutable::SharedPtr any_exec,
get_next_subscription(rclcpp::executor::AnyExecutable::SharedPtr any_exec,
const WeakNodeVector & weak_nodes) = 0;
virtual void
get_next_service(executor::AnyExecutable::SharedPtr any_exec,
get_next_service(rclcpp::executor::AnyExecutable::SharedPtr any_exec,
const WeakNodeVector & weak_nodes) = 0;
virtual void
get_next_client(executor::AnyExecutable::SharedPtr any_exec,
get_next_client(rclcpp::executor::AnyExecutable::SharedPtr any_exec,
const WeakNodeVector & weak_nodes) = 0;
static rclcpp::subscription::SubscriptionBase::SharedPtr
get_subscription_by_handle(void * subscriber_handle,
const WeakNodeVector & weak_nodes)
{
for (auto & weak_node : weak_nodes) {
auto node = weak_node.lock();
if (!node) {
continue;
}
for (auto & weak_group : node->get_callback_groups()) {
auto group = weak_group.lock();
if (!group) {
continue;
}
for (auto & weak_subscription : group->get_subscription_ptrs()) {
auto subscription = weak_subscription.lock();
if (subscription) {
if (subscription->get_subscription_handle()->data == subscriber_handle) {
return subscription;
}
if (subscription->get_intra_process_subscription_handle() &&
subscription->get_intra_process_subscription_handle()->data == subscriber_handle)
{
return subscription;
}
}
}
}
}
return nullptr;
}
const WeakNodeVector & weak_nodes);
static rclcpp::service::ServiceBase::SharedPtr
get_service_by_handle(void * service_handle, const WeakNodeVector & weak_nodes)
{
for (auto & weak_node : weak_nodes) {
auto node = weak_node.lock();
if (!node) {
continue;
}
for (auto & weak_group : node->get_callback_groups()) {
auto group = weak_group.lock();
if (!group) {
continue;
}
for (auto & service : group->get_service_ptrs()) {
if (service->get_service_handle()->data == service_handle) {
return service;
}
}
}
}
return nullptr;
}
get_service_by_handle(void * service_handle, const WeakNodeVector & weak_nodes);
static rclcpp::client::ClientBase::SharedPtr
get_client_by_handle(void * client_handle, const WeakNodeVector & weak_nodes)
{
for (auto & weak_node : weak_nodes) {
auto node = weak_node.lock();
if (!node) {
continue;
}
for (auto & weak_group : node->get_callback_groups()) {
auto group = weak_group.lock();
if (!group) {
continue;
}
for (auto & client : group->get_client_ptrs()) {
if (client->get_client_handle()->data == client_handle) {
return client;
}
}
}
}
return nullptr;
}
get_client_by_handle(void * client_handle, const WeakNodeVector & weak_nodes);
static rclcpp::node::Node::SharedPtr
get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group,
const WeakNodeVector & weak_nodes)
{
if (!group) {
return nullptr;
}
for (auto & weak_node : weak_nodes) {
auto node = weak_node.lock();
if (!node) {
continue;
}
for (auto & weak_group : node->get_callback_groups()) {
auto callback_group = weak_group.lock();
if (callback_group == group) {
return node;
}
}
}
return nullptr;
}
const WeakNodeVector & weak_nodes);
static rclcpp::callback_group::CallbackGroup::SharedPtr
get_group_by_subscription(
rclcpp::subscription::SubscriptionBase::SharedPtr subscription,
const WeakNodeVector & weak_nodes)
{
for (auto & weak_node : weak_nodes) {
auto node = weak_node.lock();
if (!node) {
continue;
}
for (auto & weak_group : node->get_callback_groups()) {
auto group = weak_group.lock();
if (!group) {
continue;
}
for (auto & weak_sub : group->get_subscription_ptrs()) {
auto sub = weak_sub.lock();
if (sub == subscription) {
return group;
}
}
}
}
return nullptr;
}
const WeakNodeVector & weak_nodes);
static rclcpp::callback_group::CallbackGroup::SharedPtr
get_group_by_service(
rclcpp::service::ServiceBase::SharedPtr service,
const WeakNodeVector & weak_nodes)
{
for (auto & weak_node : weak_nodes) {
auto node = weak_node.lock();
if (!node) {
continue;
}
for (auto & weak_group : node->get_callback_groups()) {
auto group = weak_group.lock();
if (!group) {
continue;
}
for (auto & serv : group->get_service_ptrs()) {
if (serv == service) {
return group;
}
}
}
}
return nullptr;
}
const WeakNodeVector & weak_nodes);
static rclcpp::callback_group::CallbackGroup::SharedPtr
get_group_by_client(rclcpp::client::ClientBase::SharedPtr client,
const WeakNodeVector & weak_nodes)
{
for (auto & weak_node : weak_nodes) {
auto node = weak_node.lock();
if (!node) {
continue;
}
for (auto & weak_group : node->get_callback_groups()) {
auto group = weak_group.lock();
if (!group) {
continue;
}
for (auto & cli : group->get_client_ptrs()) {
if (cli == client) {
return group;
}
}
}
}
return nullptr;
}
const WeakNodeVector & weak_nodes);
};
} // namespace memory_strategy
} // namespace rclcpp
#endif // RCLCPP__MEMORY_STRATEGY_HPP_

1
rclcpp/include/rclcpp/message_memory_strategy.hpp
View file

@ -19,6 +19,7 @@
#include "rclcpp/allocator/allocator_common.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{

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

@ -26,7 +26,6 @@
#include "rcl_interfaces/msg/parameter_descriptor.hpp"
#include "rcl_interfaces/msg/parameter_event.hpp"
#include "rcl_interfaces/msg/set_parameters_result.hpp"
#include "rosidl_generator_cpp/message_type_support.hpp"
#include "rclcpp/callback_group.hpp"
#include "rclcpp/client.hpp"
@ -38,7 +37,7 @@
#include "rclcpp/service.hpp"
#include "rclcpp/subscription.hpp"
#include "rclcpp/timer.hpp"
#include "rclcpp/visibility_control.hpp"
// Forward declaration of ROS middleware class
namespace rmw
@ -63,6 +62,7 @@ public:
* \param[in] use_intra_process_comms True to use the optimized intra-process communication
* pipeline to pass messages between nodes in the same process using shared memory.
*/
RCLCPP_PUBLIC
explicit Node(const std::string & node_name, bool use_intra_process_comms = false);
/// Create a node based on the node name and a rclcpp::context::Context.
@ -72,16 +72,19 @@ public:
* \param[in] use_intra_process_comms True to use the optimized intra-process communication
* pipeline to pass messages between nodes in the same process using shared memory.
*/
RCLCPP_PUBLIC
Node(
const std::string & node_name, rclcpp::context::Context::SharedPtr context,
bool use_intra_process_comms = false);
/// Get the name of the node.
// \return The name of the node.
RCLCPP_PUBLIC
const std::string &
get_name() const {return name_; }
get_name() const;
/// Create and return a callback group.
RCLCPP_PUBLIC
rclcpp::callback_group::CallbackGroup::SharedPtr
create_callback_group(rclcpp::callback_group::CallbackGroupType group_type);
@ -168,13 +171,14 @@ public:
* \param[in] callback User-defined callback function.
* \param[in] group Callback group to execute this timer's callback in.
*/
RCLCPP_PUBLIC
rclcpp::timer::WallTimer::SharedPtr
create_wall_timer(
std::chrono::nanoseconds period,
rclcpp::timer::CallbackType callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
/// Create a timer with a sub-nanosecond precision update period.
/// Create a timer.
/**
* \param[in] period Time interval between triggers of the callback.
* \param[in] callback User-defined callback function.
@ -206,37 +210,50 @@ public:
FunctorT callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
std::vector<rcl_interfaces::msg::SetParametersResult> set_parameters(
const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
RCLCPP_PUBLIC
std::vector<rcl_interfaces::msg::SetParametersResult>
set_parameters(const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
rcl_interfaces::msg::SetParametersResult set_parameters_atomically(
const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
RCLCPP_PUBLIC
rcl_interfaces::msg::SetParametersResult
set_parameters_atomically(const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
std::vector<rclcpp::parameter::ParameterVariant> get_parameters(
const std::vector<std::string> & names) const;
RCLCPP_PUBLIC
std::vector<rclcpp::parameter::ParameterVariant>
get_parameters(const std::vector<std::string> & names) const;
std::vector<rcl_interfaces::msg::ParameterDescriptor> describe_parameters(
const std::vector<std::string> & names) const;
RCLCPP_PUBLIC
std::vector<rcl_interfaces::msg::ParameterDescriptor>
describe_parameters(const std::vector<std::string> & names) const;
std::vector<uint8_t> get_parameter_types(
const std::vector<std::string> & names) const;
RCLCPP_PUBLIC
std::vector<uint8_t>
get_parameter_types(const std::vector<std::string> & names) const;
rcl_interfaces::msg::ListParametersResult list_parameters(
const std::vector<std::string> & prefixes, uint64_t depth) const;
RCLCPP_PUBLIC
rcl_interfaces::msg::ListParametersResult
list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const;
std::map<std::string, std::string> get_topic_names_and_types() const;
RCLCPP_PUBLIC
std::map<std::string, std::string>
get_topic_names_and_types() const;
size_t count_publishers(const std::string & topic_name) const;
RCLCPP_PUBLIC
size_t
count_publishers(const std::string & topic_name) const;
size_t count_subscribers(const std::string & topic_name) const;
RCLCPP_PUBLIC
size_t
count_subscribers(const std::string & topic_name) const;
const CallbackGroupWeakPtrList & get_callback_groups() const;
RCLCPP_PUBLIC
const CallbackGroupWeakPtrList &
get_callback_groups() const;
private:
RCLCPP_DISABLE_COPY(Node);
static const rosidl_message_type_support_t * ipm_ts_;
RCLCPP_PUBLIC
bool
group_in_node(callback_group::CallbackGroup::SharedPtr group);
@ -263,20 +280,8 @@ private:
publisher::Publisher<rcl_interfaces::msg::ParameterEvent>::SharedPtr events_publisher_;
};
const rosidl_message_type_support_t * Node::ipm_ts_ =
rosidl_generator_cpp::get_message_type_support_handle<rcl_interfaces::msg::IntraProcessMessage>();
} /* namespace node */
} /* 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())); \
}
} // namespace node
} // namespace rclcpp
#ifndef RCLCPP__NODE_IMPL_HPP_
// Template implementations

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

@ -0,0 +1,359 @@
// 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__NODE_IMPL_HPP_
#define RCLCPP__NODE_IMPL_HPP_
#include <rmw/error_handling.h>
#include <rmw/rmw.h>
#include <algorithm>
#include <cstdlib>
#include <iostream>
#include <limits>
#include <map>
#include <memory>
#include <sstream>
#include <stdexcept>
#include <string>
#include <utility>
#include <vector>
#include "rcl_interfaces/msg/intra_process_message.hpp"
#include "rclcpp/contexts/default_context.hpp"
#include "rclcpp/intra_process_manager.hpp"
#include "rclcpp/parameter.hpp"
#include "rclcpp/type_support_decl.hpp"
#include "rclcpp/visibility_control.hpp"
#ifndef RCLCPP__NODE_HPP_
#include "node.hpp"
#endif
namespace rclcpp
{
namespace node
{
template<typename MessageT, typename Alloc>
typename rclcpp::publisher::Publisher<MessageT, Alloc>::SharedPtr
Node::create_publisher(
const std::string & topic_name, size_t qos_history_depth,
std::shared_ptr<Alloc> allocator)
{
if (!allocator) {
allocator = std::make_shared<Alloc>();
}
rmw_qos_profile_t qos = rmw_qos_profile_default;
qos.depth = qos_history_depth;
return this->create_publisher<MessageT, Alloc>(topic_name, qos, allocator);
}
template<typename MessageT, typename Alloc>
typename publisher::Publisher<MessageT, Alloc>::SharedPtr
Node::create_publisher(
const std::string & topic_name, const rmw_qos_profile_t & qos_profile,
std::shared_ptr<Alloc> allocator)
{
if (!allocator) {
allocator = std::make_shared<Alloc>();
}
using rosidl_generator_cpp::get_message_type_support_handle;
auto type_support_handle = get_message_type_support_handle<MessageT>();
rmw_publisher_t * publisher_handle = rmw_create_publisher(
node_handle_.get(), type_support_handle, topic_name.c_str(), qos_profile);
if (!publisher_handle) {
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
throw std::runtime_error(
std::string("could not create publisher: ") +
rmw_get_error_string_safe());
// *INDENT-ON*
}
auto publisher = publisher::Publisher<MessageT, Alloc>::make_shared(
node_handle_, publisher_handle, topic_name, qos_profile.depth, allocator);
if (use_intra_process_comms_) {
rmw_publisher_t * intra_process_publisher_handle = rmw_create_publisher(
node_handle_.get(), rclcpp::type_support::get_intra_process_message_msg_type_support(),
(topic_name + "__intra").c_str(), qos_profile);
if (!intra_process_publisher_handle) {
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
throw std::runtime_error(
std::string("could not create intra process publisher: ") +
rmw_get_error_string_safe());
// *INDENT-ON*
}
auto intra_process_manager =
context_->get_sub_context<rclcpp::intra_process_manager::IntraProcessManager>();
uint64_t intra_process_publisher_id =
intra_process_manager->add_publisher<MessageT, Alloc>(publisher);
rclcpp::intra_process_manager::IntraProcessManager::WeakPtr weak_ipm = intra_process_manager;
// *INDENT-OFF*
auto shared_publish_callback =
[weak_ipm](uint64_t publisher_id, void * msg, const std::type_info & type_info) -> uint64_t
{
auto ipm = weak_ipm.lock();
if (!ipm) {
// TODO(wjwwood): should this just return silently? Or maybe return with a logged warning?
throw std::runtime_error(
"intra process publish called after destruction of intra process manager");
}
if (!msg) {
throw std::runtime_error("cannot publisher msg which is a null pointer");
}
auto & message_type_info = typeid(MessageT);
if (message_type_info != type_info) {
throw std::runtime_error(
std::string("published type '") + type_info.name() +
"' is incompatible from the publisher type '" + message_type_info.name() + "'");
}
MessageT * typed_message_ptr = static_cast<MessageT *>(msg);
using MessageDeleter = typename publisher::Publisher<MessageT, Alloc>::MessageDeleter;
std::unique_ptr<MessageT, MessageDeleter> unique_msg(typed_message_ptr);
uint64_t message_seq =
ipm->store_intra_process_message<MessageT, Alloc>(publisher_id, unique_msg);
return message_seq;
};
// *INDENT-ON*
publisher->setup_intra_process(
intra_process_publisher_id,
shared_publish_callback,
intra_process_publisher_handle);
}
return publisher;
}
template<typename MessageT, typename CallbackT, typename Alloc>
typename rclcpp::subscription::Subscription<MessageT, Alloc>::SharedPtr
Node::create_subscription(
const std::string & topic_name,
CallbackT callback,
const rmw_qos_profile_t & qos_profile,
rclcpp::callback_group::CallbackGroup::SharedPtr group,
bool ignore_local_publications,
typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
msg_mem_strat,
typename std::shared_ptr<Alloc> allocator)
{
if (!allocator) {
allocator = std::make_shared<Alloc>();
}
rclcpp::subscription::AnySubscriptionCallback<MessageT,
Alloc> any_subscription_callback(allocator);
any_subscription_callback.set(callback);
using rosidl_generator_cpp::get_message_type_support_handle;
if (!msg_mem_strat) {
msg_mem_strat =
rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::create_default();
}
auto type_support_handle = get_message_type_support_handle<MessageT>();
rmw_subscription_t * subscriber_handle = rmw_create_subscription(
node_handle_.get(), type_support_handle,
topic_name.c_str(), qos_profile, ignore_local_publications);
if (!subscriber_handle) {
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
throw std::runtime_error(
std::string("could not create subscription: ") + rmw_get_error_string_safe());
// *INDENT-ON*
}
using rclcpp::subscription::Subscription;
using rclcpp::subscription::SubscriptionBase;
auto sub = Subscription<MessageT, Alloc>::make_shared(
node_handle_,
subscriber_handle,
topic_name,
ignore_local_publications,
any_subscription_callback,
msg_mem_strat);
auto sub_base_ptr = std::dynamic_pointer_cast<SubscriptionBase>(sub);
// Setup intra process.
if (use_intra_process_comms_) {
rmw_subscription_t * intra_process_subscriber_handle = rmw_create_subscription(
node_handle_.get(), rclcpp::type_support::get_intra_process_message_msg_type_support(),
(topic_name + "__intra").c_str(), qos_profile, false);
if (!subscriber_handle) {
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
throw std::runtime_error(
std::string("could not create intra process subscription: ") + rmw_get_error_string_safe());
// *INDENT-ON*
}
auto intra_process_manager =
context_->get_sub_context<rclcpp::intra_process_manager::IntraProcessManager>();
rclcpp::intra_process_manager::IntraProcessManager::WeakPtr weak_ipm = intra_process_manager;
uint64_t intra_process_subscription_id =
intra_process_manager->add_subscription(sub_base_ptr);
// *INDENT-OFF*
sub->setup_intra_process(
intra_process_subscription_id,
intra_process_subscriber_handle,
[weak_ipm](
uint64_t publisher_id,
uint64_t message_sequence,
uint64_t subscription_id,
typename Subscription<MessageT, Alloc>::MessageUniquePtr & message)
{
auto ipm = weak_ipm.lock();
if (!ipm) {
// TODO(wjwwood): should this just return silently? Or maybe return with a logged warning?
throw std::runtime_error(
"intra process take called after destruction of intra process manager");
}
ipm->take_intra_process_message<MessageT, Alloc>(
publisher_id, message_sequence, subscription_id, message);
},
[weak_ipm](const rmw_gid_t * sender_gid) -> bool {
auto ipm = weak_ipm.lock();
if (!ipm) {
throw std::runtime_error(
"intra process publisher check called after destruction of intra process manager");
}
return ipm->matches_any_publishers(sender_gid);
}
);
// *INDENT-ON*
}
// Assign to a group.
if (group) {
if (!group_in_node(group)) {
// TODO(jacquelinekay): use custom exception
throw std::runtime_error("Cannot create subscription, group not in node.");
}
group->add_subscription(sub_base_ptr);
} else {
default_callback_group_->add_subscription(sub_base_ptr);
}
number_of_subscriptions_++;
return sub;
}
template<typename MessageT, typename CallbackT, typename Alloc>
typename rclcpp::subscription::Subscription<MessageT, Alloc>::SharedPtr
Node::create_subscription(
const std::string & topic_name,
size_t qos_history_depth,
CallbackT callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group,
bool ignore_local_publications,
typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
msg_mem_strat,
std::shared_ptr<Alloc> allocator)
{
rmw_qos_profile_t qos = rmw_qos_profile_default;
qos.depth = qos_history_depth;
return this->create_subscription<MessageT, CallbackT, Alloc>(
topic_name,
callback,
qos,
group,
ignore_local_publications,
msg_mem_strat,
allocator);
}
template<typename ServiceT>
typename client::Client<ServiceT>::SharedPtr
Node::create_client(
const std::string & service_name,
rclcpp::callback_group::CallbackGroup::SharedPtr group)
{
using rosidl_generator_cpp::get_service_type_support_handle;
auto service_type_support_handle =
get_service_type_support_handle<ServiceT>();
rmw_client_t * client_handle = rmw_create_client(
this->node_handle_.get(), service_type_support_handle, service_name.c_str());
if (!client_handle) {
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
throw std::runtime_error(
std::string("could not create client: ") +
rmw_get_error_string_safe());
// *INDENT-ON*
}
using rclcpp::client::Client;
using rclcpp::client::ClientBase;
auto cli = Client<ServiceT>::make_shared(
node_handle_,
client_handle,
service_name);
auto cli_base_ptr = std::dynamic_pointer_cast<ClientBase>(cli);
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 {
default_callback_group_->add_client(cli_base_ptr);
}
number_of_clients_++;
return cli;
}
template<typename ServiceT, typename FunctorT>
typename rclcpp::service::Service<ServiceT>::SharedPtr
Node::create_service(
const std::string & service_name,
FunctorT callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group)
{
using rosidl_generator_cpp::get_service_type_support_handle;
auto service_type_support_handle =
get_service_type_support_handle<ServiceT>();
rclcpp::service::AnyServiceCallback<ServiceT> any_service_callback;
any_service_callback.set(callback);
rmw_service_t * service_handle = rmw_create_service(
node_handle_.get(), service_type_support_handle, service_name.c_str());
if (!service_handle) {
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
throw std::runtime_error(
std::string("could not create service: ") +
rmw_get_error_string_safe());
// *INDENT-ON*
}
auto serv = service::Service<ServiceT>::make_shared(
node_handle_, service_handle, service_name, any_service_callback);
auto serv_base_ptr = std::dynamic_pointer_cast<service::ServiceBase>(serv);
if (group) {
if (!group_in_node(group)) {
// TODO(jacquelinekay): use custom exception
throw std::runtime_error("Cannot create service, group not in node.");
}
group->add_service(serv_base_ptr);
} else {
default_callback_group_->add_service(serv_base_ptr);
}
number_of_services_++;
return serv;
}
} // namespace node
} // namespace rclcpp
#endif // RCLCPP__NODE_IMPL_HPP_

281
rclcpp/include/rclcpp/parameter.hpp
View file

@ -24,6 +24,8 @@
#include <rcl_interfaces/msg/parameter.hpp>
#include <rcl_interfaces/msg/parameter_type.hpp>
#include <rcl_interfaces/msg/parameter_value.hpp>
#include "rclcpp/visibility_control.hpp"
#include "rmw/rmw.h"
namespace rclcpp
{
@ -45,87 +47,40 @@ enum ParameterType
class ParameterVariant
{
public:
ParameterVariant()
: name_("")
{
value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET;
}
explicit ParameterVariant(const std::string & name, const bool bool_value)
: name_(name)
{
value_.bool_value = bool_value;
value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
}
explicit ParameterVariant(const std::string & name, const int int_value)
: name_(name)
{
value_.integer_value = int_value;
value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER;
}
explicit ParameterVariant(const std::string & name, const int64_t int_value)
: name_(name)
{
value_.integer_value = int_value;
value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER;
}
explicit ParameterVariant(const std::string & name, const float double_value)
: name_(name)
{
value_.double_value = double_value;
value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
}
explicit ParameterVariant(const std::string & name, const double double_value)
: name_(name)
{
value_.double_value = double_value;
value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
}
explicit ParameterVariant(const std::string & name, const std::string & string_value)
: name_(name)
{
value_.string_value = string_value;
value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING;
}
explicit ParameterVariant(const std::string & name, const char * string_value)
: ParameterVariant(name, std::string(string_value)) {}
explicit ParameterVariant(const std::string & name, const std::vector<uint8_t> & bytes_value)
: name_(name)
{
value_.bytes_value = bytes_value;
value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BYTES;
}
RCLCPP_PUBLIC
ParameterVariant();
RCLCPP_PUBLIC
explicit ParameterVariant(const std::string & name, const bool bool_value);
RCLCPP_PUBLIC
explicit ParameterVariant(const std::string & name, const int int_value);
RCLCPP_PUBLIC
explicit ParameterVariant(const std::string & name, const int64_t int_value);
RCLCPP_PUBLIC
explicit ParameterVariant(const std::string & name, const float double_value);
RCLCPP_PUBLIC
explicit ParameterVariant(const std::string & name, const double double_value);
RCLCPP_PUBLIC
explicit ParameterVariant(const std::string & name, const std::string & string_value);
RCLCPP_PUBLIC
explicit ParameterVariant(const std::string & name, const char * string_value);
RCLCPP_PUBLIC
explicit ParameterVariant(const std::string & name, const std::vector<uint8_t> & bytes_value);
inline ParameterType get_type() const {return static_cast<ParameterType>(value_.type); }
RCLCPP_PUBLIC
ParameterType
get_type() const;
inline std::string get_type_name() const
{
switch (get_type()) {
case rclcpp::parameter::ParameterType::PARAMETER_BOOL:
return "bool";
case rclcpp::parameter::ParameterType::PARAMETER_INTEGER:
return "integer";
case rclcpp::parameter::ParameterType::PARAMETER_DOUBLE:
return "double";
case rclcpp::parameter::ParameterType::PARAMETER_STRING:
return "string";
case rclcpp::parameter::ParameterType::PARAMETER_BYTES:
return "bytes";
case rclcpp::parameter::ParameterType::PARAMETER_NOT_SET:
return "not set";
default:
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
throw std::runtime_error(
"Unexpected type from ParameterVariant: " + std::to_string(get_type()));
// *INDENT-ON*
}
}
RCLCPP_PUBLIC
std::string
get_type_name() const;
inline std::string get_name() const & {return name_; }
RCLCPP_PUBLIC
const std::string &
get_name() const;
inline rcl_interfaces::msg::ParameterValue get_parameter_value() const
{
return value_;
}
RCLCPP_PUBLIC
rcl_interfaces::msg::ParameterValue
get_parameter_value() const;
template<ParameterType type>
typename std::enable_if<type == ParameterType::PARAMETER_INTEGER, int64_t>::type
@ -179,86 +134,37 @@ public:
return value_.bytes_value;
}
int64_t as_int() const {return get_value<ParameterType::PARAMETER_INTEGER>(); }
RCLCPP_PUBLIC
int64_t
as_int() const;
double as_double() const {return get_value<ParameterType::PARAMETER_DOUBLE>(); }
RCLCPP_PUBLIC
double
as_double() const;
const std::string & as_string() const {return get_value<ParameterType::PARAMETER_STRING>(); }
RCLCPP_PUBLIC
const std::string &
as_string() const;
bool as_bool() const {return get_value<ParameterType::PARAMETER_BOOL>(); }
RCLCPP_PUBLIC
bool
as_bool() const;
const std::vector<uint8_t> & as_bytes() const
{
return get_value<ParameterType::PARAMETER_BYTES>();
}
RCLCPP_PUBLIC
const std::vector<uint8_t> &
as_bytes() const;
static ParameterVariant from_parameter(const rcl_interfaces::msg::Parameter & parameter)
{
switch (parameter.value.type) {
case PARAMETER_BOOL:
return ParameterVariant(parameter.name, parameter.value.bool_value);
case PARAMETER_INTEGER:
return ParameterVariant(parameter.name, parameter.value.integer_value);
case PARAMETER_DOUBLE:
return ParameterVariant(parameter.name, parameter.value.double_value);
case PARAMETER_STRING:
return ParameterVariant(parameter.name, parameter.value.string_value);
case PARAMETER_BYTES:
return ParameterVariant(parameter.name, parameter.value.bytes_value);
case PARAMETER_NOT_SET:
throw std::runtime_error("Type from ParameterValue is not set");
default:
// TODO(wjwwood): use custom exception
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
throw std::runtime_error(
"Unexpected type from ParameterVariant: " + std::to_string(parameter.value.type));
// *INDENT-ON*
}
}
RCLCPP_PUBLIC
static ParameterVariant
from_parameter(const rcl_interfaces::msg::Parameter & parameter);
rcl_interfaces::msg::Parameter to_parameter()
{
rcl_interfaces::msg::Parameter parameter;
parameter.name = name_;
parameter.value = value_;
return parameter;
}
RCLCPP_PUBLIC
rcl_interfaces::msg::Parameter
to_parameter();
std::string value_to_string() const
{
switch (get_type()) {
case rclcpp::parameter::ParameterType::PARAMETER_BOOL:
return as_bool() ? "true" : "false";
case rclcpp::parameter::ParameterType::PARAMETER_INTEGER:
return std::to_string(as_int());
case rclcpp::parameter::ParameterType::PARAMETER_DOUBLE:
return std::to_string(as_double());
case rclcpp::parameter::ParameterType::PARAMETER_STRING:
return as_string();
case rclcpp::parameter::ParameterType::PARAMETER_BYTES:
{
std::stringstream bytes;
bool first_byte = true;
bytes << "[" << std::hex;
for (auto & byte : as_bytes()) {
bytes << "0x" << byte;
if (!first_byte) {
bytes << ", ";
} else {
first_byte = false;
}
}
return bytes.str();
}
case rclcpp::parameter::ParameterType::PARAMETER_NOT_SET:
return "not set";
default:
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
throw std::runtime_error(
"Unexpected type from ParameterVariant: " + std::to_string(get_type()));
// *INDENT-ON*
}
}
RCLCPP_PUBLIC
std::string
value_to_string() const;
private:
std::string name_;
@ -267,71 +173,34 @@ private:
/* Return a json encoded version of the parameter intended for a dict. */
std::string _to_json_dict_entry(const ParameterVariant & param)
{
std::stringstream ss;
ss << "\"" << param.get_name() << "\": ";
ss << "{\"type\": \"" << param.get_type_name() << "\", ";
ss << "\"value\": \"" << param.value_to_string() << "\"}";
return ss.str();
}
RCLCPP_PUBLIC
std::string
_to_json_dict_entry(const ParameterVariant & param);
RCLCPP_PUBLIC
std::ostream &
operator<<(std::ostream & os, const rclcpp::parameter::ParameterVariant & pv);
} /* namespace parameter */
RCLCPP_PUBLIC
std::ostream &
operator<<(std::ostream & os, const std::vector<ParameterVariant> & parameters);
} /* namespace rclcpp */
} // namespace parameter
} // namespace rclcpp
namespace std
{
/* Return a json encoded version of the parameter intended for a list. */
inline std::string to_string(const rclcpp::parameter::ParameterVariant & param)
{
std::stringstream ss;
ss << "{\"name\": \"" << param.get_name() << "\", ";
ss << "\"type\": \"" << param.get_type_name() << "\", ";
ss << "\"value\": \"" << param.value_to_string() << "\"}";
return ss.str();
}
RCLCPP_PUBLIC
std::string
to_string(const rclcpp::parameter::ParameterVariant & param);
/* Return a json encoded version of a vector of parameters, as a string*/
inline std::string to_string(const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
{
std::stringstream ss;
ss << "{";
bool first = true;
for (const auto & pv : parameters) {
if (first == false) {
ss << ", ";
} else {
first = false;
}
ss << rclcpp::parameter::_to_json_dict_entry(pv);
}
ss << "}";
return ss.str();
}
RCLCPP_PUBLIC
std::string
to_string(const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
} /* namespace std */
namespace rclcpp
{
namespace parameter
{
std::ostream & operator<<(std::ostream & os, const rclcpp::parameter::ParameterVariant & pv)
{
os << std::to_string(pv);
return os;
}
std::ostream & operator<<(std::ostream & os, const std::vector<ParameterVariant> & parameters)
{
os << std::to_string(parameters);
return os;
}
} /* namespace parameter */
} /* namespace rclcpp */
} // namespace std
#endif /* RCLCPP_RCLCPP_PARAMETER_HPP_ */

300
rclcpp/include/rclcpp/parameter_client.hpp
View file

@ -16,216 +16,80 @@
#define RCLCPP_RCLCPP_PARAMETER_CLIENT_HPP_
#include <string>
#include <vector>
#include <rmw/rmw.h>
#include <rclcpp/executors.hpp>
#include <rclcpp/macros.hpp>
#include <rclcpp/node.hpp>
#include <rclcpp/parameter.hpp>
#include <rcl_interfaces/msg/parameter.hpp>
#include <rcl_interfaces/msg/parameter_event.hpp>
#include <rcl_interfaces/msg/parameter_value.hpp>
#include <rcl_interfaces/srv/describe_parameters.hpp>
#include <rcl_interfaces/srv/get_parameters.hpp>
#include <rcl_interfaces/srv/get_parameter_types.hpp>
#include <rcl_interfaces/srv/list_parameters.hpp>
#include <rcl_interfaces/srv/set_parameters.hpp>
#include <rcl_interfaces/srv/set_parameters_atomically.hpp>
#include "rcl_interfaces/msg/parameter.hpp"
#include "rcl_interfaces/msg/parameter_event.hpp"
#include "rcl_interfaces/msg/parameter_value.hpp"
#include "rcl_interfaces/srv/describe_parameters.hpp"
#include "rcl_interfaces/srv/get_parameter_types.hpp"
#include "rcl_interfaces/srv/get_parameters.hpp"
#include "rcl_interfaces/srv/list_parameters.hpp"
#include "rcl_interfaces/srv/set_parameters.hpp"
#include "rcl_interfaces/srv/set_parameters_atomically.hpp"
#include "rclcpp/executors.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/parameter.hpp"
#include "rclcpp/type_support_decl.hpp"
#include "rclcpp/visibility_control.hpp"
#include "rmw/rmw.h"
namespace rclcpp
{
namespace parameter_client
{
class AsyncParametersClient
{
public:
RCLCPP_SMART_PTR_DEFINITIONS(AsyncParametersClient);
AsyncParametersClient(const rclcpp::node::Node::SharedPtr node,
const std::string & remote_node_name = "")
: node_(node)
{
if (remote_node_name != "") {
remote_node_name_ = remote_node_name;
} else {
remote_node_name_ = node_->get_name();
}
get_parameters_client_ = node_->create_client<rcl_interfaces::srv::GetParameters>(
remote_node_name_ + "__get_parameters");
get_parameter_types_client_ = node_->create_client<rcl_interfaces::srv::GetParameterTypes>(
remote_node_name_ + "__get_parameter_types");
set_parameters_client_ = node_->create_client<rcl_interfaces::srv::SetParameters>(
remote_node_name_ + "__set_parameters");
list_parameters_client_ = node_->create_client<rcl_interfaces::srv::ListParameters>(
remote_node_name_ + "__list_parameters");
describe_parameters_client_ = node_->create_client<rcl_interfaces::srv::DescribeParameters>(
remote_node_name_ + "__describe_parameters");
}
RCLCPP_PUBLIC
AsyncParametersClient(
const rclcpp::node::Node::SharedPtr node,
const std::string & remote_node_name = "");
RCLCPP_PUBLIC
std::shared_future<std::vector<rclcpp::parameter::ParameterVariant>>
get_parameters(
const std::vector<std::string> & names,
std::function<void(
std::shared_future<std::vector<rclcpp::parameter::ParameterVariant>>)> callback = nullptr)
{
auto promise_result =
std::make_shared<std::promise<std::vector<rclcpp::parameter::ParameterVariant>>>();
auto future_result = promise_result->get_future().share();
auto request = std::make_shared<rcl_interfaces::srv::GetParameters::Request>();
request->names = names;
get_parameters_client_->async_send_request(
request,
[request, promise_result, future_result, &callback](
rclcpp::client::Client<rcl_interfaces::srv::GetParameters>::SharedFuture cb_f) {
std::vector<rclcpp::parameter::ParameterVariant> parameter_variants;
auto & pvalues = cb_f.get()->values;
for (auto & pvalue : pvalues) {
auto i = &pvalue - &pvalues[0];
rcl_interfaces::msg::Parameter parameter;
parameter.name = request->names[i];
parameter.value = pvalue;
parameter_variants.push_back(rclcpp::parameter::ParameterVariant::from_parameter(
parameter));
}
promise_result->set_value(parameter_variants);
if (callback != nullptr) {
callback(future_result);
}
}
);
return future_result;
}
std::function<
void(std::shared_future<std::vector<rclcpp::parameter::ParameterVariant>>)
> callback = nullptr);
RCLCPP_PUBLIC
std::shared_future<std::vector<rclcpp::parameter::ParameterType>>
get_parameter_types(
const std::vector<std::string> & names,
std::function<void(
std::shared_future<std::vector<rclcpp::parameter::ParameterType>>)> callback = nullptr)
{
auto promise_result =
std::make_shared<std::promise<std::vector<rclcpp::parameter::ParameterType>>>();
auto future_result = promise_result->get_future().share();
auto request = std::make_shared<rcl_interfaces::srv::GetParameterTypes::Request>();
request->names = names;
get_parameter_types_client_->async_send_request(
request,
[promise_result, future_result, &callback](
rclcpp::client::Client<rcl_interfaces::srv::GetParameterTypes>::SharedFuture cb_f) {
std::vector<rclcpp::parameter::ParameterType> types;
auto & pts = cb_f.get()->types;
for (auto & pt : pts) {
pts.push_back(static_cast<rclcpp::parameter::ParameterType>(pt));
}
promise_result->set_value(types);
if (callback != nullptr) {
callback(future_result);
}
}
);
return future_result;
}
std::function<
void(std::shared_future<std::vector<rclcpp::parameter::ParameterType>>)
> callback = nullptr);
RCLCPP_PUBLIC
std::shared_future<std::vector<rcl_interfaces::msg::SetParametersResult>>
set_parameters(
const std::vector<rclcpp::parameter::ParameterVariant> & parameters,
std::function<void(std::shared_future<std::vector<rcl_interfaces::msg::SetParametersResult>>)> callback =
nullptr)
{
auto promise_result =
std::make_shared<std::promise<std::vector<rcl_interfaces::msg::SetParametersResult>>>();
auto future_result = promise_result->get_future().share();
auto request = std::make_shared<rcl_interfaces::srv::SetParameters::Request>();
std::transform(parameters.begin(), parameters.end(), std::back_inserter(
request->parameters), [](
rclcpp::parameter::ParameterVariant p) {return p.to_parameter(); });
set_parameters_client_->async_send_request(
request,
[promise_result, future_result, &callback](
rclcpp::client::Client<rcl_interfaces::srv::SetParameters>::SharedFuture cb_f) {
promise_result->set_value(cb_f.get()->results);
if (callback != nullptr) {
callback(future_result);
}
}
);
return future_result;
}
std::function<
void(std::shared_future<std::vector<rcl_interfaces::msg::SetParametersResult>>)
> callback = nullptr);
RCLCPP_PUBLIC
std::shared_future<rcl_interfaces::msg::SetParametersResult>
set_parameters_atomically(
const std::vector<rclcpp::parameter::ParameterVariant> & parameters,
std::function<void(std::shared_future<rcl_interfaces::msg::SetParametersResult>)> callback =
nullptr)
{
auto promise_result =
std::make_shared<std::promise<rcl_interfaces::msg::SetParametersResult>>();
auto future_result = promise_result->get_future().share();
auto request = std::make_shared<rcl_interfaces::srv::SetParametersAtomically::Request>();
std::transform(parameters.begin(), parameters.end(), std::back_inserter(
request->parameters), [](
rclcpp::parameter::ParameterVariant p) {return p.to_parameter(); });
set_parameters_atomically_client_->async_send_request(
request,
[promise_result, future_result, &callback](
rclcpp::client::Client<rcl_interfaces::srv::SetParametersAtomically>::SharedFuture cb_f) {
promise_result->set_value(cb_f.get()->result);
if (callback != nullptr) {
callback(future_result);
}
}
);
return future_result;
}
std::function<
void(std::shared_future<rcl_interfaces::msg::SetParametersResult>)
> callback = nullptr);
RCLCPP_PUBLIC
std::shared_future<rcl_interfaces::msg::ListParametersResult>
list_parameters(
const std::vector<std::string> & prefixes,
uint64_t depth,
std::function<void(std::shared_future<rcl_interfaces::msg::ListParametersResult>)> callback =
nullptr)
{
auto promise_result =
std::make_shared<std::promise<rcl_interfaces::msg::ListParametersResult>>();
auto future_result = promise_result->get_future().share();
auto request = std::make_shared<rcl_interfaces::srv::ListParameters::Request>();
request->prefixes = prefixes;
request->depth = depth;
list_parameters_client_->async_send_request(
request,
[promise_result, future_result, &callback](
rclcpp::client::Client<rcl_interfaces::srv::ListParameters>::SharedFuture cb_f) {
promise_result->set_value(cb_f.get()->result);
if (callback != nullptr) {
callback(future_result);
}
}
);
return future_result;
}
std::function<
void(std::shared_future<rcl_interfaces::msg::ListParametersResult>)
> callback = nullptr);
template<typename FunctorT>
typename rclcpp::subscription::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr
@ -254,90 +118,36 @@ class SyncParametersClient
public:
RCLCPP_SMART_PTR_DEFINITIONS(SyncParametersClient);
RCLCPP_PUBLIC
SyncParametersClient(
rclcpp::node::Node::SharedPtr node)
: node_(node)
{
executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
async_parameters_client_ = std::make_shared<AsyncParametersClient>(node);
}
rclcpp::node::Node::SharedPtr node);
RCLCPP_PUBLIC
SyncParametersClient(
rclcpp::executor::Executor::SharedPtr executor,
rclcpp::node::Node::SharedPtr node)
: executor_(executor), node_(node)
{
async_parameters_client_ = std::make_shared<AsyncParametersClient>(node);
}
rclcpp::node::Node::SharedPtr node);
RCLCPP_PUBLIC
std::vector<rclcpp::parameter::ParameterVariant>
get_parameters(const std::vector<std::string> & parameter_names)
{
auto f = async_parameters_client_->get_parameters(parameter_names);
if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
rclcpp::executors::FutureReturnCode::SUCCESS)
{
return f.get();
}
// Return an empty vector if unsuccessful
return std::vector<rclcpp::parameter::ParameterVariant>();
}
get_parameters(const std::vector<std::string> & parameter_names);
RCLCPP_PUBLIC
std::vector<rclcpp::parameter::ParameterType>
get_parameter_types(const std::vector<std::string> & parameter_names)
{
auto f = async_parameters_client_->get_parameter_types(parameter_names);
if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
rclcpp::executors::FutureReturnCode::SUCCESS)
{
return f.get();
}
return std::vector<rclcpp::parameter::ParameterType>();
}
get_parameter_types(const std::vector<std::string> & parameter_names);
RCLCPP_PUBLIC
std::vector<rcl_interfaces::msg::SetParametersResult>
set_parameters(const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
{
auto f = async_parameters_client_->set_parameters(parameters);
if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
rclcpp::executors::FutureReturnCode::SUCCESS)
{
return f.get();
}
return std::vector<rcl_interfaces::msg::SetParametersResult>();
}
set_parameters(const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
RCLCPP_PUBLIC
rcl_interfaces::msg::SetParametersResult
set_parameters_atomically(const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
{
auto f = async_parameters_client_->set_parameters_atomically(parameters);
if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
rclcpp::executors::FutureReturnCode::SUCCESS)
{
return f.get();
}
throw std::runtime_error("Unable to get result of set parameters service call.");
}
set_parameters_atomically(const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
RCLCPP_PUBLIC
rcl_interfaces::msg::ListParametersResult
list_parameters(
const std::vector<std::string> & parameter_prefixes,
uint64_t depth)
{
auto f = async_parameters_client_->list_parameters(parameter_prefixes, depth);
if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
rclcpp::executors::FutureReturnCode::SUCCESS)
{
return f.get();
}
throw std::runtime_error("Unable to get result of list parameters service call.");
}
uint64_t depth);
template<typename FunctorT>
typename rclcpp::subscription::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr

140
rclcpp/include/rclcpp/parameter_service.hpp
View file

@ -17,19 +17,18 @@
#include <string>
#include <rmw/rmw.h>
#include <rclcpp/executors.hpp>
#include <rclcpp/macros.hpp>
#include <rclcpp/node.hpp>
#include <rclcpp/parameter.hpp>
#include <rcl_interfaces/srv/describe_parameters.hpp>
#include <rcl_interfaces/srv/get_parameters.hpp>
#include <rcl_interfaces/srv/get_parameter_types.hpp>
#include <rcl_interfaces/srv/list_parameters.hpp>
#include <rcl_interfaces/srv/set_parameters.hpp>
#include <rcl_interfaces/srv/set_parameters_atomically.hpp>
#include "rcl_interfaces/srv/describe_parameters.hpp"
#include "rcl_interfaces/srv/get_parameter_types.hpp"
#include "rcl_interfaces/srv/get_parameters.hpp"
#include "rcl_interfaces/srv/list_parameters.hpp"
#include "rcl_interfaces/srv/set_parameters.hpp"
#include "rcl_interfaces/srv/set_parameters_atomically.hpp"
#include "rclcpp/executors.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/parameter.hpp"
#include "rclcpp/visibility_control.hpp"
#include "rmw/rmw.h"
namespace rclcpp
{
@ -43,119 +42,8 @@ class ParameterService
public:
RCLCPP_SMART_PTR_DEFINITIONS(ParameterService);
ParameterService(const rclcpp::node::Node::SharedPtr node)
: node_(node)
{
std::weak_ptr<rclcpp::node::Node> captured_node = node_;
get_parameters_service_ = node_->create_service<rcl_interfaces::srv::GetParameters>(
node_->get_name() + "__get_parameters", [captured_node](
const std::shared_ptr<rmw_request_id_t>,
const std::shared_ptr<rcl_interfaces::srv::GetParameters::Request> request,
std::shared_ptr<rcl_interfaces::srv::GetParameters::Response> response)
{
auto node = captured_node.lock();
if (!node) {
return;
}
auto values = node->get_parameters(request->names);
for (auto & pvariant : values) {
response->values.push_back(pvariant.get_parameter_value());
}
}
);
get_parameter_types_service_ = node_->create_service<rcl_interfaces::srv::GetParameterTypes>(
node_->get_name() + "__get_parameter_types", [captured_node](
const std::shared_ptr<rmw_request_id_t>,
const std::shared_ptr<rcl_interfaces::srv::GetParameterTypes::Request> request,
std::shared_ptr<rcl_interfaces::srv::GetParameterTypes::Response> response)
{
auto node = captured_node.lock();
if (!node) {
return;
}
auto types = node->get_parameter_types(request->names);
std::transform(types.cbegin(), types.cend(),
std::back_inserter(response->types), [](const uint8_t & type) {
return static_cast<rclcpp::parameter::ParameterType>(type);
});
}
);
set_parameters_service_ = node_->create_service<rcl_interfaces::srv::SetParameters>(
node_->get_name() + "__set_parameters", [captured_node](
const std::shared_ptr<rmw_request_id_t>,
const std::shared_ptr<rcl_interfaces::srv::SetParameters::Request> request,
std::shared_ptr<rcl_interfaces::srv::SetParameters::Response> response)
{
auto node = captured_node.lock();
if (!node) {
return;
}
std::vector<rclcpp::parameter::ParameterVariant> pvariants;
for (auto & p : request->parameters) {
pvariants.push_back(rclcpp::parameter::ParameterVariant::from_parameter(p));
}
auto results = node->set_parameters(pvariants);
response->results = results;
}
);
set_parameters_atomically_service_ =
node_->create_service<rcl_interfaces::srv::SetParametersAtomically>(
node_->get_name() + "__set_parameters_atomically", [captured_node](
const std::shared_ptr<rmw_request_id_t>,
const std::shared_ptr<rcl_interfaces::srv::SetParametersAtomically::Request> request,
std::shared_ptr<rcl_interfaces::srv::SetParametersAtomically::Response> response)
{
auto node = captured_node.lock();
if (!node) {
return;
}
std::vector<rclcpp::parameter::ParameterVariant> pvariants;
std::transform(request->parameters.cbegin(), request->parameters.cend(),
std::back_inserter(pvariants),
[](const rcl_interfaces::msg::Parameter & p) {
return rclcpp::parameter::ParameterVariant::
from_parameter(p);
});
auto result = node->set_parameters_atomically(pvariants);
response->result = result;
}
);
describe_parameters_service_ = node_->create_service<rcl_interfaces::srv::DescribeParameters>(
node_->get_name() + "__describe_parameters", [captured_node](
const std::shared_ptr<rmw_request_id_t>,
const std::shared_ptr<rcl_interfaces::srv::DescribeParameters::Request> request,
std::shared_ptr<rcl_interfaces::srv::DescribeParameters::Response> response)
{
auto node = captured_node.lock();
if (!node) {
return;
}
auto descriptors = node->describe_parameters(request->names);
response->descriptors = descriptors;
}
);
list_parameters_service_ = node_->create_service<rcl_interfaces::srv::ListParameters>(
node_->get_name() + "__list_parameters", [captured_node](
const std::shared_ptr<rmw_request_id_t>,
const std::shared_ptr<rcl_interfaces::srv::ListParameters::Request> request,
std::shared_ptr<rcl_interfaces::srv::ListParameters::Response> response)
{
auto node = captured_node.lock();
if (!node) {
return;
}
auto result = node->list_parameters(request->prefixes, request->depth);
response->result = result;
}
);
}
RCLCPP_PUBLIC
explicit ParameterService(const rclcpp::node::Node::SharedPtr node);
private:
const rclcpp::node::Node::SharedPtr node_;

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

@ -27,8 +27,10 @@
#include "rcl_interfaces/msg/intra_process_message.hpp"
#include "rmw/impl/cpp/demangle.hpp"
#include "rclcpp/allocator/allocator_common.hpp"
#include "rclcpp/allocator/allocator_deleter.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
@ -55,77 +57,40 @@ public:
* \param[in] topic The topic that this publisher publishes on.
* \param[in] queue_size The maximum number of unpublished messages to queue.
*/
RCLCPP_PUBLIC
PublisherBase(
std::shared_ptr<rmw_node_t> node_handle,
rmw_publisher_t * publisher_handle,
std::string topic,
size_t queue_size)
: node_handle_(node_handle), publisher_handle_(publisher_handle),
intra_process_publisher_handle_(nullptr),
topic_(topic), queue_size_(queue_size),
intra_process_publisher_id_(0), store_intra_process_message_(nullptr)
{
// Life time of this object is tied to the publisher handle.
if (rmw_get_gid_for_publisher(publisher_handle_, &rmw_gid_) != RMW_RET_OK) {
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
throw std::runtime_error(
std::string("failed to get publisher gid: ") + rmw_get_error_string_safe());
// *INDENT-ON*
}
}
size_t queue_size);
/// Default destructor.
virtual ~PublisherBase()
{
if (intra_process_publisher_handle_) {
if (rmw_destroy_publisher(node_handle_.get(), intra_process_publisher_handle_)) {
fprintf(
stderr,
"Error in destruction of intra process rmw publisher handle: %s\n",
rmw_get_error_string_safe());
}
}
if (publisher_handle_) {
if (rmw_destroy_publisher(node_handle_.get(), publisher_handle_) != RMW_RET_OK) {
fprintf(
stderr,
"Error in destruction of rmw publisher handle: %s\n",
rmw_get_error_string_safe());
}
}
}
RCLCPP_PUBLIC
virtual ~PublisherBase();
/// Get the topic that this publisher publishes on.
// \return The topic name.
RCLCPP_PUBLIC
const std::string &
get_topic_name() const
{
return topic_;
}
get_topic_name() const;
/// Get the queue size for this publisher.
// \return The queue size.
RCLCPP_PUBLIC
size_t
get_queue_size() const
{
return queue_size_;
}
get_queue_size() const;
/// Get the global identifier for this publisher (used in rmw and by DDS).
// \return The gid.
RCLCPP_PUBLIC
const rmw_gid_t &
get_gid() const
{
return rmw_gid_;
}
get_gid() const;
/// Get the global identifier for this publisher used by intra-process communication.
// \return The intra-process gid.
RCLCPP_PUBLIC
const rmw_gid_t &
get_intra_process_gid() const
{
return intra_process_rmw_gid_;
}
get_intra_process_gid() const;
/// Compare this publisher to a gid.
/**
@ -133,11 +98,9 @@ public:
* \param[in] gid Reference to a gid.
* \return True if the publisher's gid matches the input.
*/
RCLCPP_PUBLIC
bool
operator==(const rmw_gid_t & gid) const
{
return *this == &gid;
}
operator==(const rmw_gid_t & gid) const;
/// Compare this publisher to a pointer gid.
/**
@ -145,47 +108,19 @@ public:
* \param[in] gid A pointer to a gid.
* \return True if this publisher's gid matches the input.
*/
RCLCPP_PUBLIC
bool
operator==(const rmw_gid_t * gid) const
{
bool result = false;
auto ret = rmw_compare_gids_equal(gid, &this->get_gid(), &result);
if (ret != RMW_RET_OK) {
throw std::runtime_error(
std::string("failed to compare gids: ") + rmw_get_error_string_safe());
}
if (!result) {
ret = rmw_compare_gids_equal(gid, &this->get_intra_process_gid(), &result);
if (ret != RMW_RET_OK) {
throw std::runtime_error(
std::string("failed to compare gids: ") + rmw_get_error_string_safe());
}
}
return result;
}
operator==(const rmw_gid_t * gid) const;
typedef std::function<uint64_t(uint64_t, void *, const std::type_info &)> StoreMessageCallbackT;
protected:
RCLCPP_PUBLIC
void
setup_intra_process(
uint64_t intra_process_publisher_id,
StoreMessageCallbackT callback,
rmw_publisher_t * intra_process_publisher_handle)
{
intra_process_publisher_id_ = intra_process_publisher_id;
store_intra_process_message_ = callback;
intra_process_publisher_handle_ = intra_process_publisher_handle;
// Life time of this object is tied to the publisher handle.
auto ret = rmw_get_gid_for_publisher(intra_process_publisher_handle_, &intra_process_rmw_gid_);
if (ret != RMW_RET_OK) {
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
throw std::runtime_error(
std::string("failed to create intra process publisher gid: ") +
rmw_get_error_string_safe());
// *INDENT-ON*
}
}
rmw_publisher_t * intra_process_publisher_handle);
std::shared_ptr<rmw_node_t> node_handle_;

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

@ -21,6 +21,7 @@
#include <rclcpp/macros.hpp>
#include <rclcpp/utilities.hpp>
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
@ -47,11 +48,11 @@ class GenericRate : public RateBase
public:
RCLCPP_SMART_PTR_DEFINITIONS(GenericRate);
GenericRate(double rate)
explicit GenericRate(double rate)
: GenericRate<Clock>(
duration_cast<nanoseconds>(duration<double>(1.0 / rate)))
{}
GenericRate(std::chrono::nanoseconds period)
explicit GenericRate(std::chrono::nanoseconds period)
: period_(period), last_interval_(Clock::now())
{}

46
rclcpp/include/rclcpp/rclcpp.hpp
View file

@ -25,9 +25,8 @@
#include <rclcpp/executors.hpp>
#include <rclcpp/rate.hpp>
#include <rclcpp/utilities.hpp>
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
// NOLINTNEXTLINE(runtime/int)
const std::chrono::seconds operator"" _s(unsigned long long s)
{
@ -39,32 +38,31 @@ const std::chrono::nanoseconds operator"" _s(long double s)
std::chrono::duration<long double>(s));
}
const std::chrono::nanoseconds
// NOLINTNEXTLINE(runtime/int)
operator"" _ms(unsigned long long ms)
const std::chrono::nanoseconds operator"" _ms(unsigned long long ms)
{
return std::chrono::milliseconds(ms);
}
const std::chrono::nanoseconds
operator"" _ms(long double ms)
const std::chrono::nanoseconds operator"" _ms(long double ms)
{
return std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<long double, std::milli>(ms));
}
const std::chrono::nanoseconds
// NOLINTNEXTLINE(runtime/int)
operator"" _ns(unsigned long long ns)
const std::chrono::nanoseconds operator"" _ns(unsigned long long ns)
{
return std::chrono::nanoseconds(ns);
}
const std::chrono::nanoseconds
operator"" _ns(long double ns)
const std::chrono::nanoseconds operator"" _ns(long double ns)
{
return std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<long double, std::nano>(ns));
}
namespace rclcpp
{
// Namespace escalations.
// For example, this next line escalates type "rclcpp:node::Node" to "rclcpp::Node"
using rclcpp::node::Node;
@ -82,34 +80,6 @@ using rclcpp::utilities::shutdown;
using rclcpp::utilities::init;
using rclcpp::utilities::sleep_for;
/// Create a default single-threaded executor and execute any immediately available work.
// \param[in] node_ptr Shared pointer to the node to spin.
void spin_some(Node::SharedPtr node_ptr)
{
rclcpp::executors::SingleThreadedExecutor executor;
executor.spin_node_some(node_ptr);
}
/// Create a default single-threaded executor and spin the specified node.
// \param[in] node_ptr Shared pointer to the node to spin.
void spin(Node::SharedPtr node_ptr)
{
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(node_ptr);
executor.spin();
}
template<typename FutureT, typename TimeT = std::milli>
rclcpp::executors::FutureReturnCode
spin_until_future_complete(
Node::SharedPtr node_ptr, std::shared_future<FutureT> & future,
std::chrono::duration<int64_t, TimeT> timeout = std::chrono::duration<int64_t, TimeT>(-1))
{
rclcpp::executors::SingleThreadedExecutor executor;
return rclcpp::executors::spin_node_until_future_complete<FutureT>(
executor, node_ptr, future, timeout);
}
} /* namespace rclcpp */
#endif /* RCLCPP_RCLCPP_RCLCPP_HPP_ */

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

@ -26,6 +26,7 @@
#include <rclcpp/macros.hpp>
#include <rclcpp/any_service_callback.hpp>
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
@ -39,34 +40,22 @@ class ServiceBase
public:
RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ServiceBase);
RCLCPP_PUBLIC
ServiceBase(
std::shared_ptr<rmw_node_t> node_handle,
rmw_service_t * service_handle,
const std::string service_name)
: node_handle_(node_handle), service_handle_(service_handle), service_name_(service_name)
{}
const std::string service_name);
virtual ~ServiceBase()
{
if (service_handle_) {
if (rmw_destroy_service(service_handle_) != RMW_RET_OK) {
std::stringstream ss;
ss << "Error in destruction of rmw service_handle_ handle: " <<
rmw_get_error_string_safe() << '\n';
(std::cerr << ss.str()).flush();
}
}
}
RCLCPP_PUBLIC
virtual ~ServiceBase();
std::string get_service_name()
{
return this->service_name_;
}
RCLCPP_PUBLIC
std::string
get_service_name();
const rmw_service_t * get_service_handle()
{
return this->service_handle_;
}
RCLCPP_PUBLIC
const rmw_service_t *
get_service_handle();
virtual std::shared_ptr<void> create_request() = 0;
virtual std::shared_ptr<void> create_request_header() = 0;

1
rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
View file

@ -21,6 +21,7 @@
#include "rclcpp/allocator/allocator_common.hpp"
#include "rclcpp/memory_strategy.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{

11
rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp
View file

@ -17,6 +17,7 @@
#include <rclcpp/macros.hpp>
#include <rclcpp/message_memory_strategy.hpp>
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
@ -32,9 +33,13 @@ namespace message_pool_memory_strategy
* The size of the message pool should be at least the largest number of concurrent accesses to
* the subscription (usually the number of threads).
*/
template<typename MessageT, size_t Size,
typename std::enable_if<rosidl_generator_traits::has_fixed_size<MessageT>::value>::type * =
nullptr>
template<
typename MessageT,
size_t Size,
typename std::enable_if<
rosidl_generator_traits::has_fixed_size<MessageT>::value
>::type * = nullptr
>
class MessagePoolMemoryStrategy
: public message_memory_strategy::MessageMemoryStrategy<MessageT>
{

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

@ -29,6 +29,7 @@
#include "rclcpp/macros.hpp"
#include "rclcpp/message_memory_strategy.hpp"
#include "rclcpp/any_subscription_callback.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
@ -54,75 +55,48 @@ public:
* \param[in] topic_name Name of the topic to subscribe to.
* \param[in] ignore_local_publications True to ignore local publications (unused).
*/
RCLCPP_PUBLIC
SubscriptionBase(
std::shared_ptr<rmw_node_t> node_handle,
rmw_subscription_t * subscription_handle,
const std::string & topic_name,
bool ignore_local_publications)
: intra_process_subscription_handle_(nullptr),
node_handle_(node_handle),
subscription_handle_(subscription_handle),
topic_name_(topic_name),
ignore_local_publications_(ignore_local_publications)
{
// To avoid unused private member warnings.
(void)ignore_local_publications_;
}
bool ignore_local_publications);
/// Default destructor.
virtual ~SubscriptionBase()
{
if (subscription_handle_) {
if (rmw_destroy_subscription(node_handle_.get(), subscription_handle_) != RMW_RET_OK) {
std::stringstream ss;
ss << "Error in destruction of rmw subscription handle: " <<
rmw_get_error_string_safe() << '\n';
(std::cerr << ss.str()).flush();
}
}
if (intra_process_subscription_handle_) {
auto ret = rmw_destroy_subscription(node_handle_.get(), intra_process_subscription_handle_);
if (ret != RMW_RET_OK) {
std::stringstream ss;
ss << "Error in destruction of rmw intra process subscription handle: " <<
rmw_get_error_string_safe() << '\n';
(std::cerr << ss.str()).flush();
}
}
}
RCLCPP_PUBLIC
virtual ~SubscriptionBase();
/// Get the topic that this subscription is subscribed on.
const std::string & get_topic_name() const
{
return this->topic_name_;
}
RCLCPP_PUBLIC
const std::string &
get_topic_name() const;
const rmw_subscription_t * get_subscription_handle() const
{
return subscription_handle_;
}
RCLCPP_PUBLIC
const rmw_subscription_t *
get_subscription_handle() const;
const rmw_subscription_t * get_intra_process_subscription_handle() const
{
return intra_process_subscription_handle_;
}
RCLCPP_PUBLIC
const rmw_subscription_t *
get_intra_process_subscription_handle() const;
/// Borrow a new message.
// \return Shared pointer to the fresh message.
virtual std::shared_ptr<void> create_message() = 0;
virtual std::shared_ptr<void>
create_message() = 0;
/// Check if we need to handle the message, and execute the callback if we do.
/**
* \param[in] message Shared pointer to the message to handle.
* \param[in] message_info Metadata associated with this message.
*/
virtual void handle_message(
std::shared_ptr<void> & message,
const rmw_message_info_t & message_info) = 0;
virtual void
handle_message(std::shared_ptr<void> & message, const rmw_message_info_t & message_info) = 0;
/// Return the message borrowed in create_message.
// \param[in] Shared pointer to the returned message.
virtual void return_message(std::shared_ptr<void> & message) = 0;
virtual void handle_intra_process_message(
virtual void
return_message(std::shared_ptr<void> & message) = 0;
virtual void
handle_intra_process_message(
rcl_interfaces::msg::IntraProcessMessage & ipm,
const rmw_message_info_t & message_info) = 0;
@ -140,7 +114,7 @@ private:
bool ignore_local_publications_;
};
using namespace any_subscription_callback;
using any_subscription_callback::AnySubscriptionCallback;
/// Subscription implementation, templated on the type of message this subscription receives.
template<typename MessageT, typename Alloc = std::allocator<void>>
@ -172,8 +146,8 @@ public:
const std::string & topic_name,
bool ignore_local_publications,
AnySubscriptionCallback<MessageT, Alloc> callback,
typename message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr memory_strategy =
message_memory_strategy::MessageMemoryStrategy<MessageT,
typename message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
memory_strategy = message_memory_strategy::MessageMemoryStrategy<MessageT,
Alloc>::create_default())
: SubscriptionBase(node_handle, subscription_handle, topic_name, ignore_local_publications),
any_callback_(callback),

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

@ -27,6 +27,7 @@
#include <rclcpp/macros.hpp>
#include <rclcpp/rate.hpp>
#include <rclcpp/utilities.hpp>
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
@ -42,32 +43,23 @@ class TimerBase
public:
RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(TimerBase);
TimerBase(std::chrono::nanoseconds period, CallbackType callback)
: period_(period),
callback_(callback),
canceled_(false)
{
}
RCLCPP_PUBLIC
TimerBase(std::chrono::nanoseconds period, CallbackType callback);
virtual ~TimerBase()
{
}
RCLCPP_PUBLIC
virtual ~TimerBase();
RCLCPP_PUBLIC
void
cancel()
{
this->canceled_ = true;
}
cancel();
void execute_callback() const
{
callback_();
}
RCLCPP_PUBLIC
void
execute_callback() const;
const CallbackType & get_callback() const
{
return callback_;
}
RCLCPP_PUBLIC
const CallbackType &
get_callback() const;
/// Check how long the timer has until its next scheduled callback.
// \return A std::chrono::duration representing the relative time until the next callback.

62
rclcpp/include/rclcpp/type_support_decl.hpp Normal file
View file

@ -0,0 +1,62 @@
// 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__TYPE_SUPPORT_DECL_HPP_
#define RCLCPP__TYPE_SUPPORT_DECL_HPP_
#include "rosidl_generator_cpp/message_type_support_decl.hpp"
#include "rosidl_generator_cpp/service_type_support_decl.hpp"
namespace rclcpp
{
namespace type_support
{
const rosidl_message_type_support_t *
get_intra_process_message_msg_type_support();
const rosidl_message_type_support_t *
get_parameter_event_msg_type_support();
const rosidl_message_type_support_t *
get_set_parameters_result_msg_type_support();
const rosidl_message_type_support_t *
get_parameter_descriptor_msg_type_support();
const rosidl_message_type_support_t *
get_list_parameters_result_msg_type_support();
const rosidl_service_type_support_t *
get_get_parameters_srv_type_support();
const rosidl_service_type_support_t *
get_get_parameter_types_srv_type_support();
const rosidl_service_type_support_t *
get_set_parameters_srv_type_support();
const rosidl_service_type_support_t *
get_list_parameters_srv_type_support();
const rosidl_service_type_support_t *
get_describe_parameters_srv_type_support();
const rosidl_service_type_support_t *
get_set_parameters_atomically_srv_type_support();
} // namespace type_support
} // namespace rclcpp
#endif // RCLCPP__TYPE_SUPPORT_DECL_HPP_

123
rclcpp/include/rclcpp/type_support_def.hpp Normal file
View file

@ -0,0 +1,123 @@
// 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__TYPE_SUPPORT_DEF_HPP_
#define RCLCPP__TYPE_SUPPORT_DEF_HPP_
#include "rclcpp/type_support_decl.hpp"
#include "rclcpp/visibility_control.hpp"
#include "rcl_interfaces/msg/intra_process_message.hpp"
#include "rcl_interfaces/msg/list_parameters_result.hpp"
#include "rcl_interfaces/msg/parameter_descriptor.hpp"
#include "rcl_interfaces/msg/parameter_event.hpp"
#include "rcl_interfaces/msg/set_parameters_result.hpp"
#include "rcl_interfaces/srv/describe_parameters.hpp"
#include "rcl_interfaces/srv/get_parameter_types.hpp"
#include "rcl_interfaces/srv/get_parameters.hpp"
#include "rcl_interfaces/srv/list_parameters.hpp"
#include "rcl_interfaces/srv/set_parameters.hpp"
#include "rcl_interfaces/srv/set_parameters_atomically.hpp"
#include "rosidl_generator_cpp/message_type_support.hpp"
#include "rosidl_generator_cpp/service_type_support.hpp"
const rosidl_message_type_support_t *
rclcpp::type_support::get_intra_process_message_msg_type_support()
{
return rosidl_generator_cpp::get_message_type_support_handle<
rcl_interfaces::msg::IntraProcessMessage
>();
}
const rosidl_message_type_support_t *
rclcpp::type_support::get_parameter_event_msg_type_support()
{
return rosidl_generator_cpp::get_message_type_support_handle<
rcl_interfaces::msg::ParameterEvent
>();
}
const rosidl_message_type_support_t *
rclcpp::type_support::get_set_parameters_result_msg_type_support()
{
return rosidl_generator_cpp::get_message_type_support_handle<
rcl_interfaces::msg::SetParametersResult
>();
}
const rosidl_message_type_support_t *
rclcpp::type_support::get_parameter_descriptor_msg_type_support()
{
return rosidl_generator_cpp::get_message_type_support_handle<
rcl_interfaces::msg::ParameterDescriptor
>();
}
const rosidl_message_type_support_t *
rclcpp::type_support::get_list_parameters_result_msg_type_support()
{
return rosidl_generator_cpp::get_message_type_support_handle<
rcl_interfaces::msg::ListParametersResult
>();
}
const rosidl_service_type_support_t *
rclcpp::type_support::get_get_parameters_srv_type_support()
{
return rosidl_generator_cpp::get_service_type_support_handle<
rcl_interfaces::srv::GetParameters
>();
}
const rosidl_service_type_support_t *
rclcpp::type_support::get_get_parameter_types_srv_type_support()
{
return rosidl_generator_cpp::get_service_type_support_handle<
rcl_interfaces::srv::GetParameterTypes
>();
}
const rosidl_service_type_support_t *
rclcpp::type_support::get_set_parameters_srv_type_support()
{
return rosidl_generator_cpp::get_service_type_support_handle<
rcl_interfaces::srv::SetParameters
>();
}
const rosidl_service_type_support_t *
rclcpp::type_support::get_list_parameters_srv_type_support()
{
return rosidl_generator_cpp::get_service_type_support_handle<
rcl_interfaces::srv::ListParameters
>();
}
const rosidl_service_type_support_t *
rclcpp::type_support::get_describe_parameters_srv_type_support()
{
return rosidl_generator_cpp::get_service_type_support_handle<
rcl_interfaces::srv::DescribeParameters
>();
}
const rosidl_service_type_support_t *
rclcpp::type_support::get_set_parameters_atomically_srv_type_support()
{
return rosidl_generator_cpp::get_service_type_support_handle<
rcl_interfaces::srv::SetParametersAtomically
>();
}
#endif // RCLCPP__TYPE_SUPPORT_DEF_HPP_

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

@ -15,97 +15,14 @@
#ifndef RCLCPP_RCLCPP_UTILITIES_HPP_
#define RCLCPP_RCLCPP_UTILITIES_HPP_
// TODO(wjwwood): remove
#include <iostream>
#include <cerrno>
#include <chrono>
#include <condition_variable>
#include <csignal>
#include <cstring>
#include <mutex>
#include <string.h>
#include <thread>
#include <rmw/error_handling.h>
#include <rmw/macros.h>
#include <rmw/rmw.h>
// Determine if sigaction is available
#if __APPLE__ || _POSIX_C_SOURCE >= 1 || _XOPEN_SOURCE || _POSIX_SOURCE
#define HAS_SIGACTION
#endif
namespace
{
/// Represent the status of the global interrupt signal.
volatile sig_atomic_t g_signal_status = 0;
/// Guard condition for interrupting the rmw implementation when the global interrupt signal fired.
rmw_guard_condition_t * g_sigint_guard_cond_handle = \
rmw_create_guard_condition();
/// Condition variable for timed sleep (see sleep_for).
std::condition_variable g_interrupt_condition_variable;
std::atomic<bool> g_is_interrupted(false);
/// Mutex for protecting the global condition variable.
std::mutex g_interrupt_mutex;
#ifdef HAS_SIGACTION
struct sigaction old_action;
#else
void (* old_signal_handler)(int) = 0;
#endif
/// Handle the interrupt signal.
/** When the interrupt signal fires, the signal handler notifies the condition variable to wake up
* and triggers the interrupt guard condition, so that all global threads managed by rclcpp
* are interrupted.
*/
void
#ifdef HAS_SIGACTION
signal_handler(int signal_value, siginfo_t * siginfo, void * context)
#else
signal_handler(int signal_value)
#endif
{
// 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);
}
} else {
// *INDENT-OFF*
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
// *INDENT-ON*
{
old_action.sa_handler(signal_value);
}
}
#else
if (old_signal_handler) {
old_signal_handler(signal_value);
}
#endif
g_signal_status = signal_value;
rmw_ret_t status = rmw_trigger_guard_condition(g_sigint_guard_cond_handle);
if (status != RMW_RET_OK) {
fprintf(stderr,
"[rclcpp::error] failed to trigger guard condition: %s\n", rmw_get_error_string_safe());
}
g_is_interrupted.store(true);
g_interrupt_condition_variable.notify_all();
}
} // namespace
#include "rclcpp/visibility_control.hpp"
#include "rmw/macros.h"
#include "rmw/rmw.h"
namespace rclcpp
{
RMW_THREAD_LOCAL size_t thread_id = 0;
namespace utilities
{
@ -114,113 +31,37 @@ namespace utilities
* \param[in] argc Number of arguments.
* \param[in] argv Argument vector. Will eventually be used for passing options to rclcpp.
*/
RCLCPP_PUBLIC
void
init(int argc, char * argv[])
{
(void)argc;
(void)argv;
g_is_interrupted.store(false);
rmw_ret_t status = rmw_init();
if (status != RMW_RET_OK) {
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
throw std::runtime_error(
std::string("failed to initialize rmw implementation: ") + rmw_get_error_string_safe());
// *INDENT-ON*
}
#ifdef HAS_SIGACTION
struct sigaction action;
memset(&action, 0, sizeof(action));
sigemptyset(&action.sa_mask);
action.sa_sigaction = ::signal_handler;
action.sa_flags = SA_SIGINFO;
ssize_t ret = sigaction(SIGINT, &action, &old_action);
if (ret == -1)
#else
::old_signal_handler = std::signal(SIGINT, ::signal_handler);
if (::old_signal_handler == SIG_ERR)
#endif
{
const size_t error_length = 1024;
char error_string[error_length];
#ifndef _WIN32
auto rc = strerror_r(errno, error_string, error_length);
if (rc) {
// *INDENT-OFF*
throw std::runtime_error(
"Failed to set SIGINT signal handler: (" + std::to_string(errno) +
") unable to retrieve error string");
// *INDENT-ON*
}
#else
strerror_s(error_string, error_length, errno);
#endif
// *INDENT-OFF*
throw std::runtime_error(
std::string("Failed to set SIGINT signal handler: (" + std::to_string(errno) + ")") +
error_string);
// *INDENT-ON*
}
}
init(int argc, char * argv[]);
/// Check rclcpp's status.
// \return True if SIGINT hasn't fired yet, false otherwise.
RCLCPP_PUBLIC
bool
ok()
{
return ::g_signal_status == 0;
}
ok();
/// Notify the signal handler and rmw that rclcpp is shutting down.
RCLCPP_PUBLIC
void
shutdown()
{
g_signal_status = SIGINT;
rmw_ret_t status = rmw_trigger_guard_condition(g_sigint_guard_cond_handle);
if (status != RMW_RET_OK) {
fprintf(stderr,
"[rclcpp::error] failed to trigger guard condition: %s\n", rmw_get_error_string_safe());
}
g_is_interrupted.store(true);
g_interrupt_condition_variable.notify_all();
}
shutdown();
/// Get a handle to the rmw guard condition that manages the signal handler.
RCLCPP_PUBLIC
rmw_guard_condition_t *
get_global_sigint_guard_condition()
{
return ::g_sigint_guard_cond_handle;
}
get_global_sigint_guard_condition();
/// Use the global condition variable to block for the specified amount of time.
/**
* \param[in] nanoseconds A std::chrono::duration representing how long to sleep for.
* \return True if the condition variable did not timeout.
*/
RCLCPP_PUBLIC
bool
sleep_for(const std::chrono::nanoseconds & nanoseconds)
{
// TODO: determine if posix's nanosleep(2) is more efficient here
std::chrono::nanoseconds time_left = nanoseconds;
{
std::unique_lock<std::mutex> lock(::g_interrupt_mutex);
auto start = std::chrono::steady_clock::now();
::g_interrupt_condition_variable.wait_for(lock, nanoseconds);
time_left -= std::chrono::steady_clock::now() - start;
}
if (time_left > std::chrono::nanoseconds::zero() && !g_is_interrupted) {
return sleep_for(time_left);
}
// Return true if the timeout elapsed successfully, otherwise false.
return !g_is_interrupted;
}
sleep_for(const std::chrono::nanoseconds & nanoseconds);
} /* namespace utilities */
} /* namespace rclcpp */
#ifdef HAS_SIGACTION
#undef HAS_SIGACTION
#endif
} // namespace utilities
} // namespace rclcpp
#endif /* RCLCPP_RCLCPP_UTILITIES_HPP_ */

83
rclcpp/include/rclcpp/visibility_control.hpp Normal file
View file

@ -0,0 +1,83 @@
// Copyright 2015 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.
/* This header must be included by all rclcpp headers which declare symbols
* which are defined in the rclcpp library. When not building the rclcpp
* library, i.e. when using the headers in other package's code, the contents
* of this header change the visibility of certain symbols which the rclcpp
* library cannot have, but the consuming code must have inorder to link.
*/
#ifndef RCLCPP__VISIBILITY_CONTROL_HPP_
#define RCLCPP__VISIBILITY_CONTROL_HPP_
#include "rmw/rmw.h"
#if !defined(RCLCPP_BUILDING_LIBRARY)
// The following header is necessary inorder to get the correct rmw
// implementation specific message symbols.
// Any library or executable using librclcpp must include the header.
// However, librclcpp must not include it.
#include "rclcpp/type_support_def.hpp"
// Anonymous namespace to prevent duplicate symbols across compile units.
namespace
{
// This call to an rmw function is used to force the Linux linker to include
// the rmw implementation library in the user's executable.
// If this is not done, then only librclcpp is missing rmw symbols and when
// linking the next library or exectuable the linker will discard the rmw
// implementation shared library as unused.
// On OS X this isn't an issue because linking is done, by default, in a flat
// namespace, so when linking something that uses librclcpp, it will try to
// resolve the rclcpp symbols each time, taking them from the rmw implementation
// when it is available and not discarding it as unused during the link step.
static const char * __rmw_identifier = rmw_get_implementation_identifier();
} // namespace
#endif // !defined(RCLCPP_BUILDING_LIBRARY)
// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
// https://gcc.gnu.org/wiki/Visibility
#if defined _WIN32 || defined __CYGWIN__
#ifdef __GNUC__
#define RCLCPP_EXPORT __attribute__ ((dllexport))
#define RCLCPP_IMPORT __attribute__ ((dllimport))
#else
#define RCLCPP_EXPORT __declspec(dllexport)
#define RCLCPP_IMPORT __declspec(dllimport)
#endif
#ifdef RCLCPP_BUILDING_LIBRARY
#define RCLCPP_PUBLIC RCLCPP_EXPORT
#else
#define RCLCPP_PUBLIC RCLCPP_IMPORT
#endif
#define RCLCPP_PUBLIC_TYPE RCLCPP_PUBLIC
#define RCLCPP_LOCAL
#else
#define RCLCPP_EXPORT __attribute__ ((visibility("default")))
#define RCLCPP_IMPORT
#if __GNUC__ >= 4
#define RCLCPP_PUBLIC __attribute__ ((visibility("default")))
#define RCLCPP_LOCAL __attribute__ ((visibility("hidden")))
#else
#define RCLCPP_PUBLIC
#define RCLCPP_LOCAL
#endif
#define RCLCPP_PUBLIC_TYPE
#endif
#endif // RCLCPP__VISIBILITY_CONTROL_HPP_

4
rclcpp/package.xml
View file

@ -11,7 +11,11 @@
<build_export_depend>rmw</build_export_depend>
<build_depend>rcl_interfaces</build_depend>
<!-- This ensures the rmw impls are built first. -->
<build_depend>rmw_implementation</build_depend>
<build_depend>rosidl_generator_cpp</build_depend>
<build_export_depend>rcl_interfaces</build_export_depend>
<build_export_depend>rosidl_generator_cpp</build_export_depend>
<test_depend>ament_cmake_gtest</test_depend>
<test_depend>ament_lint_auto</test_depend>

43
rclcpp/src/rclcpp/any_executable.cpp
View file

@ -12,37 +12,16 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_RCLCPP_ANY_EXECUTABLE_HPP_
#define RCLCPP_RCLCPP_ANY_EXECUTABLE_HPP_
#include "rclcpp/any_executable.hpp"
#include <memory>
using rclcpp::executor::AnyExecutable;
#include <rclcpp/macros.hpp>
#include <rclcpp/node.hpp>
namespace rclcpp
{
namespace executor
{
struct AnyExecutable
{
RCLCPP_SMART_PTR_DEFINITIONS(AnyExecutable);
AnyExecutable()
: subscription(0), timer(0), callback_group(0), node(0)
{}
// Only one of the following pointers will be set.
rclcpp::subscription::SubscriptionBase::SharedPtr subscription;
rclcpp::subscription::SubscriptionBase::SharedPtr subscription_intra_process;
rclcpp::timer::TimerBase::SharedPtr timer;
rclcpp::service::ServiceBase::SharedPtr service;
rclcpp::client::ClientBase::SharedPtr client;
// These are used to keep the scope on the containing items
rclcpp::callback_group::CallbackGroup::SharedPtr callback_group;
rclcpp::node::Node::SharedPtr node;
};
} /* executor */
} /* rclcpp */
#endif
AnyExecutable::AnyExecutable()
: subscription(nullptr),
subscription_intra_process(nullptr),
timer(nullptr),
service(nullptr),
client(nullptr),
callback_group(nullptr),
node(nullptr)
{}

141
rclcpp/src/rclcpp/callback_group.cpp
View file

@ -1,4 +1,4 @@
// Copyright 2014 Open Source Robotics Foundation, Inc.
// Copyright 2015 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.
@ -12,119 +12,74 @@
// 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 "rclcpp/callback_group.hpp"
#include <atomic>
#include <string>
#include <vector>
#include <rclcpp/subscription.hpp>
#include <rclcpp/timer.hpp>
#include <rclcpp/service.hpp>
#include <rclcpp/client.hpp>
using rclcpp::callback_group::CallbackGroup;
using rclcpp::callback_group::CallbackGroupType;
namespace rclcpp
CallbackGroup::CallbackGroup(CallbackGroupType group_type)
: type_(group_type), can_be_taken_from_(true)
{}
const std::vector<rclcpp::subscription::SubscriptionBase::WeakPtr> &
CallbackGroup::get_subscription_ptrs() const
{
// Forward declarations for friend statement in class CallbackGroup
namespace node
{
class Node;
} // namespace node
namespace callback_group
{
enum class CallbackGroupType
{
MutuallyExclusive,
Reentrant
};
class CallbackGroup
{
friend class rclcpp::node::Node;
public:
RCLCPP_SMART_PTR_DEFINITIONS(CallbackGroup);
CallbackGroup(CallbackGroupType group_type)
: type_(group_type), can_be_taken_from_(true)
{}
const std::vector<subscription::SubscriptionBase::WeakPtr> &
get_subscription_ptrs() const
{
return subscription_ptrs_;
}
}
const std::vector<timer::TimerBase::WeakPtr> &
get_timer_ptrs() const
{
const std::vector<rclcpp::timer::TimerBase::WeakPtr> &
CallbackGroup::get_timer_ptrs() const
{
return timer_ptrs_;
}
}
const std::vector<service::ServiceBase::SharedPtr> &
get_service_ptrs() const
{
const std::vector<rclcpp::service::ServiceBase::SharedPtr> &
CallbackGroup::get_service_ptrs() const
{
return service_ptrs_;
}
}
const std::vector<client::ClientBase::SharedPtr> &
get_client_ptrs() const
{
const std::vector<rclcpp::client::ClientBase::SharedPtr> &
CallbackGroup::get_client_ptrs() const
{
return client_ptrs_;
}
}
std::atomic_bool & can_be_taken_from()
{
std::atomic_bool &
CallbackGroup::can_be_taken_from()
{
return can_be_taken_from_;
}
}
const CallbackGroupType & type() const
{
const CallbackGroupType &
CallbackGroup::type() const
{
return type_;
}
}
private:
RCLCPP_DISABLE_COPY(CallbackGroup);
void
add_subscription(
const subscription::SubscriptionBase::SharedPtr subscription_ptr)
{
void
CallbackGroup::add_subscription(
const rclcpp::subscription::SubscriptionBase::SharedPtr subscription_ptr)
{
subscription_ptrs_.push_back(subscription_ptr);
}
}
void
add_timer(const timer::TimerBase::SharedPtr timer_ptr)
{
void
CallbackGroup::add_timer(const rclcpp::timer::TimerBase::SharedPtr timer_ptr)
{
timer_ptrs_.push_back(timer_ptr);
}
}
void
add_service(const service::ServiceBase::SharedPtr service_ptr)
{
void
CallbackGroup::add_service(const rclcpp::service::ServiceBase::SharedPtr service_ptr)
{
service_ptrs_.push_back(service_ptr);
}
}
void
add_client(const client::ClientBase::SharedPtr client_ptr)
{
void
CallbackGroup::add_client(const rclcpp::client::ClientBase::SharedPtr client_ptr)
{
client_ptrs_.push_back(client_ptr);
}
CallbackGroupType type_;
std::vector<subscription::SubscriptionBase::WeakPtr> subscription_ptrs_;
std::vector<timer::TimerBase::WeakPtr> timer_ptrs_;
std::vector<service::ServiceBase::SharedPtr> service_ptrs_;
std::vector<client::ClientBase::SharedPtr> client_ptrs_;
std::atomic_bool can_be_taken_from_;
};
} /* namespace callback_group */
} /* namespace rclcpp */
#endif /* RCLCPP_RCLCPP_CALLBACK_GROUP_HPP_ */
}

155
rclcpp/src/rclcpp/client.cpp
View file

@ -1,4 +1,4 @@
// Copyright 2014 Open Source Robotics Foundation, Inc.
// Copyright 2015 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.
@ -12,153 +12,40 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_RCLCPP_CLIENT_HPP_
#define RCLCPP_RCLCPP_CLIENT_HPP_
#include "rclcpp/client.hpp"
#include <future>
#include <iostream>
#include <map>
#include <memory>
#include <sstream>
#include <utility>
#include <cstdio>
#include <string>
#include <rmw/error_handling.h>
#include <rmw/rmw.h>
#include "rmw/rmw.h"
#include <rclcpp/macros.hpp>
#include <rclcpp/utilities.hpp>
using rclcpp::client::ClientBase;
namespace rclcpp
{
namespace client
{
class ClientBase
{
public:
RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientBase);
ClientBase(
ClientBase::ClientBase(
std::shared_ptr<rmw_node_t> node_handle,
rmw_client_t * client_handle,
const std::string & service_name)
: node_handle_(node_handle), client_handle_(client_handle), service_name_(service_name)
{}
: node_handle_(node_handle), client_handle_(client_handle), service_name_(service_name)
{}
virtual ~ClientBase()
{
ClientBase::~ClientBase()
{
if (client_handle_) {
if (rmw_destroy_client(client_handle_) != RMW_RET_OK) {
fprintf(stderr,
"Error in destruction of rmw client handle: %s\n", rmw_get_error_string_safe());
}
}
}
}
const std::string & get_service_name() const
{
return this->service_name_;
}
const rmw_client_t * get_client_handle() const
{
return this->client_handle_;
}
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;
private:
RCLCPP_DISABLE_COPY(ClientBase);
std::shared_ptr<rmw_node_t> node_handle_;
rmw_client_t * client_handle_;
std::string service_name_;
};
template<typename ServiceT>
class Client : public ClientBase
const std::string &
ClientBase::get_service_name() const
{
public:
using Promise = std::promise<typename ServiceT::Response::SharedPtr>;
using SharedPromise = std::shared_ptr<Promise>;
using SharedFuture = std::shared_future<typename ServiceT::Response::SharedPtr>;
return this->service_name_;
}
using CallbackType = std::function<void(SharedFuture)>;
RCLCPP_SMART_PTR_DEFINITIONS(Client);
Client(
std::shared_ptr<rmw_node_t> node_handle,
rmw_client_t * client_handle,
const std::string & service_name)
: ClientBase(node_handle, client_handle, service_name)
{}
std::shared_ptr<void> create_response()
{
return std::shared_ptr<void>(new typename ServiceT::Response());
}
std::shared_ptr<void> create_request_header()
{
// TODO(wjwwood): This should probably use rmw_request_id's allocator.
// (since it is a C type)
return std::shared_ptr<void>(new rmw_request_id_t);
}
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);
int64_t sequence_number = typed_request_header->sequence_number;
// TODO this must check if the sequence_number is valid otherwise the call_promise will be null
auto tuple = this->pending_requests_[sequence_number];
auto call_promise = std::get<0>(tuple);
auto callback = std::get<1>(tuple);
auto future = std::get<2>(tuple);
this->pending_requests_.erase(sequence_number);
call_promise->set_value(typed_response);
callback(future);
}
SharedFuture async_send_request(
typename ServiceT::Request::SharedPtr request)
{
return async_send_request(request, [](SharedFuture) {});
}
SharedFuture async_send_request(
typename ServiceT::Request::SharedPtr request,
CallbackType cb)
{
int64_t sequence_number;
if (RMW_RET_OK != rmw_send_request(get_client_handle(), request.get(), &sequence_number)) {
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
throw std::runtime_error(
std::string("failed to send request: ") + rmw_get_error_string_safe());
// *INDENT-ON*
}
SharedPromise call_promise = std::make_shared<Promise>();
SharedFuture f(call_promise->get_future());
pending_requests_[sequence_number] = std::make_tuple(call_promise, cb, f);
return f;
}
private:
RCLCPP_DISABLE_COPY(Client);
std::map<int64_t, std::tuple<SharedPromise, CallbackType, SharedFuture>> pending_requests_;
};
} /* namespace client */
} /* namespace rclcpp */
#endif /* RCLCPP_RCLCPP_CLIENT_HPP_ */
const rmw_client_t *
ClientBase::get_client_handle() const
{
return this->client_handle_;
}

69
rclcpp/src/rclcpp/context.cpp
View file

@ -1,4 +1,4 @@
// Copyright 2014 Open Source Robotics Foundation, Inc.
// Copyright 2015 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.
@ -12,69 +12,8 @@
// 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 "rclcpp/context.hpp"
#include <rclcpp/macros.hpp>
using rclcpp::context::Context;
#include <iostream>
#include <memory>
#include <mutex>
#include <typeinfo>
#include <typeindex>
#include <unordered_map>
#include <rmw/rmw.h>
namespace rclcpp
{
namespace context
{
class Context
{
public:
RCLCPP_SMART_PTR_DEFINITIONS(Context);
Context() {}
template<typename SubContext, typename ... Args>
std::shared_ptr<SubContext>
get_sub_context(Args && ... args)
{
std::lock_guard<std::mutex> lock(mutex_);
std::type_index type_i(typeid(SubContext));
std::shared_ptr<SubContext> sub_context;
auto it = sub_contexts_.find(type_i);
if (it == sub_contexts_.end()) {
// It doesn't exist yet, make it
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
sub_context = std::shared_ptr<SubContext>(
new SubContext(std::forward<Args>(args) ...),
[] (SubContext * sub_context_ptr) {
delete sub_context_ptr;
});
// *INDENT-ON*
sub_contexts_[type_i] = sub_context;
} else {
// It exists, get it out and cast it.
sub_context = std::static_pointer_cast<SubContext>(it->second);
}
return sub_context;
}
private:
RCLCPP_DISABLE_COPY(Context);
std::unordered_map<std::type_index, std::shared_ptr<void>> sub_contexts_;
std::mutex mutex_;
};
} /* namespace context */
} /* namespace rclcpp */
#endif /* RCLCPP_RCLCPP_CONTEXT_HPP_ */
Context::Context() {}

32
rclcpp/src/rclcpp/contexts/default_context.cpp
View file

@ -1,4 +1,4 @@
// Copyright 2014 Open Source Robotics Foundation, Inc.
// Copyright 2015 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.
@ -12,36 +12,16 @@
// 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/contexts/default_context.hpp"
#include <rclcpp/context.hpp>
using rclcpp::contexts::default_context::DefaultContext;
namespace rclcpp
{
namespace contexts
{
namespace default_context
{
class DefaultContext : public rclcpp::context::Context
{
public:
RCLCPP_SMART_PTR_DEFINITIONS(DefaultContext);
DefaultContext() {}
};
DefaultContext::DefaultContext()
{}
DefaultContext::SharedPtr
get_global_default_context()
rclcpp::contexts::default_context::get_global_default_context()
{
static DefaultContext::SharedPtr default_context = DefaultContext::make_shared();
return default_context;
}
} // namespace default_context
} // namespace contexts
} // namespace rclcpp
#endif /* RCLCPP_RCLCPP_CONTEXTS_DEFAULT_CONTEXT_HPP_ */

304
rclcpp/src/rclcpp/executor.cpp
View file

@ -1,4 +1,4 @@
// Copyright 2014 Open Source Robotics Foundation, Inc.
// Copyright 2015 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.
@ -12,58 +12,21 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP__EXECUTOR_HPP_
#define RCLCPP__EXECUTOR_HPP_
#include <algorithm>
#include <cassert>
#include <cstdlib>
#include <iostream>
#include <list>
#include <memory>
#include <vector>
#include "rclcpp/executor.hpp"
#include "rcl_interfaces/msg/intra_process_message.hpp"
#include "rclcpp/any_executable.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/memory_strategies.hpp"
#include "rclcpp/memory_strategy.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/utilities.hpp"
using rclcpp::executor::AnyExecutable;
using rclcpp::executor::Executor;
namespace rclcpp
{
namespace executor
{
/// Coordinate the order and timing of available communication tasks.
/**
* Executor provides spin functions (including spin_node_once and spin_some).
* It coordinates the nodes and callback groups by looking for available work and completing it,
* based on the threading or concurrency scheme provided by the subclass implementation.
* An example of available work is executing a subscription callback, or a timer callback.
* The executor structure allows for a decoupling of the communication graph and the execution
* model.
* See SingleThreadedExecutor and MultiThreadedExecutor for examples of execution paradigms.
*/
class Executor
{
public:
RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Executor);
/// Default constructor.
// \param[in] ms The memory strategy to be used with this executor.
explicit Executor(memory_strategy::MemoryStrategy::SharedPtr ms =
memory_strategies::create_default_strategy())
: interrupt_guard_condition_(rmw_create_guard_condition()),
Executor::Executor(memory_strategy::MemoryStrategy::SharedPtr ms)
: interrupt_guard_condition_(rmw_create_guard_condition()),
memory_strategy_(ms)
{
}
{
}
/// Default destructor.
virtual ~Executor()
{
Executor::~Executor()
{
// Try to deallocate the interrupt guard condition.
if (interrupt_guard_condition_ != nullptr) {
rmw_ret_t status = rmw_destroy_guard_condition(interrupt_guard_condition_);
@ -72,23 +35,11 @@ public:
"[rclcpp::error] failed to destroy guard condition: %s\n", rmw_get_error_string_safe());
}
}
}
}
/// Do work periodically as it becomes available to us. Blocking call, may block indefinitely.
// It is up to the implementation of Executor to implement spin.
virtual void spin() = 0;
/// Add a node to the executor.
/**
* An executor can have zero or more nodes which provide work during `spin` functions.
* \param[in] node_ptr Shared pointer to the node to be added.
* \param[in] notify True to trigger the interrupt guard condition during this function. If
* the executor is blocked at the rmw layer while waiting for work and it is notified that a new
* node was added, it will wake up.
*/
virtual void
add_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify = true)
{
void
Executor::add_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify)
{
// Check to ensure node not already added
for (auto & weak_node : weak_nodes_) {
auto node = weak_node.lock();
@ -105,28 +56,25 @@ public:
throw std::runtime_error(rmw_get_error_string_safe());
}
}
}
}
/// Remove a node from the executor.
/**
* \param[in] node_ptr Shared pointer to the node to remove.
* \param[in] notify True to trigger the interrupt guard condition and wake up the executor.
* This is useful if the last node was removed from the executor while the executor was blocked
* waiting for work in another thread, because otherwise the executor would never be notified.
*/
virtual void
remove_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify = true)
{
void
Executor::remove_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify)
{
bool node_removed = false;
weak_nodes_.erase(
std::remove_if(
weak_nodes_.begin(), weak_nodes_.end(),
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
[&](std::weak_ptr<rclcpp::node::Node> & i)
{
bool matched = (i.lock() == node_ptr);
node_removed |= matched;
return matched;
}));
}
// *INDENT-ON*
)
);
if (notify) {
// If the node was matched and removed, interrupt waiting
if (node_removed) {
@ -136,19 +84,13 @@ public:
}
}
}
}
}
/// Add a node to executor, execute the next available unit of work, and remove the node.
/**
* \param[in] node Shared pointer to the node to add.
* \param[in] timeout How long to wait for work to become available. Negative values cause
* spin_node_once to block indefinitely (the default behavior). A timeout of 0 causes this
* function to be non-blocking.
*/
template<typename T = std::milli>
void spin_node_once(rclcpp::node::Node::SharedPtr node,
std::chrono::duration<int64_t, T> timeout = std::chrono::duration<int64_t, T>(-1))
{
void
Executor::spin_node_once_nanoseconds(
rclcpp::node::Node::SharedPtr node,
std::chrono::nanoseconds timeout)
{
this->add_node(node, false);
// non-blocking = true
auto any_exec = get_next_executable(timeout);
@ -156,58 +98,38 @@ public:
execute_any_executable(any_exec);
}
this->remove_node(node, false);
}
}
/// Add a node, complete all immediately available work, and remove the node.
/**
* \param[in] node Shared pointer to the node to add.
*/
void spin_node_some(rclcpp::node::Node::SharedPtr node)
{
void
Executor::spin_node_some(rclcpp::node::Node::SharedPtr node)
{
this->add_node(node, false);
spin_some();
this->remove_node(node, false);
}
}
/// Complete all available queued work without blocking.
/**
* This function can be overridden. The default implementation is suitable for a
* single-threaded model of execution.
* Adding subscriptions, timers, services, etc. with blocking callbacks will cause this function
* to block (which may have unintended consequences).
*/
virtual void spin_some()
{
void
Executor::spin_some()
{
while (AnyExecutable::SharedPtr any_exec =
get_next_executable(std::chrono::milliseconds::zero()))
{
execute_any_executable(any_exec);
}
}
}
/// Support dynamic switching of the memory strategy.
/**
* Switching the memory strategy while the executor is spinning in another threading could have
* unintended consequences.
* \param[in] memory_strategy Shared pointer to the memory strategy to set.
*/
void
set_memory_strategy(memory_strategy::MemoryStrategy::SharedPtr memory_strategy)
{
void
Executor::set_memory_strategy(memory_strategy::MemoryStrategy::SharedPtr memory_strategy)
{
if (memory_strategy == nullptr) {
throw std::runtime_error("Received NULL memory strategy in executor.");
}
memory_strategy_ = memory_strategy;
}
}
protected:
/// Find the next available executable and do the work associated with it.
/** \param[in] any_exec Union structure that can hold any executable type (timer, subscription,
* service, client).
*/
void
execute_any_executable(AnyExecutable::SharedPtr any_exec)
{
void
Executor::execute_any_executable(AnyExecutable::SharedPtr any_exec)
{
if (!any_exec) {
return;
}
@ -234,12 +156,12 @@ protected:
if (status != RMW_RET_OK) {
throw std::runtime_error(rmw_get_error_string_safe());
}
}
}
static void
execute_subscription(
void
Executor::execute_subscription(
rclcpp::subscription::SubscriptionBase::SharedPtr subscription)
{
{
std::shared_ptr<void> message = subscription->create_message();
bool taken = false;
rmw_message_info_t message_info;
@ -257,12 +179,12 @@ protected:
subscription->get_topic_name().c_str(), rmw_get_error_string_safe());
}
subscription->return_message(message);
}
}
static void
execute_intra_process_subscription(
void
Executor::execute_intra_process_subscription(
rclcpp::subscription::SubscriptionBase::SharedPtr subscription)
{
{
rcl_interfaces::msg::IntraProcessMessage ipm;
bool taken = false;
rmw_message_info_t message_info;
@ -281,19 +203,19 @@ protected:
"[rclcpp::error] take failed for intra process subscription on topic '%s': %s\n",
subscription->get_topic_name().c_str(), rmw_get_error_string_safe());
}
}
}
static void
execute_timer(
void
Executor::execute_timer(
rclcpp::timer::TimerBase::SharedPtr timer)
{
{
timer->execute_callback();
}
}
static void
execute_service(
void
Executor::execute_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;
@ -311,12 +233,12 @@ protected:
"[rclcpp::error] take request failed for server of service '%s': %s\n",
service->get_service_name().c_str(), rmw_get_error_string_safe());
}
}
}
static void
execute_client(
void
Executor::execute_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;
@ -334,14 +256,11 @@ protected:
"[rclcpp::error] take response failed for client of service '%s': %s\n",
client->get_service_name().c_str(), rmw_get_error_string_safe());
}
}
}
/*** Populate class storage from stored weak node pointers and wait. ***/
template<typename T = std::milli>
void
wait_for_work(std::chrono::duration<int64_t, T> timeout = std::chrono::duration<int64_t, T>(-1))
{
void
Executor::wait_for_work(std::chrono::nanoseconds timeout)
{
memory_strategy_->clear_active_entities();
// Collect the subscriptions and timers to be waited on
@ -350,11 +269,16 @@ protected:
// 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(),
remove_if(
weak_nodes_.begin(), weak_nodes_.end(),
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
[](std::weak_ptr<rclcpp::node::Node> i)
{
return i.expired();
}));
}
// *INDENT-ON*
)
);
}
// Use the number of subscriptions to allocate memory in the handles
@ -399,13 +323,13 @@ protected:
// replace the requested timeout with the next timeout.
bool has_valid_timer = next_timer_duration >= std::chrono::nanoseconds::zero();
if ((next_timer_duration < timeout ||
timeout < std::chrono::duration<int64_t, T>::zero()) && has_valid_timer)
timeout < std::chrono::nanoseconds::zero()) && has_valid_timer)
{
rmw_timeout.sec =
std::chrono::duration_cast<std::chrono::seconds>(next_timer_duration).count();
rmw_timeout.nsec = next_timer_duration.count() % (1000 * 1000 * 1000);
wait_timeout = &rmw_timeout;
} else if (timeout >= std::chrono::duration<int64_t, T>::zero()) {
} else if (timeout >= std::chrono::nanoseconds::zero()) {
// Convert timeout representation to rmw_time
rmw_timeout.sec = std::chrono::duration_cast<std::chrono::seconds>(timeout).count();
rmw_timeout.nsec = std::chrono::duration_cast<std::chrono::nanoseconds>(timeout).count() %
@ -431,12 +355,11 @@ protected:
}
memory_strategy_->revalidate_handles();
}
}
/******************************/
rclcpp::node::Node::SharedPtr
get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group)
{
rclcpp::node::Node::SharedPtr
Executor::get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group)
{
if (!group) {
return rclcpp::node::Node::SharedPtr();
}
@ -453,12 +376,11 @@ protected:
}
}
return rclcpp::node::Node::SharedPtr();
}
}
rclcpp::callback_group::CallbackGroup::SharedPtr
get_group_by_timer(
rclcpp::timer::TimerBase::SharedPtr timer)
{
rclcpp::callback_group::CallbackGroup::SharedPtr
Executor::get_group_by_timer(rclcpp::timer::TimerBase::SharedPtr timer)
{
for (auto & weak_node : weak_nodes_) {
auto node = weak_node.lock();
if (!node) {
@ -478,11 +400,11 @@ protected:
}
}
return rclcpp::callback_group::CallbackGroup::SharedPtr();
}
}
void
get_next_timer(AnyExecutable::SharedPtr any_exec)
{
void
Executor::get_next_timer(AnyExecutable::SharedPtr any_exec)
{
for (auto & weak_node : weak_nodes_) {
auto node = weak_node.lock();
if (!node) {
@ -504,11 +426,11 @@ protected:
}
}
}
}
}
std::chrono::nanoseconds
get_earliest_timer()
{
std::chrono::nanoseconds
Executor::get_earliest_timer()
{
std::chrono::nanoseconds latest = std::chrono::nanoseconds::max();
bool timers_empty = true;
for (auto & weak_node : weak_nodes_) {
@ -535,11 +457,11 @@ protected:
return std::chrono::nanoseconds(-1);
}
return latest;
}
}
AnyExecutable::SharedPtr
get_next_ready_executable()
{
AnyExecutable::SharedPtr
Executor::get_next_ready_executable()
{
auto any_exec = memory_strategy_->instantiate_next_executable();
// Check the timers to see if there are any that are ready, if so return
get_next_timer(any_exec);
@ -563,13 +485,11 @@ protected:
}
// If there is no ready executable, return a null ptr
return nullptr;
}
}
template<typename T = std::milli>
AnyExecutable::SharedPtr
get_next_executable(std::chrono::duration<int64_t, T> timeout = std::chrono::duration<int64_t,
T>(-1))
{
AnyExecutable::SharedPtr
Executor::get_next_executable(std::chrono::nanoseconds timeout)
{
// 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();
@ -595,22 +515,4 @@ protected:
}
}
return any_exec;
}
/// Guard condition for signaling the rmw layer to wake up for special events.
rmw_guard_condition_t * interrupt_guard_condition_;
/// The memory strategy: an interface for handling user-defined memory allocation strategies.
memory_strategy::MemoryStrategy::SharedPtr memory_strategy_;
private:
RCLCPP_DISABLE_COPY(Executor);
std::vector<std::weak_ptr<rclcpp::node::Node>> weak_nodes_;
std::array<void *, 2> guard_cond_handles_;
};
} // namespace executor
} // namespace rclcpp
#endif // RCLCPP__EXECUTOR_HPP_
}

83
rclcpp/src/rclcpp/executors.cpp
View file

@ -1,4 +1,4 @@
// Copyright 2014 Open Source Robotics Foundation, Inc.
// Copyright 2015 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.
@ -12,76 +12,19 @@
// 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.hpp"
#include <future>
#include <rclcpp/executors/multi_threaded_executor.hpp>
#include <rclcpp/executors/single_threaded_executor.hpp>
#include <rclcpp/node.hpp>
#include <rclcpp/utilities.hpp>
namespace rclcpp
void
rclcpp::spin_some(node::Node::SharedPtr node_ptr)
{
namespace executors
{
using rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor;
using rclcpp::executors::single_threaded_executor::SingleThreadedExecutor;
/// Return codes to be used with spin_until_future_complete.
/**
* SUCCESS: The future is complete and can be accessed with "get" without blocking.
* INTERRUPTED: The future is not complete, spinning was interrupted by Ctrl-C or another error.
* TIMEOUT: Spinning timed out.
*/
enum FutureReturnCode {SUCCESS, INTERRUPTED, TIMEOUT};
/// Spin (blocking) until the future is complete, until the function times out (if applicable),
/// or until rclcpp is interrupted.
/**
* \param[in] executor The executor which will spin the node.
* \param[in] node_ptr The node to spin.
* \param[in] future The future to wait on. If SUCCESS, the future is safe to access after this function
* \param[in] timeout Optional timeout parameter, which gets passed to Executor::spin_node_once.
-1 is block forever, 0 is non-blocking.
If the time spent inside the blocking loop exceeds this timeout, return a TIMEOUT return code.
* \return The return code, one of SUCCESS, INTERRUPTED, or TIMEOUT.
*/
template<typename ResponseT, typename TimeT = std::milli>
FutureReturnCode
spin_node_until_future_complete(
rclcpp::executor::Executor & executor, rclcpp::node::Node::SharedPtr node_ptr,
std::shared_future<ResponseT> & future,
std::chrono::duration<int64_t, TimeT> timeout = std::chrono::duration<int64_t, TimeT>(-1))
{
// TODO(wjwwood): does not work recursively right, can't call spin_node_until_future_complete
// inside a callback executed by an executor.
// Check the future before entering the while loop.
// If the future is already complete, don't try to spin.
std::future_status status = future.wait_for(std::chrono::seconds(0));
auto start_time = std::chrono::system_clock::now();
while (status != std::future_status::ready && rclcpp::utilities::ok()) {
executor.spin_node_once(node_ptr, timeout);
if (timeout.count() >= 0) {
if (start_time + timeout < std::chrono::system_clock::now()) {
return TIMEOUT;
}
}
status = future.wait_for(std::chrono::seconds(0));
}
// If the future completed, and we weren't interrupted by ctrl-C, return the response
if (status == std::future_status::ready) {
return FutureReturnCode::SUCCESS;
}
return FutureReturnCode::INTERRUPTED;
rclcpp::executors::SingleThreadedExecutor exec;
exec.spin_node_some(node_ptr);
}
} // namespace executors
} // namespace rclcpp
#endif /* RCLCPP_RCLCPP_EXECUTORS_HPP_ */
void
rclcpp::spin(node::Node::SharedPtr node_ptr)
{
rclcpp::executors::SingleThreadedExecutor exec;
exec.add_node(node_ptr);
exec.spin();
}

79
rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp
View file

@ -1,4 +1,4 @@
// Copyright 2014 Open Source Robotics Foundation, Inc.
// Copyright 2015 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.
@ -12,74 +12,55 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_
#define RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_
#include "rclcpp/executors/multi_threaded_executor.hpp"
#include <rmw/rmw.h>
#include <cassert>
#include <cstdlib>
#include <memory>
#include <mutex>
#include <chrono>
#include <functional>
#include <vector>
#include "rclcpp/executor.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/utilities.hpp"
namespace rclcpp
{
namespace executors
{
namespace multi_threaded_executor
{
using rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor;
class MultiThreadedExecutor : public executor::Executor
MultiThreadedExecutor::MultiThreadedExecutor(memory_strategy::MemoryStrategy::SharedPtr ms)
: executor::Executor(ms)
{
public:
RCLCPP_SMART_PTR_DEFINITIONS(MultiThreadedExecutor);
MultiThreadedExecutor(memory_strategy::MemoryStrategy::SharedPtr ms =
memory_strategies::create_default_strategy())
: executor::Executor(ms)
{
number_of_threads_ = std::thread::hardware_concurrency();
if (number_of_threads_ == 0) {
number_of_threads_ = 1;
}
}
}
virtual ~MultiThreadedExecutor() {}
MultiThreadedExecutor::~MultiThreadedExecutor() {}
void
spin()
{
void
MultiThreadedExecutor::spin()
{
std::vector<std::thread> threads;
{
std::lock_guard<std::mutex> wait_lock(wait_mutex_);
size_t thread_id_ = 1; // Use a _ suffix to avoid shadowing `rclcpp::thread_id`
size_t thread_id = 1;
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_++);
auto func = std::bind(&MultiThreadedExecutor::run, this, thread_id++);
threads.emplace_back(func);
}
}
for (auto & thread : threads) {
thread.join();
}
}
}
size_t
get_number_of_threads()
{
size_t
MultiThreadedExecutor::get_number_of_threads()
{
return number_of_threads_;
}
}
private:
void run(size_t this_thread_id)
{
rclcpp::thread_id = this_thread_id;
void
MultiThreadedExecutor::run(size_t this_thread_number)
{
thread_number_by_thread_id_[std::this_thread::get_id()] = this_thread_number;
while (rclcpp::utilities::ok()) {
executor::AnyExecutable::SharedPtr any_exec;
{
@ -91,16 +72,4 @@ private:
}
execute_any_executable(any_exec);
}
}
RCLCPP_DISABLE_COPY(MultiThreadedExecutor);
std::mutex wait_mutex_;
size_t number_of_threads_;
};
} // namespace multi_threaded_executor
} // namespace executors
} // namespace rclcpp
#endif // RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_
}

61
rclcpp/src/rclcpp/executors/single_threaded_executor.cpp
View file

@ -1,4 +1,4 @@
// Copyright 2014 Open Source Robotics Foundation, Inc.
// Copyright 2015 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.
@ -12,62 +12,21 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP__EXECUTORS__SINGLE_THREADED_EXECUTOR_HPP_
#define RCLCPP__EXECUTORS__SINGLE_THREADED_EXECUTOR_HPP_
#include <rclcpp/executors/single_threaded_executor.hpp>
#include <rmw/rmw.h>
using rclcpp::executors::single_threaded_executor::SingleThreadedExecutor;
#include <cassert>
#include <cstdlib>
#include <memory>
#include <vector>
SingleThreadedExecutor::SingleThreadedExecutor(memory_strategy::MemoryStrategy::SharedPtr ms)
: executor::Executor(ms) {}
#include "rclcpp/executor.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/memory_strategies.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/utilities.hpp"
#include "rclcpp/rate.hpp"
namespace rclcpp
SingleThreadedExecutor::~SingleThreadedExecutor() {}
void
SingleThreadedExecutor::spin()
{
namespace executors
{
namespace single_threaded_executor
{
/// Single-threaded executor implementation
// This is the default executor created by rclcpp::spin.
class SingleThreadedExecutor : public executor::Executor
{
public:
RCLCPP_SMART_PTR_DEFINITIONS(SingleThreadedExecutor);
/// Default constructor. See the default constructor for Executor.
SingleThreadedExecutor(memory_strategy::MemoryStrategy::SharedPtr ms =
memory_strategies::create_default_strategy())
: executor::Executor(ms) {}
/// Default destrcutor.
virtual ~SingleThreadedExecutor() {}
/// Single-threaded implementation of spin.
// This function will block until work comes in, execute it, and keep blocking.
// It will only be interrupt by a CTRL-C (managed by the global signal handler).
void spin()
{
while (rclcpp::utilities::ok()) {
auto any_exec = get_next_executable();
execute_any_executable(any_exec);
}
}
private:
RCLCPP_DISABLE_COPY(SingleThreadedExecutor);
};
} // namespace single_threaded_executor
} // namespace executors
} // namespace rclcpp
#endif // RCLCPP__EXECUTORS__SINGLE_THREADED_EXECUTOR_HPP_
}

362
rclcpp/src/rclcpp/intra_process_manager.cpp
View file

@ -12,340 +12,50 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP__INTRA_PROCESS_MANAGER_HPP_
#define RCLCPP__INTRA_PROCESS_MANAGER_HPP_
#include "rclcpp/intra_process_manager.hpp"
#include <rmw/types.h>
using rclcpp::intra_process_manager::IntraProcessManager;
#include <algorithm>
#include <atomic>
#include <cstdint>
#include <exception>
#include <map>
#include <memory>
#include <unordered_map>
#include <set>
static std::atomic<uint64_t> _next_unique_id {1};
#include "rclcpp/allocator/allocator_deleter.hpp"
#include "rclcpp/intra_process_manager_state.hpp"
#include "rclcpp/mapped_ring_buffer.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/publisher.hpp"
#include "rclcpp/subscription.hpp"
IntraProcessManager::IntraProcessManager(IntraProcessManagerStateBase::SharedPtr state)
: state_(state)
{}
namespace rclcpp
IntraProcessManager::~IntraProcessManager()
{}
uint64_t
IntraProcessManager::add_subscription(
rclcpp::subscription::SubscriptionBase::SharedPtr subscription)
{
namespace intra_process_manager
{
/// This class facilitates intra process communication between nodes.
/* This class is used in the creation of publishers and subscriptions.
* A singleton instance of this class is owned by a rclcpp::Context and a
* rclcpp::Node can use an associated Context to get an instance of this class.
* Nodes which do not have a common Context will not exchange intra process
* messages because they will not share access to an instance of this class.
*
* When a Node creates a publisher or subscription, it will register them
* with this class.
* The node will also hook into the publisher's publish call
* in order to do intra process related work.
*
* When a publisher is created, it advertises on the topic the user provided,
* as well as a "shadowing" topic of type rcl_interfaces/IntraProcessMessage.
* For instance, if the user specified the topic '/namespace/chatter', then the
* corresponding intra process topic might be '/namespace/chatter__intra'.
* The publisher is also allocated an id which is unique among all publishers
* and subscriptions in this process.
* Additionally, when registered with this class a ring buffer is created and
* owned by this class as a temporary place to hold messages destined for intra
* process subscriptions.
*
* When a subscription is created, it subscribes to the topic provided by the
* user as well as to the corresponding intra process topic.
* It is also gets a unique id from the singleton instance of this class which
* is unique among publishers and subscriptions.
*
* When the user publishes a message, the message is stored by calling
* store_intra_process_message on this class.
* The instance of that message is uniquely identified by a publisher id and a
* message sequence number.
* The publisher id, message sequence pair is unique with in the process.
* At that point a list of the id's of intra process subscriptions which have
* been registered with the singleton instance of this class are stored with
* the message instance so that delivery is only made to those subscriptions.
* Then an instance of rcl_interfaces/IntraProcessMessage is published to the
* intra process topic which is specific to the topic specified by the user.
*
* When an instance of rcl_interfaces/IntraProcessMessage is received by a
* subscription, then it is handled by calling take_intra_process_message
* on a singleton of this class.
* The subscription passes a publisher id, message sequence pair which
* uniquely identifies the message instance it was suppose to receive as well
* as the subscriptions unique id.
* If the message is still being held by this class and the subscription's id
* is in the list of intended subscriptions then the message is returned.
* If either of those predicates are not satisfied then the message is not
* returned and the subscription does not call the users callback.
*
* Since the publisher builds a list of destined subscriptions on publish, and
* other requests are ignored, this class knows how many times a message
* instance should be requested.
* The final time a message is requested, the ownership is passed out of this
* class and passed to the final subscription, effectively freeing space in
* this class's internal storage.
*
* Since a topic is being used to ferry notifications about new intra process
* messages between publishers and subscriptions, it is possible for that
* notification to be lost.
* It is also possible that a subscription which was available when publish was
* called will no longer exist once the notification gets posted.
* In both cases this might result in a message instance getting requested
* fewer times than expected.
* This is why the internal storage of this class is a ring buffer.
* That way if a message is orphaned it will eventually be dropped from storage
* when a new message instance is stored and will not result in a memory leak.
*
* However, since the storage system is finite, this also means that a message
* instance might get displaced by an incoming message instance before all
* interested parties have called take_intra_process_message.
* Because of this the size of the internal storage should be carefully
* considered.
*
* /TODO(wjwwood): update to include information about handling latching.
* /TODO(wjwwood): consider thread safety of the class.
*
* This class is neither CopyConstructable nor CopyAssignable.
*/
class IntraProcessManager
{
private:
RCLCPP_DISABLE_COPY(IntraProcessManager);
public:
RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessManager);
explicit IntraProcessManager(
IntraProcessManagerStateBase::SharedPtr state = create_default_state()
)
: state_(state)
{
}
/// Register a subscription with the manager, returns subscriptions unique id.
/* In addition to generating a unique intra process id for the subscription,
* this method also stores the topic name of the subscription.
*
* This method is normally called during the creation of a subscription,
* but after it creates the internal intra process rmw_subscription_t.
*
* This method will allocate memory.
*
* \param subscription the Subscription to register.
* \return an unsigned 64-bit integer which is the subscription's unique id.
*/
uint64_t
add_subscription(subscription::SubscriptionBase::SharedPtr subscription)
{
auto id = IntraProcessManager::get_next_unique_id();
state_->add_subscription(id, subscription);
return id;
}
}
/// Unregister a subscription using the subscription's unique id.
/* This method does not allocate memory.
*
* \param intra_process_subscription_id id of the subscription to remove.
*/
void
remove_subscription(uint64_t intra_process_subscription_id)
{
void
IntraProcessManager::remove_subscription(uint64_t intra_process_subscription_id)
{
state_->remove_subscription(intra_process_subscription_id);
}
}
/// Register a publisher with the manager, returns the publisher unique id.
/* In addition to generating and returning a unique id for the publisher,
* this method creates internal ring buffer storage for "in-flight" intra
* process messages which are stored when store_intra_process_message is
* called with this publisher's unique id.
*
* The buffer_size must be less than or equal to the max uint64_t value.
* If the buffer_size is 0 then a buffer size is calculated using the
* publisher's QoS settings.
* The default is to use the depth field of the publisher's QoS.
* TODO(wjwwood): Consider doing depth *= 1.2, round up, or similar.
* TODO(wjwwood): Consider what to do for keep all.
*
* This method is templated on the publisher's message type so that internal
* storage of the same type can be allocated.
*
* This method will allocate memory.
*
* \param publisher publisher to be registered with the manager.
* \param buffer_size if 0 (default) a size is calculated based on the QoS.
* \return an unsigned 64-bit integer which is the publisher's unique id.
*/
template<typename MessageT, typename Alloc>
uint64_t
add_publisher(typename publisher::Publisher<MessageT, Alloc>::SharedPtr publisher,
size_t buffer_size = 0)
{
auto id = IntraProcessManager::get_next_unique_id();
size_t size = buffer_size > 0 ? buffer_size : publisher->get_queue_size();
auto mrb = mapped_ring_buffer::MappedRingBuffer<MessageT,
typename publisher::Publisher<MessageT, Alloc>::MessageAlloc>::make_shared(
size, publisher->get_allocator());
state_->add_publisher(id, publisher, mrb, size);
return id;
}
/// Unregister a publisher using the publisher's unique id.
/* This method does not allocate memory.
*
* \param intra_process_publisher_id id of the publisher to remove.
*/
void
remove_publisher(uint64_t intra_process_publisher_id)
{
void
IntraProcessManager::remove_publisher(uint64_t intra_process_publisher_id)
{
state_->remove_publisher(intra_process_publisher_id);
}
}
/// Store a message in the manager, and return the message sequence number.
/* The given message is stored in internal storage using the given publisher
* id and the newly generated message sequence, which is also returned.
* The combination of publisher id and message sequence number can later
* be used with a subscription id to retrieve the message by calling
* take_intra_process_message.
* The number of times take_intra_process_message can be called with this
* unique pair of id's is determined by the number of subscriptions currently
* subscribed to the same topic and which share the same Context, i.e. once
* for each subscription which should receive the intra process message.
*
* The ownership of the incoming message is transfered to the internal
* storage in order to avoid copying the message data.
* Therefore, the message parameter will no longer contain the original
* message after calling this method.
* Instead it will either be a nullptr or it will contain the ownership of
* the message instance which was displaced.
* If the message parameter is not equal to nullptr after calling this method
* then a message was prematurely displaced, i.e. take_intra_process_message
* had not been called on it as many times as was expected.
*
* This method can throw an exception if the publisher id is not found or
* if the publisher shared_ptr given to add_publisher has gone out of scope.
*
* This method does allocate memory.
*
* \param intra_process_publisher_id the id of the publisher of this message.
* \param message the message that is being stored.
* \return the message sequence number.
*/
template<typename MessageT, typename Alloc = std::allocator<void>,
typename Deleter = std::default_delete<MessageT>>
uint64_t
store_intra_process_message(
uint64_t intra_process_publisher_id,
std::unique_ptr<MessageT, Deleter> & message)
{
using MRBMessageAlloc = typename std::allocator_traits<Alloc>::template rebind_alloc<MessageT>;
using TypedMRB = typename mapped_ring_buffer::MappedRingBuffer<MessageT, MRBMessageAlloc>;
uint64_t message_seq = 0;
mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer = state_->get_publisher_info_for_id(
intra_process_publisher_id, message_seq);
typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast<TypedMRB>(buffer);
if (!typed_buffer) {
throw std::runtime_error("Typecast failed due to incorrect message type");
}
// Insert the message into the ring buffer using the message_seq to identify it.
bool did_replace = typed_buffer->push_and_replace(message_seq, message);
// TODO(wjwwood): do something when a message was displaced. log debug?
(void)did_replace; // Avoid unused variable warning.
state_->store_intra_process_message(intra_process_publisher_id, message_seq);
// Return the message sequence which is sent to the subscription.
return message_seq;
}
/// Take an intra process message.
/* The intra_process_publisher_id and message_sequence_number parameters
* uniquely identify a message instance, which should be taken.
*
* The requesting_subscriptions_intra_process_id parameter is used to make
* sure the requesting subscription was intended to receive this message
* instance.
* This check is made because it could happen that the requester
* comes up after the publish event, so it still receives the notification of
* a new intra process message, but it wasn't registered with the manager at
* the time of publishing, causing it to take when it wasn't intended.
* This should be avioded unless latching-like behavior is involved.
*
* The message parameter is used to store the taken message.
* On the last expected call to this method, the ownership is transfered out
* of internal storage and into the message parameter.
* On all previous calls a copy of the internally stored message is made and
* the ownership of the copy is transfered to the message parameter.
* TODO(wjwwood): update this documentation when latching is supported.
*
* The message parameter can be set to nullptr if:
*
* - The publisher id is not found.
* - The message sequence is not found for the given publisher id.
* - The requesting subscription's id is not in the list of intended takers.
* - The requesting subscription's id has been used before with this message.
*
* This method may allocate memory to copy the stored message.
*
* \param intra_process_publisher_id the id of the message's publisher.
* \param message_sequence_number the sequence number of the message.
* \param requesting_subscriptions_intra_process_id the subscription's id.
* \param message the message typed unique_ptr used to return the message.
*/
template<typename MessageT, typename Alloc = std::allocator<void>,
typename Deleter = std::default_delete<MessageT>>
void
take_intra_process_message(
uint64_t intra_process_publisher_id,
uint64_t message_sequence_number,
uint64_t requesting_subscriptions_intra_process_id,
std::unique_ptr<MessageT, Deleter> & message)
{
using MRBMessageAlloc = typename std::allocator_traits<Alloc>::template rebind_alloc<MessageT>;
using TypedMRB = mapped_ring_buffer::MappedRingBuffer<MessageT, MRBMessageAlloc>;
message = nullptr;
size_t target_subs_size = 0;
mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer = state_->take_intra_process_message(
intra_process_publisher_id,
message_sequence_number,
requesting_subscriptions_intra_process_id,
target_subs_size
);
typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast<TypedMRB>(buffer);
if (!typed_buffer) {
return;
}
// Return a copy or the unique_ptr (ownership) depending on how many subscriptions are left.
if (target_subs_size) {
// There are more subscriptions to serve, return a copy.
typed_buffer->get_copy_at_key(message_sequence_number, message);
} else {
// This is the last one to be returned, transfer ownership.
typed_buffer->pop_at_key(message_sequence_number, message);
}
}
/// Return true if the given rmw_gid_t matches any stored Publishers.
bool
matches_any_publishers(const rmw_gid_t * id) const
{
bool
IntraProcessManager::matches_any_publishers(const rmw_gid_t * id) const
{
return state_->matches_any_publishers(id);
}
}
private:
static uint64_t get_next_unique_id()
{
auto next_id = next_unique_id_.fetch_add(1, std::memory_order_relaxed);
uint64_t
IntraProcessManager::get_next_unique_id()
{
auto next_id = _next_unique_id.fetch_add(1, std::memory_order_relaxed);
// Check for rollover (we started at 1).
if (0 == next_id) {
// This puts a technical limit on the number of times you can add a publisher or subscriber.
@ -362,16 +72,4 @@ private:
// *INDENT-ON*
}
return next_id;
}
static std::atomic<uint64_t> next_unique_id_;
IntraProcessManagerStateBase::SharedPtr state_;
};
std::atomic<uint64_t> IntraProcessManager::next_unique_id_ {1};
} // namespace intra_process_manager
} // namespace rclcpp
#endif // RCLCPP__INTRA_PROCESS_MANAGER_HPP_
}

253
rclcpp/src/rclcpp/intra_process_manager_state.cpp
View file

@ -12,259 +12,12 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP__INTRA_PROCESS_MANAGER_STATE_HPP_
#define RCLCPP__INTRA_PROCESS_MANAGER_STATE_HPP_
#include "rclcpp/intra_process_manager_state.hpp"
#include <rclcpp/mapped_ring_buffer.hpp>
#include <rclcpp/publisher.hpp>
#include <rclcpp/subscription.hpp>
#include <algorithm>
#include <atomic>
#include <functional>
#include <limits>
#include <memory>
#include <map>
#include <unordered_map>
#include <set>
#include <string>
#include <utility>
namespace rclcpp
{
namespace intra_process_manager
{
class IntraProcessManagerStateBase
{
public:
RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(IntraProcessManagerStateBase);
virtual void
add_subscription(uint64_t id, subscription::SubscriptionBase::SharedPtr subscription) = 0;
virtual void
remove_subscription(uint64_t intra_process_subscription_id) = 0;
virtual void add_publisher(uint64_t id,
publisher::PublisherBase::WeakPtr publisher,
mapped_ring_buffer::MappedRingBufferBase::SharedPtr mrb,
size_t size) = 0;
virtual void
remove_publisher(uint64_t intra_process_publisher_id) = 0;
virtual mapped_ring_buffer::MappedRingBufferBase::SharedPtr
get_publisher_info_for_id(
uint64_t intra_process_publisher_id,
uint64_t & message_seq) = 0;
virtual void
store_intra_process_message(uint64_t intra_process_publisher_id, uint64_t message_seq) = 0;
virtual mapped_ring_buffer::MappedRingBufferBase::SharedPtr
take_intra_process_message(uint64_t intra_process_publisher_id,
uint64_t message_sequence_number,
uint64_t requesting_subscriptions_intra_process_id,
size_t & size) = 0;
virtual bool
matches_any_publishers(const rmw_gid_t * id) const = 0;
};
template<typename Allocator = std::allocator<void>>
class IntraProcessManagerState : public IntraProcessManagerStateBase
{
public:
void
add_subscription(uint64_t id, subscription::SubscriptionBase::SharedPtr subscription)
{
subscriptions_[id] = subscription;
subscription_ids_by_topic_[subscription->get_topic_name()].insert(id);
}
void
remove_subscription(uint64_t intra_process_subscription_id)
{
subscriptions_.erase(intra_process_subscription_id);
for (auto & pair : subscription_ids_by_topic_) {
pair.second.erase(intra_process_subscription_id);
}
// Iterate over all publisher infos and all stored subscription id's and
// remove references to this subscription's id.
for (auto & publisher_pair : publishers_) {
for (auto & sub_pair : publisher_pair.second.target_subscriptions_by_message_sequence) {
sub_pair.second.erase(intra_process_subscription_id);
}
}
}
void add_publisher(uint64_t id,
publisher::PublisherBase::WeakPtr publisher,
mapped_ring_buffer::MappedRingBufferBase::SharedPtr mrb,
size_t size)
{
publishers_[id].publisher = publisher;
// As long as the size of the ring buffer is less than the max sequence number, we're safe.
if (size > std::numeric_limits<uint64_t>::max()) {
throw std::invalid_argument("the calculated buffer size is too large");
}
publishers_[id].sequence_number.store(0);
publishers_[id].buffer = mrb;
publishers_[id].target_subscriptions_by_message_sequence.reserve(size);
}
void
remove_publisher(uint64_t intra_process_publisher_id)
{
publishers_.erase(intra_process_publisher_id);
}
// return message_seq and mrb
mapped_ring_buffer::MappedRingBufferBase::SharedPtr
get_publisher_info_for_id(
uint64_t intra_process_publisher_id,
uint64_t & message_seq)
{
auto it = publishers_.find(intra_process_publisher_id);
if (it == publishers_.end()) {
throw std::runtime_error("store_intra_process_message called with invalid publisher id");
}
PublisherInfo & info = it->second;
// Calculate the next message sequence number.
message_seq = info.sequence_number.fetch_add(1, std::memory_order_relaxed);
return info.buffer;
}
void
store_intra_process_message(uint64_t intra_process_publisher_id, uint64_t message_seq)
{
auto it = publishers_.find(intra_process_publisher_id);
if (it == publishers_.end()) {
throw std::runtime_error("store_intra_process_message called with invalid publisher id");
}
PublisherInfo & info = it->second;
auto publisher = info.publisher.lock();
if (!publisher) {
throw std::runtime_error("publisher has unexpectedly gone out of scope");
}
// Figure out what subscriptions should receive the message.
auto & destined_subscriptions = subscription_ids_by_topic_[publisher->get_topic_name()];
// Store the list for later comparison.
info.target_subscriptions_by_message_sequence[message_seq].clear();
std::copy(
destined_subscriptions.begin(), destined_subscriptions.end(),
// Memory allocation occurs in info.target_subscriptions_by_message_sequence[message_seq]
std::inserter(
info.target_subscriptions_by_message_sequence[message_seq],
// This ends up only being a hint to std::set, could also be .begin().
info.target_subscriptions_by_message_sequence[message_seq].end()
)
);
}
mapped_ring_buffer::MappedRingBufferBase::SharedPtr
take_intra_process_message(uint64_t intra_process_publisher_id,
uint64_t message_sequence_number,
uint64_t requesting_subscriptions_intra_process_id,
size_t & size
)
{
PublisherInfo * info;
{
auto it = publishers_.find(intra_process_publisher_id);
if (it == publishers_.end()) {
// Publisher is either invalid or no longer exists.
return 0;
}
info = &it->second;
}
// Figure out how many subscriptions are left.
AllocSet * target_subs;
{
auto it = info->target_subscriptions_by_message_sequence.find(message_sequence_number);
if (it == info->target_subscriptions_by_message_sequence.end()) {
// Message is no longer being stored by this publisher.
return 0;
}
target_subs = &it->second;
}
{
auto it = std::find(
target_subs->begin(), target_subs->end(),
requesting_subscriptions_intra_process_id);
if (it == target_subs->end()) {
// This publisher id/message seq pair was not intended for this subscription.
return 0;
}
target_subs->erase(it);
}
size = target_subs->size();
return info->buffer;
}
bool
matches_any_publishers(const rmw_gid_t * id) const
{
for (auto & publisher_pair : publishers_) {
auto publisher = publisher_pair.second.publisher.lock();
if (!publisher) {
continue;
}
if (*publisher.get() == id) {
return true;
}
}
return false;
}
private:
template<typename T>
using RebindAlloc = typename std::allocator_traits<Allocator>::template rebind_alloc<T>;
using AllocSet = std::set<uint64_t, std::less<uint64_t>, RebindAlloc<uint64_t>>;
using SubscriptionMap = std::unordered_map<uint64_t, subscription::SubscriptionBase::WeakPtr,
std::hash<uint64_t>, std::equal_to<uint64_t>,
RebindAlloc<std::pair<const uint64_t, subscription::SubscriptionBase::WeakPtr>>>;
using IDTopicMap = std::map<std::string, AllocSet,
std::less<std::string>, RebindAlloc<std::pair<std::string, AllocSet>>>;
SubscriptionMap subscriptions_;
IDTopicMap subscription_ids_by_topic_;
struct PublisherInfo
{
RCLCPP_DISABLE_COPY(PublisherInfo);
PublisherInfo() = default;
publisher::PublisherBase::WeakPtr publisher;
std::atomic<uint64_t> sequence_number;
mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer;
using TargetSubscriptionsMap = std::unordered_map<uint64_t, AllocSet,
std::hash<uint64_t>, std::equal_to<uint64_t>,
RebindAlloc<std::pair<const uint64_t, AllocSet>>>;
TargetSubscriptionsMap target_subscriptions_by_message_sequence;
};
using PublisherMap = std::unordered_map<uint64_t, PublisherInfo,
std::hash<uint64_t>, std::equal_to<uint64_t>,
RebindAlloc<std::pair<const uint64_t, PublisherInfo>>>;
PublisherMap publishers_;
};
static IntraProcessManagerStateBase::SharedPtr create_default_state()
rclcpp::intra_process_manager::IntraProcessManagerStateBase::SharedPtr
rclcpp::intra_process_manager::create_default_state()
{
return std::make_shared<IntraProcessManagerState<>>();
}
} // namespace intra_process_manager
} // namespace rclcpp
#endif // RCLCPP__INTRA_PROCESS_MANAGER_STATE_HPP_

19
rclcpp/src/rclcpp/memory_strategies.cpp
View file

@ -12,25 +12,14 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP__MEMORY_STRATEGIES_HPP_
#define RCLCPP__MEMORY_STRATEGIES_HPP_
#include "rclcpp/memory_strategies.hpp"
#include <rclcpp/memory_strategy.hpp>
#include <rclcpp/strategies/allocator_memory_strategy.hpp>
namespace rclcpp
{
namespace memory_strategies
{
#include "rclcpp/strategies/allocator_memory_strategy.hpp"
using rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy;
static memory_strategy::MemoryStrategy::SharedPtr create_default_strategy()
rclcpp::memory_strategy::MemoryStrategy::SharedPtr
rclcpp::memory_strategies::create_default_strategy()
{
return std::make_shared<AllocatorMemoryStrategy<>>();
}
} // namespace memory_strategies
} // namespace rclcpp
#endif // RCLCPP__MEMORY_STRATEGIES_HPP_

124
rclcpp/src/rclcpp/memory_strategy.cpp
View file

@ -12,68 +12,14 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP__MEMORY_STRATEGY_HPP_
#define RCLCPP__MEMORY_STRATEGY_HPP_
#include "rclcpp/memory_strategy.hpp"
#include <memory>
#include <vector>
using rclcpp::memory_strategy::MemoryStrategy;
#include "rclcpp/any_executable.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/node.hpp"
namespace rclcpp
rclcpp::subscription::SubscriptionBase::SharedPtr
MemoryStrategy::get_subscription_by_handle(
void * subscriber_handle, const WeakNodeVector & weak_nodes)
{
namespace memory_strategy
{
/// Delegate for handling memory allocations while the Executor is executing.
/**
* By default, the memory strategy dynamically allocates memory for structures that come in from
* the rmw implementation after the executor waits for work, based on the number of entities that
* come through.
*/
class MemoryStrategy
{
public:
RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(MemoryStrategy);
using WeakNodeVector = std::vector<std::weak_ptr<node::Node>>;
// return the new number of subscribers
virtual size_t fill_subscriber_handles(void ** & ptr) = 0;
// return the new number of services
virtual size_t fill_service_handles(void ** & ptr) = 0;
// return the new number of clients
virtual size_t fill_client_handles(void ** & ptr) = 0;
virtual void clear_active_entities() = 0;
virtual void clear_handles() = 0;
virtual void revalidate_handles() = 0;
virtual bool collect_entities(const WeakNodeVector & weak_nodes) = 0;
/// Provide a newly initialized AnyExecutable object.
// \return Shared pointer to the fresh executable.
virtual executor::AnyExecutable::SharedPtr instantiate_next_executable() = 0;
virtual void
get_next_subscription(executor::AnyExecutable::SharedPtr any_exec,
const WeakNodeVector & weak_nodes) = 0;
virtual void
get_next_service(executor::AnyExecutable::SharedPtr any_exec,
const WeakNodeVector & weak_nodes) = 0;
virtual void
get_next_client(executor::AnyExecutable::SharedPtr any_exec,
const WeakNodeVector & weak_nodes) = 0;
static rclcpp::subscription::SubscriptionBase::SharedPtr
get_subscription_by_handle(void * subscriber_handle,
const WeakNodeVector & weak_nodes)
{
for (auto & weak_node : weak_nodes) {
auto node = weak_node.lock();
if (!node) {
@ -100,11 +46,11 @@ public:
}
}
return nullptr;
}
}
static rclcpp::service::ServiceBase::SharedPtr
get_service_by_handle(void * service_handle, const WeakNodeVector & weak_nodes)
{
rclcpp::service::ServiceBase::SharedPtr
MemoryStrategy::get_service_by_handle(void * service_handle, const WeakNodeVector & weak_nodes)
{
for (auto & weak_node : weak_nodes) {
auto node = weak_node.lock();
if (!node) {
@ -123,11 +69,11 @@ public:
}
}
return nullptr;
}
}
static rclcpp::client::ClientBase::SharedPtr
get_client_by_handle(void * client_handle, const WeakNodeVector & weak_nodes)
{
rclcpp::client::ClientBase::SharedPtr
MemoryStrategy::get_client_by_handle(void * client_handle, const WeakNodeVector & weak_nodes)
{
for (auto & weak_node : weak_nodes) {
auto node = weak_node.lock();
if (!node) {
@ -146,12 +92,12 @@ public:
}
}
return nullptr;
}
}
static rclcpp::node::Node::SharedPtr
get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group,
rclcpp::node::Node::SharedPtr
MemoryStrategy::get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group,
const WeakNodeVector & weak_nodes)
{
{
if (!group) {
return nullptr;
}
@ -168,13 +114,13 @@ public:
}
}
return nullptr;
}
}
static rclcpp::callback_group::CallbackGroup::SharedPtr
get_group_by_subscription(
rclcpp::callback_group::CallbackGroup::SharedPtr
MemoryStrategy::get_group_by_subscription(
rclcpp::subscription::SubscriptionBase::SharedPtr subscription,
const WeakNodeVector & weak_nodes)
{
{
for (auto & weak_node : weak_nodes) {
auto node = weak_node.lock();
if (!node) {
@ -194,13 +140,13 @@ public:
}
}
return nullptr;
}
}
static rclcpp::callback_group::CallbackGroup::SharedPtr
get_group_by_service(
rclcpp::callback_group::CallbackGroup::SharedPtr
MemoryStrategy::get_group_by_service(
rclcpp::service::ServiceBase::SharedPtr service,
const WeakNodeVector & weak_nodes)
{
{
for (auto & weak_node : weak_nodes) {
auto node = weak_node.lock();
if (!node) {
@ -219,12 +165,13 @@ public:
}
}
return nullptr;
}
}
static rclcpp::callback_group::CallbackGroup::SharedPtr
get_group_by_client(rclcpp::client::ClientBase::SharedPtr client,
rclcpp::callback_group::CallbackGroup::SharedPtr
MemoryStrategy::get_group_by_client(
rclcpp::client::ClientBase::SharedPtr client,
const WeakNodeVector & weak_nodes)
{
{
for (auto & weak_node : weak_nodes) {
auto node = weak_node.lock();
if (!node) {
@ -243,13 +190,4 @@ public:
}
}
return nullptr;
}
};
} // namespace memory_strategy
} // namespace rclcpp
#endif // RCLCPP__MEMORY_STRATEGY_HPP_
}

370
rclcpp/src/rclcpp/node.cpp
View file

@ -1,4 +1,4 @@
// Copyright 2014 Open Source Robotics Foundation, Inc.
// Copyright 2015 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.
@ -12,38 +12,53 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP__NODE_IMPL_HPP_
#define RCLCPP__NODE_IMPL_HPP_
#include <rmw/error_handling.h>
#include <rmw/rmw.h>
#include <algorithm>
#include <cstdlib>
#include <iostream>
#include <limits>
#include <map>
#include <memory>
#include <sstream>
#include <stdexcept>
#include <string>
#include <utility>
#include <vector>
#include "rcl_interfaces/msg/intra_process_message.hpp"
#include "rosidl_generator_cpp/message_type_support.hpp"
#include "rosidl_generator_cpp/service_type_support.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/contexts/default_context.hpp"
#include "rclcpp/intra_process_manager.hpp"
#include "rclcpp/parameter.hpp"
// Specializations with references to unresolved symbols to delay evaluation until later.
// On Windows this is not necessary since it will be built against the rmw implementation directly.
#if !defined(WIN32)
namespace rosidl_generator_cpp
{
#ifndef RCLCPP__NODE_HPP_
#include "node.hpp"
template<>
const rosidl_message_type_support_t *
get_message_type_support_handle<rcl_interfaces::msg::ParameterEvent>()
{
return rclcpp::type_support::get_parameter_event_msg_type_support();
}
template<>
const rosidl_message_type_support_t *
get_message_type_support_handle<rcl_interfaces::msg::SetParametersResult>()
{
return rclcpp::type_support::get_set_parameters_result_msg_type_support();
}
template<>
const rosidl_message_type_support_t *
get_message_type_support_handle<rcl_interfaces::msg::ParameterDescriptor>()
{
return rclcpp::type_support::get_parameter_descriptor_msg_type_support();
}
template<>
const rosidl_message_type_support_t *
get_message_type_support_handle<rcl_interfaces::msg::ListParametersResult>()
{
return rclcpp::type_support::get_list_parameters_result_msg_type_support();
}
} // namespace rosidl_generator_cpp
#endif
using namespace rclcpp;
using namespace node;
using rclcpp::node::Node;
Node::Node(const std::string & node_name, bool use_intra_process_comms)
: Node(
@ -106,6 +121,12 @@ Node::Node(
"parameter_events", rmw_qos_profile_parameter_events);
}
const std::string &
Node::get_name() const
{
return name_;
}
rclcpp::callback_group::CallbackGroup::SharedPtr
Node::create_callback_group(
rclcpp::callback_group::CallbackGroupType group_type)
@ -117,95 +138,6 @@ Node::create_callback_group(
return group;
}
template<typename MessageT, typename Alloc>
typename rclcpp::publisher::Publisher<MessageT, Alloc>::SharedPtr
Node::create_publisher(
const std::string & topic_name, size_t qos_history_depth,
std::shared_ptr<Alloc> allocator)
{
if (!allocator) {
allocator = std::make_shared<Alloc>();
}
rmw_qos_profile_t qos = rmw_qos_profile_default;
qos.depth = qos_history_depth;
return this->create_publisher<MessageT, Alloc>(topic_name, qos, allocator);
}
template<typename MessageT, typename Alloc>
typename publisher::Publisher<MessageT, Alloc>::SharedPtr
Node::create_publisher(
const std::string & topic_name, const rmw_qos_profile_t & qos_profile,
std::shared_ptr<Alloc> allocator)
{
if (!allocator) {
allocator = std::make_shared<Alloc>();
}
using rosidl_generator_cpp::get_message_type_support_handle;
auto type_support_handle = get_message_type_support_handle<MessageT>();
rmw_publisher_t * publisher_handle = rmw_create_publisher(
node_handle_.get(), type_support_handle, topic_name.c_str(), qos_profile);
if (!publisher_handle) {
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
throw std::runtime_error(
std::string("could not create publisher: ") +
rmw_get_error_string_safe());
// *INDENT-ON*
}
auto publisher = publisher::Publisher<MessageT, Alloc>::make_shared(
node_handle_, publisher_handle, topic_name, qos_profile.depth, allocator);
if (use_intra_process_comms_) {
rmw_publisher_t * intra_process_publisher_handle = rmw_create_publisher(
node_handle_.get(), ipm_ts_, (topic_name + "__intra").c_str(), qos_profile);
if (!intra_process_publisher_handle) {
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
throw std::runtime_error(
std::string("could not create intra process publisher: ") +
rmw_get_error_string_safe());
// *INDENT-ON*
}
auto intra_process_manager =
context_->get_sub_context<rclcpp::intra_process_manager::IntraProcessManager>();
uint64_t intra_process_publisher_id =
intra_process_manager->add_publisher<MessageT, Alloc>(publisher);
rclcpp::intra_process_manager::IntraProcessManager::WeakPtr weak_ipm = intra_process_manager;
// *INDENT-OFF*
auto shared_publish_callback =
[weak_ipm](uint64_t publisher_id, void * msg, const std::type_info & type_info) -> uint64_t
{
auto ipm = weak_ipm.lock();
if (!ipm) {
// TODO(wjwwood): should this just return silently? Or maybe return with a logged warning?
throw std::runtime_error(
"intra process publish called after destruction of intra process manager");
}
if (!msg) {
throw std::runtime_error("cannot publisher msg which is a null pointer");
}
auto & message_type_info = typeid(MessageT);
if (message_type_info != type_info) {
throw std::runtime_error(
std::string("published type '") + type_info.name() +
"' is incompatible from the publisher type '" + message_type_info.name() + "'");
}
MessageT * typed_message_ptr = static_cast<MessageT *>(msg);
using MessageDeleter = typename publisher::Publisher<MessageT, Alloc>::MessageDeleter;
std::unique_ptr<MessageT, MessageDeleter> unique_msg(typed_message_ptr);
uint64_t message_seq =
ipm->store_intra_process_message<MessageT, Alloc>(publisher_id, unique_msg);
return message_seq;
};
// *INDENT-ON*
publisher->setup_intra_process(
intra_process_publisher_id,
shared_publish_callback,
intra_process_publisher_handle);
}
return publisher;
}
bool
Node::group_in_node(callback_group::CallbackGroup::SharedPtr group)
{
@ -219,138 +151,6 @@ Node::group_in_node(callback_group::CallbackGroup::SharedPtr group)
return group_belongs_to_this_node;
}
template<typename MessageT, typename CallbackT, typename Alloc>
typename rclcpp::subscription::Subscription<MessageT, Alloc>::SharedPtr
Node::create_subscription(
const std::string & topic_name,
CallbackT callback,
const rmw_qos_profile_t & qos_profile,
rclcpp::callback_group::CallbackGroup::SharedPtr group,
bool ignore_local_publications,
typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
msg_mem_strat,
typename std::shared_ptr<Alloc> allocator)
{
if (!allocator) {
allocator = std::make_shared<Alloc>();
}
rclcpp::subscription::AnySubscriptionCallback<MessageT,
Alloc> any_subscription_callback(allocator);
any_subscription_callback.set(callback);
using rosidl_generator_cpp::get_message_type_support_handle;
if (!msg_mem_strat) {
msg_mem_strat =
rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::create_default();
}
auto type_support_handle = get_message_type_support_handle<MessageT>();
rmw_subscription_t * subscriber_handle = rmw_create_subscription(
node_handle_.get(), type_support_handle,
topic_name.c_str(), qos_profile, ignore_local_publications);
if (!subscriber_handle) {
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
throw std::runtime_error(
std::string("could not create subscription: ") + rmw_get_error_string_safe());
// *INDENT-ON*
}
using namespace rclcpp::subscription;
auto sub = Subscription<MessageT, Alloc>::make_shared(
node_handle_,
subscriber_handle,
topic_name,
ignore_local_publications,
any_subscription_callback,
msg_mem_strat);
auto sub_base_ptr = std::dynamic_pointer_cast<SubscriptionBase>(sub);
// Setup intra process.
if (use_intra_process_comms_) {
rmw_subscription_t * intra_process_subscriber_handle = rmw_create_subscription(
node_handle_.get(), ipm_ts_,
(topic_name + "__intra").c_str(), qos_profile, false);
if (!subscriber_handle) {
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
throw std::runtime_error(
std::string("could not create intra process subscription: ") + rmw_get_error_string_safe());
// *INDENT-ON*
}
auto intra_process_manager =
context_->get_sub_context<rclcpp::intra_process_manager::IntraProcessManager>();
rclcpp::intra_process_manager::IntraProcessManager::WeakPtr weak_ipm = intra_process_manager;
uint64_t intra_process_subscription_id =
intra_process_manager->add_subscription(sub_base_ptr);
// *INDENT-OFF*
sub->setup_intra_process(
intra_process_subscription_id,
intra_process_subscriber_handle,
[weak_ipm](
uint64_t publisher_id,
uint64_t message_sequence,
uint64_t subscription_id,
typename Subscription<MessageT, Alloc>::MessageUniquePtr & message)
{
auto ipm = weak_ipm.lock();
if (!ipm) {
// TODO(wjwwood): should this just return silently? Or maybe return with a logged warning?
throw std::runtime_error(
"intra process take called after destruction of intra process manager");
}
ipm->take_intra_process_message<MessageT, Alloc>(
publisher_id, message_sequence, subscription_id, message);
},
[weak_ipm](const rmw_gid_t * sender_gid) -> bool {
auto ipm = weak_ipm.lock();
if (!ipm) {
throw std::runtime_error(
"intra process publisher check called after destruction of intra process manager");
}
return ipm->matches_any_publishers(sender_gid);
}
);
// *INDENT-ON*
}
// Assign to a group.
if (group) {
if (!group_in_node(group)) {
// TODO(jacquelinekay): use custom exception
throw std::runtime_error("Cannot create subscription, group not in node.");
}
group->add_subscription(sub_base_ptr);
} else {
default_callback_group_->add_subscription(sub_base_ptr);
}
number_of_subscriptions_++;
return sub;
}
template<typename MessageT, typename CallbackT, typename Alloc>
typename rclcpp::subscription::Subscription<MessageT, Alloc>::SharedPtr
Node::create_subscription(
const std::string & topic_name,
size_t qos_history_depth,
CallbackT callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group,
bool ignore_local_publications,
typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
msg_mem_strat,
std::shared_ptr<Alloc> allocator)
{
rmw_qos_profile_t qos = rmw_qos_profile_default;
qos.depth = qos_history_depth;
return this->create_subscription<MessageT, CallbackT, Alloc>(
topic_name,
callback,
qos,
group,
ignore_local_publications,
msg_mem_strat,
allocator);
}
rclcpp::timer::WallTimer::SharedPtr
Node::create_wall_timer(
std::chrono::nanoseconds period,
@ -384,88 +184,6 @@ Node::create_wall_timer(
// group);
// }
template<typename ServiceT>
typename client::Client<ServiceT>::SharedPtr
Node::create_client(
const std::string & service_name,
rclcpp::callback_group::CallbackGroup::SharedPtr group)
{
using rosidl_generator_cpp::get_service_type_support_handle;
auto service_type_support_handle =
get_service_type_support_handle<ServiceT>();
rmw_client_t * client_handle = rmw_create_client(
this->node_handle_.get(), service_type_support_handle, service_name.c_str());
if (!client_handle) {
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
throw std::runtime_error(
std::string("could not create client: ") +
rmw_get_error_string_safe());
// *INDENT-ON*
}
using namespace rclcpp::client;
auto cli = Client<ServiceT>::make_shared(
node_handle_,
client_handle,
service_name);
auto cli_base_ptr = std::dynamic_pointer_cast<ClientBase>(cli);
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 {
default_callback_group_->add_client(cli_base_ptr);
}
number_of_clients_++;
return cli;
}
template<typename ServiceT, typename FunctorT>
typename rclcpp::service::Service<ServiceT>::SharedPtr
Node::create_service(
const std::string & service_name,
FunctorT callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group)
{
using rosidl_generator_cpp::get_service_type_support_handle;
auto service_type_support_handle =
get_service_type_support_handle<ServiceT>();
rclcpp::service::AnyServiceCallback<ServiceT> any_service_callback;
any_service_callback.set(callback);
rmw_service_t * service_handle = rmw_create_service(
node_handle_.get(), service_type_support_handle, service_name.c_str());
if (!service_handle) {
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
throw std::runtime_error(
std::string("could not create service: ") +
rmw_get_error_string_safe());
// *INDENT-ON*
}
auto serv = service::Service<ServiceT>::make_shared(
node_handle_, service_handle, service_name, any_service_callback);
auto serv_base_ptr = std::dynamic_pointer_cast<service::ServiceBase>(serv);
if (group) {
if (!group_in_node(group)) {
// TODO(jacquelinekay): use custom exception
throw std::runtime_error("Cannot create service, group not in node.");
}
group->add_service(serv_base_ptr);
} else {
default_callback_group_->add_service(serv_base_ptr);
}
number_of_services_++;
return serv;
}
std::vector<rcl_interfaces::msg::SetParametersResult>
Node::set_parameters(
const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
@ -672,5 +390,3 @@ Node::get_callback_groups() const
{
return callback_groups_;
}
#endif // RCLCPP__NODE_IMPL_HPP_

299
rclcpp/src/rclcpp/parameter.cpp
View file

@ -12,93 +12,85 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_RCLCPP_PARAMETER_HPP_
#define RCLCPP_RCLCPP_PARAMETER_HPP_
#include "rclcpp/parameter.hpp"
#include <ostream>
#include <sstream>
#include <string>
#include <vector>
#include <rmw/rmw.h>
using rclcpp::parameter::ParameterType;
using rclcpp::parameter::ParameterVariant;
#include <rcl_interfaces/msg/parameter.hpp>
#include <rcl_interfaces/msg/parameter_type.hpp>
#include <rcl_interfaces/msg/parameter_value.hpp>
namespace rclcpp
ParameterVariant::ParameterVariant()
: name_("")
{
namespace parameter
{
enum ParameterType
{
PARAMETER_NOT_SET=rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET,
PARAMETER_BOOL=rcl_interfaces::msg::ParameterType::PARAMETER_BOOL,
PARAMETER_INTEGER=rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER,
PARAMETER_DOUBLE=rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE,
PARAMETER_STRING=rcl_interfaces::msg::ParameterType::PARAMETER_STRING,
PARAMETER_BYTES=rcl_interfaces::msg::ParameterType::PARAMETER_BYTES,
};
// Structure to store an arbitrary parameter with templated get/set methods
class ParameterVariant
{
public:
ParameterVariant()
: name_("")
{
value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET;
}
explicit ParameterVariant(const std::string & name, const bool bool_value)
: name_(name)
{
}
ParameterVariant::ParameterVariant(const std::string & name, const bool bool_value)
: name_(name)
{
value_.bool_value = bool_value;
value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
}
explicit ParameterVariant(const std::string & name, const int int_value)
: name_(name)
{
}
ParameterVariant::ParameterVariant(const std::string & name, const int int_value)
: name_(name)
{
value_.integer_value = int_value;
value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER;
}
explicit ParameterVariant(const std::string & name, const int64_t int_value)
: name_(name)
{
}
ParameterVariant::ParameterVariant(const std::string & name, const int64_t int_value)
: name_(name)
{
value_.integer_value = int_value;
value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER;
}
explicit ParameterVariant(const std::string & name, const float double_value)
: name_(name)
{
}
ParameterVariant::ParameterVariant(const std::string & name, const float double_value)
: name_(name)
{
value_.double_value = double_value;
value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
}
explicit ParameterVariant(const std::string & name, const double double_value)
: name_(name)
{
}
ParameterVariant::ParameterVariant(const std::string & name, const double double_value)
: name_(name)
{
value_.double_value = double_value;
value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
}
explicit ParameterVariant(const std::string & name, const std::string & string_value)
: name_(name)
{
}
ParameterVariant::ParameterVariant(const std::string & name, const std::string & string_value)
: name_(name)
{
value_.string_value = string_value;
value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING;
}
explicit ParameterVariant(const std::string & name, const char * string_value)
: ParameterVariant(name, std::string(string_value)) {}
explicit ParameterVariant(const std::string & name, const std::vector<uint8_t> & bytes_value)
: name_(name)
{
}
ParameterVariant::ParameterVariant(const std::string & name, const char * string_value)
: ParameterVariant(name, std::string(string_value))
{}
ParameterVariant::ParameterVariant(
const std::string & name, const std::vector<uint8_t> & bytes_value)
: name_(name)
{
value_.bytes_value = bytes_value;
value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BYTES;
}
}
inline ParameterType get_type() const {return static_cast<ParameterType>(value_.type); }
ParameterType
ParameterVariant::get_type() const
{
return static_cast<ParameterType>(value_.type);
}
inline std::string get_type_name() const
{
std::string
ParameterVariant::get_type_name() const
{
switch (get_type()) {
case rclcpp::parameter::ParameterType::PARAMETER_BOOL:
return "bool";
@ -118,82 +110,53 @@ public:
"Unexpected type from ParameterVariant: " + std::to_string(get_type()));
// *INDENT-ON*
}
}
}
inline std::string get_name() const & {return name_; }
const std::string &
ParameterVariant::get_name() const
{
return name_;
}
inline rcl_interfaces::msg::ParameterValue get_parameter_value() const
{
rcl_interfaces::msg::ParameterValue
ParameterVariant::get_parameter_value() const
{
return value_;
}
}
template<ParameterType type>
typename std::enable_if<type == ParameterType::PARAMETER_INTEGER, int64_t>::type
get_value() const
{
if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER) {
// TODO: use custom exception
throw std::runtime_error("Invalid type");
}
return value_.integer_value;
}
template<ParameterType type>
typename std::enable_if<type == ParameterType::PARAMETER_DOUBLE, double>::type
get_value() const
{
if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) {
// TODO: use custom exception
throw std::runtime_error("Invalid type");
}
return value_.double_value;
}
template<ParameterType type>
typename std::enable_if<type == ParameterType::PARAMETER_STRING, const std::string &>::type
get_value() const
{
if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_STRING) {
// TODO: use custom exception
throw std::runtime_error("Invalid type");
}
return value_.string_value;
}
template<ParameterType type>
typename std::enable_if<type == ParameterType::PARAMETER_BOOL, bool>::type
get_value() const
{
if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) {
// TODO: use custom exception
throw std::runtime_error("Invalid type");
}
return value_.bool_value;
}
template<ParameterType type>
typename std::enable_if<type == ParameterType::PARAMETER_BYTES,
const std::vector<uint8_t> &>::type
get_value() const
{
if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BYTES) {
// TODO: use custom exception
throw std::runtime_error("Invalid type");
}
return value_.bytes_value;
}
int64_t
ParameterVariant::as_int() const
{
return get_value<ParameterType::PARAMETER_INTEGER>();
}
int64_t as_int() const {return get_value<ParameterType::PARAMETER_INTEGER>(); }
double
ParameterVariant::as_double() const
{
return get_value<ParameterType::PARAMETER_DOUBLE>();
}
double as_double() const {return get_value<ParameterType::PARAMETER_DOUBLE>(); }
const std::string &
ParameterVariant::as_string() const
{
return get_value<ParameterType::PARAMETER_STRING>();
}
const std::string & as_string() const {return get_value<ParameterType::PARAMETER_STRING>(); }
bool
ParameterVariant::as_bool() const
{
return get_value<ParameterType::PARAMETER_BOOL>();
}
bool as_bool() const {return get_value<ParameterType::PARAMETER_BOOL>(); }
const std::vector<uint8_t> & as_bytes() const
{
const std::vector<uint8_t> &
ParameterVariant::as_bytes() const
{
return get_value<ParameterType::PARAMETER_BYTES>();
}
}
static ParameterVariant from_parameter(const rcl_interfaces::msg::Parameter & parameter)
{
ParameterVariant
ParameterVariant::from_parameter(const rcl_interfaces::msg::Parameter & parameter)
{
switch (parameter.value.type) {
case PARAMETER_BOOL:
return ParameterVariant(parameter.name, parameter.value.bool_value);
@ -214,18 +177,20 @@ public:
"Unexpected type from ParameterVariant: " + std::to_string(parameter.value.type));
// *INDENT-ON*
}
}
}
rcl_interfaces::msg::Parameter to_parameter()
{
rcl_interfaces::msg::Parameter
ParameterVariant::to_parameter()
{
rcl_interfaces::msg::Parameter parameter;
parameter.name = name_;
parameter.value = value_;
return parameter;
}
}
std::string value_to_string() const
{
std::string
ParameterVariant::value_to_string() const
{
switch (get_type()) {
case rclcpp::parameter::ParameterType::PARAMETER_BOOL:
return as_bool() ? "true" : "false";
@ -258,16 +223,10 @@ public:
"Unexpected type from ParameterVariant: " + std::to_string(get_type()));
// *INDENT-ON*
}
}
}
private:
std::string name_;
rcl_interfaces::msg::ParameterValue value_;
};
/* Return a json encoded version of the parameter intended for a dict. */
std::string _to_json_dict_entry(const ParameterVariant & param)
std::string
rclcpp::parameter::_to_json_dict_entry(const ParameterVariant & param)
{
std::stringstream ss;
ss << "\"" << param.get_name() << "\": ";
@ -276,15 +235,22 @@ std::string _to_json_dict_entry(const ParameterVariant & param)
return ss.str();
}
} /* namespace parameter */
} /* namespace rclcpp */
namespace std
std::ostream &
rclcpp::parameter::operator<<(std::ostream & os, const rclcpp::parameter::ParameterVariant & pv)
{
/* Return a json encoded version of the parameter intended for a list. */
inline std::string to_string(const rclcpp::parameter::ParameterVariant & param)
os << std::to_string(pv);
return os;
}
std::ostream &
rclcpp::parameter::operator<<(std::ostream & os, const std::vector<ParameterVariant> & parameters)
{
os << std::to_string(parameters);
return os;
}
std::string
std::to_string(const rclcpp::parameter::ParameterVariant & param)
{
std::stringstream ss;
ss << "{\"name\": \"" << param.get_name() << "\", ";
@ -293,8 +259,8 @@ inline std::string to_string(const rclcpp::parameter::ParameterVariant & param)
return ss.str();
}
/* Return a json encoded version of a vector of parameters, as a string*/
inline std::string to_string(const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
std::string
std::to_string(const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
{
std::stringstream ss;
ss << "{";
@ -310,28 +276,3 @@ inline std::string to_string(const std::vector<rclcpp::parameter::ParameterVaria
ss << "}";
return ss.str();
}
} /* namespace std */
namespace rclcpp
{
namespace parameter
{
std::ostream & operator<<(std::ostream & os, const rclcpp::parameter::ParameterVariant & pv)
{
os << std::to_string(pv);
return os;
}
std::ostream & operator<<(std::ostream & os, const std::vector<ParameterVariant> & parameters)
{
os << std::to_string(parameters);
return os;
}
} /* namespace parameter */
} /* namespace rclcpp */
#endif /* RCLCPP_RCLCPP_PARAMETER_HPP_ */

295
rclcpp/src/rclcpp/parameter_client.cpp
View file

@ -12,44 +12,71 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_RCLCPP_PARAMETER_CLIENT_HPP_
#define RCLCPP_RCLCPP_PARAMETER_CLIENT_HPP_
#include "rclcpp/parameter_client.hpp"
#include <algorithm>
#include <string>
#include <vector>
#include <rmw/rmw.h>
#include <rclcpp/executors.hpp>
#include <rclcpp/macros.hpp>
#include <rclcpp/node.hpp>
#include <rclcpp/parameter.hpp>
#include <rcl_interfaces/msg/parameter.hpp>
#include <rcl_interfaces/msg/parameter_event.hpp>
#include <rcl_interfaces/msg/parameter_value.hpp>
#include <rcl_interfaces/srv/describe_parameters.hpp>
#include <rcl_interfaces/srv/get_parameters.hpp>
#include <rcl_interfaces/srv/get_parameter_types.hpp>
#include <rcl_interfaces/srv/list_parameters.hpp>
#include <rcl_interfaces/srv/set_parameters.hpp>
#include <rcl_interfaces/srv/set_parameters_atomically.hpp>
namespace rclcpp
// Specializations with references to unresolved symbols to delay evaluation until later.
// On Windows this is not necessary since it will be built against the rmw implementation directly.
#if !defined(WIN32)
namespace rosidl_generator_cpp
{
namespace parameter_client
template<>
const rosidl_service_type_support_t *
get_service_type_support_handle<rcl_interfaces::srv::GetParameters>()
{
return rclcpp::type_support::get_get_parameters_srv_type_support();
}
class AsyncParametersClient
template<>
const rosidl_service_type_support_t *
get_service_type_support_handle<rcl_interfaces::srv::GetParameterTypes>()
{
return rclcpp::type_support::get_get_parameter_types_srv_type_support();
}
public:
RCLCPP_SMART_PTR_DEFINITIONS(AsyncParametersClient);
template<>
const rosidl_service_type_support_t *
get_service_type_support_handle<rcl_interfaces::srv::SetParameters>()
{
return rclcpp::type_support::get_set_parameters_srv_type_support();
}
AsyncParametersClient(const rclcpp::node::Node::SharedPtr node,
const std::string & remote_node_name = "")
: node_(node)
{
template<>
const rosidl_service_type_support_t *
get_service_type_support_handle<rcl_interfaces::srv::ListParameters>()
{
return rclcpp::type_support::get_list_parameters_srv_type_support();
}
template<>
const rosidl_service_type_support_t *
get_service_type_support_handle<rcl_interfaces::srv::DescribeParameters>()
{
return rclcpp::type_support::get_describe_parameters_srv_type_support();
}
template<>
const rosidl_service_type_support_t *
get_service_type_support_handle<rcl_interfaces::srv::SetParametersAtomically>()
{
return rclcpp::type_support::get_set_parameters_atomically_srv_type_support();
}
} // namespace rosidl_generator_cpp
#endif
using rclcpp::parameter_client::AsyncParametersClient;
using rclcpp::parameter_client::SyncParametersClient;
AsyncParametersClient::AsyncParametersClient(
const rclcpp::node::Node::SharedPtr node,
const std::string & remote_node_name)
: node_(node)
{
if (remote_node_name != "") {
remote_node_name_ = remote_node_name;
} else {
@ -65,14 +92,15 @@ public:
remote_node_name_ + "__list_parameters");
describe_parameters_client_ = node_->create_client<rcl_interfaces::srv::DescribeParameters>(
remote_node_name_ + "__describe_parameters");
}
}
std::shared_future<std::vector<rclcpp::parameter::ParameterVariant>>
get_parameters(
std::shared_future<std::vector<rclcpp::parameter::ParameterVariant>>
AsyncParametersClient::get_parameters(
const std::vector<std::string> & names,
std::function<void(
std::shared_future<std::vector<rclcpp::parameter::ParameterVariant>>)> callback = nullptr)
{
std::function<
void(std::shared_future<std::vector<rclcpp::parameter::ParameterVariant>>)
> callback)
{
auto promise_result =
std::make_shared<std::promise<std::vector<rclcpp::parameter::ParameterVariant>>>();
auto future_result = promise_result->get_future().share();
@ -80,10 +108,12 @@ public:
auto request = std::make_shared<rcl_interfaces::srv::GetParameters::Request>();
request->names = names;
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
get_parameters_client_->async_send_request(
request,
[request, promise_result, future_result, &callback](
rclcpp::client::Client<rcl_interfaces::srv::GetParameters>::SharedFuture cb_f) {
rclcpp::client::Client<rcl_interfaces::srv::GetParameters>::SharedFuture cb_f)
{
std::vector<rclcpp::parameter::ParameterVariant> parameter_variants;
auto & pvalues = cb_f.get()->values;
@ -102,16 +132,18 @@ public:
}
}
);
// *INDENT-ON*
return future_result;
}
}
std::shared_future<std::vector<rclcpp::parameter::ParameterType>>
get_parameter_types(
std::shared_future<std::vector<rclcpp::parameter::ParameterType>>
AsyncParametersClient::get_parameter_types(
const std::vector<std::string> & names,
std::function<void(
std::shared_future<std::vector<rclcpp::parameter::ParameterType>>)> callback = nullptr)
{
std::function<
void(std::shared_future<std::vector<rclcpp::parameter::ParameterType>>)
> callback)
{
auto promise_result =
std::make_shared<std::promise<std::vector<rclcpp::parameter::ParameterType>>>();
auto future_result = promise_result->get_future().share();
@ -119,10 +151,12 @@ public:
auto request = std::make_shared<rcl_interfaces::srv::GetParameterTypes::Request>();
request->names = names;
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
get_parameter_types_client_->async_send_request(
request,
[promise_result, future_result, &callback](
rclcpp::client::Client<rcl_interfaces::srv::GetParameterTypes>::SharedFuture cb_f) {
rclcpp::client::Client<rcl_interfaces::srv::GetParameterTypes>::SharedFuture cb_f)
{
std::vector<rclcpp::parameter::ParameterType> types;
auto & pts = cb_f.get()->types;
for (auto & pt : pts) {
@ -134,77 +168,91 @@ public:
}
}
);
// *INDENT-ON*
return future_result;
}
}
std::shared_future<std::vector<rcl_interfaces::msg::SetParametersResult>>
set_parameters(
std::shared_future<std::vector<rcl_interfaces::msg::SetParametersResult>>
AsyncParametersClient::set_parameters(
const std::vector<rclcpp::parameter::ParameterVariant> & parameters,
std::function<void(std::shared_future<std::vector<rcl_interfaces::msg::SetParametersResult>>)> callback =
nullptr)
{
std::function<
void(std::shared_future<std::vector<rcl_interfaces::msg::SetParametersResult>>)
> callback)
{
auto promise_result =
std::make_shared<std::promise<std::vector<rcl_interfaces::msg::SetParametersResult>>>();
auto future_result = promise_result->get_future().share();
auto request = std::make_shared<rcl_interfaces::srv::SetParameters::Request>();
std::transform(parameters.begin(), parameters.end(), std::back_inserter(
request->parameters), [](
rclcpp::parameter::ParameterVariant p) {return p.to_parameter(); });
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
std::transform(parameters.begin(), parameters.end(), std::back_inserter(request->parameters),
[](rclcpp::parameter::ParameterVariant p) {
return p.to_parameter();
}
);
set_parameters_client_->async_send_request(
request,
[promise_result, future_result, &callback](
rclcpp::client::Client<rcl_interfaces::srv::SetParameters>::SharedFuture cb_f) {
rclcpp::client::Client<rcl_interfaces::srv::SetParameters>::SharedFuture cb_f)
{
promise_result->set_value(cb_f.get()->results);
if (callback != nullptr) {
callback(future_result);
}
}
);
// *INDENT-ON*
return future_result;
}
}
std::shared_future<rcl_interfaces::msg::SetParametersResult>
set_parameters_atomically(
std::shared_future<rcl_interfaces::msg::SetParametersResult>
AsyncParametersClient::set_parameters_atomically(
const std::vector<rclcpp::parameter::ParameterVariant> & parameters,
std::function<void(std::shared_future<rcl_interfaces::msg::SetParametersResult>)> callback =
nullptr)
{
std::function<
void(std::shared_future<rcl_interfaces::msg::SetParametersResult>)
> callback)
{
auto promise_result =
std::make_shared<std::promise<rcl_interfaces::msg::SetParametersResult>>();
auto future_result = promise_result->get_future().share();
auto request = std::make_shared<rcl_interfaces::srv::SetParametersAtomically::Request>();
std::transform(parameters.begin(), parameters.end(), std::back_inserter(
request->parameters), [](
rclcpp::parameter::ParameterVariant p) {return p.to_parameter(); });
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
std::transform(parameters.begin(), parameters.end(), std::back_inserter(request->parameters),
[](rclcpp::parameter::ParameterVariant p) {
return p.to_parameter();
}
);
set_parameters_atomically_client_->async_send_request(
request,
[promise_result, future_result, &callback](
rclcpp::client::Client<rcl_interfaces::srv::SetParametersAtomically>::SharedFuture cb_f) {
rclcpp::client::Client<rcl_interfaces::srv::SetParametersAtomically>::SharedFuture cb_f)
{
promise_result->set_value(cb_f.get()->result);
if (callback != nullptr) {
callback(future_result);
}
}
);
// *INDENT-ON*
return future_result;
}
}
std::shared_future<rcl_interfaces::msg::ListParametersResult>
list_parameters(
std::shared_future<rcl_interfaces::msg::ListParametersResult>
AsyncParametersClient::list_parameters(
const std::vector<std::string> & prefixes,
uint64_t depth,
std::function<void(std::shared_future<rcl_interfaces::msg::ListParametersResult>)> callback =
nullptr)
{
std::function<
void(std::shared_future<rcl_interfaces::msg::ListParametersResult>)
> callback)
{
auto promise_result =
std::make_shared<std::promise<rcl_interfaces::msg::ListParametersResult>>();
auto future_result = promise_result->get_future().share();
@ -213,66 +261,42 @@ public:
request->prefixes = prefixes;
request->depth = depth;
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
list_parameters_client_->async_send_request(
request,
[promise_result, future_result, &callback](
rclcpp::client::Client<rcl_interfaces::srv::ListParameters>::SharedFuture cb_f) {
rclcpp::client::Client<rcl_interfaces::srv::ListParameters>::SharedFuture cb_f)
{
promise_result->set_value(cb_f.get()->result);
if (callback != nullptr) {
callback(future_result);
}
}
);
// *INDENT-ON*
return future_result;
}
}
template<typename FunctorT>
typename rclcpp::subscription::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr
on_parameter_event(FunctorT callback)
{
return node_->create_subscription<rcl_interfaces::msg::ParameterEvent>(
"parameter_events", callback, rmw_qos_profile_parameter_events);
}
private:
const rclcpp::node::Node::SharedPtr node_;
rclcpp::client::Client<rcl_interfaces::srv::GetParameters>::SharedPtr get_parameters_client_;
rclcpp::client::Client<rcl_interfaces::srv::GetParameterTypes>::SharedPtr
get_parameter_types_client_;
rclcpp::client::Client<rcl_interfaces::srv::SetParameters>::SharedPtr set_parameters_client_;
rclcpp::client::Client<rcl_interfaces::srv::SetParametersAtomically>::SharedPtr
set_parameters_atomically_client_;
rclcpp::client::Client<rcl_interfaces::srv::ListParameters>::SharedPtr list_parameters_client_;
rclcpp::client::Client<rcl_interfaces::srv::DescribeParameters>::SharedPtr
describe_parameters_client_;
std::string remote_node_name_;
};
class SyncParametersClient
{
public:
RCLCPP_SMART_PTR_DEFINITIONS(SyncParametersClient);
SyncParametersClient(
SyncParametersClient::SyncParametersClient(
rclcpp::node::Node::SharedPtr node)
: node_(node)
{
: node_(node)
{
executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
async_parameters_client_ = std::make_shared<AsyncParametersClient>(node);
}
}
SyncParametersClient(
SyncParametersClient::SyncParametersClient(
rclcpp::executor::Executor::SharedPtr executor,
rclcpp::node::Node::SharedPtr node)
: executor_(executor), node_(node)
{
: executor_(executor), node_(node)
{
async_parameters_client_ = std::make_shared<AsyncParametersClient>(node);
}
}
std::vector<rclcpp::parameter::ParameterVariant>
get_parameters(const std::vector<std::string> & parameter_names)
{
std::vector<rclcpp::parameter::ParameterVariant>
SyncParametersClient::get_parameters(const std::vector<std::string> & parameter_names)
{
auto f = async_parameters_client_->get_parameters(parameter_names);
if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
rclcpp::executors::FutureReturnCode::SUCCESS)
@ -281,11 +305,11 @@ public:
}
// Return an empty vector if unsuccessful
return std::vector<rclcpp::parameter::ParameterVariant>();
}
}
std::vector<rclcpp::parameter::ParameterType>
get_parameter_types(const std::vector<std::string> & parameter_names)
{
std::vector<rclcpp::parameter::ParameterType>
SyncParametersClient::get_parameter_types(const std::vector<std::string> & parameter_names)
{
auto f = async_parameters_client_->get_parameter_types(parameter_names);
if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
@ -294,11 +318,12 @@ public:
return f.get();
}
return std::vector<rclcpp::parameter::ParameterType>();
}
}
std::vector<rcl_interfaces::msg::SetParametersResult>
set_parameters(const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
{
std::vector<rcl_interfaces::msg::SetParametersResult>
SyncParametersClient::set_parameters(
const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
{
auto f = async_parameters_client_->set_parameters(parameters);
if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
@ -307,11 +332,12 @@ public:
return f.get();
}
return std::vector<rcl_interfaces::msg::SetParametersResult>();
}
}
rcl_interfaces::msg::SetParametersResult
set_parameters_atomically(const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
{
rcl_interfaces::msg::SetParametersResult
SyncParametersClient::set_parameters_atomically(
const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
{
auto f = async_parameters_client_->set_parameters_atomically(parameters);
if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
@ -321,13 +347,13 @@ public:
}
throw std::runtime_error("Unable to get result of set parameters service call.");
}
}
rcl_interfaces::msg::ListParametersResult
list_parameters(
rcl_interfaces::msg::ListParametersResult
SyncParametersClient::list_parameters(
const std::vector<std::string> & parameter_prefixes,
uint64_t depth)
{
{
auto f = async_parameters_client_->list_parameters(parameter_prefixes, depth);
if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
@ -337,23 +363,4 @@ public:
}
throw std::runtime_error("Unable to get result of list parameters service call.");
}
template<typename FunctorT>
typename rclcpp::subscription::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr
on_parameter_event(FunctorT callback)
{
return async_parameters_client_->on_parameter_event(callback);
}
private:
rclcpp::executor::Executor::SharedPtr executor_;
rclcpp::node::Node::SharedPtr node_;
AsyncParametersClient::SharedPtr async_parameters_client_;
};
} /* namespace parameter_client */
} /* namespace rclcpp */
#endif /* RCLCPP_RCLCPP_PARAMETER_CLIENT_HPP_ */
}

79
rclcpp/src/rclcpp/parameter_service.cpp
View file

@ -12,43 +12,21 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_RCLCPP_PARAMETER_SERVICE_HPP_
#define RCLCPP_RCLCPP_PARAMETER_SERVICE_HPP_
#include "rclcpp/parameter_service.hpp"
#include <string>
#include <algorithm>
#include <vector>
#include <rmw/rmw.h>
using rclcpp::parameter_service::ParameterService;
#include <rclcpp/executors.hpp>
#include <rclcpp/macros.hpp>
#include <rclcpp/node.hpp>
#include <rclcpp/parameter.hpp>
#include <rcl_interfaces/srv/describe_parameters.hpp>
#include <rcl_interfaces/srv/get_parameters.hpp>
#include <rcl_interfaces/srv/get_parameter_types.hpp>
#include <rcl_interfaces/srv/list_parameters.hpp>
#include <rcl_interfaces/srv/set_parameters.hpp>
#include <rcl_interfaces/srv/set_parameters_atomically.hpp>
namespace rclcpp
ParameterService::ParameterService(const rclcpp::node::Node::SharedPtr node)
: node_(node)
{
namespace parameter_service
{
class ParameterService
{
public:
RCLCPP_SMART_PTR_DEFINITIONS(ParameterService);
ParameterService(const rclcpp::node::Node::SharedPtr node)
: node_(node)
{
std::weak_ptr<rclcpp::node::Node> captured_node = node_;
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
get_parameters_service_ = node_->create_service<rcl_interfaces::srv::GetParameters>(
node_->get_name() + "__get_parameters", [captured_node](
node_->get_name() + "__get_parameters",
[captured_node](
const std::shared_ptr<rmw_request_id_t>,
const std::shared_ptr<rcl_interfaces::srv::GetParameters::Request> request,
std::shared_ptr<rcl_interfaces::srv::GetParameters::Response> response)
@ -65,7 +43,8 @@ public:
);
get_parameter_types_service_ = node_->create_service<rcl_interfaces::srv::GetParameterTypes>(
node_->get_name() + "__get_parameter_types", [captured_node](
node_->get_name() + "__get_parameter_types",
[captured_node](
const std::shared_ptr<rmw_request_id_t>,
const std::shared_ptr<rcl_interfaces::srv::GetParameterTypes::Request> request,
std::shared_ptr<rcl_interfaces::srv::GetParameterTypes::Response> response)
@ -83,7 +62,8 @@ public:
);
set_parameters_service_ = node_->create_service<rcl_interfaces::srv::SetParameters>(
node_->get_name() + "__set_parameters", [captured_node](
node_->get_name() + "__set_parameters",
[captured_node](
const std::shared_ptr<rmw_request_id_t>,
const std::shared_ptr<rcl_interfaces::srv::SetParameters::Request> request,
std::shared_ptr<rcl_interfaces::srv::SetParameters::Response> response)
@ -103,7 +83,8 @@ public:
set_parameters_atomically_service_ =
node_->create_service<rcl_interfaces::srv::SetParametersAtomically>(
node_->get_name() + "__set_parameters_atomically", [captured_node](
node_->get_name() + "__set_parameters_atomically",
[captured_node](
const std::shared_ptr<rmw_request_id_t>,
const std::shared_ptr<rcl_interfaces::srv::SetParametersAtomically::Request> request,
std::shared_ptr<rcl_interfaces::srv::SetParametersAtomically::Response> response)
@ -125,7 +106,8 @@ public:
);
describe_parameters_service_ = node_->create_service<rcl_interfaces::srv::DescribeParameters>(
node_->get_name() + "__describe_parameters", [captured_node](
node_->get_name() + "__describe_parameters",
[captured_node](
const std::shared_ptr<rmw_request_id_t>,
const std::shared_ptr<rcl_interfaces::srv::DescribeParameters::Request> request,
std::shared_ptr<rcl_interfaces::srv::DescribeParameters::Response> response)
@ -140,7 +122,8 @@ public:
);
list_parameters_service_ = node_->create_service<rcl_interfaces::srv::ListParameters>(
node_->get_name() + "__list_parameters", [captured_node](
node_->get_name() + "__list_parameters",
[captured_node](
const std::shared_ptr<rmw_request_id_t>,
const std::shared_ptr<rcl_interfaces::srv::ListParameters::Request> request,
std::shared_ptr<rcl_interfaces::srv::ListParameters::Response> response)
@ -153,25 +136,5 @@ public:
response->result = result;
}
);
}
private:
const rclcpp::node::Node::SharedPtr node_;
rclcpp::service::Service<rcl_interfaces::srv::GetParameters>::SharedPtr get_parameters_service_;
rclcpp::service::Service<rcl_interfaces::srv::GetParameterTypes>::SharedPtr
get_parameter_types_service_;
rclcpp::service::Service<rcl_interfaces::srv::SetParameters>::SharedPtr set_parameters_service_;
rclcpp::service::Service<rcl_interfaces::srv::SetParametersAtomically>::SharedPtr
set_parameters_atomically_service_;
rclcpp::service::Service<rcl_interfaces::srv::DescribeParameters>::SharedPtr
describe_parameters_service_;
rclcpp::service::Service<rcl_interfaces::srv::ListParameters>::SharedPtr list_parameters_service_;
};
} /* namespace parameter_service */
} /* namespace rclcpp */
#endif /* RCLCPP_RCLCPP_PARAMETER_SERVICE_HPP_ */
// *INDENT-ON*
}

291
rclcpp/src/rclcpp/publisher.cpp
View file

@ -1,4 +1,4 @@
// Copyright 2014 Open Source Robotics Foundation, Inc.
// Copyright 2015 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.
@ -12,8 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP__PUBLISHER_HPP_
#define RCLCPP__PUBLISHER_HPP_
#include "rclcpp/publisher.hpp"
#include <rmw/error_handling.h>
#include <rmw/rmw.h>
@ -27,44 +26,22 @@
#include "rcl_interfaces/msg/intra_process_message.hpp"
#include "rmw/impl/cpp/demangle.hpp"
#include "rclcpp/allocator/allocator_common.hpp"
#include "rclcpp/allocator/allocator_deleter.hpp"
#include "rclcpp/macros.hpp"
namespace rclcpp
{
using rclcpp::publisher::PublisherBase;
// Forward declaration for friend statement
namespace node
{
class Node;
} // namespace node
namespace publisher
{
class PublisherBase
{
public:
RCLCPP_SMART_PTR_DEFINITIONS(PublisherBase);
/// Default constructor.
/**
* Typically, a publisher is not created through this method, but instead is created through a
* call to `Node::create_publisher`.
* \param[in] node_handle The corresponding rmw representation of the owner node.
* \param[in] publisher_handle The rmw publisher handle corresponding to this publisher.
* \param[in] topic The topic that this publisher publishes on.
* \param[in] queue_size The maximum number of unpublished messages to queue.
*/
PublisherBase(
PublisherBase::PublisherBase(
std::shared_ptr<rmw_node_t> node_handle,
rmw_publisher_t * publisher_handle,
std::string topic,
size_t queue_size)
: node_handle_(node_handle), publisher_handle_(publisher_handle),
: node_handle_(node_handle), publisher_handle_(publisher_handle),
intra_process_publisher_handle_(nullptr),
topic_(topic), queue_size_(queue_size),
intra_process_publisher_id_(0), store_intra_process_message_(nullptr)
{
{
// Life time of this object is tied to the publisher handle.
if (rmw_get_gid_for_publisher(publisher_handle_, &rmw_gid_) != RMW_RET_OK) {
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
@ -72,11 +49,10 @@ public:
std::string("failed to get publisher gid: ") + rmw_get_error_string_safe());
// *INDENT-ON*
}
}
}
/// Default destructor.
virtual ~PublisherBase()
{
PublisherBase::~PublisherBase()
{
if (intra_process_publisher_handle_) {
if (rmw_destroy_publisher(node_handle_.get(), intra_process_publisher_handle_)) {
fprintf(
@ -93,61 +69,41 @@ public:
rmw_get_error_string_safe());
}
}
}
}
/// Get the topic that this publisher publishes on.
// \return The topic name.
const std::string &
get_topic_name() const
{
const std::string &
PublisherBase::get_topic_name() const
{
return topic_;
}
}
/// Get the queue size for this publisher.
// \return The queue size.
size_t
get_queue_size() const
{
size_t
PublisherBase::get_queue_size() const
{
return queue_size_;
}
}
/// Get the global identifier for this publisher (used in rmw and by DDS).
// \return The gid.
const rmw_gid_t &
get_gid() const
{
const rmw_gid_t &
PublisherBase::get_gid() const
{
return rmw_gid_;
}
}
/// Get the global identifier for this publisher used by intra-process communication.
// \return The intra-process gid.
const rmw_gid_t &
get_intra_process_gid() const
{
const rmw_gid_t &
PublisherBase::get_intra_process_gid() const
{
return intra_process_rmw_gid_;
}
}
/// Compare this publisher to a gid.
/**
* Note that this function calls the next function.
* \param[in] gid Reference to a gid.
* \return True if the publisher's gid matches the input.
*/
bool
operator==(const rmw_gid_t & gid) const
{
bool
PublisherBase::operator==(const rmw_gid_t & gid) const
{
return *this == &gid;
}
}
/// Compare this publisher to a pointer gid.
/**
* A wrapper for comparing this publisher's gid to the input using rmw_compare_gids_equal.
* \param[in] gid A pointer to a gid.
* \return True if this publisher's gid matches the input.
*/
bool
operator==(const rmw_gid_t * gid) const
{
bool
PublisherBase::operator==(const rmw_gid_t * gid) const
{
bool result = false;
auto ret = rmw_compare_gids_equal(gid, &this->get_gid(), &result);
if (ret != RMW_RET_OK) {
@ -162,17 +118,14 @@ public:
}
}
return result;
}
}
typedef std::function<uint64_t(uint64_t, void *, const std::type_info &)> StoreMessageCallbackT;
protected:
void
setup_intra_process(
void
PublisherBase::setup_intra_process(
uint64_t intra_process_publisher_id,
StoreMessageCallbackT callback,
rmw_publisher_t * intra_process_publisher_handle)
{
{
intra_process_publisher_id_ = intra_process_publisher_id;
store_intra_process_message_ = callback;
intra_process_publisher_handle_ = intra_process_publisher_handle;
@ -185,170 +138,4 @@ protected:
rmw_get_error_string_safe());
// *INDENT-ON*
}
}
std::shared_ptr<rmw_node_t> node_handle_;
rmw_publisher_t * publisher_handle_;
rmw_publisher_t * intra_process_publisher_handle_;
std::string topic_;
size_t queue_size_;
uint64_t intra_process_publisher_id_;
StoreMessageCallbackT store_intra_process_message_;
rmw_gid_t rmw_gid_;
rmw_gid_t intra_process_rmw_gid_;
std::mutex intra_process_publish_mutex_;
};
/// A publisher publishes messages of any type to a topic.
template<typename MessageT, typename Alloc = std::allocator<void>>
class Publisher : public PublisherBase
{
friend rclcpp::node::Node;
public:
using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
using MessageAlloc = typename MessageAllocTraits::allocator_type;
using MessageDeleter = allocator::Deleter<MessageAlloc, MessageT>;
using MessageUniquePtr = std::unique_ptr<MessageT, MessageDeleter>;
RCLCPP_SMART_PTR_DEFINITIONS(Publisher<MessageT, Alloc>);
Publisher(
std::shared_ptr<rmw_node_t> node_handle,
rmw_publisher_t * publisher_handle,
std::string topic,
size_t queue_size,
std::shared_ptr<Alloc> allocator)
: PublisherBase(node_handle, publisher_handle, topic, queue_size)
{
message_allocator_ = std::make_shared<MessageAlloc>(*allocator.get());
allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get());
}
/// Send a message to the topic for this publisher.
/**
* This function is templated on the input message type, MessageT.
* \param[in] msg A shared pointer to the message to send.
*/
void
publish(std::unique_ptr<MessageT, MessageDeleter> & msg)
{
this->do_inter_process_publish(msg.get());
if (store_intra_process_message_) {
// Take the pointer from the unique_msg, release it and pass as a void *
// to the ipm. The ipm should then capture it again as a unique_ptr of
// the correct type.
// TODO(wjwwood):
// investigate how to transfer the custom deleter (if there is one)
// from the incoming unique_ptr through to the ipm's unique_ptr.
// See: http://stackoverflow.com/questions/11002641/dynamic-casting-for-unique-ptr
MessageT * msg_ptr = msg.get();
msg.release();
uint64_t message_seq;
{
std::lock_guard<std::mutex> lock(intra_process_publish_mutex_);
message_seq =
store_intra_process_message_(intra_process_publisher_id_, msg_ptr, typeid(MessageT));
}
rcl_interfaces::msg::IntraProcessMessage ipm;
ipm.publisher_id = intra_process_publisher_id_;
ipm.message_sequence = message_seq;
auto status = rmw_publish(intra_process_publisher_handle_, &ipm);
if (status != RMW_RET_OK) {
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
throw std::runtime_error(
std::string("failed to publish intra process message: ") + rmw_get_error_string_safe());
// *INDENT-ON*
}
} else {
// Always destroy the message, even if we don't consume it, for consistency.
msg.reset();
}
}
void
publish(const std::shared_ptr<MessageT> & msg)
{
// Avoid allocating when not using intra process.
if (!store_intra_process_message_) {
// In this case we're not using intra process.
return this->do_inter_process_publish(msg.get());
}
// Otherwise we have to allocate memory in a unique_ptr and pass it along.
// TODO(wjwwood):
// The intra process manager should probably also be able to store
// shared_ptr's and do the "smart" thing based on other intra process
// subscriptions. For now call the other publish().
auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1);
MessageAllocTraits::construct(*message_allocator_.get(), ptr, *msg.get());
MessageUniquePtr unique_msg(ptr, message_deleter_);
return this->publish(unique_msg);
}
void
publish(std::shared_ptr<const MessageT> msg)
{
// Avoid allocating when not using intra process.
if (!store_intra_process_message_) {
// In this case we're not using intra process.
return this->do_inter_process_publish(msg.get());
}
// Otherwise we have to allocate memory in a unique_ptr and pass it along.
// TODO(wjwwood):
// The intra process manager should probably also be able to store
// shared_ptr's and do the "smart" thing based on other intra process
// subscriptions. For now call the other publish().
auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1);
MessageAllocTraits::construct(*message_allocator_.get(), ptr, *msg.get());
MessageUniquePtr unique_msg(ptr, message_deleter_);
return this->publish(unique_msg);
}
void
publish(const MessageT & msg)
{
// Avoid allocating when not using intra process.
if (!store_intra_process_message_) {
// In this case we're not using intra process.
return this->do_inter_process_publish(&msg);
}
// Otherwise we have to allocate memory in a unique_ptr and pass it along.
auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1);
MessageAllocTraits::construct(*message_allocator_.get(), ptr, msg);
MessageUniquePtr unique_msg(ptr, message_deleter_);
return this->publish(unique_msg);
}
std::shared_ptr<MessageAlloc> get_allocator() const
{
return message_allocator_;
}
protected:
void
do_inter_process_publish(const MessageT * msg)
{
auto status = rmw_publish(publisher_handle_, msg);
if (status != RMW_RET_OK) {
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
throw std::runtime_error(
std::string("failed to publish message: ") + rmw_get_error_string_safe());
// *INDENT-ON*
}
}
std::shared_ptr<MessageAlloc> message_allocator_;
MessageDeleter message_deleter_;
};
} // namespace publisher
} // namespace rclcpp
#endif // RCLCPP__PUBLISHER_HPP_
}

143
rclcpp/src/rclcpp/service.cpp
View file

@ -1,4 +1,4 @@
// Copyright 2014 Open Source Robotics Foundation, Inc.
// Copyright 2015 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.
@ -12,8 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_RCLCPP_SERVICE_HPP_
#define RCLCPP_RCLCPP_SERVICE_HPP_
#include "rclcpp/service.hpp"
#include <functional>
#include <iostream>
@ -21,33 +20,22 @@
#include <sstream>
#include <string>
#include <rmw/error_handling.h>
#include <rmw/rmw.h>
#include "rclcpp/any_service_callback.hpp"
#include "rclcpp/macros.hpp"
#include "rmw/error_handling.h"
#include "rmw/rmw.h"
#include <rclcpp/macros.hpp>
#include <rclcpp/any_service_callback.hpp>
using rclcpp::service::ServiceBase;
namespace rclcpp
{
namespace service
{
class ServiceBase
{
public:
RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ServiceBase);
ServiceBase(
ServiceBase::ServiceBase(
std::shared_ptr<rmw_node_t> node_handle,
rmw_service_t * service_handle,
const std::string service_name)
: node_handle_(node_handle), service_handle_(service_handle), service_name_(service_name)
{}
: node_handle_(node_handle), service_handle_(service_handle), service_name_(service_name)
{}
virtual ~ServiceBase()
{
ServiceBase::~ServiceBase()
{
if (service_handle_) {
if (rmw_destroy_service(service_handle_) != RMW_RET_OK) {
std::stringstream ss;
@ -56,103 +44,16 @@ public:
(std::cerr << ss.str()).flush();
}
}
}
}
std::string get_service_name()
{
return this->service_name_;
}
const rmw_service_t * get_service_handle()
{
return this->service_handle_;
}
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;
private:
RCLCPP_DISABLE_COPY(ServiceBase);
std::shared_ptr<rmw_node_t> node_handle_;
rmw_service_t * service_handle_;
std::string service_name_;
};
using namespace any_service_callback;
template<typename ServiceT>
class Service : public ServiceBase
std::string
ServiceBase::get_service_name()
{
public:
using CallbackType = std::function<
void(
const std::shared_ptr<typename ServiceT::Request>,
std::shared_ptr<typename ServiceT::Response>)>;
return this->service_name_;
}
using CallbackWithHeaderType = std::function<
void(
const std::shared_ptr<rmw_request_id_t>,
const std::shared_ptr<typename ServiceT::Request>,
std::shared_ptr<typename ServiceT::Response>)>;
RCLCPP_SMART_PTR_DEFINITIONS(Service);
Service(
std::shared_ptr<rmw_node_t> node_handle,
rmw_service_t * service_handle,
const std::string & service_name,
AnyServiceCallback<ServiceT> any_callback)
: ServiceBase(node_handle, service_handle, service_name), any_callback_(any_callback)
{}
Service() = delete;
std::shared_ptr<void> create_request()
{
return std::shared_ptr<void>(new typename ServiceT::Request());
}
std::shared_ptr<void> create_request_header()
{
// TODO(wjwwood): This should probably use rmw_request_id's allocator.
// (since it is a C type)
return std::shared_ptr<void>(new rmw_request_id_t);
}
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);
auto response = std::shared_ptr<typename ServiceT::Response>(new typename ServiceT::Response);
any_callback_.dispatch(typed_request_header, typed_request, response);
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)
{
rmw_ret_t status = rmw_send_response(get_service_handle(), req_id.get(), response.get());
if (status != RMW_RET_OK) {
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
throw std::runtime_error(
std::string("failed to send response: ") + rmw_get_error_string_safe());
// *INDENT-ON*
}
}
private:
RCLCPP_DISABLE_COPY(Service);
AnyServiceCallback<ServiceT> any_callback_;
};
} /* namespace service */
} /* namespace rclcpp */
#endif /* RCLCPP_RCLCPP_SERVICE_HPP_ */
const rmw_service_t *
ServiceBase::get_service_handle()
{
return this->service_handle_;
}

260
rclcpp/src/rclcpp/subscription.cpp
View file

@ -1,4 +1,4 @@
// Copyright 2014 Open Source Robotics Foundation, Inc.
// Copyright 2015 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.
@ -12,66 +12,34 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP__SUBSCRIPTION_HPP_
#define RCLCPP__SUBSCRIPTION_HPP_
#include "rclcpp/subscription.hpp"
#include <rmw/error_handling.h>
#include <rmw/rmw.h>
#include <functional>
#include <iostream>
#include <memory>
#include <sstream>
#include <cstdio>
#include <string>
#include "rcl_interfaces/msg/intra_process_message.hpp"
#include "rmw/error_handling.h"
#include "rmw/rmw.h"
#include "rclcpp/macros.hpp"
#include "rclcpp/message_memory_strategy.hpp"
#include "rclcpp/any_subscription_callback.hpp"
namespace rclcpp
{
using rclcpp::subscription::SubscriptionBase;
namespace node
{
class Node;
} // namespace node
namespace subscription
{
/// Virtual base class for subscriptions. This pattern allows us to iterate over different template
/// specializations of Subscription, among other things.
class SubscriptionBase
{
public:
RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(SubscriptionBase);
/// Default constructor.
/**
* \param[in] node_handle The rmw representation of the node that owns this subscription.
* \param[in] topic_name Name of the topic to subscribe to.
* \param[in] ignore_local_publications True to ignore local publications (unused).
*/
SubscriptionBase(
SubscriptionBase::SubscriptionBase(
std::shared_ptr<rmw_node_t> node_handle,
rmw_subscription_t * subscription_handle,
const std::string & topic_name,
bool ignore_local_publications)
: intra_process_subscription_handle_(nullptr),
: intra_process_subscription_handle_(nullptr),
node_handle_(node_handle),
subscription_handle_(subscription_handle),
topic_name_(topic_name),
ignore_local_publications_(ignore_local_publications)
{
{
// To avoid unused private member warnings.
(void)ignore_local_publications_;
}
}
/// Default destructor.
virtual ~SubscriptionBase()
{
SubscriptionBase::~SubscriptionBase()
{
if (subscription_handle_) {
if (rmw_destroy_subscription(node_handle_.get(), subscription_handle_) != RMW_RET_OK) {
std::stringstream ss;
@ -89,196 +57,22 @@ public:
(std::cerr << ss.str()).flush();
}
}
}
}
/// Get the topic that this subscription is subscribed on.
const std::string & get_topic_name() const
{
return this->topic_name_;
}
const rmw_subscription_t * get_subscription_handle() const
{
return subscription_handle_;
}
const rmw_subscription_t * get_intra_process_subscription_handle() const
{
return intra_process_subscription_handle_;
}
/// Borrow a new message.
// \return Shared pointer to the fresh message.
virtual std::shared_ptr<void> create_message() = 0;
/// Check if we need to handle the message, and execute the callback if we do.
/**
* \param[in] message Shared pointer to the message to handle.
* \param[in] message_info Metadata associated with this message.
*/
virtual void handle_message(
std::shared_ptr<void> & message,
const rmw_message_info_t & message_info) = 0;
/// Return the message borrowed in create_message.
// \param[in] Shared pointer to the returned message.
virtual void return_message(std::shared_ptr<void> & message) = 0;
virtual void handle_intra_process_message(
rcl_interfaces::msg::IntraProcessMessage & ipm,
const rmw_message_info_t & message_info) = 0;
protected:
rmw_subscription_t * intra_process_subscription_handle_;
private:
RCLCPP_DISABLE_COPY(SubscriptionBase);
std::shared_ptr<rmw_node_t> node_handle_;
rmw_subscription_t * subscription_handle_;
std::string topic_name_;
bool ignore_local_publications_;
};
using namespace any_subscription_callback;
/// Subscription implementation, templated on the type of message this subscription receives.
template<typename MessageT, typename Alloc = std::allocator<void>>
class Subscription : public SubscriptionBase
const std::string &
SubscriptionBase::get_topic_name() const
{
friend class rclcpp::node::Node;
return this->topic_name_;
}
public:
using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
using MessageAlloc = typename MessageAllocTraits::allocator_type;
using MessageDeleter = allocator::Deleter<MessageAlloc, MessageT>;
using MessageUniquePtr = std::unique_ptr<MessageT, MessageDeleter>;
const rmw_subscription_t *
SubscriptionBase::get_subscription_handle() const
{
return subscription_handle_;
}
RCLCPP_SMART_PTR_DEFINITIONS(Subscription);
/// Default constructor.
/**
* The constructor for a subscription is almost never called directly. Instead, subscriptions
* should be instantiated through Node::create_subscription.
* \param[in] node_handle rmw representation of the node that owns this subscription.
* \param[in] topic_name Name of the topic to subscribe to.
* \param[in] ignore_local_publications True to ignore local publications (unused).
* \param[in] callback User-defined callback to call when a message is received.
* \param[in] memory_strategy The memory strategy to be used for managing message memory.
*/
Subscription(
std::shared_ptr<rmw_node_t> node_handle,
rmw_subscription_t * subscription_handle,
const std::string & topic_name,
bool ignore_local_publications,
AnySubscriptionCallback<MessageT, Alloc> callback,
typename message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr memory_strategy =
message_memory_strategy::MessageMemoryStrategy<MessageT,
Alloc>::create_default())
: SubscriptionBase(node_handle, subscription_handle, topic_name, ignore_local_publications),
any_callback_(callback),
message_memory_strategy_(memory_strategy),
get_intra_process_message_callback_(nullptr),
matches_any_intra_process_publishers_(nullptr)
{
}
/// Support dynamically setting the message memory strategy.
/**
* Behavior may be undefined if called while the subscription could be executing.
* \param[in] message_memory_strategy Shared pointer to the memory strategy to set.
*/
void set_message_memory_strategy(
typename message_memory_strategy::MessageMemoryStrategy<MessageT,
Alloc>::SharedPtr message_memory_strategy)
{
message_memory_strategy_ = message_memory_strategy;
}
std::shared_ptr<void> create_message()
{
/* The default message memory strategy provides a dynamically allocated message on each call to
* create_message, though alternative memory strategies that re-use a preallocated message may be
* used (see rclcpp/strategies/message_pool_memory_strategy.hpp).
*/
return message_memory_strategy_->borrow_message();
}
void handle_message(std::shared_ptr<void> & message, const rmw_message_info_t & message_info)
{
if (matches_any_intra_process_publishers_) {
if (matches_any_intra_process_publishers_(&message_info.publisher_gid)) {
// In this case, the message will be delivered via intra process and
// we should ignore this copy of the message.
return;
}
}
auto typed_message = std::static_pointer_cast<MessageT>(message);
any_callback_.dispatch(typed_message, message_info);
}
void return_message(std::shared_ptr<void> & message)
{
auto typed_message = std::static_pointer_cast<MessageT>(message);
message_memory_strategy_->return_message(typed_message);
}
void handle_intra_process_message(
rcl_interfaces::msg::IntraProcessMessage & ipm,
const rmw_message_info_t & message_info)
{
if (!get_intra_process_message_callback_) {
// throw std::runtime_error(
// "handle_intra_process_message called before setup_intra_process");
// TODO(wjwwood): for now, this could mean that intra process was just not enabled.
// However, this can only really happen if this node has it disabled, but the other doesn't.
return;
}
MessageUniquePtr msg;
get_intra_process_message_callback_(
ipm.publisher_id,
ipm.message_sequence,
intra_process_subscription_id_,
msg);
if (!msg) {
// This either occurred because the publisher no longer exists or the
// message requested is no longer being stored.
// TODO(wjwwood): should we notify someone of this? log error, log warning?
return;
}
any_callback_.dispatch_intra_process(msg, message_info);
}
private:
typedef
std::function<
void (uint64_t, uint64_t, uint64_t, MessageUniquePtr &)
> GetMessageCallbackType;
typedef std::function<bool (const rmw_gid_t *)> MatchesAnyPublishersCallbackType;
void setup_intra_process(
uint64_t intra_process_subscription_id,
rmw_subscription_t * intra_process_subscription,
GetMessageCallbackType get_message_callback,
MatchesAnyPublishersCallbackType matches_any_publisher_callback)
{
intra_process_subscription_id_ = intra_process_subscription_id;
intra_process_subscription_handle_ = intra_process_subscription;
get_intra_process_message_callback_ = get_message_callback;
matches_any_intra_process_publishers_ = matches_any_publisher_callback;
}
RCLCPP_DISABLE_COPY(Subscription);
AnySubscriptionCallback<MessageT, Alloc> any_callback_;
typename message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
message_memory_strategy_;
GetMessageCallbackType get_intra_process_message_callback_;
MatchesAnyPublishersCallbackType matches_any_intra_process_publishers_;
uint64_t intra_process_subscription_id_;
};
} // namespace subscription
} // namespace rclcpp
#endif // RCLCPP__SUBSCRIPTION_HPP_
const rmw_subscription_t *
SubscriptionBase::get_intra_process_subscription_handle() const
{
return intra_process_subscription_handle_;
}

176
rclcpp/src/rclcpp/timer.cpp
View file

@ -1,4 +1,4 @@
// Copyright 2014 Open Source Robotics Foundation, Inc.
// Copyright 2015 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.
@ -12,166 +12,36 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_RCLCPP_TIMER_HPP_
#define RCLCPP_RCLCPP_TIMER_HPP_
#include "rclcpp/timer.hpp"
#include <chrono>
#include <functional>
#include <memory>
#include <sstream>
#include <thread>
#include <rmw/error_handling.h>
#include <rmw/rmw.h>
using rclcpp::timer::CallbackType;
using rclcpp::timer::TimerBase;
#include <rclcpp/macros.hpp>
#include <rclcpp/rate.hpp>
#include <rclcpp/utilities.hpp>
namespace rclcpp
{
namespace timer
{
using CallbackType = std::function<void()>;
class TimerBase
{
public:
RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(TimerBase);
TimerBase(std::chrono::nanoseconds period, CallbackType callback)
: period_(period),
TimerBase::TimerBase(std::chrono::nanoseconds period, CallbackType callback)
: period_(period),
callback_(callback),
canceled_(false)
{
}
{}
virtual ~TimerBase()
{
}
TimerBase::~TimerBase()
{}
void
cancel()
{
this->canceled_ = true;
}
void execute_callback() const
{
callback_();
}
const CallbackType & get_callback() const
{
return callback_;
}
/// Check how long the timer has until its next scheduled callback.
// \return A std::chrono::duration representing the relative time until the next callback.
virtual std::chrono::nanoseconds
time_until_trigger() = 0;
/// Is the clock steady (i.e. is the time between ticks constant?)
// \return True if the clock used by this timer is steady.
virtual bool is_steady() = 0;
/// Check if the timer needs to trigger the callback.
/**
* This function expects its caller to immediately trigger the callback after this function,
* since it maintains the last time the callback was triggered.
* \return True if the timer needs to trigger.
*/
virtual bool check_and_trigger() = 0;
protected:
std::chrono::nanoseconds period_;
CallbackType callback_;
bool canceled_;
};
/// Generic timer templated on the clock type. Periodically executes a user-specified callback.
template<class Clock = std::chrono::high_resolution_clock>
class GenericTimer : public TimerBase
void
TimerBase::cancel()
{
this->canceled_ = true;
}
public:
RCLCPP_SMART_PTR_DEFINITIONS(GenericTimer);
void
TimerBase::execute_callback() const
{
callback_();
}
/// Default constructor.
/**
* \param[in] period The interval at which the timer fires.
* \param[in] callback User-specified callback function.
*/
GenericTimer(std::chrono::nanoseconds period, CallbackType callback)
: TimerBase(period, callback), loop_rate_(period)
{
/* Subtracting the loop rate period ensures that the callback gets triggered
on the first call to check_and_trigger. */
last_triggered_time_ = Clock::now() - period;
}
/// Default destructor.
virtual ~GenericTimer()
{
// Stop the timer from running.
cancel();
}
bool
check_and_trigger()
{
if (canceled_) {
return false;
}
if (Clock::now() < last_triggered_time_) {
return false;
}
if (std::chrono::duration_cast<std::chrono::nanoseconds>(Clock::now() - last_triggered_time_) >=
loop_rate_.period())
{
last_triggered_time_ = Clock::now();
return true;
}
return false;
}
std::chrono::nanoseconds
time_until_trigger()
{
std::chrono::nanoseconds time_until_trigger;
// Calculate the time between the next trigger and the current time
if (last_triggered_time_ + loop_rate_.period() < Clock::now()) {
// time is overdue, need to trigger immediately
time_until_trigger = std::chrono::nanoseconds::zero();
} else {
time_until_trigger = std::chrono::duration_cast<std::chrono::nanoseconds>(
last_triggered_time_ - Clock::now()) + loop_rate_.period();
}
return time_until_trigger;
}
virtual bool
is_steady()
{
return Clock::is_steady;
}
private:
RCLCPP_DISABLE_COPY(GenericTimer);
rclcpp::rate::GenericRate<Clock> loop_rate_;
std::chrono::time_point<Clock> last_triggered_time_;
};
using WallTimer = GenericTimer<std::chrono::steady_clock>;
} /* namespace timer */
} /* namespace rclcpp */
#endif /* RCLCPP_RCLCPP_TIMER_HPP_ */
const CallbackType &
TimerBase::get_callback() const
{
return callback_;
}

102
rclcpp/src/rclcpp/utilities.cpp
View file

@ -1,4 +1,4 @@
// Copyright 2014 Open Source Robotics Foundation, Inc.
// Copyright 2015 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.
@ -12,54 +12,40 @@
// 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 "rclcpp/utilities.hpp"
// TODO(wjwwood): remove
#include <iostream>
#include <cerrno>
#include <chrono>
#include <atomic>
#include <condition_variable>
#include <csignal>
#include <cstdio>
#include <cstring>
#include <mutex>
#include <string.h>
#include <thread>
#include <string>
#include <rmw/error_handling.h>
#include <rmw/macros.h>
#include <rmw/rmw.h>
#include "rmw/error_handling.h"
#include "rmw/rmw.h"
// Determine if sigaction is available
#if __APPLE__ || _POSIX_C_SOURCE >= 1 || _XOPEN_SOURCE || _POSIX_SOURCE
#define HAS_SIGACTION
#endif
namespace
{
/// Represent the status of the global interrupt signal.
volatile sig_atomic_t g_signal_status = 0;
static volatile sig_atomic_t g_signal_status = 0;
/// Guard condition for interrupting the rmw implementation when the global interrupt signal fired.
rmw_guard_condition_t * g_sigint_guard_cond_handle = \
rmw_create_guard_condition();
static rmw_guard_condition_t * g_sigint_guard_cond_handle = rmw_create_guard_condition();
/// Condition variable for timed sleep (see sleep_for).
std::condition_variable g_interrupt_condition_variable;
std::atomic<bool> g_is_interrupted(false);
static std::condition_variable g_interrupt_condition_variable;
static std::atomic<bool> g_is_interrupted(false);
/// Mutex for protecting the global condition variable.
std::mutex g_interrupt_mutex;
static std::mutex g_interrupt_mutex;
#ifdef HAS_SIGACTION
struct sigaction old_action;
static struct sigaction old_action;
#else
void (* old_signal_handler)(int) = 0;
static void (* old_signal_handler)(int) = 0;
#endif
/// Handle the interrupt signal.
/** When the interrupt signal fires, the signal handler notifies the condition variable to wake up
* and triggers the interrupt guard condition, so that all global threads managed by rclcpp
* are interrupted.
*/
void
#ifdef HAS_SIGACTION
signal_handler(int signal_value, siginfo_t * siginfo, void * context)
@ -67,8 +53,8 @@ signal_handler(int signal_value, siginfo_t * siginfo, void * context)
signal_handler(int signal_value)
#endif
{
// TODO(wjwwood): remove
std::cout << "signal_handler(" << signal_value << ")" << std::endl;
// TODO(wjwwood): remove? move to console logging at some point?
printf("signal_handler(%d)\n", signal_value);
#ifdef HAS_SIGACTION
if (old_action.sa_flags & SA_SIGINFO) {
if (old_action.sa_sigaction != NULL) {
@ -99,23 +85,9 @@ signal_handler(int signal_value)
g_is_interrupted.store(true);
g_interrupt_condition_variable.notify_all();
}
} // namespace
namespace rclcpp
{
RMW_THREAD_LOCAL size_t thread_id = 0;
namespace utilities
{
/// Initialize communications via the rmw implementation and set up a global signal handler.
/**
* \param[in] argc Number of arguments.
* \param[in] argv Argument vector. Will eventually be used for passing options to rclcpp.
*/
void
init(int argc, char * argv[])
rclcpp::utilities::init(int argc, char * argv[])
{
(void)argc;
(void)argv;
@ -137,21 +109,15 @@ init(int argc, char * argv[])
if (ret == -1)
#else
::old_signal_handler = std::signal(SIGINT, ::signal_handler);
// NOLINTNEXTLINE(readability/braces)
if (::old_signal_handler == SIG_ERR)
#endif
{
const size_t error_length = 1024;
// NOLINTNEXTLINE(runtime/arrays)
char error_string[error_length];
#ifndef _WIN32
auto rc = strerror_r(errno, error_string, error_length);
if (rc) {
// *INDENT-OFF*
throw std::runtime_error(
"Failed to set SIGINT signal handler: (" + std::to_string(errno) +
") unable to retrieve error string");
// *INDENT-ON*
}
strerror_r(errno, error_string, error_length);
#else
strerror_s(error_string, error_length, errno);
#endif
@ -163,17 +129,14 @@ init(int argc, char * argv[])
}
}
/// Check rclcpp's status.
// \return True if SIGINT hasn't fired yet, false otherwise.
bool
ok()
rclcpp::utilities::ok()
{
return ::g_signal_status == 0;
}
/// Notify the signal handler and rmw that rclcpp is shutting down.
void
shutdown()
rclcpp::utilities::shutdown()
{
g_signal_status = SIGINT;
rmw_ret_t status = rmw_trigger_guard_condition(g_sigint_guard_cond_handle);
@ -185,23 +148,15 @@ shutdown()
g_interrupt_condition_variable.notify_all();
}
/// Get a handle to the rmw guard condition that manages the signal handler.
rmw_guard_condition_t *
get_global_sigint_guard_condition()
rclcpp::utilities::get_global_sigint_guard_condition()
{
return ::g_sigint_guard_cond_handle;
}
/// Use the global condition variable to block for the specified amount of time.
/**
* \param[in] nanoseconds A std::chrono::duration representing how long to sleep for.
* \return True if the condition variable did not timeout.
*/
bool
sleep_for(const std::chrono::nanoseconds & nanoseconds)
rclcpp::utilities::sleep_for(const std::chrono::nanoseconds & nanoseconds)
{
// TODO: determine if posix's nanosleep(2) is more efficient here
std::chrono::nanoseconds time_left = nanoseconds;
{
std::unique_lock<std::mutex> lock(::g_interrupt_mutex);
@ -215,12 +170,3 @@ sleep_for(const std::chrono::nanoseconds & nanoseconds)
// Return true if the timeout elapsed successfully, otherwise false.
return !g_is_interrupted;
}
} /* namespace utilities */
} /* namespace rclcpp */
#ifdef HAS_SIGACTION
#undef HAS_SIGACTION
#endif
#endif /* RCLCPP_RCLCPP_UTILITIES_HPP_ */

17
rclcpp/src/rclcpp/windows_helper.cpp Normal file
View file

@ -0,0 +1,17 @@
// Copyright 2015 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.
#if defined(WIN32)
#include "rclcpp/type_support_def.hpp"
#endif

4
rclcpp/test/test_intra_process_manager.cpp
View file

@ -118,11 +118,13 @@ public:
// Prevent rclcpp/publisher.hpp and rclcpp/subscription.hpp from being imported.
#define RCLCPP__PUBLISHER_HPP_
#define RCLCPP__SUBSCRIPTION_HPP_
#define RCLCPP_BUILDING_LIBRARY 1
// Force ipm to use our mock publisher class.
#define Publisher mock::Publisher
#define PublisherBase mock::PublisherBase
#define SubscriptionBase mock::SubscriptionBase
#include <rclcpp/intra_process_manager.hpp>
#include "../src/rclcpp/intra_process_manager.cpp"
#include "../src/rclcpp/intra_process_manager_state.cpp"
#undef SubscriptionBase
#undef Publisher
#undef PublisherBase

1
rclcpp/test/test_mapped_ring_buffer.cpp
View file

@ -14,6 +14,7 @@
#include <gtest/gtest.h>
#define RCLCPP_BUILDING_LIBRARY 1 // Prevent including unavailable symbols
#include <rclcpp/mapped_ring_buffer.hpp>
/*