Skip to content

Commit 50f61d9

Browse files
committed
Implement subscription base changes
Signed-off-by: methylDragon <[email protected]>
1 parent 71a0698 commit 50f61d9

File tree

7 files changed

+355
-75
lines changed

7 files changed

+355
-75
lines changed

rclcpp/include/rclcpp/generic_subscription.hpp

Lines changed: 27 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -84,7 +84,7 @@ class GenericSubscription : public rclcpp::SubscriptionBase
8484
options.to_rcl_subscription_options(qos),
8585
options.event_callbacks,
8686
options.use_default_callbacks,
87-
true),
87+
SubscriptionType::SERIALIZED_MESSAGE),
8888
callback_(callback),
8989
ts_lib_(ts_lib)
9090
{}
@@ -123,6 +123,32 @@ class GenericSubscription : public rclcpp::SubscriptionBase
123123
RCLCPP_PUBLIC
124124
void return_serialized_message(std::shared_ptr<rclcpp::SerializedMessage> & message) override;
125125

126+
127+
// DYNAMIC TYPE ==================================================================================
128+
// TODO(methylDragon): Reorder later
129+
RCLCPP_PUBLIC
130+
rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr get_shared_dynamic_message_type()
131+
override;
132+
133+
RCLCPP_PUBLIC
134+
rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr get_shared_dynamic_message() override;
135+
136+
RCLCPP_PUBLIC
137+
rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr
138+
get_shared_dynamic_serialization_support() override;
139+
140+
RCLCPP_PUBLIC
141+
rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr create_dynamic_message() override;
142+
143+
RCLCPP_PUBLIC
144+
void return_dynamic_message(
145+
rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message) override;
146+
147+
RCLCPP_PUBLIC
148+
void handle_dynamic_message(
149+
const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message,
150+
const rclcpp::MessageInfo & message_info) override;
151+
126152
private:
127153
RCLCPP_DISABLE_COPY(GenericSubscription)
128154

rclcpp/include/rclcpp/subscription.hpp

Lines changed: 52 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -144,7 +144,7 @@ class Subscription : public SubscriptionBase
144144
// NOTE(methylDragon): Passing these args separately is necessary for event binding
145145
options.event_callbacks,
146146
options.use_default_callbacks,
147-
callback.is_serialized_message_callback()),
147+
callback.is_serialized_message_callback() ? SubscriptionType::SERIALIZED_MESSAGE : SubscriptionType::ROS_MESSAGE), // NOLINT
148148
any_callback_(callback),
149149
options_(options),
150150
message_memory_strategy_(message_memory_strategy)
@@ -388,6 +388,57 @@ class Subscription : public SubscriptionBase
388388
return any_callback_.use_take_shared_method();
389389
}
390390

391+
// DYNAMIC TYPE ==================================================================================
392+
// TODO(methylDragon): Reorder later
393+
// TODO(methylDragon): Implement later...
394+
rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr
395+
get_shared_dynamic_message_type() override
396+
{
397+
throw rclcpp::exceptions::UnimplementedError(
398+
"get_shared_dynamic_message_type is not implemented for Subscription");
399+
}
400+
401+
rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr
402+
get_shared_dynamic_message() override
403+
{
404+
throw rclcpp::exceptions::UnimplementedError(
405+
"get_shared_dynamic_message is not implemented for Subscription");
406+
}
407+
408+
rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr
409+
get_shared_dynamic_serialization_support() override
410+
{
411+
throw rclcpp::exceptions::UnimplementedError(
412+
"get_shared_dynamic_serialization_support is not implemented for Subscription");
413+
}
414+
415+
rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr
416+
create_dynamic_message() override
417+
{
418+
throw rclcpp::exceptions::UnimplementedError(
419+
"create_dynamic_message is not implemented for Subscription");
420+
}
421+
422+
void
423+
return_dynamic_message(
424+
rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message) override
425+
{
426+
(void) message;
427+
throw rclcpp::exceptions::UnimplementedError(
428+
"return_dynamic_message is not implemented for Subscription");
429+
}
430+
431+
void
432+
handle_dynamic_message(
433+
const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message,
434+
const rclcpp::MessageInfo & message_info) override
435+
{
436+
(void) message;
437+
(void) message_info;
438+
throw rclcpp::exceptions::UnimplementedError(
439+
"handle_dynamic_message is not implemented for Subscription");
440+
}
441+
391442
private:
392443
RCLCPP_DISABLE_COPY(Subscription)
393444

rclcpp/include/rclcpp/subscription_base.hpp

Lines changed: 61 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -31,6 +31,9 @@
3131

3232
#include "rclcpp/any_subscription_callback.hpp"
3333
#include "rclcpp/detail/cpp_callback_trampoline.hpp"
34+
#include "rclcpp/dynamic_typesupport/dynamic_message.hpp"
35+
#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp"
36+
#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
3437
#include "rclcpp/experimental/intra_process_manager.hpp"
3538
#include "rclcpp/experimental/subscription_intra_process_base.hpp"
3639
#include "rclcpp/macros.hpp"
@@ -60,6 +63,14 @@ namespace experimental
6063
class IntraProcessManager;
6164
} // namespace experimental
6265

66+
enum class SubscriptionType : uint8_t
67+
{
68+
ROS_MESSAGE = 1, // take message as ROS message and handle as ROS message
69+
SERIALIZED_MESSAGE = 2, // take message as serialized and handle as serialized
70+
DYNAMIC_MESSAGE_DIRECT = 3, // take message as DynamicMessage and handle as DynamicMessage
71+
DYNAMIC_MESSAGE_FROM_SERIALIZED = 4 // take message as serialized and handle as DynamicMessage
72+
};
73+
6374
/// Virtual base class for subscriptions. This pattern allows us to iterate over different template
6475
/// specializations of Subscription, among other things.
6576
class SubscriptionBase : public std::enable_shared_from_this<SubscriptionBase>
@@ -76,7 +87,7 @@ class SubscriptionBase : public std::enable_shared_from_this<SubscriptionBase>
7687
* \param[in] type_support_handle rosidl type support struct, for the Message type of the topic.
7788
* \param[in] topic_name Name of the topic to subscribe to.
7889
* \param[in] subscription_options Options for the subscription.
79-
* \param[in] is_serialized is true if the message will be delivered still serialized
90+
* \param[in] subscription_type Enum flag to change how the message will be received and delivered
8091
*/
8192
RCLCPP_PUBLIC
8293
SubscriptionBase(
@@ -86,7 +97,7 @@ class SubscriptionBase : public std::enable_shared_from_this<SubscriptionBase>
8697
const rcl_subscription_options_t & subscription_options,
8798
const SubscriptionEventCallbacks & event_callbacks,
8899
bool use_default_callbacks,
89-
bool is_serialized = false);
100+
SubscriptionType subscription_type = SubscriptionType::ROS_MESSAGE);
90101

91102
/// Destructor.
92103
RCLCPP_PUBLIC
@@ -227,13 +238,13 @@ class SubscriptionBase : public std::enable_shared_from_this<SubscriptionBase>
227238
const rosidl_message_type_support_t &
228239
get_message_type_support_handle() const;
229240

230-
/// Return if the subscription is serialized
241+
/// Return the type of the subscription.
231242
/**
232-
* \return `true` if the subscription is serialized, `false` otherwise
243+
* \return `SubscriptionType`, which adjusts how messages are received and delivered.
233244
*/
234245
RCLCPP_PUBLIC
235-
bool
236-
is_serialized() const;
246+
SubscriptionType
247+
get_subscription_type() const;
237248

238249
/// Get matching publisher count.
239250
/** \return The number of publishers on this topic. */
@@ -535,6 +546,49 @@ class SubscriptionBase : public std::enable_shared_from_this<SubscriptionBase>
535546
rclcpp::ContentFilterOptions
536547
get_content_filter() const;
537548

549+
// DYNAMIC TYPE ==================================================================================
550+
// TODO(methylDragon): Reorder later
551+
RCLCPP_PUBLIC
552+
virtual
553+
rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr
554+
get_shared_dynamic_message_type() = 0;
555+
556+
RCLCPP_PUBLIC
557+
virtual
558+
rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr
559+
get_shared_dynamic_message() = 0;
560+
561+
RCLCPP_PUBLIC
562+
virtual
563+
rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr
564+
get_shared_dynamic_serialization_support() = 0;
565+
566+
/// Borrow a new serialized message (this clones!)
567+
/** \return Shared pointer to a rclcpp::dynamic_typesupport::DynamicMessage. */
568+
RCLCPP_PUBLIC
569+
virtual
570+
rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr
571+
create_dynamic_message() = 0;
572+
573+
RCLCPP_PUBLIC
574+
virtual
575+
void
576+
return_dynamic_message(rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message) = 0;
577+
578+
RCLCPP_PUBLIC
579+
virtual
580+
void
581+
handle_dynamic_message(
582+
const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message,
583+
const rclcpp::MessageInfo & message_info) = 0;
584+
585+
RCLCPP_PUBLIC
586+
bool
587+
take_dynamic_message(
588+
rclcpp::dynamic_typesupport::DynamicMessage & message_out,
589+
rclcpp::MessageInfo & message_info_out);
590+
// ===============================================================================================
591+
538592
protected:
539593
template<typename EventCallbackT>
540594
void
@@ -587,7 +641,7 @@ class SubscriptionBase : public std::enable_shared_from_this<SubscriptionBase>
587641
RCLCPP_DISABLE_COPY(SubscriptionBase)
588642

589643
rosidl_message_type_support_t type_support_;
590-
bool is_serialized_;
644+
SubscriptionType subscription_type_;
591645

592646
std::atomic<bool> subscription_in_use_by_wait_set_{false};
593647
std::atomic<bool> intra_process_subscription_waitable_in_use_by_wait_set_{false};

rclcpp/package.xml

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -39,6 +39,7 @@
3939
<depend>rcpputils</depend>
4040
<depend>rcutils</depend>
4141
<depend>rmw</depend>
42+
<depend>rosidl_dynamic_typesupport</depend>
4243
<depend>statistics_msgs</depend>
4344
<depend>tracetools</depend>
4445

0 commit comments

Comments
 (0)