Skip to content

Commit

Permalink
test_any_subscription_callback isolated test working
Browse files Browse the repository at this point in the history 
Signed-off-by: Gonzalo de Pedro <[email protected]>
  • Loading branch information
@clalancette
Gonzalo de Pedro authored and clalancette committed Dec 7, 2021
1 parent 3fcf467 commit 2ac80ad
Show file tree
Hide file tree
Showing 6 changed files with 50 additions and 942 deletions.
50 changes: 31 additions & 19 deletions rclcpp/include/rclcpp/any_subscription_callback.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
#define RCLCPP__ANY_SUBSCRIPTION_CALLBACK_HPP_

#include <functional>
#include <iostream> //TODO remove when removed std::cout s
#include <memory>
#include <stdexcept>
#include <type_traits>
Expand Down Expand Up @@ -686,21 +687,21 @@ class AnySubscriptionCallback
std::is_same_v<T, SharedPtrCallback>
))
{
callback(message);
// callback(message); //TODO Convert
} else if constexpr ( // NOLINT[readability/braces]
is_ta && (
std::is_same_v<T, UniquePtrWithInfoCallback>||
std::is_same_v<T, SharedPtrWithInfoCallback>
))
{
callback(message, message_info);
// callback(message, message_info); //TODO Convert
} else if constexpr ( // NOLINT[readability/braces]
is_ta && (
std::is_same_v<T, SharedConstPtrCallback>||
std::is_same_v<T, ConstRefSharedConstPtrCallback>
))
{
callback(message);
// callback(message); //TODO Convert
} else if constexpr ( // NOLINT[readability/braces]
is_ta && (
std::is_same_v<T, SharedConstPtrWithInfoCallback>||
Expand All @@ -711,29 +712,29 @@ class AnySubscriptionCallback
}
// conditions for ros message type
else if constexpr (std::is_same_v<T, ConstRefROSMessageCallback>) { // NOLINT
callback(*message);
//callback(*message); //TODO convert to rosMessage
} else if constexpr (std::is_same_v<T, ConstRefWithInfoROSMessageCallback>) {
callback(*message, message_info);
//callback(*message, message_info); //TODO convert to rosMessage
} 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));
// 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);
// 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);
// callback(message);
} else if constexpr ( // NOLINT[readability/braces]
std::is_same_v<T, SharedConstPtrWithInfoROSMessageCallback>||
std::is_same_v<T, ConstRefSharedConstPtrWithInfoROSMessageCallback>)
{
callback(message, message_info);
// callback(message, message_info);
}
// condition to catch SerializedMessage types
else if constexpr ( // NOLINT[readability/braces]
Expand Down Expand Up @@ -765,6 +766,9 @@ class AnySubscriptionCallback
std::unique_ptr<SubscribedType, SubscribedTypeDeleter> message,
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 @@ -779,72 +783,80 @@ 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::cout << "ASC unorshcb" << std::endl;
//callback(convert_ros_message_to_custom_type_unique_ptr(*message));
callback(message);
callback(std::move(message)); //TODO convert
} 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(message, message_info);
callback(std::move(message), message_info); //TODO convert
} 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);
// callback(message); //TODO Convert
} 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);
//callback(message, message_info); //TODO Convert
}
// conditions for ros message type
else if constexpr (std::is_same_v<T, ConstRefROSMessageCallback>) { // NOLINT
callback(*message);
//callback(*message);
} else if constexpr (std::is_same_v<T, ConstRefWithInfoROSMessageCallback>) {
callback(*message, message_info);
// 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));
// 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);
// 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));
// 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);
// callback(std::move(message), message_info);
}
// condition to catch SerializedMessage types
else if constexpr ( // NOLINT[readability/braces]
Expand Down
11 changes: 10 additions & 1 deletion rclcpp/include/rclcpp/experimental/intra_process_manager.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -427,12 +427,18 @@ class IntraProcessManager
throw std::runtime_error("subscription has unexpectedly gone out of scope");
}
auto subscription_base = subscription_it->second.lock();
std::cout << "Subscription Base?" << std::endl;
if (subscription_base) {

std::cout << "Typed Subscription" << std::endl;

auto subscription = std::dynamic_pointer_cast<
rclcpp::experimental::SubscriptionIntraProcessBuffer<MessageT, Alloc, Deleter>
>(subscription_base);
if (nullptr == subscription) {

std::cout << "ROSMessage Subscription" << std::endl;

auto ros_message_subscription = std::dynamic_pointer_cast<
rclcpp::experimental::ROSMessageIntraProcessBuffer<MessageT, Alloc, Deleter>
>(subscription_base);
Expand All @@ -445,6 +451,8 @@ class IntraProcessManager
"allocator types, which is not supported");
} else {

std::cout << "ROSMessage TypeAdapted Subscription" << std::endl;

using ROSMessageType = typename rclcpp::TypeAdapter<MessageT>::ros_message_type;

if constexpr (rclcpp::TypeAdapter<MessageT>::is_specialized::value) {
Expand Down Expand Up @@ -474,7 +482,7 @@ class IntraProcessManager

}
} else {

std::cout << "Typed Subscription" << std::endl;
if (std::next(it) == subscription_ids.end()) {
// If this is the last subscription, give up ownership
subscription->provide_intra_process_data(std::move(message));
Expand All @@ -491,6 +499,7 @@ class IntraProcessManager
}

} else {
std::cout << "Erasing subscription" << std::endl;
subscriptions_.erase(subscription_it);
}
}
Expand Down
9 changes: 7 additions & 2 deletions rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,10 +18,11 @@
#include <rmw/rmw.h>

#include <functional>
#include <iostream> //TODO remove
#include <map>
#include <memory>
#include <stdexcept>
#include <string>
#include <stdexcept>
#include <utility>

#include "rcl/error_handling.h"
Expand Down Expand Up @@ -106,6 +107,8 @@ class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBuffer<ROSMe
provide_intra_process_message(ConstMessageSharedPtr message)
{

std::cout << "--------------Provide Intra Process Message (ConstMessageSharedPtr)" << std::endl;

if constexpr (!rclcpp::TypeAdapter<MessageT>::is_specialized::value) {
buffer_->add_shared(std::move(message));
trigger_guard_condition();
Expand All @@ -120,7 +123,7 @@ class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBuffer<ROSMe
void
provide_intra_process_message(MessageUniquePtr message)
{

std::cout << "--------------Provide Intra Process Message (MessageUniquePtr)" << std::endl;
if constexpr (!rclcpp::TypeAdapter<MessageT>::is_specialized::value) {
buffer_->add_unique(std::move(message));
trigger_guard_condition();
Expand All @@ -137,13 +140,15 @@ class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBuffer<ROSMe
void
provide_intra_process_data(ConstDataSharedPtr message)
{
std::cout << "--------------Provide Intra Process DATA (ConstDataSharedPtr)" << std::endl;
buffer_->add_shared(std::move(message));
trigger_guard_condition();
}

void
provide_intra_process_data(DataUniquePtr message)
{
std::cout << "--------------Provide Intra Process DATA (DataUniquePtr)" << std::endl;
buffer_->add_unique(std::move(message));
trigger_guard_condition();
}
Expand Down
3 changes: 2 additions & 1 deletion rclcpp/include/rclcpp/subscription.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -408,7 +408,8 @@ class Subscription : public SubscriptionBase
using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess<
SubscribedType,
AllocatorT,
SubscribedTypeDeleter>;
SubscribedTypeDeleter,
MessageT>;
std::shared_ptr<SubscriptionIntraProcessT> subscription_intra_process_;

};
Expand Down
Loading

0 comments on commit 2ac80ad

Please sign in to comment.