Skip to content

Commit f8c11e9

Browse files
methylDragonmethylDragon
committed
Implement dynamic subscription and C++ wrapper classes
Signed-off-by: methylDragon <[email protected]>
1 parent b812790 commit f8c11e9

21 files changed

+3720
-26
lines changed

‎rclcpp/CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -105,6 +105,7 @@ set(${PROJECT_NAME}_SRCS
105105
src/rclcpp/qos.cpp
106106
src/rclcpp/event_handler.cpp
107107
src/rclcpp/qos_overriding_options.cpp
108+
src/rclcpp/dynamic_subscription.cpp
108109
src/rclcpp/serialization.cpp
109110
src/rclcpp/serialized_message.cpp
110111
src/rclcpp/service.cpp

‎rclcpp/include/rclcpp/dynamic_subscription.hpp

+175
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,175 @@
1+
// Copyright 2022 Open Source Robotics Foundation, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef RCLCPP__DYNAMIC_SUBSCRIPTION_HPP_
16+
#define RCLCPP__DYNAMIC_SUBSCRIPTION_HPP_
17+
18+
#include <rosidl_dynamic_typesupport/identifier.h>
19+
20+
#include <functional>
21+
#include <memory>
22+
#include <string>
23+
24+
#include "rcpputils/shared_library.hpp"
25+
26+
#include "rclcpp/callback_group.hpp"
27+
#include "rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp"
28+
#include "rclcpp/macros.hpp"
29+
#include "rclcpp/node_interfaces/node_base_interface.hpp"
30+
#include "rclcpp/node_interfaces/node_topics_interface.hpp"
31+
#include "rclcpp/qos.hpp"
32+
#include "rclcpp/serialized_message.hpp"
33+
#include "rclcpp/subscription_base.hpp"
34+
#include "rclcpp/typesupport_helpers.hpp"
35+
#include "rclcpp/visibility_control.hpp"
36+
37+
namespace rclcpp
38+
{
39+
40+
/// %Subscription for messages whose type descriptions are obtained at runtime.
41+
/**
42+
* Since the type is not known at compile time, this is not a template, and the dynamic library
43+
* containing type support information has to be identified and loaded based on the type name.
44+
*
45+
* NOTE(methylDragon): No considerations for intra-process handling are made.
46+
*/
47+
class DynamicSubscription : public rclcpp::SubscriptionBase
48+
{
49+
public:
50+
// cppcheck-suppress unknownMacro
51+
RCLCPP_SMART_PTR_DEFINITIONS(DynamicSubscription)
52+
53+
template<typename AllocatorT = std::allocator<void>>
54+
DynamicSubscription(
55+
rclcpp::node_interfaces::NodeBaseInterface * node_base,
56+
rclcpp::dynamic_typesupport::DynamicMessageTypeSupport::SharedPtr type_support,
57+
const std::string & topic_name,
58+
const rclcpp::QoS & qos,
59+
std::function<void(
60+
rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr,
61+
const rosidl_runtime_c__type_description__TypeDescription &
62+
)> callback,
63+
const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options)
64+
: SubscriptionBase(
65+
node_base,
66+
type_support->get_const_rosidl_message_type_support(),
67+
topic_name,
68+
options.to_rcl_subscription_options(
69+
qos),
70+
options.event_callbacks,
71+
options.use_default_callbacks,
72+
DeliveredMessageKind::DYNAMIC_MESSAGE),
73+
ts_(type_support),
74+
callback_(callback),
75+
serialization_support_(nullptr),
76+
dynamic_message_(nullptr),
77+
dynamic_message_type_(nullptr)
78+
{
79+
if (!type_support) {
80+
throw std::runtime_error("DynamicMessageTypeSupport cannot be nullptr!");
81+
}
82+
83+
if (type_support->get_const_rosidl_message_type_support().typesupport_identifier !=
84+
rosidl_get_dynamic_typesupport_identifier())
85+
{
86+
throw std::runtime_error(
87+
"DynamicSubscription must use dynamic type introspection type support!");
88+
}
89+
90+
serialization_support_ = type_support->get_shared_dynamic_serialization_support();
91+
dynamic_message_type_ = type_support->get_shared_dynamic_message_type()->clone_shared();
92+
dynamic_message_ = type_support->get_shared_dynamic_message()->clone_shared();
93+
}
94+
95+
// TODO(methylDragon):
96+
/// Deferred type description constructor, only usable if the middleware implementation supports
97+
/// type discovery
98+
99+
RCLCPP_PUBLIC
100+
virtual ~DynamicSubscription() = default;
101+
102+
// Same as create_serialized_message() as the subscription is to serialized_messages only
103+
RCLCPP_PUBLIC
104+
std::shared_ptr<void> create_message() override;
105+
106+
RCLCPP_PUBLIC
107+
std::shared_ptr<rclcpp::SerializedMessage> create_serialized_message() override;
108+
109+
/// Cast the message to a rclcpp::SerializedMessage and call the callback.
110+
RCLCPP_PUBLIC
111+
void handle_message(
112+
std::shared_ptr<void> & message, const rclcpp::MessageInfo & message_info) override;
113+
114+
/// Handle dispatching rclcpp::SerializedMessage to user callback.
115+
RCLCPP_PUBLIC
116+
void
117+
handle_serialized_message(
118+
const std::shared_ptr<rclcpp::SerializedMessage> & serialized_message,
119+
const rclcpp::MessageInfo & message_info) override;
120+
121+
/// This function is currently not implemented.
122+
RCLCPP_PUBLIC
123+
void handle_loaned_message(
124+
void * loaned_message, const rclcpp::MessageInfo & message_info) override;
125+
126+
// Same as return_serialized_message() as the subscription is to serialized_messages only
127+
RCLCPP_PUBLIC
128+
void return_message(std::shared_ptr<void> & message) override;
129+
130+
RCLCPP_PUBLIC
131+
void return_serialized_message(std::shared_ptr<rclcpp::SerializedMessage> & message) override;
132+
133+
134+
// DYNAMIC TYPE ==================================================================================
135+
// TODO(methylDragon): Reorder later
136+
RCLCPP_PUBLIC
137+
rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr
138+
get_shared_dynamic_message_type() override;
139+
140+
RCLCPP_PUBLIC
141+
rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr get_shared_dynamic_message() override;
142+
143+
RCLCPP_PUBLIC
144+
rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr
145+
get_shared_dynamic_serialization_support() override;
146+
147+
RCLCPP_PUBLIC
148+
rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr create_dynamic_message() override;
149+
150+
RCLCPP_PUBLIC
151+
void return_dynamic_message(
152+
rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message) override;
153+
154+
RCLCPP_PUBLIC
155+
void handle_dynamic_message(
156+
const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message,
157+
const rclcpp::MessageInfo & message_info) override;
158+
159+
private:
160+
RCLCPP_DISABLE_COPY(DynamicSubscription)
161+
162+
rclcpp::dynamic_typesupport::DynamicMessageTypeSupport::SharedPtr ts_;
163+
std::function<void(
164+
rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr,
165+
const rosidl_runtime_c__type_description__TypeDescription &
166+
)> callback_;
167+
168+
rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr serialization_support_;
169+
rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr dynamic_message_;
170+
rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr dynamic_message_type_;
171+
};
172+
173+
} // namespace rclcpp
174+
175+
#endif // RCLCPP__DYNAMIC_SUBSCRIPTION_HPP_

0 commit comments

Comments
 (0)