Skip to content

Commit

Permalink
Migrate to rosidl_dynamic_typesupport and update field IDs
Browse files Browse the repository at this point in the history
Signed-off-by: methylDragon <methylDragon@gmail.com>
  • Loading branch information
methylDragon committed Feb 28, 2023
1 parent 08db827 commit d2b1743
Show file tree
Hide file tree
Showing 12 changed files with 118 additions and 118 deletions.
2 changes: 1 addition & 1 deletion rclcpp/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -94,7 +94,7 @@ set(${PROJECT_NAME}_SRCS
src/rclcpp/qos.cpp
src/rclcpp/qos_event.cpp
src/rclcpp/qos_overriding_options.cpp
src/rclcpp/runtime_type_subscription.cpp
src/rclcpp/dynamic_subscription.cpp
src/rclcpp/serialization.cpp
src/rclcpp/serialized_message.cpp
src/rclcpp/service.cpp
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef RCLCPP__RUNTIME_TYPE_SUBSCRIPTION_HPP_
#define RCLCPP__RUNTIME_TYPE_SUBSCRIPTION_HPP_
#ifndef RCLCPP__DYNAMIC_SUBSCRIPTION_HPP_
#define RCLCPP__DYNAMIC_SUBSCRIPTION_HPP_

#include <functional>
#include <memory>
Expand Down Expand Up @@ -41,25 +41,25 @@ namespace rclcpp
*
* NOTE(methylDragon): No considerations for intra-process handling are made.
*/
class RuntimeTypeSubscription : public rclcpp::SubscriptionBase
class DynamicSubscription : public rclcpp::SubscriptionBase
{
public:
// cppcheck-suppress unknownMacro
RCLCPP_SMART_PTR_DEFINITIONS(RuntimeTypeSubscription)
RCLCPP_SMART_PTR_DEFINITIONS(DynamicSubscription)

template<typename AllocatorT = std::allocator<void>>
RuntimeTypeSubscription(
DynamicSubscription(
rclcpp::node_interfaces::NodeBaseInterface * node_base,
rosidl_message_type_support_t & type_support_handle,
const std::string & topic_name,
const rclcpp::QoS & qos,
// TODO(methylDragons): Eventually roll out an rclcpp::DynamicData that encompasses the ser
// TODO(methylDragons): Eventually roll out an rclcpp::DynamicData that encompasses the serialization_support
// support and DynamicData, and pass that to the callback
std::function<void(
std::shared_ptr<serialization_support_t>, std::shared_ptr<ser_dynamic_data_t>
std::shared_ptr<rosidl_dynamic_typesupport_serialization_support_t>, std::shared_ptr<rosidl_dynamic_typesupport_dynamic_data_t>
)> callback,
const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options,
bool use_take_runtime_type_message = false)
bool use_take_dynamic_message = false)
: SubscriptionBase(
node_base,
type_support_handle,
Expand All @@ -69,38 +69,38 @@ class RuntimeTypeSubscription : public rclcpp::SubscriptionBase
options.use_default_callbacks,
false,
true,
use_take_runtime_type_message),
use_take_dynamic_message),
ts_(type_support_handle),
callback_(callback)
{
if(type_support_handle.typesupport_identifier
!= rmw_typesupport_runtime_type_introspection_c__identifier)
!= rmw_dynamic_typesupport_c__identifier)
{
throw std::runtime_error(
"RuntimeTypeSubscription must use runtime type introspection type support!");
"DynamicSubscription must use runtime type introspection type support!");
}
}

// TODO(methylDragon):
/// Deferred type description constructor, only usable if the middleware implementation supports
/// type discovery
// template<typename AllocatorT = std::allocator<void>>
// RuntimeTypeSubscription(
// DynamicSubscription(
// rclcpp::node_interfaces::NodeBaseInterface * node_base,
// const std::string & topic_name,
// const rclcpp::QoS & qos,
// // TODO(methylDragons): Eventually roll out an rclcpp::DynamicData that encompasses the ser
// // TODO(methylDragons): Eventually roll out an rclcpp::DynamicData that encompasses the serialization_support
// // support and DynamicData, and pass that to the callback
// std::function<void(
// std::shared_ptr<serialization_support_t>, std::shared_ptr<ser_dynamic_data_t>
// std::shared_ptr<rosidl_dynamic_typesupport_serialization_support_t>, std::shared_ptr<rosidl_dynamic_typesupport_dynamic_data_t>
// )> callback,
// const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options,
// const char * serialization_lib_name = nullptr)
// : SubscriptionBase(
// node_base,
// // NOTE(methylDragon): Since the typesupport is deferred, it needs to be modified post-hoc
// // which means it technically isn't const correct...
// *rmw_get_runtime_type_message_typesupport_handle(serialization_lib_name),
// *rmw_get_dynamic_message_typesupport_handle(serialization_lib_name),
// topic_name,
// options.to_rcl_subscription_options(qos),
// options.event_callbacks,
Expand All @@ -111,7 +111,7 @@ class RuntimeTypeSubscription : public rclcpp::SubscriptionBase
// {}

RCLCPP_PUBLIC
virtual ~RuntimeTypeSubscription() = default;
virtual ~DynamicSubscription() = default;

// Same as create_serialized_message() as the subscription is to serialized_messages only
RCLCPP_PUBLIC
Expand Down Expand Up @@ -148,35 +148,35 @@ class RuntimeTypeSubscription : public rclcpp::SubscriptionBase
// RUNTIME TYPE ==================================================================================
// TODO(methylDragon): Reorder later
RCLCPP_PUBLIC
std::shared_ptr<ser_dynamic_type_t>
std::shared_ptr<rosidl_dynamic_typesupport_dynamic_type_t>
get_dynamic_type() override;

RCLCPP_PUBLIC
std::shared_ptr<ser_dynamic_data_t>
std::shared_ptr<rosidl_dynamic_typesupport_dynamic_data_t>
get_dynamic_data() override;

RCLCPP_PUBLIC
std::shared_ptr<serialization_support_t>
std::shared_ptr<rosidl_dynamic_typesupport_serialization_support_t>
get_serialization_support() override;

RCLCPP_PUBLIC
void handle_runtime_type_message(
const std::shared_ptr<serialization_support_t> & ser,
const std::shared_ptr<ser_dynamic_data_t> & dyn_data,
void handle_dynamic_message(
const std::shared_ptr<rosidl_dynamic_typesupport_serialization_support_t> & serialization_support,
const std::shared_ptr<rosidl_dynamic_typesupport_dynamic_data_t> & dyn_data,
const rclcpp::MessageInfo & message_info
) override;


private:
RCLCPP_DISABLE_COPY(RuntimeTypeSubscription)
RCLCPP_DISABLE_COPY(DynamicSubscription)

rosidl_message_type_support_t & ts_;

std::function<void(
std::shared_ptr<serialization_support_t>, std::shared_ptr<ser_dynamic_data_t>
std::shared_ptr<rosidl_dynamic_typesupport_serialization_support_t>, std::shared_ptr<rosidl_dynamic_typesupport_dynamic_data_t>
)> callback_;
};

} // namespace rclcpp

#endif // RCLCPP__RUNTIME_TYPE_SUBSCRIPTION_HPP_
#endif // RCLCPP__DYNAMIC_SUBSCRIPTION_HPP_
12 changes: 6 additions & 6 deletions rclcpp/include/rclcpp/generic_subscription.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -127,18 +127,18 @@ class GenericSubscription : public rclcpp::SubscriptionBase
// RUNTIME TYPE ==================================================================================
// TODO(methylDragon): Reorder later
RCLCPP_PUBLIC
std::shared_ptr<ser_dynamic_type_t> get_dynamic_type() override;
std::shared_ptr<rosidl_dynamic_typesupport_dynamic_type_t> get_dynamic_type() override;

RCLCPP_PUBLIC
std::shared_ptr<ser_dynamic_data_t> get_dynamic_data() override;
std::shared_ptr<rosidl_dynamic_typesupport_dynamic_data_t> get_dynamic_data() override;

RCLCPP_PUBLIC
std::shared_ptr<serialization_support_t> get_serialization_support() override;
std::shared_ptr<rosidl_dynamic_typesupport_serialization_support_t> get_serialization_support() override;

RCLCPP_PUBLIC
void handle_runtime_type_message(
const std::shared_ptr<serialization_support_t> & ser,
const std::shared_ptr<ser_dynamic_data_t> & dyn_data,
void handle_dynamic_message(
const std::shared_ptr<rosidl_dynamic_typesupport_serialization_support_t> & serialization_support,
const std::shared_ptr<rosidl_dynamic_typesupport_dynamic_data_t> & dyn_data,
const rclcpp::MessageInfo & message_info) override;

private:
Expand Down
2 changes: 1 addition & 1 deletion rclcpp/include/rclcpp/rclcpp.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -167,6 +167,6 @@
#include "rclcpp/waitable.hpp"
#include "rclcpp/wait_set.hpp"

#include "rclcpp/runtime_type_subscription.hpp"
#include "rclcpp/dynamic_subscription.hpp"

#endif // RCLCPP__RCLCPP_HPP_
16 changes: 8 additions & 8 deletions rclcpp/include/rclcpp/subscription.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -391,38 +391,38 @@ class Subscription : public SubscriptionBase
// RUNTIME TYPE ==================================================================================
// TODO(methylDragon): Reorder later
// TODO(methylDragon): Implement later...
std::shared_ptr<ser_dynamic_type_t>
std::shared_ptr<rosidl_dynamic_typesupport_dynamic_type_t>
get_dynamic_type() override
{
throw rclcpp::exceptions::UnimplementedError(
"get_dynamic_type is not implemented for Subscription");
}

std::shared_ptr<ser_dynamic_data_t>
std::shared_ptr<rosidl_dynamic_typesupport_dynamic_data_t>
get_dynamic_data() override
{
throw rclcpp::exceptions::UnimplementedError(
"get_dynamic_data is not implemented for Subscription");
}

std::shared_ptr<serialization_support_t>
std::shared_ptr<rosidl_dynamic_typesupport_serialization_support_t>
get_serialization_support() override
{
throw rclcpp::exceptions::UnimplementedError(
"get_serialization_support is not implemented for Subscription");
}

void
handle_runtime_type_message(
const std::shared_ptr<serialization_support_t> & ser,
const std::shared_ptr<ser_dynamic_data_t> & dyn_data,
handle_dynamic_message(
const std::shared_ptr<rosidl_dynamic_typesupport_serialization_support_t> & serialization_support,
const std::shared_ptr<rosidl_dynamic_typesupport_dynamic_data_t> & dyn_data,
const rclcpp::MessageInfo & message_info) override
{
(void) ser;
(void) serialization_support;
(void) dyn_data;
(void) message_info;
throw rclcpp::exceptions::UnimplementedError(
"handle_runtime_type_message is not implemented for Subscription");
"handle_dynamic_message is not implemented for Subscription");
}

private:
Expand Down
32 changes: 16 additions & 16 deletions rclcpp/include/rclcpp/subscription_base.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -77,7 +77,7 @@ class SubscriptionBase : public std::enable_shared_from_this<SubscriptionBase>
* \param[in] topic_name Name of the topic to subscribe to.
* \param[in] subscription_options Options for the subscription.
* \param[in] is_serialized is true if the message will be delivered still serialized
* \param[in] use_runtime_type_cb is true if the message will be taken serialized and then handled
* \param[in] use_dynamic_message_cb is true if the message will be taken serialized and then handled
* using dynamic type and dynamic data (type constructed at runtime)
*/
RCLCPP_PUBLIC
Expand All @@ -89,8 +89,8 @@ class SubscriptionBase : public std::enable_shared_from_this<SubscriptionBase>
const SubscriptionEventCallbacks & event_callbacks,
bool use_default_callbacks,
bool is_serialized = false,
bool use_runtime_type_cb = false,
bool use_take_runtime_type_message = false);
bool use_dynamic_message_cb = false,
bool use_take_dynamic_message = false);
// TODO(methylDragon): If we don't need this, remove it,
// rclcpp::node_interfaces::NodeGraphInterface * node_graph = 0,
// rclcpp::node_interfaces::NodeServicesInterface * node_services = 0);
Expand Down Expand Up @@ -546,47 +546,47 @@ class SubscriptionBase : public std::enable_shared_from_this<SubscriptionBase>
// TODO(methylDragon): Reorder later
RCLCPP_PUBLIC
virtual
std::shared_ptr<ser_dynamic_type_t>
std::shared_ptr<rosidl_dynamic_typesupport_dynamic_type_t>
get_dynamic_type() = 0;

RCLCPP_PUBLIC
virtual
std::shared_ptr<ser_dynamic_data_t>
std::shared_ptr<rosidl_dynamic_typesupport_dynamic_data_t>
get_dynamic_data() = 0;

RCLCPP_PUBLIC
virtual
std::shared_ptr<serialization_support_t>
std::shared_ptr<rosidl_dynamic_typesupport_serialization_support_t>
get_serialization_support() = 0;

RCLCPP_PUBLIC
virtual
void
handle_runtime_type_message(
const std::shared_ptr<serialization_support_t> & ser,
const std::shared_ptr<ser_dynamic_data_t> & dyn_data,
handle_dynamic_message(
const std::shared_ptr<rosidl_dynamic_typesupport_serialization_support_t> & serialization_support,
const std::shared_ptr<rosidl_dynamic_typesupport_dynamic_data_t> & dyn_data,
const rclcpp::MessageInfo & message_info
) = 0;

// TODO(methylDragon):
// RCLCPP_PUBLIC
// bool
// take_runtime_type_message(ser_dynamic_data_t * message_out, rclcpp::MessageInfo & message_info_out);
// take_dynamic_message(rosidl_dynamic_typesupport_dynamic_data_t * message_out, rclcpp::MessageInfo & message_info_out);

/// Return if the subscription should use runtime type
/**
* This will cause the subscription to use the handle_runtime_type_message methods, which must be
* used with take_serialized or take_runtime_type.
* This will cause the subscription to use the handle_dynamic_message methods, which must be
* used with take_serialized or take_dynamic_message.
*
* \return `true` if the subscription should use a runtime type callback, `false` otherwise
*/
RCLCPP_PUBLIC
bool
use_runtime_type_cb() const;
use_dynamic_message_cb() const;

RCLCPP_PUBLIC
bool
use_take_runtime_type_message() const;
use_take_dynamic_message() const;
// ===============================================================================================


Expand Down Expand Up @@ -644,8 +644,8 @@ class SubscriptionBase : public std::enable_shared_from_this<SubscriptionBase>

rosidl_message_type_support_t type_support_;
bool is_serialized_;
bool use_runtime_type_cb_;
bool use_take_runtime_type_message_;
bool use_dynamic_message_cb_;
bool use_take_dynamic_message_;

std::atomic<bool> subscription_in_use_by_wait_set_{false};
std::atomic<bool> intra_process_subscription_waitable_in_use_by_wait_set_{false};
Expand Down
Loading

0 comments on commit d2b1743

Please sign in to comment.