typename rclcpp::timer::WallTimer<CallbackType>::SharedPtr
Node::create_wall_timer(
std::chrono::nanoseconds period,
CallbackType callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group)
{
auto timer = rclcpp::timer::WallTimer<CallbackType>::make_shared(
period, std::move(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_++;
if (rcl_trigger_guard_condition(¬ify_guard_condition_) != RCL_RET_OK) {
throw std::runtime_error(
std::string(
"Failed to notify waitset on timer creation: ") + rmw_get_error_string());
}
return timer;
}
typename rclcpp::service::Service<ServiceT>::SharedPtr
Node::create_service(
const std::string & service_name,
CallbackT && callback,
const rmw_qos_profile_t & qos_profile,
rclcpp::callback_group::CallbackGroup::SharedPtr group)
{
rclcpp::service::AnyServiceCallback<ServiceT> any_service_callback;
any_service_callback.set(std::forward<CallbackT>(callback));
rcl_service_options_t service_options = rcl_service_get_default_options();
service_options.qos = qos_profile;
auto serv = service::Service<ServiceT>::make_shared(
node_handle_,
service_name, any_service_callback, service_options);
auto serv_base_ptr = std::dynamic_pointer_cast<service::ServiceBase>(serv);
if (group) {
if (!group_in_node(group)) {
// TODO(jacquelinekay): use custom exception
throw std::runtime_error("Cannot create service, group not in node.");
}
group->add_service(serv_base_ptr);
} else {
default_callback_group_->add_service(serv_base_ptr);
}
number_of_services_++;
if (rcl_trigger_guard_condition(¬ify_guard_condition_) != RCL_RET_OK) {
throw std::runtime_error(
std::string(
"Failed to notify waitset on service creation: ") + rmw_get_error_string());
}
return serv;
}
void
NodeTopics::add_subscription(
rclcpp::subscription::SubscriptionBase::SharedPtr subscription,
rclcpp::callback_group::CallbackGroup::SharedPtr callback_group)
{
// Assign to a group.
if (callback_group) {
if (!node_base_->callback_group_in_node(callback_group)) {
// TODO(jacquelinekay): use custom exception
throw std::runtime_error("Cannot create subscription, callback group not in node.");
}
callback_group->add_subscription(subscription);
} else {
node_base_->get_default_callback_group()->add_subscription(subscription);
}
// Notify the executor that a new subscription was created using the parent Node.
{
auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock();
if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
throw std::runtime_error(
std::string(
"Failed to notify waitset on subscription creation: ") + rmw_get_error_string()
);
}
}
}
void
NodeServices::add_client(
rclcpp::client::ClientBase::SharedPtr client_base_ptr,
rclcpp::callback_group::CallbackGroup::SharedPtr group)
{
if (group) {
if (!node_base_->callback_group_in_node(group)) {
// TODO(jacquelinekay): use custom exception
throw std::runtime_error("Cannot create client, group not in node.");
}
group->add_client(client_base_ptr);
} else {
node_base_->get_default_callback_group()->add_client(client_base_ptr);
}
// Notify the executor that a new client was created using the parent Node.
{
auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock();
if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
throw std::runtime_error(
std::string("Failed to notify waitset on client creation: ") + rmw_get_error_string()
);
}
}
}
typename client::Client<ServiceT>::SharedPtr
Node::create_client(
const std::string & service_name,
const rmw_qos_profile_t & qos_profile,
rclcpp::callback_group::CallbackGroup::SharedPtr group)
{
using rosidl_generator_cpp::get_service_type_support_handle;
auto service_type_support_handle =
get_service_type_support_handle<ServiceT>();
rmw_client_t * client_handle = rmw_create_client(
this->node_handle_.get(), service_type_support_handle, service_name.c_str(), &qos_profile);
if (!client_handle) {
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
throw std::runtime_error(
std::string("could not create client: ") +
rmw_get_error_string_safe());
// *INDENT-ON*
}
using rclcpp::client::Client;
using rclcpp::client::ClientBase;
auto cli = Client<ServiceT>::make_shared(
node_handle_,
client_handle,
service_name);
auto cli_base_ptr = std::dynamic_pointer_cast<ClientBase>(cli);
if (group) {
if (!group_in_node(group)) {
// TODO(esteve): use custom exception
throw std::runtime_error("Cannot create client, group not in node.");
}
group->add_client(cli_base_ptr);
} else {
default_callback_group_->add_client(cli_base_ptr);
}
number_of_clients_++;
return cli;
}
typename rclcpp::timer::WallTimer<CallbackType>::SharedPtr
Node::create_wall_timer(
std::chrono::nanoseconds period,
CallbackType callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group)
{
auto timer = rclcpp::timer::WallTimer<CallbackType>::make_shared(
period, std::move(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;
}
void
NodeTimers::add_timer(
rclcpp::timer::TimerBase::SharedPtr timer,
rclcpp::callback_group::CallbackGroup::SharedPtr callback_group)
{
if (callback_group) {
if (!node_base_->callback_group_in_node(callback_group)) {
// TODO(jacquelinekay): use custom exception
throw std::runtime_error("Cannot create timer, group not in node.");
}
callback_group->add_timer(timer);
} else {
node_base_->get_default_callback_group()->add_timer(timer);
}
if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
throw std::runtime_error(
std::string(
"Failed to notify waitset on timer creation: ") + rmw_get_error_string());
}
}
typename rclcpp::service::Service<ServiceT>::SharedPtr
Node::create_service(
const std::string & service_name,
CallbackT && callback,
const rmw_qos_profile_t & qos_profile,
rclcpp::callback_group::CallbackGroup::SharedPtr group)
{
using rosidl_generator_cpp::get_service_type_support_handle;
auto service_type_support_handle =
get_service_type_support_handle<ServiceT>();
rclcpp::service::AnyServiceCallback<ServiceT> any_service_callback;
any_service_callback.set(std::forward<CallbackT>(callback));
rmw_service_t * service_handle = rmw_create_service(
node_handle_.get(), service_type_support_handle, service_name.c_str(), &qos_profile);
if (!service_handle) {
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
throw std::runtime_error(
std::string("could not create service: ") +
rmw_get_error_string_safe());
// *INDENT-ON*
}
auto serv = service::Service<ServiceT>::make_shared(
node_handle_, service_handle, service_name, any_service_callback);
auto serv_base_ptr = std::dynamic_pointer_cast<service::ServiceBase>(serv);
if (group) {
if (!group_in_node(group)) {
// TODO(jacquelinekay): use custom exception
throw std::runtime_error("Cannot create service, group not in node.");
}
group->add_service(serv_base_ptr);
} else {
default_callback_group_->add_service(serv_base_ptr);
}
number_of_services_++;
return serv;
}
typename client::Client<ServiceT>::SharedPtr
Node::create_client(
const std::string & service_name,
const rmw_qos_profile_t & qos_profile,
rclcpp::callback_group::CallbackGroup::SharedPtr group)
{
rcl_client_options_t options = rcl_client_get_default_options();
options.qos = qos_profile;
using rclcpp::client::Client;
using rclcpp::client::ClientBase;
auto cli = Client<ServiceT>::make_shared(
node_handle_,
service_name,
options);
auto cli_base_ptr = std::dynamic_pointer_cast<ClientBase>(cli);
if (group) {
if (!group_in_node(group)) {
// TODO(esteve): use custom exception
throw std::runtime_error("Cannot create client, group not in node.");
}
group->add_client(cli_base_ptr);
} else {
default_callback_group_->add_client(cli_base_ptr);
}
number_of_clients_++;
if (rcl_trigger_guard_condition(¬ify_guard_condition_) != RCL_RET_OK) {
throw std::runtime_error(
std::string(
"Failed to notify waitset on client creation: ") + rmw_get_error_string());
}
return cli;
}
typename rclcpp::subscription::Subscription<MessageT, Alloc>::SharedPtr
Node::create_subscription(
const std::string & topic_name,
CallbackT && callback,
const rmw_qos_profile_t & qos_profile,
rclcpp::callback_group::CallbackGroup::SharedPtr group,
bool ignore_local_publications,
typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
msg_mem_strat,
typename std::shared_ptr<Alloc> allocator)
{
if (!allocator) {
allocator = std::make_shared<Alloc>();
}
rclcpp::subscription::AnySubscriptionCallback<MessageT,
Alloc> any_subscription_callback(allocator);
any_subscription_callback.set(std::forward<CallbackT>(callback));
using rosidl_generator_cpp::get_message_type_support_handle;
if (!msg_mem_strat) {
msg_mem_strat =
rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::create_default();
}
auto type_support_handle = get_message_type_support_handle<MessageT>();
rmw_subscription_t * subscriber_handle = rmw_create_subscription(
node_handle_.get(), type_support_handle,
topic_name.c_str(), &qos_profile, ignore_local_publications);
if (!subscriber_handle) {
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
throw std::runtime_error(
std::string("could not create subscription: ") + rmw_get_error_string_safe());
// *INDENT-ON*
}
using rclcpp::subscription::Subscription;
using rclcpp::subscription::SubscriptionBase;
auto sub = Subscription<MessageT, Alloc>::make_shared(
node_handle_,
subscriber_handle,
topic_name,
ignore_local_publications,
any_subscription_callback,
msg_mem_strat);
auto sub_base_ptr = std::dynamic_pointer_cast<SubscriptionBase>(sub);
// Setup intra process.
if (use_intra_process_comms_) {
rmw_subscription_t * intra_process_subscriber_handle = rmw_create_subscription(
node_handle_.get(), rclcpp::type_support::get_intra_process_message_msg_type_support(),
(topic_name + "__intra").c_str(), &qos_profile, false);
if (!subscriber_handle) {
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
throw std::runtime_error(
std::string("could not create intra process subscription: ") + rmw_get_error_string_safe());
// *INDENT-ON*
}
auto intra_process_manager =
context_->get_sub_context<rclcpp::intra_process_manager::IntraProcessManager>();
rclcpp::intra_process_manager::IntraProcessManager::WeakPtr weak_ipm = intra_process_manager;
uint64_t intra_process_subscription_id =
intra_process_manager->add_subscription(sub_base_ptr);
// *INDENT-OFF*
sub->setup_intra_process(
intra_process_subscription_id,
intra_process_subscriber_handle,
[weak_ipm](
uint64_t publisher_id,
uint64_t message_sequence,
uint64_t subscription_id,
typename Subscription<MessageT, Alloc>::MessageUniquePtr & message)
{
auto ipm = weak_ipm.lock();
if (!ipm) {
// TODO(wjwwood): should this just return silently? Or maybe return with a logged warning?
throw std::runtime_error(
"intra process take called after destruction of intra process manager");
}
ipm->take_intra_process_message<MessageT, Alloc>(
publisher_id, message_sequence, subscription_id, message);
},
[weak_ipm](const rmw_gid_t * sender_gid) -> bool {
auto ipm = weak_ipm.lock();
if (!ipm) {
throw std::runtime_error(
"intra process publisher check called after destruction of intra process manager");
}
return ipm->matches_any_publishers(sender_gid);
}
);
// *INDENT-ON*
}
// Assign to a group.
if (group) {
if (!group_in_node(group)) {
// TODO(jacquelinekay): use custom exception
throw std::runtime_error("Cannot create subscription, group not in node.");
}
group->add_subscription(sub_base_ptr);
} else {
default_callback_group_->add_subscription(sub_base_ptr);
}
number_of_subscriptions_++;
return sub;
}
typename rclcpp::subscription::Subscription<MessageT, Alloc>::SharedPtr
Node::create_subscription(
const std::string & topic_name,
CallbackT && callback,
const rmw_qos_profile_t & qos_profile,
rclcpp::callback_group::CallbackGroup::SharedPtr group,
bool ignore_local_publications,
typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
msg_mem_strat,
typename std::shared_ptr<Alloc> allocator)
{
if (!allocator) {
allocator = std::make_shared<Alloc>();
}
rclcpp::subscription::AnySubscriptionCallback<MessageT,
Alloc> any_subscription_callback(allocator);
any_subscription_callback.set(std::forward<CallbackT>(callback));
if (!msg_mem_strat) {
msg_mem_strat =
rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::create_default();
}
auto message_alloc =
std::make_shared<typename subscription::Subscription<MessageT, Alloc>::MessageAlloc>();
auto subscription_options = rcl_subscription_get_default_options();
subscription_options.qos = qos_profile;
subscription_options.allocator = rclcpp::allocator::get_rcl_allocator<MessageT>(
*message_alloc.get());
subscription_options.ignore_local_publications = ignore_local_publications;
using rclcpp::subscription::Subscription;
using rclcpp::subscription::SubscriptionBase;
auto sub = Subscription<MessageT, Alloc>::make_shared(
node_handle_,
topic_name,
subscription_options,
any_subscription_callback,
msg_mem_strat);
auto sub_base_ptr = std::dynamic_pointer_cast<SubscriptionBase>(sub);
// Setup intra process.
if (use_intra_process_comms_) {
auto intra_process_options = rcl_subscription_get_default_options();
intra_process_options.allocator = rclcpp::allocator::get_rcl_allocator<MessageT>(
*message_alloc.get());
intra_process_options.qos = qos_profile;
intra_process_options.ignore_local_publications = false;
auto intra_process_manager =
context_->get_sub_context<rclcpp::intra_process_manager::IntraProcessManager>();
rclcpp::intra_process_manager::IntraProcessManager::WeakPtr weak_ipm = intra_process_manager;
uint64_t intra_process_subscription_id =
intra_process_manager->add_subscription(sub_base_ptr);
// *INDENT-OFF*
sub->setup_intra_process(
intra_process_subscription_id,
[weak_ipm](
uint64_t publisher_id,
uint64_t message_sequence,
uint64_t subscription_id,
typename Subscription<MessageT, Alloc>::MessageUniquePtr & message)
{
auto ipm = weak_ipm.lock();
if (!ipm) {
// TODO(wjwwood): should this just return silently? Or maybe return with a logged warning?
throw std::runtime_error(
"intra process take called after destruction of intra process manager");
}
ipm->take_intra_process_message<MessageT, Alloc>(
publisher_id, message_sequence, subscription_id, message);
},
[weak_ipm](const rmw_gid_t * sender_gid) -> bool {
auto ipm = weak_ipm.lock();
if (!ipm) {
throw std::runtime_error(
"intra process publisher check called after destruction of intra process manager");
}
return ipm->matches_any_publishers(sender_gid);
},
intra_process_options
);
// *INDENT-ON*
}
// Assign to a group.
if (group) {
if (!group_in_node(group)) {
// TODO(jacquelinekay): use custom exception
throw std::runtime_error("Cannot create subscription, group not in node.");
}
group->add_subscription(sub_base_ptr);
} else {
default_callback_group_->add_subscription(sub_base_ptr);
}
number_of_subscriptions_++;
if (rcl_trigger_guard_condition(¬ify_guard_condition_) != RCL_RET_OK) {
throw std::runtime_error(
std::string(
"Failed to notify waitset on subscription creation: ") + rmw_get_error_string());
}
return sub;
}
