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

Merge pull request #140 from ros2/rclcpp_library

Browse source 
Make rclcpp in to a library
This commit is contained in:
William Woodall 2015-11-05 21:01:13 -08:00
commit ca64af3145
65 changed files with 4013 additions and 2405 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
)

2
rclcpp/include/rclcpp/allocator/allocator_deleter.hpp
View file

@ -16,10 +16,10 @@
#define RCLCPP__ALLOCATOR__ALLOCATOR_DELETER_HPP_
#include <memory>
#include <stdexcept>
namespace rclcpp
{
namespace allocator
{

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

@ -12,13 +12,14 @@
// 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_
#ifndef RCLCPP__ANY_EXECUTABLE_HPP_
#define RCLCPP__ANY_EXECUTABLE_HPP_
#include <memory>
#include <rclcpp/macros.hpp>
#include <rclcpp/node.hpp>
#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;
@ -42,7 +44,7 @@ struct AnyExecutable
rclcpp::node::Node::SharedPtr node;
};
} /* executor */
} /* rclcpp */
} // namespace executor
} // namespace rclcpp
#endif
#endif // RCLCPP__ANY_EXECUTABLE_HPP_

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

@ -12,16 +12,17 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_RCLCPP_ANY_SERVICE_CALLBACK_HPP_
#define RCLCPP_RCLCPP_ANY_SERVICE_CALLBACK_HPP_
#include <rclcpp/function_traits.hpp>
#ifndef RCLCPP__ANY_SERVICE_CALLBACK_HPP_
#define RCLCPP__ANY_SERVICE_CALLBACK_HPP_
#include <functional>
#include <memory>
#include <stdexcept>
#include <type_traits>
#include <rmw/types.h>
#include "rclcpp/function_traits.hpp"
#include "rclcpp/visibility_control.hpp"
#include "rmw/types.h"
namespace rclcpp
{
@ -97,7 +98,7 @@ public:
}
};
} /* namespace any_service_callback */
} /* namespace rclcpp */
} // namespace any_service_callback
} // namespace rclcpp
#endif /* RCLCPP_RCLCPP_ANY_SERVICE_CALLBACK_HPP_ */
#endif // RCLCPP__ANY_SERVICE_CALLBACK_HPP_

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

@ -19,10 +19,12 @@
#include <functional>
#include <memory>
#include <stdexcept>
#include <type_traits>
#include "rclcpp/allocator/allocator_common.hpp"
#include "rclcpp/function_traits.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{

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

@ -12,17 +12,18 @@
// 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_
#ifndef RCLCPP__CALLBACK_GROUP_HPP_
#define 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>
#include "rclcpp/client.hpp"
#include "rclcpp/service.hpp"
#include "rclcpp/subscription.hpp"
#include "rclcpp/timer.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
@ -31,7 +32,7 @@ namespace rclcpp
namespace node
{
class Node;
} // namespace node
} // namespace node
namespace callback_group
{
@ -49,82 +50,61 @@ 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_;
};
} /* namespace callback_group */
} /* namespace rclcpp */
} // namespace callback_group
} // namespace rclcpp
#endif /* RCLCPP_RCLCPP_CALLBACK_GROUP_HPP_ */
#endif // RCLCPP__CALLBACK_GROUP_HPP_

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

@ -12,60 +12,49 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_RCLCPP_CLIENT_HPP_
#define RCLCPP_RCLCPP_CLIENT_HPP_
#ifndef RCLCPP__CLIENT_HPP_
#define RCLCPP__CLIENT_HPP_
#include <future>
#include <iostream>
#include <map>
#include <memory>
#include <sstream>
#include <string>
#include <tuple>
#include <utility>
#include <rmw/error_handling.h>
#include <rmw/rmw.h>
#include <rclcpp/macros.hpp>
#include <rclcpp/utilities.hpp>
#include "rclcpp/macros.hpp"
#include "rclcpp/utilities.hpp"
#include "rclcpp/visibility_control.hpp"
#include "rmw/error_handling.h"
#include "rmw/rmw.h"
namespace rclcpp
{
namespace client
{
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;
@ -79,7 +68,6 @@ private:
rmw_client_t * client_handle_;
std::string service_name_;
};
template<typename ServiceT>
@ -118,7 +106,8 @@ public:
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
// TODO(esteve) 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);
@ -158,7 +147,7 @@ private:
std::map<int64_t, std::tuple<SharedPromise, CallbackType, SharedFuture>> pending_requests_;
};
} /* namespace client */
} /* namespace rclcpp */
} // namespace client
} // namespace rclcpp
#endif /* RCLCPP_RCLCPP_CLIENT_HPP_ */
#endif // RCLCPP__CLIENT_HPP_

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

@ -12,20 +12,19 @@
// 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/macros.hpp>
#ifndef RCLCPP__CONTEXT_HPP_
#define RCLCPP__CONTEXT_HPP_
#include <iostream>
#include <memory>
#include <mutex>
#include <typeinfo>
#include <typeindex>
#include <typeinfo>
#include <unordered_map>
#include <rmw/rmw.h>
#include "rclcpp/macros.hpp"
#include "rclcpp/visibility_control.hpp"
#include "rmw/rmw.h"
namespace rclcpp
{
@ -38,7 +37,7 @@ class Context
public:
RCLCPP_SMART_PTR_DEFINITIONS(Context);
Context() {}
Context();
template<typename SubContext, typename ... Args>
std::shared_ptr<SubContext>
@ -71,10 +70,9 @@ private:
std::unordered_map<std::type_index, std::shared_ptr<void>> sub_contexts_;
std::mutex mutex_;
};
} /* namespace context */
} /* namespace rclcpp */
} // namespace context
} // namespace rclcpp
#endif /* RCLCPP_RCLCPP_CONTEXT_HPP_ */
#endif // RCLCPP__CONTEXT_HPP_

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

@ -12,10 +12,11 @@
// 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_
#ifndef RCLCPP__CONTEXTS__DEFAULT_CONTEXT_HPP_
#define RCLCPP__CONTEXTS__DEFAULT_CONTEXT_HPP_
#include <rclcpp/context.hpp>
#include "rclcpp/context.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
@ -29,19 +30,16 @@ 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
} // namespace rclcpp
} // namespace default_context
} // namespace contexts
} // namespace rclcpp
#endif /* RCLCPP_RCLCPP_CONTEXTS_DEFAULT_CONTEXT_HPP_ */
#endif // RCLCPP__CONTEXTS__DEFAULT_CONTEXT_HPP_

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_;

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

@ -12,17 +12,32 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_RCLCPP_EXECUTORS_HPP_
#define RCLCPP_RCLCPP_EXECUTORS_HPP_
#ifndef RCLCPP__EXECUTORS_HPP_
#define 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>
#include "rclcpp/executors/multi_threaded_executor.hpp"
#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 +52,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 +69,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.
@ -81,7 +95,18 @@ spin_node_until_future_complete(
return FutureReturnCode::INTERRUPTED;
}
} // namespace executors
} // namespace rclcpp
} // namespace executors
#endif /* RCLCPP_RCLCPP_EXECUTORS_HPP_ */
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__EXECUTORS_HPP_

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

@ -15,18 +15,14 @@
#ifndef RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_
#define RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_
#include <rmw/rmw.h>
#include <cassert>
#include <cstdlib>
#include <memory>
#include <mutex>
#include <vector>
#include <thread>
#include <unordered_map>
#include "rclcpp/executor.hpp"
#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 +36,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);

11
rclcpp/include/rclcpp/function_traits.hpp
View file

@ -12,9 +12,11 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_RCLCPP_FUNCTION_TRAITS_HPP_
#define RCLCPP_RCLCPP_FUNCTION_TRAITS_HPP_
#ifndef RCLCPP__FUNCTION_TRAITS_HPP_
#define RCLCPP__FUNCTION_TRAITS_HPP_
#include <memory>
#include <tuple>
namespace rclcpp
{
@ -95,6 +97,7 @@ struct same_arguments : std::is_same<
typename function_traits<FunctorBT>::arguments
>
{};
} /* namespace rclcpp */
#endif /* RCLCPP_RCLCPP_FUNCTION_TRAITS_HPP_ */
} // namespace rclcpp
#endif // RCLCPP__FUNCTION_TRAITS_HPP_

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

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

@ -15,21 +15,23 @@
#ifndef RCLCPP__INTRA_PROCESS_MANAGER_STATE_HPP_
#define 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 <memory>
#include <set>
#include <string>
#include <unordered_map>
#include <utility>
#include "rclcpp/macros.hpp"
#include "rclcpp/mapped_ring_buffer.hpp"
#include "rclcpp/publisher.hpp"
#include "rclcpp/subscription.hpp"
#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

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

@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_RCLCPP_MACROS_HPP_
#define RCLCPP_RCLCPP_MACROS_HPP_
#ifndef RCLCPP__MACROS_HPP_
#define RCLCPP__MACROS_HPP_
/* Disables the copy constructor and operator= for the given class.
*
@ -76,6 +76,7 @@
{ \
return std::unique_ptr<__VA_ARGS__>(new __VA_ARGS__(std::forward<Args>(args) ...)); \
}
/// Defines aliases and static functions for using the Class with unique_ptrs.
#define RCLCPP_UNIQUE_PTR_DEFINITIONS(...) \
__RCLCPP_UNIQUE_PTR_ALIAS(__VA_ARGS__) \
@ -83,4 +84,4 @@
#define RCLCPP_INFO(Args) std::cout << Args << std::endl;
#endif /* RCLCPP_RCLCPP_MACROS_HPP_ */
#endif // RCLCPP__MACROS_HPP_

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);

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

@ -15,20 +15,17 @@
#ifndef RCLCPP__MEMORY_STRATEGIES_HPP_
#define RCLCPP__MEMORY_STRATEGIES_HPP_
#include <rclcpp/memory_strategy.hpp>
#include <rclcpp/strategies/allocator_memory_strategy.hpp>
#include "rclcpp/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_

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

@ -16,9 +16,11 @@
#define RCLCPP__MESSAGE_MEMORY_STRATEGY_HPP_
#include <memory>
#include <stdexcept>
#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

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

@ -31,91 +31,21 @@
#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/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
using namespace rclcpp;
using namespace node;
Node::Node(const std::string & node_name, bool use_intra_process_comms)
: Node(
node_name,
rclcpp::contexts::default_context::get_global_default_context(),
use_intra_process_comms)
{}
Node::Node(
const std::string & node_name,
context::Context::SharedPtr context,
bool use_intra_process_comms)
: name_(node_name), context_(context),
number_of_subscriptions_(0), number_of_timers_(0), number_of_services_(0),
use_intra_process_comms_(use_intra_process_comms)
namespace rclcpp
{
size_t domain_id = 0;
char * ros_domain_id = nullptr;
const char * env_var = "ROS_DOMAIN_ID";
#ifndef _WIN32
ros_domain_id = getenv(env_var);
#else
size_t ros_domain_id_size;
_dupenv_s(&ros_domain_id, &ros_domain_id_size, env_var);
#endif
if (ros_domain_id) {
uint32_t number = strtoul(ros_domain_id, NULL, 0);
if (number == (std::numeric_limits<uint32_t>::max)()) {
throw std::runtime_error("failed to interpret ROS_DOMAIN_ID as integral number");
}
domain_id = static_cast<size_t>(number);
#ifdef _WIN32
free(ros_domain_id);
#endif
}
auto node = rmw_create_node(name_.c_str(), domain_id);
if (!node) {
// *INDENT-OFF*
throw std::runtime_error(
std::string("could not create node: ") +
rmw_get_error_string_safe());
// *INDENT-ON*
}
// Initialize node handle shared_ptr with custom deleter.
// *INDENT-OFF*
node_handle_.reset(node, [](rmw_node_t * node) {
auto ret = rmw_destroy_node(node);
if (ret != RMW_RET_OK) {
fprintf(
stderr, "Error in destruction of rmw node handle: %s\n", rmw_get_error_string_safe());
}
});
// *INDENT-ON*
using rclcpp::callback_group::CallbackGroupType;
default_callback_group_ = create_callback_group(
CallbackGroupType::MutuallyExclusive);
events_publisher_ = create_publisher<rcl_interfaces::msg::ParameterEvent>(
"parameter_events", rmw_qos_profile_parameter_events);
}
rclcpp::callback_group::CallbackGroup::SharedPtr
Node::create_callback_group(
rclcpp::callback_group::CallbackGroupType group_type)
namespace node
{
using rclcpp::callback_group::CallbackGroup;
using rclcpp::callback_group::CallbackGroupType;
auto group = CallbackGroup::SharedPtr(new CallbackGroup(group_type));
callback_groups_.push_back(group);
return group;
}
template<typename MessageT, typename Alloc>
typename rclcpp::publisher::Publisher<MessageT, Alloc>::SharedPtr
@ -157,7 +87,8 @@ Node::create_publisher(
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);
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(
@ -206,19 +137,6 @@ Node::create_publisher(
return publisher;
}
bool
Node::group_in_node(callback_group::CallbackGroup::SharedPtr group)
{
bool group_belongs_to_this_node = false;
for (auto & weak_group : this->callback_groups_) {
auto cur_group = weak_group.lock();
if (cur_group && (cur_group == group)) {
group_belongs_to_this_node = true;
}
}
return group_belongs_to_this_node;
}
template<typename MessageT, typename CallbackT, typename Alloc>
typename rclcpp::subscription::Subscription<MessageT, Alloc>::SharedPtr
Node::create_subscription(
@ -257,7 +175,8 @@ Node::create_subscription(
// *INDENT-ON*
}
using namespace rclcpp::subscription;
using rclcpp::subscription::Subscription;
using rclcpp::subscription::SubscriptionBase;
auto sub = Subscription<MessageT, Alloc>::make_shared(
node_handle_,
@ -270,7 +189,7 @@ Node::create_subscription(
// Setup intra process.
if (use_intra_process_comms_) {
rmw_subscription_t * intra_process_subscriber_handle = rmw_create_subscription(
node_handle_.get(), ipm_ts_,
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)
@ -351,39 +270,6 @@ Node::create_subscription(
allocator);
}
rclcpp::timer::WallTimer::SharedPtr
Node::create_wall_timer(
std::chrono::nanoseconds period,
rclcpp::timer::CallbackType callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group)
{
auto timer = rclcpp::timer::WallTimer::make_shared(period, callback);
if (group) {
if (!group_in_node(group)) {
// TODO(jacquelinekay): use custom exception
throw std::runtime_error("Cannot create timer, group not in node.");
}
group->add_timer(timer);
} else {
default_callback_group_->add_timer(timer);
}
number_of_timers_++;
return timer;
}
// TODO(wjwwood): reenable this once I figure out why the demo doesn't build with it.
// rclcpp::timer::WallTimer::SharedPtr
// Node::create_wall_timer(
// std::chrono::duration<long double, std::nano> period,
// rclcpp::timer::CallbackType callback,
// rclcpp::callback_group::CallbackGroup::SharedPtr group)
// {
// return create_wall_timer(
// std::chrono::duration_cast<std::chrono::nanoseconds>(period),
// callback,
// group);
// }
template<typename ServiceT>
typename client::Client<ServiceT>::SharedPtr
Node::create_client(
@ -404,7 +290,8 @@ Node::create_client(
// *INDENT-ON*
}
using namespace rclcpp::client;
using rclcpp::client::Client;
using rclcpp::client::ClientBase;
auto cli = Client<ServiceT>::make_shared(
node_handle_,
@ -466,211 +353,7 @@ Node::create_service(
return serv;
}
std::vector<rcl_interfaces::msg::SetParametersResult>
Node::set_parameters(
const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
{
std::vector<rcl_interfaces::msg::SetParametersResult> results;
for (auto p : parameters) {
auto result = set_parameters_atomically({{p}});
results.push_back(result);
}
return results;
}
rcl_interfaces::msg::SetParametersResult
Node::set_parameters_atomically(
const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
{
std::lock_guard<std::mutex> lock(mutex_);
std::map<std::string, rclcpp::parameter::ParameterVariant> tmp_map;
auto parameter_event = std::make_shared<rcl_interfaces::msg::ParameterEvent>();
for (auto p : parameters) {
if (parameters_.find(p.get_name()) == parameters_.end()) {
if (p.get_type() != rclcpp::parameter::ParameterType::PARAMETER_NOT_SET) {
parameter_event->new_parameters.push_back(p.to_parameter());
}
} else if (p.get_type() != rclcpp::parameter::ParameterType::PARAMETER_NOT_SET) {
parameter_event->changed_parameters.push_back(p.to_parameter());
} else {
parameter_event->deleted_parameters.push_back(p.to_parameter());
}
tmp_map[p.get_name()] = p;
}
// std::map::insert will not overwrite elements, so we'll keep the new
// ones and add only those that already exist in the Node's internal map
tmp_map.insert(parameters_.begin(), parameters_.end());
std::swap(tmp_map, parameters_);
// TODO(jacquelinekay): handle parameter constraints
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
events_publisher_->publish(parameter_event);
return result;
}
std::vector<rclcpp::parameter::ParameterVariant>
Node::get_parameters(
const std::vector<std::string> & names) const
{
std::lock_guard<std::mutex> lock(mutex_);
std::vector<rclcpp::parameter::ParameterVariant> results;
for (auto & name : names) {
if (std::any_of(parameters_.cbegin(), parameters_.cend(),
[&name](const std::pair<std::string, rclcpp::parameter::ParameterVariant> & kv) {
return name == kv.first;
}))
{
results.push_back(parameters_.at(name));
}
}
return results;
}
std::vector<rcl_interfaces::msg::ParameterDescriptor>
Node::describe_parameters(
const std::vector<std::string> & names) const
{
std::lock_guard<std::mutex> lock(mutex_);
std::vector<rcl_interfaces::msg::ParameterDescriptor> results;
for (auto & kv : parameters_) {
if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) {
return name == kv.first;
}))
{
rcl_interfaces::msg::ParameterDescriptor parameter_descriptor;
parameter_descriptor.name = kv.first;
parameter_descriptor.type = kv.second.get_type();
results.push_back(parameter_descriptor);
}
}
return results;
}
std::vector<uint8_t>
Node::get_parameter_types(
const std::vector<std::string> & names) const
{
std::lock_guard<std::mutex> lock(mutex_);
std::vector<uint8_t> results;
for (auto & kv : parameters_) {
if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) {
return name == kv.first;
}))
{
results.push_back(kv.second.get_type());
} else {
results.push_back(rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET);
}
}
return results;
}
rcl_interfaces::msg::ListParametersResult
Node::list_parameters(
const std::vector<std::string> & prefixes, uint64_t depth) const
{
std::lock_guard<std::mutex> lock(mutex_);
rcl_interfaces::msg::ListParametersResult result;
// TODO(esteve): define parameter separator, use "." for now
for (auto & kv : parameters_) {
if (std::any_of(prefixes.cbegin(), prefixes.cend(), [&kv, &depth](const std::string & prefix) {
if (kv.first == prefix) {
return true;
} else if (kv.first.find(prefix + ".") == 0) {
size_t length = prefix.length();
std::string substr = kv.first.substr(length);
// Cast as unsigned integer to avoid warning
return static_cast<uint64_t>(std::count(substr.begin(), substr.end(), '.')) < depth;
}
return false;
}))
{
result.names.push_back(kv.first);
size_t last_separator = kv.first.find_last_of('.');
if (std::string::npos != last_separator) {
std::string prefix = kv.first.substr(0, last_separator);
if (std::find(result.prefixes.cbegin(), result.prefixes.cend(), prefix) ==
result.prefixes.cend())
{
result.prefixes.push_back(prefix);
}
}
}
}
return result;
}
std::map<std::string, std::string>
Node::get_topic_names_and_types() const
{
rmw_topic_names_and_types_t topic_names_and_types;
topic_names_and_types.topic_count = 0;
topic_names_and_types.topic_names = nullptr;
topic_names_and_types.type_names = nullptr;
auto ret = rmw_get_topic_names_and_types(node_handle_.get(), &topic_names_and_types);
if (ret != RMW_RET_OK) {
// *INDENT-OFF*
throw std::runtime_error(
std::string("could not get topic names and types: ") + rmw_get_error_string_safe());
// *INDENT-ON*
}
std::map<std::string, std::string> topics;
for (size_t i = 0; i < topic_names_and_types.topic_count; ++i) {
topics[topic_names_and_types.topic_names[i]] = topic_names_and_types.type_names[i];
}
ret = rmw_destroy_topic_names_and_types(&topic_names_and_types);
if (ret != RMW_RET_OK) {
// *INDENT-OFF*
throw std::runtime_error(
std::string("could not destroy topic names and types: ") + rmw_get_error_string_safe());
// *INDENT-ON*
}
return topics;
}
size_t
Node::count_publishers(const std::string & topic_name) const
{
size_t count;
auto ret = rmw_count_publishers(node_handle_.get(), topic_name.c_str(), &count);
if (ret != RMW_RET_OK) {
// *INDENT-OFF*
throw std::runtime_error(
std::string("could not count publishers: ") + rmw_get_error_string_safe());
// *INDENT-ON*
}
return count;
}
size_t
Node::count_subscribers(const std::string & topic_name) const
{
size_t count;
auto ret = rmw_count_subscribers(node_handle_.get(), topic_name.c_str(), &count);
if (ret != RMW_RET_OK) {
// *INDENT-OFF*
throw std::runtime_error(
std::string("could not count subscribers: ") + rmw_get_error_string_safe());
// *INDENT-ON*
}
return count;
}
const Node::CallbackGroupWeakPtrList &
Node::get_callback_groups() const
{
return callback_groups_;
}
} // namespace node
} // namespace rclcpp
#endif // RCLCPP__NODE_IMPL_HPP_

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

@ -12,253 +12,161 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_RCLCPP_PARAMETER_HPP_
#define RCLCPP_RCLCPP_PARAMETER_HPP_
#ifndef RCLCPP__PARAMETER_HPP_
#define RCLCPP__PARAMETER_HPP_
#include <ostream>
#include <sstream>
#include <string>
#include <vector>
#include <rmw/rmw.h>
#include <rcl_interfaces/msg/parameter.hpp>
#include <rcl_interfaces/msg/parameter_type.hpp>
#include <rcl_interfaces/msg/parameter_value.hpp>
#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
{
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,
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)
{
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
get_value() const
{
if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER) {
// TODO: use custom exception
// TODO(wjwwood): 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
// TODO(wjwwood): 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
// TODO(wjwwood): 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
// TODO(wjwwood): 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
// TODO(wjwwood): use custom exception
throw std::runtime_error("Invalid type");
}
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 +175,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 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_ */
#endif // RCLCPP__PARAMETER_HPP_

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

@ -12,220 +12,84 @@
// 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_
#ifndef RCLCPP__PARAMETER_CLIENT_HPP_
#define 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
@ -352,8 +162,7 @@ private:
AsyncParametersClient::SharedPtr async_parameters_client_;
};
} /* namespace parameter_client */
} // namespace parameter_client
} // namespace rclcpp
} /* namespace rclcpp */
#endif /* RCLCPP_RCLCPP_PARAMETER_CLIENT_HPP_ */
#endif // RCLCPP__PARAMETER_CLIENT_HPP_

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

@ -12,150 +12,36 @@
// 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_
#ifndef RCLCPP__PARAMETER_SERVICE_HPP_
#define RCLCPP__PARAMETER_SERVICE_HPP_
#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
{
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_;
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_;
@ -170,8 +56,7 @@ private:
rclcpp::service::Service<rcl_interfaces::srv::ListParameters>::SharedPtr list_parameters_service_;
};
} /* namespace parameter_service */
} // namespace parameter_service
} // namespace rclcpp
} /* namespace rclcpp */
#endif /* RCLCPP_RCLCPP_PARAMETER_SERVICE_HPP_ */
#endif // RCLCPP__PARAMETER_SERVICE_HPP_

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_;

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

@ -12,15 +12,16 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_RCLCPP_RATE_HPP_
#define RCLCPP_RCLCPP_RATE_HPP_
#ifndef RCLCPP__RATE_HPP_
#define RCLCPP__RATE_HPP_
#include <chrono>
#include <memory>
#include <thread>
#include <rclcpp/macros.hpp>
#include <rclcpp/utilities.hpp>
#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())
{}
@ -109,13 +110,12 @@ private:
std::chrono::nanoseconds period_;
std::chrono::time_point<Clock> last_interval_;
};
using Rate = GenericRate<std::chrono::system_clock>;
using WallRate = GenericRate<std::chrono::steady_clock>;
} // namespace rate
} // namespace rclcpp
} // namespace rate
} // namespace rclcpp
#endif /* RCLCPP_RCLCPP_RATE_HPP_ */
#endif // RCLCPP__RATE_HPP_

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

@ -12,22 +12,21 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_RCLCPP_RCLCPP_HPP_
#define RCLCPP_RCLCPP_RCLCPP_HPP_
#ifndef RCLCPP__RCLCPP_HPP_
#define RCLCPP__RCLCPP_HPP_
#include <csignal>
#include <memory>
#include <rclcpp/node.hpp>
#include <rclcpp/parameter.hpp>
#include <rclcpp/parameter_client.hpp>
#include <rclcpp/parameter_service.hpp>
#include <rclcpp/executors.hpp>
#include <rclcpp/rate.hpp>
#include <rclcpp/utilities.hpp>
#include "rclcpp/executors.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/parameter.hpp"
#include "rclcpp/parameter_client.hpp"
#include "rclcpp/parameter_service.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);
}
} // namespace rclcpp
/// 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_ */
#endif // RCLCPP__RCLCPP_HPP_

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

@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_RCLCPP_SERVICE_HPP_
#define RCLCPP_RCLCPP_SERVICE_HPP_
#ifndef RCLCPP__SERVICE_HPP_
#define RCLCPP__SERVICE_HPP_
#include <functional>
#include <iostream>
@ -21,52 +21,38 @@
#include <sstream>
#include <string>
#include <rmw/error_handling.h>
#include <rmw/rmw.h>
#include <rclcpp/macros.hpp>
#include <rclcpp/any_service_callback.hpp>
#include "rclcpp/any_service_callback.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/visibility_control.hpp"
#include "rmw/error_handling.h"
#include "rmw/rmw.h"
namespace rclcpp
{
namespace service
{
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;
@ -81,10 +67,9 @@ private:
rmw_service_t * service_handle_;
std::string service_name_;
};
using namespace any_service_callback;
using any_service_callback::AnyServiceCallback;
template<typename ServiceT>
class Service : public ServiceBase
@ -152,7 +137,7 @@ private:
AnyServiceCallback<ServiceT> any_callback_;
};
} /* namespace service */
} /* namespace rclcpp */
} // namespace service
} // namespace rclcpp
#endif /* RCLCPP_RCLCPP_SERVICE_HPP_ */
#endif // RCLCPP__SERVICE_HPP_

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

@ -21,13 +21,12 @@
#include "rclcpp/allocator/allocator_common.hpp"
#include "rclcpp/memory_strategy.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
namespace memory_strategies
{
namespace allocator_memory_strategy
{
@ -312,7 +311,6 @@ private:
} // namespace allocator_memory_strategy
} // namespace memory_strategies
} // namespace rclcpp
#endif // RCLCPP__STRATEGIES__ALLOCATOR_MEMORY_STRATEGY_HPP_

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

@ -15,8 +15,9 @@
#ifndef RCLCPP__STRATEGIES__MESSAGE_POOL_MEMORY_STRATEGY_HPP_
#define RCLCPP__STRATEGIES__MESSAGE_POOL_MEMORY_STRATEGY_HPP_
#include <rclcpp/macros.hpp>
#include <rclcpp/message_memory_strategy.hpp>
#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>
{
@ -101,4 +106,5 @@ protected:
} // namespace message_pool_memory_strategy
} // namespace strategies
} // namespace rclcpp
#endif // RCLCPP__STRATEGIES__MESSAGE_POOL_MEMORY_STRATEGY_HPP_

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),

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

@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_RCLCPP_TIMER_HPP_
#define RCLCPP_RCLCPP_TIMER_HPP_
#ifndef RCLCPP__TIMER_HPP_
#define RCLCPP__TIMER_HPP_
#include <chrono>
#include <functional>
@ -21,16 +21,15 @@
#include <sstream>
#include <thread>
#include <rmw/error_handling.h>
#include <rmw/rmw.h>
#include <rclcpp/macros.hpp>
#include <rclcpp/rate.hpp>
#include <rclcpp/utilities.hpp>
#include "rclcpp/macros.hpp"
#include "rclcpp/rate.hpp"
#include "rclcpp/utilities.hpp"
#include "rclcpp/visibility_control.hpp"
#include "rmw/error_handling.h"
#include "rmw/rmw.h"
namespace rclcpp
{
namespace timer
{
@ -38,36 +37,26 @@ using CallbackType = std::function<void()>;
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.
@ -91,14 +80,12 @@ protected:
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
{
public:
RCLCPP_SMART_PTR_DEFINITIONS(GenericTimer);
@ -166,7 +153,6 @@ private:
rclcpp::rate::GenericRate<Clock> loop_rate_;
std::chrono::time_point<Clock> last_triggered_time_;
};
using WallTimer = GenericTimer<std::chrono::steady_clock>;
@ -174,4 +160,4 @@ using WallTimer = GenericTimer<std::chrono::steady_clock>;
} /* namespace timer */
} /* namespace rclcpp */
#endif /* RCLCPP_RCLCPP_TIMER_HPP_ */
#endif // RCLCPP__TIMER_HPP_

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_

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

@ -12,100 +12,17 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_RCLCPP_UTILITIES_HPP_
#define RCLCPP_RCLCPP_UTILITIES_HPP_
#ifndef RCLCPP__UTILITIES_HPP_
#define 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 */
} // namespace utilities
} // namespace rclcpp
#ifdef HAS_SIGACTION
#undef HAS_SIGACTION
#endif
#endif /* RCLCPP_RCLCPP_UTILITIES_HPP_ */
#endif // 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>

27
rclcpp/src/rclcpp/any_executable.cpp Normal file
View file

@ -0,0 +1,27 @@
// 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.
#include "rclcpp/any_executable.hpp"
using rclcpp::executor::AnyExecutable;
AnyExecutable::AnyExecutable()
: subscription(nullptr),
subscription_intra_process(nullptr),
timer(nullptr),
service(nullptr),
client(nullptr),
callback_group(nullptr),
node(nullptr)
{}

85
rclcpp/src/rclcpp/callback_group.cpp Normal file
View file

@ -0,0 +1,85 @@
// 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.
#include "rclcpp/callback_group.hpp"
#include <vector>
using rclcpp::callback_group::CallbackGroup;
using rclcpp::callback_group::CallbackGroupType;
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
{
return subscription_ptrs_;
}
const std::vector<rclcpp::timer::TimerBase::WeakPtr> &
CallbackGroup::get_timer_ptrs() const
{
return timer_ptrs_;
}
const std::vector<rclcpp::service::ServiceBase::SharedPtr> &
CallbackGroup::get_service_ptrs() const
{
return service_ptrs_;
}
const std::vector<rclcpp::client::ClientBase::SharedPtr> &
CallbackGroup::get_client_ptrs() const
{
return client_ptrs_;
}
std::atomic_bool &
CallbackGroup::can_be_taken_from()
{
return can_be_taken_from_;
}
const CallbackGroupType &
CallbackGroup::type() const
{
return type_;
}
void
CallbackGroup::add_subscription(
const rclcpp::subscription::SubscriptionBase::SharedPtr subscription_ptr)
{
subscription_ptrs_.push_back(subscription_ptr);
}
void
CallbackGroup::add_timer(const rclcpp::timer::TimerBase::SharedPtr timer_ptr)
{
timer_ptrs_.push_back(timer_ptr);
}
void
CallbackGroup::add_service(const rclcpp::service::ServiceBase::SharedPtr service_ptr)
{
service_ptrs_.push_back(service_ptr);
}
void
CallbackGroup::add_client(const rclcpp::client::ClientBase::SharedPtr client_ptr)
{
client_ptrs_.push_back(client_ptr);
}

51
rclcpp/src/rclcpp/client.cpp Normal file
View file

@ -0,0 +1,51 @@
// 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.
#include "rclcpp/client.hpp"
#include <cstdio>
#include <string>
#include "rmw/rmw.h"
using rclcpp::client::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)
{}
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 &
ClientBase::get_service_name() const
{
return this->service_name_;
}
const rmw_client_t *
ClientBase::get_client_handle() const
{
return this->client_handle_;
}

19
rclcpp/src/rclcpp/context.cpp Normal file
View file

@ -0,0 +1,19 @@
// 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.
#include "rclcpp/context.hpp"
using rclcpp::context::Context;
Context::Context() {}

27
rclcpp/src/rclcpp/contexts/default_context.cpp Normal file
View file

@ -0,0 +1,27 @@
// 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.
#include "rclcpp/contexts/default_context.hpp"
using rclcpp::contexts::default_context::DefaultContext;
DefaultContext::DefaultContext()
{}
DefaultContext::SharedPtr
rclcpp::contexts::default_context::get_global_default_context()
{
static DefaultContext::SharedPtr default_context = DefaultContext::make_shared();
return default_context;
}

518
rclcpp/src/rclcpp/executor.cpp Normal file
View file

@ -0,0 +1,518 @@
// 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.
#include "rclcpp/executor.hpp"
#include "rcl_interfaces/msg/intra_process_message.hpp"
using rclcpp::executor::AnyExecutable;
using rclcpp::executor::Executor;
Executor::Executor(rclcpp::memory_strategy::MemoryStrategy::SharedPtr ms)
: interrupt_guard_condition_(rmw_create_guard_condition()),
memory_strategy_(ms)
{
}
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_);
if (status != RMW_RET_OK) {
fprintf(stderr,
"[rclcpp::error] failed to destroy guard condition: %s\n", rmw_get_error_string_safe());
}
}
}
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();
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());
}
}
}
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) {
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());
}
}
}
}
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);
if (any_exec) {
execute_any_executable(any_exec);
}
this->remove_node(node, false);
}
void
Executor::spin_node_some(rclcpp::node::Node::SharedPtr node)
{
this->add_node(node, false);
spin_some();
this->remove_node(node, false);
}
void
Executor::spin_some()
{
while (AnyExecutable::SharedPtr any_exec =
get_next_executable(std::chrono::milliseconds::zero()))
{
execute_any_executable(any_exec);
}
}
void
Executor::set_memory_strategy(rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy)
{
if (memory_strategy == nullptr) {
throw std::runtime_error("Received NULL memory strategy in executor.");
}
memory_strategy_ = memory_strategy;
}
void
Executor::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());
}
}
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;
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);
}
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;
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());
}
}
void
Executor::execute_timer(
rclcpp::timer::TimerBase::SharedPtr timer)
{
timer->execute_callback();
}
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;
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());
}
}
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;
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());
}
}
void
Executor::wait_for_work(std::chrono::nanoseconds timeout)
{
memory_strategy_->clear_active_entities();
// 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(),
// *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
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::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::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() %
(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::node::Node::SharedPtr
Executor::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();
}
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) {
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();
}
void
Executor::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;
}
}
}
}
}
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_) {
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;
}
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);
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;
}
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();
// 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;
}

30
rclcpp/src/rclcpp/executors.cpp Normal file
View file

@ -0,0 +1,30 @@
// 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.
#include "rclcpp/executors.hpp"
void
rclcpp::spin_some(node::Node::SharedPtr node_ptr)
{
rclcpp::executors::SingleThreadedExecutor exec;
exec.spin_node_some(node_ptr);
}
void
rclcpp::spin(node::Node::SharedPtr node_ptr)
{
rclcpp::executors::SingleThreadedExecutor exec;
exec.add_node(node_ptr);
exec.spin();
}

75
rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp Normal file
View file

@ -0,0 +1,75 @@
// 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.
#include "rclcpp/executors/multi_threaded_executor.hpp"
#include <chrono>
#include <functional>
#include <vector>
#include "rclcpp/utilities.hpp"
using rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor;
MultiThreadedExecutor::MultiThreadedExecutor(rclcpp::memory_strategy::MemoryStrategy::SharedPtr ms)
: executor::Executor(ms)
{
number_of_threads_ = std::thread::hardware_concurrency();
if (number_of_threads_ == 0) {
number_of_threads_ = 1;
}
}
MultiThreadedExecutor::~MultiThreadedExecutor() {}
void
MultiThreadedExecutor::spin()
{
std::vector<std::thread> threads;
{
std::lock_guard<std::mutex> wait_lock(wait_mutex_);
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++);
threads.emplace_back(func);
}
}
for (auto & thread : threads) {
thread.join();
}
}
size_t
MultiThreadedExecutor::get_number_of_threads()
{
return number_of_threads_;
}
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;
{
std::lock_guard<std::mutex> wait_lock(wait_mutex_);
if (!rclcpp::utilities::ok()) {
return;
}
any_exec = get_next_executable();
}
execute_any_executable(any_exec);
}
}

32
rclcpp/src/rclcpp/executors/single_threaded_executor.cpp Normal file
View file

@ -0,0 +1,32 @@
// 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.
#include <rclcpp/executors/single_threaded_executor.hpp>
using rclcpp::executors::single_threaded_executor::SingleThreadedExecutor;
SingleThreadedExecutor::SingleThreadedExecutor(rclcpp::memory_strategy::MemoryStrategy::SharedPtr ms)
: executor::Executor(ms) {}
SingleThreadedExecutor::~SingleThreadedExecutor() {}
void
SingleThreadedExecutor::spin()
{
while (rclcpp::utilities::ok()) {
auto any_exec = get_next_executable();
execute_any_executable(any_exec);
}
}

82
rclcpp/src/rclcpp/intra_process_manager.cpp Normal file
View file

@ -0,0 +1,82 @@
// 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.
#include "rclcpp/intra_process_manager.hpp"
namespace rclcpp
{
namespace intra_process_manager
{
static std::atomic<uint64_t> _next_unique_id {1};
IntraProcessManager::IntraProcessManager(
rclcpp::intra_process_manager::IntraProcessManagerStateBase::SharedPtr state)
: state_(state)
{}
IntraProcessManager::~IntraProcessManager()
{}
uint64_t
IntraProcessManager::add_subscription(
rclcpp::subscription::SubscriptionBase::SharedPtr subscription)
{
auto id = IntraProcessManager::get_next_unique_id();
state_->add_subscription(id, subscription);
return id;
}
void
IntraProcessManager::remove_subscription(uint64_t intra_process_subscription_id)
{
state_->remove_subscription(intra_process_subscription_id);
}
void
IntraProcessManager::remove_publisher(uint64_t intra_process_publisher_id)
{
state_->remove_publisher(intra_process_publisher_id);
}
bool
IntraProcessManager::matches_any_publishers(const rmw_gid_t * id) const
{
return state_->matches_any_publishers(id);
}
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.
// 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;
}
} // namespace intra_process_manager
} // namespace rclcpp

23
rclcpp/src/rclcpp/intra_process_manager_state.cpp Normal file
View file

@ -0,0 +1,23 @@
// 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.
#include "rclcpp/intra_process_manager_state.hpp"
#include <memory>
rclcpp::intra_process_manager::IntraProcessManagerStateBase::SharedPtr
rclcpp::intra_process_manager::create_default_state()
{
return std::make_shared<IntraProcessManagerState<>>();
}

25
rclcpp/src/rclcpp/memory_strategies.cpp Normal file
View file

@ -0,0 +1,25 @@
// 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.
#include "rclcpp/memory_strategies.hpp"
#include "rclcpp/strategies/allocator_memory_strategy.hpp"
using rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy;
rclcpp::memory_strategy::MemoryStrategy::SharedPtr
rclcpp::memory_strategies::create_default_strategy()
{
return std::make_shared<AllocatorMemoryStrategy<>>();
}

193
rclcpp/src/rclcpp/memory_strategy.cpp Normal file
View file

@ -0,0 +1,193 @@
// 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.
#include "rclcpp/memory_strategy.hpp"
using rclcpp::memory_strategy::MemoryStrategy;
rclcpp::subscription::SubscriptionBase::SharedPtr
MemoryStrategy::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;
}
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) {
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;
}
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) {
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;
}
rclcpp::node::Node::SharedPtr
MemoryStrategy::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;
}
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) {
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;
}
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) {
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;
}
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) {
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;
}

392
rclcpp/src/rclcpp/node.cpp Normal file
View file

@ -0,0 +1,392 @@
// 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.
#include <algorithm>
#include <limits>
#include <map>
#include <string>
#include <utility>
#include <vector>
#include "rclcpp/node.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
{
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 rclcpp::node::Node;
Node::Node(const std::string & node_name, bool use_intra_process_comms)
: Node(
node_name,
rclcpp::contexts::default_context::get_global_default_context(),
use_intra_process_comms)
{}
Node::Node(
const std::string & node_name,
rclcpp::context::Context::SharedPtr context,
bool use_intra_process_comms)
: name_(node_name), context_(context),
number_of_subscriptions_(0), number_of_timers_(0), number_of_services_(0),
use_intra_process_comms_(use_intra_process_comms)
{
size_t domain_id = 0;
char * ros_domain_id = nullptr;
const char * env_var = "ROS_DOMAIN_ID";
#ifndef _WIN32
ros_domain_id = getenv(env_var);
#else
size_t ros_domain_id_size;
_dupenv_s(&ros_domain_id, &ros_domain_id_size, env_var);
#endif
if (ros_domain_id) {
uint32_t number = strtoul(ros_domain_id, NULL, 0);
if (number == (std::numeric_limits<uint32_t>::max)()) {
throw std::runtime_error("failed to interpret ROS_DOMAIN_ID as integral number");
}
domain_id = static_cast<size_t>(number);
#ifdef _WIN32
free(ros_domain_id);
#endif
}
auto node = rmw_create_node(name_.c_str(), domain_id);
if (!node) {
// *INDENT-OFF*
throw std::runtime_error(
std::string("could not create node: ") +
rmw_get_error_string_safe());
// *INDENT-ON*
}
// Initialize node handle shared_ptr with custom deleter.
// *INDENT-OFF*
node_handle_.reset(node, [](rmw_node_t * node) {
auto ret = rmw_destroy_node(node);
if (ret != RMW_RET_OK) {
fprintf(
stderr, "Error in destruction of rmw node handle: %s\n", rmw_get_error_string_safe());
}
});
// *INDENT-ON*
using rclcpp::callback_group::CallbackGroupType;
default_callback_group_ = create_callback_group(
CallbackGroupType::MutuallyExclusive);
events_publisher_ = create_publisher<rcl_interfaces::msg::ParameterEvent>(
"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)
{
using rclcpp::callback_group::CallbackGroup;
using rclcpp::callback_group::CallbackGroupType;
auto group = CallbackGroup::SharedPtr(new CallbackGroup(group_type));
callback_groups_.push_back(group);
return group;
}
bool
Node::group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group)
{
bool group_belongs_to_this_node = false;
for (auto & weak_group : this->callback_groups_) {
auto cur_group = weak_group.lock();
if (cur_group && (cur_group == group)) {
group_belongs_to_this_node = true;
}
}
return group_belongs_to_this_node;
}
rclcpp::timer::WallTimer::SharedPtr
Node::create_wall_timer(
std::chrono::nanoseconds period,
rclcpp::timer::CallbackType callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group)
{
auto timer = rclcpp::timer::WallTimer::make_shared(period, callback);
if (group) {
if (!group_in_node(group)) {
// TODO(jacquelinekay): use custom exception
throw std::runtime_error("Cannot create timer, group not in node.");
}
group->add_timer(timer);
} else {
default_callback_group_->add_timer(timer);
}
number_of_timers_++;
return timer;
}
// TODO(wjwwood): reenable this once I figure out why the demo doesn't build with it.
// rclcpp::timer::WallTimer::SharedPtr
// Node::create_wall_timer(
// std::chrono::duration<long double, std::nano> period,
// rclcpp::timer::CallbackType callback,
// rclcpp::callback_group::CallbackGroup::SharedPtr group)
// {
// return create_wall_timer(
// std::chrono::duration_cast<std::chrono::nanoseconds>(period),
// callback,
// group);
// }
std::vector<rcl_interfaces::msg::SetParametersResult>
Node::set_parameters(
const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
{
std::vector<rcl_interfaces::msg::SetParametersResult> results;
for (auto p : parameters) {
auto result = set_parameters_atomically({{p}});
results.push_back(result);
}
return results;
}
rcl_interfaces::msg::SetParametersResult
Node::set_parameters_atomically(
const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
{
std::lock_guard<std::mutex> lock(mutex_);
std::map<std::string, rclcpp::parameter::ParameterVariant> tmp_map;
auto parameter_event = std::make_shared<rcl_interfaces::msg::ParameterEvent>();
for (auto p : parameters) {
if (parameters_.find(p.get_name()) == parameters_.end()) {
if (p.get_type() != rclcpp::parameter::ParameterType::PARAMETER_NOT_SET) {
parameter_event->new_parameters.push_back(p.to_parameter());
}
} else if (p.get_type() != rclcpp::parameter::ParameterType::PARAMETER_NOT_SET) {
parameter_event->changed_parameters.push_back(p.to_parameter());
} else {
parameter_event->deleted_parameters.push_back(p.to_parameter());
}
tmp_map[p.get_name()] = p;
}
// std::map::insert will not overwrite elements, so we'll keep the new
// ones and add only those that already exist in the Node's internal map
tmp_map.insert(parameters_.begin(), parameters_.end());
std::swap(tmp_map, parameters_);
// TODO(jacquelinekay): handle parameter constraints
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
events_publisher_->publish(parameter_event);
return result;
}
std::vector<rclcpp::parameter::ParameterVariant>
Node::get_parameters(
const std::vector<std::string> & names) const
{
std::lock_guard<std::mutex> lock(mutex_);
std::vector<rclcpp::parameter::ParameterVariant> results;
for (auto & name : names) {
if (std::any_of(parameters_.cbegin(), parameters_.cend(),
[&name](const std::pair<std::string, rclcpp::parameter::ParameterVariant> & kv) {
return name == kv.first;
}))
{
results.push_back(parameters_.at(name));
}
}
return results;
}
std::vector<rcl_interfaces::msg::ParameterDescriptor>
Node::describe_parameters(
const std::vector<std::string> & names) const
{
std::lock_guard<std::mutex> lock(mutex_);
std::vector<rcl_interfaces::msg::ParameterDescriptor> results;
for (auto & kv : parameters_) {
if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) {
return name == kv.first;
}))
{
rcl_interfaces::msg::ParameterDescriptor parameter_descriptor;
parameter_descriptor.name = kv.first;
parameter_descriptor.type = kv.second.get_type();
results.push_back(parameter_descriptor);
}
}
return results;
}
std::vector<uint8_t>
Node::get_parameter_types(
const std::vector<std::string> & names) const
{
std::lock_guard<std::mutex> lock(mutex_);
std::vector<uint8_t> results;
for (auto & kv : parameters_) {
if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) {
return name == kv.first;
}))
{
results.push_back(kv.second.get_type());
} else {
results.push_back(rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET);
}
}
return results;
}
rcl_interfaces::msg::ListParametersResult
Node::list_parameters(
const std::vector<std::string> & prefixes, uint64_t depth) const
{
std::lock_guard<std::mutex> lock(mutex_);
rcl_interfaces::msg::ListParametersResult result;
// TODO(esteve): define parameter separator, use "." for now
for (auto & kv : parameters_) {
if (std::any_of(prefixes.cbegin(), prefixes.cend(), [&kv, &depth](const std::string & prefix) {
if (kv.first == prefix) {
return true;
} else if (kv.first.find(prefix + ".") == 0) {
size_t length = prefix.length();
std::string substr = kv.first.substr(length);
// Cast as unsigned integer to avoid warning
return static_cast<uint64_t>(std::count(substr.begin(), substr.end(), '.')) < depth;
}
return false;
}))
{
result.names.push_back(kv.first);
size_t last_separator = kv.first.find_last_of('.');
if (std::string::npos != last_separator) {
std::string prefix = kv.first.substr(0, last_separator);
if (std::find(result.prefixes.cbegin(), result.prefixes.cend(), prefix) ==
result.prefixes.cend())
{
result.prefixes.push_back(prefix);
}
}
}
}
return result;
}
std::map<std::string, std::string>
Node::get_topic_names_and_types() const
{
rmw_topic_names_and_types_t topic_names_and_types;
topic_names_and_types.topic_count = 0;
topic_names_and_types.topic_names = nullptr;
topic_names_and_types.type_names = nullptr;
auto ret = rmw_get_topic_names_and_types(node_handle_.get(), &topic_names_and_types);
if (ret != RMW_RET_OK) {
// *INDENT-OFF*
throw std::runtime_error(
std::string("could not get topic names and types: ") + rmw_get_error_string_safe());
// *INDENT-ON*
}
std::map<std::string, std::string> topics;
for (size_t i = 0; i < topic_names_and_types.topic_count; ++i) {
topics[topic_names_and_types.topic_names[i]] = topic_names_and_types.type_names[i];
}
ret = rmw_destroy_topic_names_and_types(&topic_names_and_types);
if (ret != RMW_RET_OK) {
// *INDENT-OFF*
throw std::runtime_error(
std::string("could not destroy topic names and types: ") + rmw_get_error_string_safe());
// *INDENT-ON*
}
return topics;
}
size_t
Node::count_publishers(const std::string & topic_name) const
{
size_t count;
auto ret = rmw_count_publishers(node_handle_.get(), topic_name.c_str(), &count);
if (ret != RMW_RET_OK) {
// *INDENT-OFF*
throw std::runtime_error(
std::string("could not count publishers: ") + rmw_get_error_string_safe());
// *INDENT-ON*
}
return count;
}
size_t
Node::count_subscribers(const std::string & topic_name) const
{
size_t count;
auto ret = rmw_count_subscribers(node_handle_.get(), topic_name.c_str(), &count);
if (ret != RMW_RET_OK) {
// *INDENT-OFF*
throw std::runtime_error(
std::string("could not count subscribers: ") + rmw_get_error_string_safe());
// *INDENT-ON*
}
return count;
}
const Node::CallbackGroupWeakPtrList &
Node::get_callback_groups() const
{
return callback_groups_;
}

278
rclcpp/src/rclcpp/parameter.cpp Normal file
View file

@ -0,0 +1,278 @@
// 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.
#include "rclcpp/parameter.hpp"
#include <ostream>
#include <sstream>
#include <string>
#include <vector>
using rclcpp::parameter::ParameterType;
using rclcpp::parameter::ParameterVariant;
ParameterVariant::ParameterVariant()
: name_("")
{
value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET;
}
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;
}
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;
}
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;
}
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;
}
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;
}
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;
}
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;
}
ParameterType
ParameterVariant::get_type() const
{
return static_cast<ParameterType>(value_.type);
}
std::string
ParameterVariant::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*
}
}
const std::string &
ParameterVariant::get_name() const
{
return name_;
}
rcl_interfaces::msg::ParameterValue
ParameterVariant::get_parameter_value() const
{
return value_;
}
int64_t
ParameterVariant::as_int() const
{
return get_value<ParameterType::PARAMETER_INTEGER>();
}
double
ParameterVariant::as_double() const
{
return get_value<ParameterType::PARAMETER_DOUBLE>();
}
const std::string &
ParameterVariant::as_string() const
{
return get_value<ParameterType::PARAMETER_STRING>();
}
bool
ParameterVariant::as_bool() const
{
return get_value<ParameterType::PARAMETER_BOOL>();
}
const std::vector<uint8_t> &
ParameterVariant::as_bytes() const
{
return get_value<ParameterType::PARAMETER_BYTES>();
}
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);
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*
}
}
rcl_interfaces::msg::Parameter
ParameterVariant::to_parameter()
{
rcl_interfaces::msg::Parameter parameter;
parameter.name = name_;
parameter.value = value_;
return parameter;
}
std::string
ParameterVariant::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*
}
}
std::string
rclcpp::parameter::_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();
}
std::ostream &
rclcpp::parameter::operator<<(std::ostream & os, const rclcpp::parameter::ParameterVariant & pv)
{
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() << "\", ";
ss << "\"type\": \"" << param.get_type_name() << "\", ";
ss << "\"value\": \"" << param.value_to_string() << "\"}";
return ss.str();
}
std::string
std::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();
}

366
rclcpp/src/rclcpp/parameter_client.cpp Normal file
View file

@ -0,0 +1,366 @@
// 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.
#include "rclcpp/parameter_client.hpp"
#include <algorithm>
#include <string>
#include <vector>
// 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
{
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();
}
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();
}
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();
}
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 {
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");
}
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)
{
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;
// *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)
{
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);
}
}
);
// *INDENT-ON*
return future_result;
}
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)
{
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;
// *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)
{
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);
}
}
);
// *INDENT-ON*
return future_result;
}
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)
{
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>();
// *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)
{
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>
AsyncParametersClient::set_parameters_atomically(
const std::vector<rclcpp::parameter::ParameterVariant> & parameters,
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>();
// *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)
{
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>
AsyncParametersClient::list_parameters(
const std::vector<std::string> & prefixes,
uint64_t depth,
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();
auto request = std::make_shared<rcl_interfaces::srv::ListParameters::Request>();
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)
{
promise_result->set_value(cb_f.get()->result);
if (callback != nullptr) {
callback(future_result);
}
}
);
// *INDENT-ON*
return future_result;
}
SyncParametersClient::SyncParametersClient(
rclcpp::node::Node::SharedPtr node)
: node_(node)
{
executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
async_parameters_client_ = std::make_shared<AsyncParametersClient>(node);
}
SyncParametersClient::SyncParametersClient(
rclcpp::executor::Executor::SharedPtr executor,
rclcpp::node::Node::SharedPtr node)
: executor_(executor), node_(node)
{
async_parameters_client_ = std::make_shared<AsyncParametersClient>(node);
}
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)
{
return f.get();
}
// Return an empty vector if unsuccessful
return std::vector<rclcpp::parameter::ParameterVariant>();
}
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) ==
rclcpp::executors::FutureReturnCode::SUCCESS)
{
return f.get();
}
return std::vector<rclcpp::parameter::ParameterType>();
}
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) ==
rclcpp::executors::FutureReturnCode::SUCCESS)
{
return f.get();
}
return std::vector<rcl_interfaces::msg::SetParametersResult>();
}
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) ==
rclcpp::executors::FutureReturnCode::SUCCESS)
{
return f.get();
}
throw std::runtime_error("Unable to get result of set parameters service call.");
}
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) ==
rclcpp::executors::FutureReturnCode::SUCCESS)
{
return f.get();
}
throw std::runtime_error("Unable to get result of list parameters service call.");
}

140
rclcpp/src/rclcpp/parameter_service.cpp Normal file
View file

@ -0,0 +1,140 @@
// 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.
#include "rclcpp/parameter_service.hpp"
#include <algorithm>
#include <vector>
using rclcpp::parameter_service::ParameterService;
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](
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;
}
);
// *INDENT-ON*
}

141
rclcpp/src/rclcpp/publisher.cpp Normal file
View file

@ -0,0 +1,141 @@
// 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.
#include "rclcpp/publisher.hpp"
#include <rmw/error_handling.h>
#include <rmw/rmw.h>
#include <iostream>
#include <memory>
#include <mutex>
#include <sstream>
#include <string>
#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"
using rclcpp::publisher::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),
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*
}
}
PublisherBase::~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());
}
}
}
const std::string &
PublisherBase::get_topic_name() const
{
return topic_;
}
size_t
PublisherBase::get_queue_size() const
{
return queue_size_;
}
const rmw_gid_t &
PublisherBase::get_gid() const
{
return rmw_gid_;
}
const rmw_gid_t &
PublisherBase::get_intra_process_gid() const
{
return intra_process_rmw_gid_;
}
bool
PublisherBase::operator==(const rmw_gid_t & gid) const
{
return *this == &gid;
}
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) {
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;
}
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;
// 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*
}
}

59
rclcpp/src/rclcpp/service.cpp Normal file
View file

@ -0,0 +1,59 @@
// 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.
#include "rclcpp/service.hpp"
#include <functional>
#include <iostream>
#include <memory>
#include <sstream>
#include <string>
#include "rclcpp/any_service_callback.hpp"
#include "rclcpp/macros.hpp"
#include "rmw/error_handling.h"
#include "rmw/rmw.h"
using rclcpp::service::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)
{}
ServiceBase::~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();
}
}
}
std::string
ServiceBase::get_service_name()
{
return this->service_name_;
}
const rmw_service_t *
ServiceBase::get_service_handle()
{
return this->service_handle_;
}

78
rclcpp/src/rclcpp/subscription.cpp Normal file
View file

@ -0,0 +1,78 @@
// 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.
#include "rclcpp/subscription.hpp"
#include <cstdio>
#include <string>
#include "rmw/error_handling.h"
#include "rmw/rmw.h"
using rclcpp::subscription::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),
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_;
}
SubscriptionBase::~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();
}
}
}
const std::string &
SubscriptionBase::get_topic_name() const
{
return this->topic_name_;
}
const rmw_subscription_t *
SubscriptionBase::get_subscription_handle() const
{
return subscription_handle_;
}
const rmw_subscription_t *
SubscriptionBase::get_intra_process_subscription_handle() const
{
return intra_process_subscription_handle_;
}

47
rclcpp/src/rclcpp/timer.cpp Normal file
View file

@ -0,0 +1,47 @@
// 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.
#include "rclcpp/timer.hpp"
#include <chrono>
using rclcpp::timer::CallbackType;
using rclcpp::timer::TimerBase;
TimerBase::TimerBase(std::chrono::nanoseconds period, CallbackType callback)
: period_(period),
callback_(callback),
canceled_(false)
{}
TimerBase::~TimerBase()
{}
void
TimerBase::cancel()
{
this->canceled_ = true;
}
void
TimerBase::execute_callback() const
{
callback_();
}
const CallbackType &
TimerBase::get_callback() const
{
return callback_;
}

172
rclcpp/src/rclcpp/utilities.cpp Normal file
View file

@ -0,0 +1,172 @@
// 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.
#include "rclcpp/utilities.hpp"
#include <atomic>
#include <condition_variable>
#include <csignal>
#include <cstdio>
#include <cstring>
#include <mutex>
#include <string>
#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
/// Represent the status of the global interrupt signal.
static volatile sig_atomic_t g_signal_status = 0;
/// Guard condition for interrupting the rmw implementation when the global interrupt signal fired.
static rmw_guard_condition_t * g_sigint_guard_cond_handle = rmw_create_guard_condition();
/// Condition variable for timed sleep (see sleep_for).
static std::condition_variable g_interrupt_condition_variable;
static std::atomic<bool> g_is_interrupted(false);
/// Mutex for protecting the global condition variable.
static std::mutex g_interrupt_mutex;
#ifdef HAS_SIGACTION
static struct sigaction old_action;
#else
static void (* old_signal_handler)(int) = 0;
#endif
void
#ifdef HAS_SIGACTION
signal_handler(int signal_value, siginfo_t * siginfo, void * context)
#else
signal_handler(int signal_value)
#endif
{
// 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) {
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();
}
void
rclcpp::utilities::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);
// 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
strerror_r(errno, error_string, error_length);
#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*
}
}
bool
rclcpp::utilities::ok()
{
return ::g_signal_status == 0;
}
void
rclcpp::utilities::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();
}
rmw_guard_condition_t *
rclcpp::utilities::get_global_sigint_guard_condition()
{
return ::g_sigint_guard_cond_handle;
}
bool
rclcpp::utilities::sleep_for(const std::chrono::nanoseconds & nanoseconds)
{
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;
}

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

52
rclcpp/test/test_function_traits.cpp
View file

@ -21,6 +21,7 @@ int func_no_args()
return 0;
}
// NOLINTNEXTLINE(readability/casting)
int func_one_int(int)
{
return 1;
@ -142,15 +143,20 @@ TEST(TestFunctionTraits, arity) {
return 0;
};
auto lambda_one_int = [](int) {
auto lambda_one_int = [](int one) {
(void)one;
return 1;
};
auto lambda_two_ints = [](int, int) {
auto lambda_two_ints = [](int one, int two) {
(void)one;
(void)two;
return 2;
};
auto lambda_one_int_one_char = [](int, char) {
auto lambda_one_int_one_char = [](int one, char two) {
(void)one;
(void)two;
return 3;
};
@ -224,15 +230,20 @@ TEST(TestFunctionTraits, argument_types) {
>::value, "Functor accepts a char as second argument");
// Test lambdas
auto lambda_one_int = [](int) {
auto lambda_one_int = [](int one) {
(void)one;
return 1;
};
auto lambda_two_ints = [](int, int) {
auto lambda_two_ints = [](int one, int two) {
(void)one;
(void)two;
return 2;
};
auto lambda_one_int_one_char = [](int, char) {
auto lambda_one_int_one_char = [](int one, char two) {
(void)one;
(void)two;
return 3;
};
@ -336,15 +347,20 @@ TEST(TestFunctionTraits, check_arguments) {
"Functor accepts an int and a char as arguments");
// Test lambdas
auto lambda_one_int = [](int) {
auto lambda_one_int = [](int one) {
(void)one;
return 1;
};
auto lambda_two_ints = [](int, int) {
auto lambda_two_ints = [](int one, int two) {
(void)one;
(void)two;
return 2;
};
auto lambda_one_int_one_char = [](int, char) {
auto lambda_one_int_one_char = [](int one, char two) {
(void)one;
(void)two;
return 3;
};
@ -378,11 +394,14 @@ TEST(TestFunctionTraits, check_arguments) {
Tests that same_arguments work.
*/
TEST(TestFunctionTraits, same_arguments) {
auto lambda_one_int = [](int) {
auto lambda_one_int = [](int one) {
(void)one;
return 1;
};
auto lambda_two_ints = [](int, int) {
auto lambda_two_ints = [](int one, int two) {
(void)one;
(void)two;
return 1;
};
@ -427,15 +446,20 @@ TEST(TestFunctionTraits, sfinae_match) {
return 0;
};
auto lambda_one_int = [](int) {
auto lambda_one_int = [](int one) {
(void)one;
return 1;
};
auto lambda_two_ints = [](int, int) {
auto lambda_two_ints = [](int one, int two) {
(void)one;
(void)two;
return 2;
};
auto lambda_one_int_one_char = [](int, char) {
auto lambda_one_int_one_char = [](int one, char two) {
(void)one;
(void)two;
return 3;
};

102
rclcpp/test/test_intra_process_manager.cpp
View file

@ -13,12 +13,12 @@
// limitations under the License.
#include <memory>
#include <string>
#include <gtest/gtest.h>
#include <rclcpp/allocator/allocator_common.hpp>
#include <rclcpp/macros.hpp>
#include <rmw/types.h>
#include "gtest/gtest.h"
#include "rclcpp/allocator/allocator_common.hpp"
#include "rclcpp/macros.hpp"
#include "rmw/types.h"
// Mock up publisher and subscription base to avoid needing an rmw impl.
namespace rclcpp
@ -79,9 +79,9 @@ public:
}
};
}
}
}
} // namespace mock
} // namespace publisher
} // namespace rclcpp
namespace rclcpp
{
@ -111,22 +111,25 @@ public:
size_t mock_queue_size;
};
}
}
}
} // namespace mock
} // namespace subscription
} // namespace rclcpp
// 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
// NOLINTNEXTLINE(build/include_order)
#include <rcl_interfaces/msg/intra_process_message.hpp>
/*
@ -142,13 +145,15 @@ public:
TEST(TestIntraProcessManager, nominal) {
rclcpp::intra_process_manager::IntraProcessManager ipm;
auto p1 =
std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
auto p1 = std::make_shared<
rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>
>();
p1->mock_topic_name = "nominal1";
p1->mock_queue_size = 2;
auto p2 =
std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
auto p2 = std::make_shared<
rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>
>();
p2->mock_topic_name = "nominal2";
p2->mock_queue_size = 10;
@ -231,8 +236,9 @@ TEST(TestIntraProcessManager, nominal) {
TEST(TestIntraProcessManager, remove_publisher_before_trying_to_take) {
rclcpp::intra_process_manager::IntraProcessManager ipm;
auto p1 =
std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
auto p1 = std::make_shared<
rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>
>();
p1->mock_topic_name = "nominal1";
p1->mock_queue_size = 10;
@ -272,8 +278,9 @@ TEST(TestIntraProcessManager, remove_publisher_before_trying_to_take) {
TEST(TestIntraProcessManager, removed_subscription_affects_take) {
rclcpp::intra_process_manager::IntraProcessManager ipm;
auto p1 =
std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
auto p1 = std::make_shared<
rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>
>();
p1->mock_topic_name = "nominal1";
p1->mock_queue_size = 10;
@ -342,8 +349,9 @@ TEST(TestIntraProcessManager, removed_subscription_affects_take) {
TEST(TestIntraProcessManager, multiple_subscriptions_one_publisher) {
rclcpp::intra_process_manager::IntraProcessManager ipm;
auto p1 =
std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
auto p1 = std::make_shared<
rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>
>();
p1->mock_topic_name = "nominal1";
p1->mock_queue_size = 10;
@ -413,18 +421,21 @@ TEST(TestIntraProcessManager, multiple_subscriptions_one_publisher) {
TEST(TestIntraProcessManager, multiple_publishers_one_subscription) {
rclcpp::intra_process_manager::IntraProcessManager ipm;
auto p1 =
std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
auto p1 = std::make_shared<
rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>
>();
p1->mock_topic_name = "nominal1";
p1->mock_queue_size = 10;
auto p2 =
std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
auto p2 = std::make_shared<
rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>
>();
p2->mock_topic_name = "nominal1";
p2->mock_queue_size = 10;
auto p3 =
std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
auto p3 = std::make_shared<
rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>
>();
p3->mock_topic_name = "nominal1";
p3->mock_queue_size = 10;
@ -510,18 +521,21 @@ TEST(TestIntraProcessManager, multiple_publishers_one_subscription) {
TEST(TestIntraProcessManager, multiple_publishers_multiple_subscription) {
rclcpp::intra_process_manager::IntraProcessManager ipm;
auto p1 =
std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
auto p1 = std::make_shared<
rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>
>();
p1->mock_topic_name = "nominal1";
p1->mock_queue_size = 10;
auto p2 =
std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
auto p2 = std::make_shared<
rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>
>();
p2->mock_topic_name = "nominal1";
p2->mock_queue_size = 10;
auto p3 =
std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
auto p3 = std::make_shared<
rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>
>();
p3->mock_topic_name = "nominal1";
p3->mock_queue_size = 10;
@ -674,8 +688,9 @@ TEST(TestIntraProcessManager, multiple_publishers_multiple_subscription) {
TEST(TestIntraProcessManager, ring_buffer_displacement) {
rclcpp::intra_process_manager::IntraProcessManager ipm;
auto p1 =
std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
auto p1 = std::make_shared<
rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>
>();
p1->mock_topic_name = "nominal1";
p1->mock_queue_size = 2;
@ -744,8 +759,9 @@ TEST(TestIntraProcessManager, ring_buffer_displacement) {
TEST(TestIntraProcessManager, subscription_creation_race_condition) {
rclcpp::intra_process_manager::IntraProcessManager ipm;
auto p1 =
std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
auto p1 = std::make_shared<
rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>
>();
p1->mock_topic_name = "nominal1";
p1->mock_queue_size = 2;
@ -792,8 +808,9 @@ TEST(TestIntraProcessManager, publisher_out_of_scope_take) {
uint64_t p1_id;
uint64_t p1_m1_id;
{
auto p1 =
std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
auto p1 = std::make_shared<
rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>
>();
p1->mock_topic_name = "nominal1";
p1->mock_queue_size = 2;
@ -830,8 +847,9 @@ TEST(TestIntraProcessManager, publisher_out_of_scope_store) {
uint64_t p1_id;
{
auto p1 =
std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
auto p1 = std::make_shared<
rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>
>();
p1->mock_topic_name = "nominal1";
p1->mock_queue_size = 2;

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>
/*