Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add required conversions to any subscription callback #8

Merged
merged 2 commits into from
Dec 8, 2021
Merged

Add required conversions to any subscription callback #8

Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
4bb743b
Add required conversions to any subscription callback
Dec 8, 2021
9188728
Removed extra space
Dec 8, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
127 changes: 89 additions & 38 deletions rclcpp/include/rclcpp/any_subscription_callback.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -477,6 +477,22 @@ class AnySubscriptionCallback
}
}

std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>
convert_custom_type_to_ros_message_unique_ptr(const SubscribedType & msg)
{
if constexpr (rclcpp::TypeAdapter<MessageT>::is_specialized::value) {
auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_type_allocator_, 1);
ROSMessageTypeAllocatorTraits::construct(ros_message_type_allocator_, ptr);
rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(msg, *ptr);
return std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>(ptr, ros_message_type_deleter_);
} else {
throw std::runtime_error(
"convert_custom_type_to_ros_message_unique_ptr "
"unexpectedly called without TypeAdapter");
}
}


Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Nit: extra space here.

// Dispatch when input is a ros message and the output could be anything.
void
dispatch(
Expand Down Expand Up @@ -687,21 +703,21 @@ class AnySubscriptionCallback
std::is_same_v<T, SharedPtrCallback>
))
{
// callback(message); //TODO Convert
callback(create_custom_unique_ptr_from_custom_shared_ptr_message(message));
} else if constexpr ( // NOLINT[readability/braces]
is_ta && (
std::is_same_v<T, UniquePtrWithInfoCallback>||
std::is_same_v<T, SharedPtrWithInfoCallback>
))
{
// callback(message, message_info); //TODO Convert
callback(create_custom_unique_ptr_from_custom_shared_ptr_message(message), message_info);
} else if constexpr ( // NOLINT[readability/braces]
is_ta && (
std::is_same_v<T, SharedConstPtrCallback>||
std::is_same_v<T, ConstRefSharedConstPtrCallback>
))
{
// callback(message); //TODO Convert
callback(message);
} else if constexpr ( // NOLINT[readability/braces]
is_ta && (
std::is_same_v<T, SharedConstPtrWithInfoCallback>||
Expand All @@ -712,29 +728,55 @@ class AnySubscriptionCallback
}
// conditions for ros message type
else if constexpr (std::is_same_v<T, ConstRefROSMessageCallback>) { // NOLINT
//callback(*message); //TODO convert to rosMessage
if constexpr (is_ta) {
auto local = convert_custom_type_to_ros_message_unique_ptr(*message);
callback(*local);
} else {
callback(*message);
}
} else if constexpr (std::is_same_v<T, ConstRefWithInfoROSMessageCallback>) {
//callback(*message, message_info); //TODO convert to rosMessage
if constexpr (is_ta) {
auto local = convert_custom_type_to_ros_message_unique_ptr(*message);
callback(*local, message_info);
} else {
callback(*message, message_info);
}
} else if constexpr ( // NOLINT[readability/braces]
std::is_same_v<T, UniquePtrROSMessageCallback>||
std::is_same_v<T, SharedPtrROSMessageCallback>)
{
// callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message));
if constexpr (is_ta) {
callback(convert_custom_type_to_ros_message_unique_ptr(*message));
} else {
callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message));
}
} else if constexpr ( // NOLINT[readability/braces]
std::is_same_v<T, UniquePtrWithInfoROSMessageCallback>||
std::is_same_v<T, SharedPtrWithInfoROSMessageCallback>)
{
// callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message), message_info);
if constexpr (is_ta) {
callback(convert_custom_type_to_ros_message_unique_ptr(*message), message_info);
} else {
callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message), message_info);
}
} else if constexpr ( // NOLINT[readability/braces]
std::is_same_v<T, SharedConstPtrROSMessageCallback>||
std::is_same_v<T, ConstRefSharedConstPtrROSMessageCallback>)
{
// callback(message);
if constexpr (is_ta) {
callback(convert_custom_type_to_ros_message_unique_ptr(*message));
} else {
callback(message);
}
} else if constexpr ( // NOLINT[readability/braces]
std::is_same_v<T, SharedConstPtrWithInfoROSMessageCallback>||
std::is_same_v<T, ConstRefSharedConstPtrWithInfoROSMessageCallback>)
{
// callback(message, message_info);
if constexpr (is_ta) {
callback(convert_custom_type_to_ros_message_unique_ptr(*message), message_info);
} else {
callback(message, message_info);
}
}
// condition to catch SerializedMessage types
else if constexpr ( // NOLINT[readability/braces]
Expand Down Expand Up @@ -767,8 +809,6 @@ class AnySubscriptionCallback
const rclcpp::MessageInfo & message_info)
{

std::cout << "ASC dispatch_intra_process" << std::endl;

TRACEPOINT(callback_start, static_cast<const void *>(this), true);
// Check if the variant is "unset", throw if it is.
if (callback_variant_.index() == 0) {
Expand All @@ -783,80 +823,91 @@ class AnySubscriptionCallback
using T = std::decay_t<decltype(callback)>;
static constexpr bool is_ta = rclcpp::TypeAdapter<MessageT>::is_specialized::value;

std::cout << "IS TA: " << is_ta << std::endl;

// conditions for custom type
if constexpr (is_ta && std::is_same_v<T, ConstRefCallback>) {
//We won't need to convert
//auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message);
std::cout << "ASC crcb" << std::endl;
callback(*message);
} else if constexpr (is_ta && std::is_same_v<T, ConstRefWithInfoCallback>) { // NOLINT
//auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message);
std::cout << "ASC crcb+info" << std::endl;
callback(*message, message_info);
} else if constexpr ( // NOLINT[readability/braces]
is_ta && (
std::is_same_v<T, UniquePtrCallback>||
std::is_same_v<T, SharedPtrCallback>
))
std::is_same_v<T, SharedPtrCallback>)
)
{
std::cout << "ASC unorshcb" << std::endl;
//callback(convert_ros_message_to_custom_type_unique_ptr(*message));
callback(std::move(message)); //TODO convert
callback(std::move(message));
} else if constexpr ( // NOLINT[readability/braces]
is_ta && (
std::is_same_v<T, UniquePtrWithInfoCallback>||
std::is_same_v<T, SharedPtrWithInfoCallback>
))
{
std::cout << "ASC unorshcb+info" << std::endl;
//callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info);
callback(std::move(message), message_info); //TODO convert
callback(std::move(message), message_info);
} else if constexpr ( // NOLINT[readability/braces]
is_ta && (
std::is_same_v<T, SharedConstPtrCallback>||
std::is_same_v<T, ConstRefSharedConstPtrCallback>
))
{
std::cout << "ASC shconsrref" << std::endl;
//callback(convert_ros_message_to_custom_type_unique_ptr(*message));
// callback(message); //TODO Convert
callback(std::move(message));
} else if constexpr ( // NOLINT[readability/braces]
is_ta && (
std::is_same_v<T, SharedConstPtrWithInfoCallback>||
std::is_same_v<T, ConstRefSharedConstPtrWithInfoCallback>
))
{
std::cout << "ASC shconsrref+info" << std::endl;
//callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info);
//callback(message, message_info); //TODO Convert
callback(std::move(message), message_info);
}
// conditions for ros message type
else if constexpr (std::is_same_v<T, ConstRefROSMessageCallback>) { // NOLINT
//callback(*message);
if constexpr (is_ta) {
auto local = convert_custom_type_to_ros_message_unique_ptr(*message);
callback(*local);
} else {
callback(*message);
}
} else if constexpr (std::is_same_v<T, ConstRefWithInfoROSMessageCallback>) {
// callback(*message, message_info);
if constexpr (is_ta) {
auto local = convert_custom_type_to_ros_message_unique_ptr(*message);
callback(*local, message_info);
} else {
callback(*message, message_info);
}
} else if constexpr ( // NOLINT[readability/braces]
std::is_same_v<T, UniquePtrROSMessageCallback>||
std::is_same_v<T, SharedPtrROSMessageCallback>)
{
// callback(std::move(message));
if constexpr (is_ta) {
callback(convert_custom_type_to_ros_message_unique_ptr(*message));
} else {
callback(std::move(message));
}
} else if constexpr ( // NOLINT[readability/braces]
std::is_same_v<T, UniquePtrWithInfoROSMessageCallback>||
std::is_same_v<T, SharedPtrWithInfoROSMessageCallback>)
{
// callback(std::move(message), message_info);
if constexpr (is_ta) {
callback(convert_custom_type_to_ros_message_unique_ptr(*message), message_info);
} else {
callback(std::move(message), message_info);
}
} else if constexpr ( // NOLINT[readability/braces]
std::is_same_v<T, SharedConstPtrROSMessageCallback>||
std::is_same_v<T, ConstRefSharedConstPtrROSMessageCallback>)
{
// callback(std::move(message));
if constexpr (is_ta) {
callback(convert_custom_type_to_ros_message_unique_ptr(*message));
} else {
callback(std::move(message));
}
} else if constexpr ( // NOLINT[readability/braces]
std::is_same_v<T, SharedConstPtrWithInfoROSMessageCallback>||
std::is_same_v<T, ConstRefSharedConstPtrWithInfoROSMessageCallback>)
{
// callback(std::move(message), message_info);
if constexpr (is_ta) {
callback(convert_custom_type_to_ros_message_unique_ptr(*message), message_info);
} else {
callback(std::move(message), message_info);
}
}
// condition to catch SerializedMessage types
else if constexpr ( // NOLINT[readability/braces]
Expand Down