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