Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
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
55 changes: 54 additions & 1 deletion rclcpp/include/rclcpp/any_subscription_callback.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -392,13 +392,66 @@ class AnySubscriptionCallback
// automatically with lambda functions in cases where the arguments can be
// converted to one another, e.g. shared_ptr and unique_ptr.
using scbth = detail::SubscriptionCallbackTypeHelper<MessageT, CallbackT>;
constexpr auto is_invalid_signature =
rclcpp::function_traits::same_arguments<
typename scbth::callback_type,
std::function<void(std::shared_ptr<SubscribedType>)>
>::value ||
rclcpp::function_traits::same_arguments<
typename scbth::callback_type,
std::function<void(std::shared_ptr<SubscribedType>, const rclcpp::MessageInfo &)>
>::value ||
rclcpp::function_traits::same_arguments<
typename scbth::callback_type,
std::function<void(std::shared_ptr<ROSMessageType>)>
>::value ||
rclcpp::function_traits::same_arguments<
typename scbth::callback_type,
std::function<void(std::shared_ptr<ROSMessageType>, const rclcpp::MessageInfo &)>
>::value ||
rclcpp::function_traits::same_arguments<
typename scbth::callback_type,
std::function<void(std::shared_ptr<rclcpp::SerializedMessage>)>
>::value ||
rclcpp::function_traits::same_arguments<
typename scbth::callback_type,
std::function<void(std::shared_ptr<rclcpp::SerializedMessage>, const rclcpp::MessageInfo &)>
>::value;

callback_variant_ = static_cast<typename scbth::callback_type>(callback);
// Use the discovered type to force the type of callback when assigning
// into the variant.
if constexpr (is_invalid_signature) {
// If deprecated, call sub-routine that is deprecated.
set_deprecated(static_cast<typename scbth::callback_type>(callback));
} else {
// Otherwise just assign it.
callback_variant_ = static_cast<typename scbth::callback_type>(callback);
}

// Return copy of self for easier testing, normally will be compiled out.
return *this;
}

template<typename SetT>
/// Function for shared_ptr to non-const MessageT, which is deprecated.
[[deprecated("use 'void(std::shared_ptr<const MessageT>)' instead")]]
void
set_deprecated(std::function<void(std::shared_ptr<SetT>)> callback)
{
callback_variant_ = callback;
}

/// Function for shared_ptr to non-const MessageT with MessageInfo, which is deprecated.
template<typename SetT>
[[deprecated(
"use 'void(std::shared_ptr<const MessageT>, const rclcpp::MessageInfo &)' instead"
)]]
void
set_deprecated(std::function<void(std::shared_ptr<SetT>, const rclcpp::MessageInfo &)> callback)
{
callback_variant_ = callback;
}

std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>
create_ros_unique_ptr_from_ros_shared_ptr_message(
const std::shared_ptr<const ROSMessageType> & message)
Expand Down
79 changes: 2 additions & 77 deletions rclcpp/test/rclcpp/test_any_subscription_callback.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -177,7 +177,7 @@ TEST_F(TestAnySubscriptionCallback, is_serialized_message_callback) {
}
{
rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty> asc;
asc.set([](std::shared_ptr<rclcpp::SerializedMessage>) {});
asc.set([](std::shared_ptr<const rclcpp::SerializedMessage>) {});
EXPECT_TRUE(asc.is_serialized_message_callback());
EXPECT_NO_THROW(
asc.dispatch(
Expand All @@ -186,7 +186,7 @@ TEST_F(TestAnySubscriptionCallback, is_serialized_message_callback) {
}
{
rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty> asc;
asc.set([](std::shared_ptr<rclcpp::SerializedMessage>, const rclcpp::MessageInfo &) {});
asc.set([](std::shared_ptr<const rclcpp::SerializedMessage>, const rclcpp::MessageInfo &) {});
EXPECT_TRUE(asc.is_serialized_message_callback());
EXPECT_NO_THROW(
asc.dispatch(
Expand Down Expand Up @@ -646,78 +646,3 @@ INSTANTIATE_TEST_SUITE_P(
),
format_parameter_with_ta
);

//
// Versions of `std::shared_ptr<MessageT>`
//
void shared_ptr_free_func(std::shared_ptr<test_msgs::msg::Empty>) {}
void shared_ptr_w_info_free_func(
std::shared_ptr<test_msgs::msg::Empty>, const rclcpp::MessageInfo &)
{}

INSTANTIATE_TEST_SUITE_P(
SharedPtrCallbackTests,
DispatchTests,
::testing::Values(
// lambda
InstanceContext{"lambda", rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty>().set(
[](std::shared_ptr<test_msgs::msg::Empty>) {})},
InstanceContext{"lambda_with_info",
rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty>().set(
[](std::shared_ptr<test_msgs::msg::Empty>, const rclcpp::MessageInfo &) {})},
// free function
InstanceContext{"free_function", rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty>().set(
shared_ptr_free_func)},
InstanceContext{"free_function_with_info",
rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty>().set(
shared_ptr_w_info_free_func)},
// bind function
BindContext<test_msgs::msg::Empty, std::shared_ptr<test_msgs::msg::Empty>>("bind_method"),
BindContext<test_msgs::msg::Empty, std::shared_ptr<test_msgs::msg::Empty>,
const rclcpp::MessageInfo &>(
"bind_method_with_info")
),
format_parameter
);

void shared_ptr_ta_free_func(std::shared_ptr<MyEmpty>) {}
void shared_ptr_ta_w_info_free_func(
std::shared_ptr<MyEmpty>, const rclcpp::MessageInfo &)
{}

INSTANTIATE_TEST_SUITE_P(
SharedPtrCallbackTests,
DispatchTestsWithTA,
::testing::Values(
// lambda
InstanceContext<MyTA>{"lambda_ta", rclcpp::AnySubscriptionCallback<MyTA>().set(
[](std::shared_ptr<MyEmpty>) {})},
InstanceContext<MyTA>{"lambda_ta_with_info",
rclcpp::AnySubscriptionCallback<MyTA>().set(
[](std::shared_ptr<MyEmpty>, const rclcpp::MessageInfo &) {})},
InstanceContext<MyTA>{"lambda", rclcpp::AnySubscriptionCallback<MyTA>().set(
[](std::shared_ptr<test_msgs::msg::Empty>) {})},
InstanceContext<MyTA>{"lambda_with_info",
rclcpp::AnySubscriptionCallback<MyTA>().set(
[](std::shared_ptr<test_msgs::msg::Empty>, const rclcpp::MessageInfo &) {})},
// free function
InstanceContext<MyTA>{"free_function_ta", rclcpp::AnySubscriptionCallback<MyTA>().set(
shared_ptr_ta_free_func)},
InstanceContext<MyTA>{"free_function_ta_with_info",
rclcpp::AnySubscriptionCallback<MyTA>().set(
shared_ptr_ta_w_info_free_func)},
InstanceContext<MyTA>{"free_function", rclcpp::AnySubscriptionCallback<MyTA>().set(
shared_ptr_free_func)},
InstanceContext<MyTA>{"free_function_with_info",
rclcpp::AnySubscriptionCallback<MyTA>().set(
shared_ptr_w_info_free_func)},
// bind function
BindContext<MyTA, std::shared_ptr<MyEmpty>>("bind_method_ta"),
BindContext<MyTA, std::shared_ptr<MyEmpty>, const rclcpp::MessageInfo &>(
"bind_method_ta_with_info"),
BindContext<MyTA, std::shared_ptr<test_msgs::msg::Empty>>("bind_method"),
BindContext<MyTA, std::shared_ptr<test_msgs::msg::Empty>, const rclcpp::MessageInfo &>(
"bind_method_with_info")
),
format_parameter_with_ta
);
Original file line number Diff line number Diff line change
Expand Up @@ -212,7 +212,7 @@ TEST_F(
bool is_received = false;
auto callback =
[message_data, &is_received](
std::shared_ptr<std::string> msg) -> void {
std::shared_ptr<const std::string> msg) -> void {
is_received = true;
EXPECT_STREQ(message_data.c_str(), (*msg).c_str());
};
Expand All @@ -230,7 +230,7 @@ TEST_F(
bool is_received = false;
auto callback =
[message_data, &is_received](
std::shared_ptr<std::string> msg,
std::shared_ptr<const std::string> msg,
const rclcpp::MessageInfo & message_info) -> void {
is_received = true;
EXPECT_STREQ(message_data.c_str(), (*msg).c_str());
Expand Down Expand Up @@ -422,7 +422,7 @@ TEST_F(
bool is_received = false;
auto callback =
[message_data, &is_received](
std::shared_ptr<std::string> msg) -> void {
std::shared_ptr<const std::string> msg) -> void {
is_received = true;
EXPECT_STREQ(message_data.c_str(), (*msg).c_str());
};
Expand All @@ -440,7 +440,7 @@ TEST_F(
bool is_received = false;
auto callback =
[message_data, &is_received](
std::shared_ptr<std::string> msg,
std::shared_ptr<const std::string> msg,
const rclcpp::MessageInfo & message_info) -> void {
is_received = true;
EXPECT_STREQ(message_data.c_str(), (*msg).c_str());
Expand Down Expand Up @@ -637,7 +637,7 @@ TEST_F(
bool is_received = false;
auto callback =
[message_data, &is_received](
std::shared_ptr<double> msg) -> void {
std::shared_ptr<const double> msg) -> void {
is_received = true;
ASSERT_EQ(message_data, *msg);
};
Expand All @@ -656,7 +656,7 @@ TEST_F(
bool is_received = false;
auto callback =
[message_data, &is_received](
std::shared_ptr<double> msg,
std::shared_ptr<const double> msg,
const rclcpp::MessageInfo & message_info) -> void {
is_received = true;
ASSERT_EQ(message_data, *msg);
Expand Down Expand Up @@ -862,7 +862,7 @@ TEST_F(
bool is_received = false;
auto callback =
[message_data, &is_received](
std::shared_ptr<double> msg) -> void {
std::shared_ptr<const double> msg) -> void {
is_received = true;
ASSERT_EQ(message_data, *msg);
};
Expand All @@ -882,7 +882,7 @@ TEST_F(
bool is_received = false;
auto callback =
[message_data, &is_received](
std::shared_ptr<double> msg,
std::shared_ptr<const double> msg,
const rclcpp::MessageInfo & message_info) -> void {
is_received = true;
ASSERT_EQ(message_data, *msg);
Expand Down