diff --git a/foxglove_bridge_base/include/foxglove_bridge/websocket_server.hpp b/foxglove_bridge_base/include/foxglove_bridge/websocket_server.hpp index e96bcdd..bac5ddd 100644 --- a/foxglove_bridge_base/include/foxglove_bridge/websocket_server.hpp +++ b/foxglove_bridge_base/include/foxglove_bridge/websocket_server.hpp @@ -759,7 +759,7 @@ inline void Server::handleBinaryMessage(ConnHandle hdl, Mes length, data}; _handlers.clientMessageHandler(clientMessage, hdl); - } catch (const ServiceError const& e) { + } catch (ServiceError const& e) { sendStatusAndLogMsg(hdl, StatusLevel::Error, e.what()); } catch (std::exception const& e) { sendStatusAndLogMsg(hdl, StatusLevel::Error, diff --git a/ros2_foxglove_bridge/include/ros2_babel_fish/babel_fish.hpp b/ros2_foxglove_bridge/include/ros2_babel_fish/babel_fish.hpp index cdccafe..2ad3182 100644 --- a/ros2_foxglove_bridge/include/ros2_babel_fish/babel_fish.hpp +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/babel_fish.hpp @@ -1,9 +1,12 @@ // Copyright (c) 2021 Stefan Fabian. All rights reserved. -// Licensed under the MIT license. See LICENSE file in the project root for full license information. +// Licensed under the MIT license. See LICENSE file in the project root for full license +// information. #ifndef ROS2_BABEL_FISH_BABEL_FISH_HPP #define ROS2_BABEL_FISH_BABEL_FISH_HPP +#include + #include "ros2_babel_fish/detail/babel_fish_action_client.hpp" #include "ros2_babel_fish/detail/babel_fish_publisher.hpp" #include "ros2_babel_fish/detail/babel_fish_service.hpp" @@ -14,19 +17,14 @@ #include "ros2_babel_fish/messages/compound_message.hpp" #include "ros2_babel_fish/messages/value_message.hpp" -#include - -namespace ros2_babel_fish -{ +namespace ros2_babel_fish { /*! * Allows communication using message types that are not known at compile time. */ -class BabelFish : public std::enable_shared_from_this -{ - +class BabelFish : public std::enable_shared_from_this { public: - RCLCPP_SMART_PTR_DEFINITIONS( BabelFish ) + RCLCPP_SMART_PTR_DEFINITIONS(BabelFish) /*! * Constructs an instance of BabelFish with a new instance of the default description provider. @@ -35,111 +33,110 @@ class BabelFish : public std::enable_shared_from_this */ BabelFish(); - explicit BabelFish( std::vector type_support_providers ); + explicit BabelFish(std::vector type_support_providers); ~BabelFish(); //! Wrapper for create_subscription without type. - template - BabelFishSubscription::SharedPtr - create_subscription( rclcpp::Node &node, const std::string &topic, const rclcpp::QoS &qos, - CallbackT &&callback, rclcpp::CallbackGroup::SharedPtr group = nullptr, - rclcpp::SubscriptionOptions options = {}, - std::chrono::nanoseconds timeout = std::chrono::nanoseconds( -1 ) ) - { + template + BabelFishSubscription::SharedPtr create_subscription( + rclcpp::Node& node, const std::string& topic, const rclcpp::QoS& qos, CallbackT&& callback, + rclcpp::CallbackGroup::SharedPtr group = nullptr, rclcpp::SubscriptionOptions options = {}, + std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)) { #if RCLCPP_VERSION_MAJOR >= 9 rclcpp::AnySubscriptionCallback> any_callback; #else rclcpp::AnySubscriptionCallback> any_callback( - options.get_allocator() ); + options.get_allocator()); #endif - any_callback.set( std::forward( callback ) ); - return create_subscription( node, topic, qos, any_callback, std::move( group ), - std::move( options ), timeout ); + any_callback.set(std::forward(callback)); + return create_subscription(node, topic, qos, any_callback, std::move(group), std::move(options), + timeout); } /*! - * This method will wait for the given topic until timeout expired (if a timeout is set) or the topic becomes available. - * As soon as the topic is available, it will create a subscription for the topic with the passed options. - * @param qos The quality of service options. Can be a number which will be the queue size, i.e., number of messages - * to queue for processing before dropping messages if the processing can't keep up. - * @param timeout The maximum time this call will block before returning. Set to 0 to not block at all. - * @return A subscription if the topic became available before the timeout expired or a nullptr otherwise. + * This method will wait for the given topic until timeout expired (if a timeout is set) or the + * topic becomes available. As soon as the topic is available, it will create a subscription for + * the topic with the passed options. + * @param qos The quality of service options. Can be a number which will be the queue size, i.e., + * number of messages to queue for processing before dropping messages if the processing can't + * keep up. + * @param timeout The maximum time this call will block before returning. Set to 0 to not block at + * all. + * @return A subscription if the topic became available before the timeout expired or a nullptr + * otherwise. * - * @throws BabelFishException If the message type for the given topic could not be loaded or a subscription could not - * be created for any other reason. + * @throws BabelFishException If the message type for the given topic could not be loaded or a + * subscription could not be created for any other reason. */ BabelFishSubscription::SharedPtr create_subscription( - rclcpp::Node &node, const std::string &topic, const rclcpp::QoS &qos, - rclcpp::AnySubscriptionCallback> callback, - rclcpp::CallbackGroup::SharedPtr group = nullptr, rclcpp::SubscriptionOptions options = {}, - std::chrono::nanoseconds timeout = std::chrono::nanoseconds( -1 ) ); + rclcpp::Node& node, const std::string& topic, const rclcpp::QoS& qos, + rclcpp::AnySubscriptionCallback> callback, + rclcpp::CallbackGroup::SharedPtr group = nullptr, rclcpp::SubscriptionOptions options = {}, + std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); //! Wrapper for create_subscription using type. - template - BabelFishSubscription::SharedPtr - create_subscription( rclcpp::Node &node, const std::string &topic, const std::string &type, - const rclcpp::QoS &qos, CallbackT &&callback, - rclcpp::CallbackGroup::SharedPtr group = nullptr, - rclcpp::SubscriptionOptions options = {} ) - { + template + BabelFishSubscription::SharedPtr create_subscription( + rclcpp::Node& node, const std::string& topic, const std::string& type, const rclcpp::QoS& qos, + CallbackT&& callback, rclcpp::CallbackGroup::SharedPtr group = nullptr, + rclcpp::SubscriptionOptions options = {}) { #if RCLCPP_VERSION_MAJOR >= 9 rclcpp::AnySubscriptionCallback> any_callback; #else rclcpp::AnySubscriptionCallback> any_callback( - options.get_allocator() ); + options.get_allocator()); #endif - any_callback.set( std::forward( callback ) ); - return create_subscription( node, topic, type, qos, any_callback, std::move( group ), - std::move( options ) ); + any_callback.set(std::forward(callback)); + return create_subscription(node, topic, type, qos, any_callback, std::move(group), + std::move(options)); } /*! * This method will create a subscription for the given topic using the given message type. * Since the message type is provided, it will not wait for the topic to become available. * @param type The message type name for the given topic. E.g.: geometry_msgs/msg/Pose - * @param qos The quality of service options. Can be a number which will be the queue size, i.e., number of messages - * to queue for processing before dropping messages if the processing can't keep up.´ + * @param qos The quality of service options. Can be a number which will be the queue size, i.e., + * number of messages to queue for processing before dropping messages if the processing can't + * keep up.´ * @return A subscription to the given topic with the given message type. * - * @throws BabelFishException If the given message type could not be loaded or a subscription could not - * be created for any other reason. + * @throws BabelFishException If the given message type could not be loaded or a subscription + * could not be created for any other reason. */ BabelFishSubscription::SharedPtr create_subscription( - rclcpp::Node &node, const std::string &topic, const std::string &type, const rclcpp::QoS &qos, - rclcpp::AnySubscriptionCallback> callback, - rclcpp::CallbackGroup::SharedPtr group = nullptr, rclcpp::SubscriptionOptions options = {} ); - - BabelFishPublisher::SharedPtr create_publisher( rclcpp::Node &node, const std::string &topic, - const std::string &type, const rclcpp::QoS &qos, - rclcpp::PublisherOptions options = {} ); - - template - BabelFishService::SharedPtr - create_service( rclcpp::Node &node, const std::string &service_name, const std::string &type, - CallbackT &&callback, - const rmw_qos_profile_t &qos_profile = rmw_qos_profile_services_default, - rclcpp::CallbackGroup::SharedPtr group = nullptr ) - { - AnyServiceCallback any_callback( std::forward( callback ) ); - return create_service( node, service_name, type, any_callback, qos_profile, group ); + rclcpp::Node& node, const std::string& topic, const std::string& type, const rclcpp::QoS& qos, + rclcpp::AnySubscriptionCallback> callback, + rclcpp::CallbackGroup::SharedPtr group = nullptr, rclcpp::SubscriptionOptions options = {}); + + BabelFishPublisher::SharedPtr create_publisher(rclcpp::Node& node, const std::string& topic, + const std::string& type, const rclcpp::QoS& qos, + rclcpp::PublisherOptions options = {}); + + template + BabelFishService::SharedPtr create_service( + rclcpp::Node& node, const std::string& service_name, const std::string& type, + CallbackT&& callback, const rmw_qos_profile_t& qos_profile = rmw_qos_profile_services_default, + rclcpp::CallbackGroup::SharedPtr group = nullptr) { + AnyServiceCallback any_callback(std::forward(callback)); + return create_service(node, service_name, type, any_callback, qos_profile, group); } - BabelFishService::SharedPtr - create_service( rclcpp::Node &node, const std::string &service_name, const std::string &type, - AnyServiceCallback callback, - const rmw_qos_profile_t &qos_profile = rmw_qos_profile_services_default, - rclcpp::CallbackGroup::SharedPtr group = nullptr ); + BabelFishService::SharedPtr create_service( + rclcpp::Node& node, const std::string& service_name, const std::string& type, + AnyServiceCallback callback, + const rmw_qos_profile_t& qos_profile = rmw_qos_profile_services_default, + rclcpp::CallbackGroup::SharedPtr group = nullptr); - BabelFishServiceClient::SharedPtr - create_service_client( rclcpp::Node &node, const std::string &service_name, const std::string &type, - const rmw_qos_profile_t &qos_profile = rmw_qos_profile_services_default, - rclcpp::CallbackGroup::SharedPtr group = nullptr ); + BabelFishServiceClient::SharedPtr create_service_client( + rclcpp::Node& node, const std::string& service_name, const std::string& type, + const rmw_qos_profile_t& qos_profile = rmw_qos_profile_services_default, + rclcpp::CallbackGroup::SharedPtr group = nullptr); BabelFishActionClient::SharedPtr create_action_client( - rclcpp::Node &node, const std::string &name, const std::string &type, - const rcl_action_client_options_t &options = rcl_action_client_get_default_options(), - rclcpp::CallbackGroup::SharedPtr group = nullptr ); + rclcpp::Node& node, const std::string& name, const std::string& type, + const rcl_action_client_options_t& options = rcl_action_client_get_default_options(), + rclcpp::CallbackGroup::SharedPtr group = nullptr); /*! * Creates an empty message of the given type. @@ -148,10 +145,10 @@ class BabelFish : public std::enable_shared_from_this * * @throws BabelFishException If the message description was not found */ - CompoundMessage create_message( const std::string &type ) const; + CompoundMessage create_message(const std::string& type) const; //! @copydoc create_message - CompoundMessage::SharedPtr create_message_shared( const std::string &type ) const; + CompoundMessage::SharedPtr create_message_shared(const std::string& type) const; /*! * Creates a service request message for the given service type. @@ -160,22 +157,22 @@ class BabelFish : public std::enable_shared_from_this * * @throws BabelFishException If the service description was not found */ - CompoundMessage create_service_request( const std::string &type ) const; + CompoundMessage create_service_request(const std::string& type) const; //! @copydoc create_service_request - CompoundMessage::SharedPtr create_service_request_shared( const std::string &type ) const; + CompoundMessage::SharedPtr create_service_request_shared(const std::string& type) const; - MessageTypeSupport::ConstSharedPtr get_message_type_support( const std::string &type ) const; + MessageTypeSupport::ConstSharedPtr get_message_type_support(const std::string& type) const; - ServiceTypeSupport::ConstSharedPtr get_service_type_support( const std::string &type ) const; + ServiceTypeSupport::ConstSharedPtr get_service_type_support(const std::string& type) const; - ActionTypeSupport::ConstSharedPtr get_action_type_support( const std::string &type ) const; + ActionTypeSupport::ConstSharedPtr get_action_type_support(const std::string& type) const; std::vector type_support_providers(); private: std::vector type_support_providers_; }; -} // namespace ros2_babel_fish +} // namespace ros2_babel_fish -#endif // ROS2_BABEL_FISH_BABEL_FISH_HPP +#endif // ROS2_BABEL_FISH_BABEL_FISH_HPP diff --git a/ros2_foxglove_bridge/include/ros2_babel_fish/detail/any_service_callback.hpp b/ros2_foxglove_bridge/include/ros2_babel_fish/detail/any_service_callback.hpp index afcf958..16a4b58 100644 --- a/ros2_foxglove_bridge/include/ros2_babel_fish/detail/any_service_callback.hpp +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/detail/any_service_callback.hpp @@ -1,109 +1,105 @@ // Copyright (c) 2022 Stefan Fabian. All rights reserved. -// Licensed under the MIT license. See LICENSE file in the project root for full license information. +// Licensed under the MIT license. See LICENSE file in the project root for full license +// information. #ifndef ROS2_BABEL_FISH_BABEL_FISH_ANY_SERVICE_CALLBACK_HPP #define ROS2_BABEL_FISH_BABEL_FISH_ANY_SERVICE_CALLBACK_HPP #include -namespace ros2_babel_fish -{ +namespace ros2_babel_fish { class BabelFishService; class CompoundMessage; /** - * Designed to be compatible with rclcpp::AnyServiceCallback compatible callbacks with equivalent tracing support. + * Designed to be compatible with rclcpp::AnyServiceCallback compatible callbacks with equivalent + * tracing support. */ -class AnyServiceCallback -{ - template::value, int>::type = 0> - bool isNullptr( CallbackT &&callback ) - { +class AnyServiceCallback { + template ::value, int>::type = 0> + bool isNullptr(CallbackT&& callback) { return !callback; } - template::value, int>::type = 0> - constexpr bool isNullptr( CallbackT && ) - { + template ::value, int>::type = 0> + constexpr bool isNullptr(CallbackT&&) { return false; } public: - template - explicit AnyServiceCallback( CallbackT &&callback ) - { - if ( isNullptr( callback ) ) { - throw std::invalid_argument( "Service callback cannot be nullptr!" ); + template + explicit AnyServiceCallback(CallbackT&& callback) { + if (isNullptr(callback)) { + throw std::invalid_argument("Service callback cannot be nullptr!"); } - if constexpr ( rclcpp::function_traits::same_arguments::value ) { - callback_.template emplace( callback ); - } else if constexpr ( rclcpp::function_traits::same_arguments< - CallbackT, SharedPtrWithRequestHeaderCallback>::value ) { - callback_.template emplace( callback ); - } else if constexpr ( rclcpp::function_traits::same_arguments< - CallbackT, SharedPtrDeferResponseCallback>::value ) { - callback_.template emplace( callback ); - } else if constexpr ( rclcpp::function_traits::same_arguments< - CallbackT, SharedPtrDeferResponseCallbackWithServiceHandle>::value ) { - callback_.template emplace( callback ); + if constexpr (rclcpp::function_traits::same_arguments::value) { + callback_.template emplace(callback); + } else if constexpr (rclcpp::function_traits::same_arguments< + CallbackT, SharedPtrWithRequestHeaderCallback>::value) { + callback_.template emplace(callback); + } else if constexpr (rclcpp::function_traits::same_arguments< + CallbackT, SharedPtrDeferResponseCallback>::value) { + callback_.template emplace(callback); + } else if constexpr (rclcpp::function_traits::same_arguments< + CallbackT, SharedPtrDeferResponseCallbackWithServiceHandle>::value) { + callback_.template emplace(callback); } else { - static_assert( "Invalid callback type passed to AnyServiceCallback!" ); + static_assert("Invalid callback type passed to AnyServiceCallback!"); } } - void dispatch( const std::shared_ptr &service_handle, - const std::shared_ptr &request_header, - std::shared_ptr request, std::shared_ptr response ) - { - TRACEPOINT( callback_start, static_cast( this ), false ); - if ( std::holds_alternative( callback_ ) ) { + void dispatch(const std::shared_ptr& service_handle, + const std::shared_ptr& request_header, + std::shared_ptr request, + std::shared_ptr response) { + TRACEPOINT(callback_start, static_cast(this), false); + if (std::holds_alternative(callback_)) { (void)request_header; - const auto &cb = std::get( callback_ ); - cb( std::move( request ), response ); - } else if ( std::holds_alternative( callback_ ) ) { - const auto &cb = std::get( callback_ ); - cb( request_header, std::move( request ), response ); - } else if ( std::holds_alternative( callback_ ) ) { - const auto &cb = std::get( callback_ ); - cb( request_header, std::move( request ) ); - } else if ( std::holds_alternative( callback_ ) ) { - const auto &cb = std::get( callback_ ); - cb( service_handle, request_header, std::move( request ) ); + const auto& cb = std::get(callback_); + cb(std::move(request), response); + } else if (std::holds_alternative(callback_)) { + const auto& cb = std::get(callback_); + cb(request_header, std::move(request), response); + } else if (std::holds_alternative(callback_)) { + const auto& cb = std::get(callback_); + cb(request_header, std::move(request)); + } else if (std::holds_alternative(callback_)) { + const auto& cb = std::get(callback_); + cb(service_handle, request_header, std::move(request)); } - TRACEPOINT( callback_end, static_cast( this ) ); + TRACEPOINT(callback_end, static_cast(this)); } - void register_callback_for_tracing() - { + void register_callback_for_tracing() { #ifndef TRACETOOLS_DISABLED std::visit( - [this]( auto &&arg ) { - TRACEPOINT( rclcpp_callback_register, static_cast( this ), - tracetools::get_symbol( arg ) ); - }, - callback_ ); -#endif // TRACETOOLS_DISABLED + [this](auto&& arg) { + TRACEPOINT(rclcpp_callback_register, static_cast(this), + tracetools::get_symbol(arg)); + }, + callback_); +#endif // TRACETOOLS_DISABLED } private: using SharedPtrCallback = - std::function, std::shared_ptr )>; + std::function, std::shared_ptr)>; using SharedPtrWithRequestHeaderCallback = - std::function, std::shared_ptr, - std::shared_ptr )>; + std::function, std::shared_ptr, + std::shared_ptr)>; using SharedPtrDeferResponseCallback = - std::function, std::shared_ptr )>; + std::function, std::shared_ptr)>; using SharedPtrDeferResponseCallbackWithServiceHandle = - std::function, std::shared_ptr, - std::shared_ptr )>; + std::function, std::shared_ptr, + std::shared_ptr)>; std::variant - callback_; + callback_; }; -} // namespace ros2_babel_fish +} // namespace ros2_babel_fish -#endif // ROS2_BABEL_FISH_BABEL_FISH_ANY_SERVICE_CALLBACK_HPP +#endif // ROS2_BABEL_FISH_BABEL_FISH_ANY_SERVICE_CALLBACK_HPP diff --git a/ros2_foxglove_bridge/include/ros2_babel_fish/detail/babel_fish_action_client.hpp b/ros2_foxglove_bridge/include/ros2_babel_fish/detail/babel_fish_action_client.hpp index 0d48445..25264d3 100644 --- a/ros2_foxglove_bridge/include/ros2_babel_fish/detail/babel_fish_action_client.hpp +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/detail/babel_fish_action_client.hpp @@ -1,18 +1,18 @@ // Copyright (c) 2021 Stefan Fabian. All rights reserved. -// Licensed under the MIT license. See LICENSE file in the project root for full license information. +// Licensed under the MIT license. See LICENSE file in the project root for full license +// information. #ifndef ROS2_BABEL_FISH_BABEL_FISH_ACTION_CLIENT_HPP #define ROS2_BABEL_FISH_BABEL_FISH_ACTION_CLIENT_HPP -#include "ros2_babel_fish/messages/compound_message.hpp" #include -namespace ros2_babel_fish -{ +#include "ros2_babel_fish/messages/compound_message.hpp" + +namespace ros2_babel_fish { struct ActionTypeSupport; -namespace impl -{ +namespace impl { struct BabelFishAction { using Feedback = CompoundMessage; using Goal = CompoundMessage; @@ -25,25 +25,23 @@ struct BabelFishAction { }; } Impl; }; -} // namespace impl -} // namespace ros2_babel_fish - -namespace rclcpp_action -{ -template<> -class Client : public rclcpp_action::ClientBase -{ +} // namespace impl +} // namespace ros2_babel_fish + +namespace rclcpp_action { +template <> +class Client : public rclcpp_action::ClientBase { public: - RCLCPP_SMART_PTR_ALIASES_ONLY( Client ) + RCLCPP_SMART_PTR_ALIASES_ONLY(Client) using GoalHandle = rclcpp_action::ClientGoalHandle; using WrappedResult = GoalHandle::WrappedResult; - using GoalResponseCallback = std::function; + using GoalResponseCallback = std::function; using FeedbackCallback = GoalHandle::FeedbackCallback; using ResultCallback = GoalHandle::ResultCallback; using CancelRequest = ros2_babel_fish::CompoundMessage; using CancelResponse = ros2_babel_fish::CompoundMessage; - using CancelCallback = std::function; + using CancelCallback = std::function; /// Options for sending a goal. /** @@ -65,17 +63,18 @@ class Client : public rclcpp_action::Cli ResultCallback result_callback; }; - Client( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, - rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, - const std::string &action_name, - std::shared_ptr type_support, - const rcl_action_client_options_t &client_options = rcl_action_client_get_default_options() ); + Client( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + const std::string& action_name, + std::shared_ptr type_support, + const rcl_action_client_options_t& client_options = rcl_action_client_get_default_options()); ros2_babel_fish::CompoundMessage create_goal() const; - std::shared_future - async_send_goal( const ros2_babel_fish::CompoundMessage &goal, const SendGoalOptions &options = {} ); + std::shared_future async_send_goal( + const ros2_babel_fish::CompoundMessage& goal, const SendGoalOptions& options = {}); /// Asynchronously get the result for an active goal. /** @@ -86,8 +85,8 @@ class Client : public rclcpp_action::Cli * Will overwrite any result callback specified in async_send_goal! * @return A future that is set to the goal result when the goal is finished. */ - std::shared_future async_get_result( typename GoalHandle::SharedPtr goal_handle, - ResultCallback result_callback = nullptr ); + std::shared_future async_get_result(typename GoalHandle::SharedPtr goal_handle, + ResultCallback result_callback = nullptr); /// Asynchronously request a goal be canceled. /** @@ -102,8 +101,8 @@ class Client : public rclcpp_action::Cli * * action_msgs/CancelGoal.srv. */ - std::shared_future async_cancel_goal( GoalHandle::SharedPtr goal_handle, - CancelCallback cancel_callback = nullptr ); + std::shared_future async_cancel_goal(GoalHandle::SharedPtr goal_handle, + CancelCallback cancel_callback = nullptr); /// Asynchronously request for all goals to be canceled. /** @@ -115,7 +114,8 @@ class Client : public rclcpp_action::Cli * * action_msgs/CancelGoal.srv. */ - std::shared_future async_cancel_all_goals( CancelCallback cancel_callback = nullptr ); + std::shared_future async_cancel_all_goals( + CancelCallback cancel_callback = nullptr); /// Asynchronously request all goals at or before a specified time be canceled. /** @@ -128,8 +128,8 @@ class Client : public rclcpp_action::Cli * * action_msgs/CancelGoal.srv. */ - std::shared_future - async_cancel_goals_before( const rclcpp::Time &stamp, CancelCallback cancel_callback = nullptr ); + std::shared_future async_cancel_goals_before( + const rclcpp::Time& stamp, CancelCallback cancel_callback = nullptr); protected: std::shared_ptr create_goal_response() const override; @@ -140,29 +140,28 @@ class Client : public rclcpp_action::Cli std::shared_ptr create_feedback_message() const override; - void handle_feedback_message( std::shared_ptr message ) override; + void handle_feedback_message(std::shared_ptr message) override; std::shared_ptr create_status_message() const override; - void handle_status_message( std::shared_ptr message ) override; + void handle_status_message(std::shared_ptr message) override; - void make_result_aware( GoalHandle::SharedPtr goal_handle ); + void make_result_aware(GoalHandle::SharedPtr goal_handle); - std::shared_future async_cancel( CancelRequest cancel_request, - CancelCallback cancel_callback = nullptr ); + std::shared_future async_cancel(CancelRequest cancel_request, + CancelCallback cancel_callback = nullptr); private: std::shared_ptr type_support_; std::mutex goal_handles_mutex_; std::map goal_handles_; }; -} // namespace rclcpp_action +} // namespace rclcpp_action -namespace ros2_babel_fish -{ +namespace ros2_babel_fish { using BabelFishActionClient = rclcpp_action::Client; } -#endif // ROS2_BABEL_FISH_BABEL_FISH_ACTION_CLIENT_HPP +#endif // ROS2_BABEL_FISH_BABEL_FISH_ACTION_CLIENT_HPP diff --git a/ros2_foxglove_bridge/include/ros2_babel_fish/detail/babel_fish_action_server.hpp b/ros2_foxglove_bridge/include/ros2_babel_fish/detail/babel_fish_action_server.hpp index 4914b9b..59f3cd4 100644 --- a/ros2_foxglove_bridge/include/ros2_babel_fish/detail/babel_fish_action_server.hpp +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/detail/babel_fish_action_server.hpp @@ -1,5 +1,6 @@ // Copyright (c) 2021 Stefan Fabian. All rights reserved. -// Licensed under the MIT license. See LICENSE file in the project root for full license information. +// Licensed under the MIT license. See LICENSE file in the project root for full license +// information. #ifndef ROS2_BABEL_FISH_BABEL_FISH_ACTION_HPP #define ROS2_BABEL_FISH_BABEL_FISH_ACTION_HPP @@ -8,18 +9,16 @@ #include #include -namespace ros2_babel_fish -{ +namespace ros2_babel_fish { -class BabelFishService -{ +class BabelFishService { public: - RCLCPP_SMART_PTR_DEFINITIONS( BabelFishService ) + RCLCPP_SMART_PTR_DEFINITIONS(BabelFishService) BabelFishService( - rclcpp::Node *node, const std::string &name, ServiceTypeSupport::ConstSharedPtr type_support, - std::function callback, - rcl_service_options_t options ); + rclcpp::Node* node, const std::string& name, ServiceTypeSupport::ConstSharedPtr type_support, + std::function callback, + rcl_service_options_t options); rclcpp::ServiceBase::ConstSharedPtr getService() const; @@ -28,6 +27,6 @@ class BabelFishService private: rclcpp::ServiceBase::SharedPtr service_; }; -} // namespace ros2_babel_fish +} // namespace ros2_babel_fish -#endif // ROS2_BABEL_FISH_BABEL_FISH_ACTION_HPP +#endif // ROS2_BABEL_FISH_BABEL_FISH_ACTION_HPP diff --git a/ros2_foxglove_bridge/include/ros2_babel_fish/detail/babel_fish_publisher.hpp b/ros2_foxglove_bridge/include/ros2_babel_fish/detail/babel_fish_publisher.hpp index fbeccfc..d44664c 100644 --- a/ros2_foxglove_bridge/include/ros2_babel_fish/detail/babel_fish_publisher.hpp +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/detail/babel_fish_publisher.hpp @@ -1,50 +1,48 @@ // Copyright (c) 2021 Stefan Fabian. All rights reserved. -// Licensed under the MIT license. See LICENSE file in the project root for full license information. +// Licensed under the MIT license. See LICENSE file in the project root for full license +// information. #ifndef ROS2_BABEL_FISH_BABEL_FISH_PUBLISHER_HPP #define ROS2_BABEL_FISH_BABEL_FISH_PUBLISHER_HPP -#include "ros2_babel_fish/idl/type_support.hpp" -#include - #include #include +#include + +#include "ros2_babel_fish/idl/type_support.hpp" -namespace rclcpp -{ +namespace rclcpp { class SerializedMessage; } -namespace ros2_babel_fish -{ +namespace ros2_babel_fish { -class BabelFishPublisher : public rclcpp::PublisherBase -{ +class BabelFishPublisher : public rclcpp::PublisherBase { public: - RCLCPP_SMART_PTR_DEFINITIONS( BabelFishPublisher ) + RCLCPP_SMART_PTR_DEFINITIONS(BabelFishPublisher) - BabelFishPublisher( rclcpp::node_interfaces::NodeBaseInterface *node_base, - const rosidl_message_type_support_t &type_support, const std::string &topic, - const rclcpp::QoS &qos, const rclcpp::PublisherOptions &options ); + BabelFishPublisher(rclcpp::node_interfaces::NodeBaseInterface* node_base, + const rosidl_message_type_support_t& type_support, const std::string& topic, + const rclcpp::QoS& qos, const rclcpp::PublisherOptions& options); /// Called post construction, so that construction may continue after shared_from_this() works. - virtual void - post_init_setup( rclcpp::node_interfaces::NodeBaseInterface *node_base, const std::string &topic, - const rclcpp::QoS &qos, - const rclcpp::PublisherOptionsWithAllocator> &options ); + virtual void post_init_setup( + rclcpp::node_interfaces::NodeBaseInterface* node_base, const std::string& topic, + const rclcpp::QoS& qos, + const rclcpp::PublisherOptionsWithAllocator>& options); - virtual void publish( std::unique_ptr msg ); + virtual void publish(std::unique_ptr msg); - virtual void publish( const CompoundMessage &msg ); + virtual void publish(const CompoundMessage& msg); - void publish( const rcl_serialized_message_t &serialized_msg ); + void publish(const rcl_serialized_message_t& serialized_msg); - void publish( const rclcpp::SerializedMessage &serialized_msg ); + void publish(const rclcpp::SerializedMessage& serialized_msg); private: - void do_inter_process_publish( const CompoundMessage &msg ); + void do_inter_process_publish(const CompoundMessage& msg); - void do_serialized_publish( const rcl_serialized_message_t *serialized_msg ); + void do_serialized_publish(const rcl_serialized_message_t* serialized_msg); /// Copy of original options passed during construction. /** @@ -53,6 +51,6 @@ class BabelFishPublisher : public rclcpp::PublisherBase */ const rclcpp::PublisherOptionsWithAllocator> options_; }; -} // namespace ros2_babel_fish +} // namespace ros2_babel_fish -#endif // ROS2_BABEL_FISH_BABEL_FISH_PUBLISHER_HPP +#endif // ROS2_BABEL_FISH_BABEL_FISH_PUBLISHER_HPP diff --git a/ros2_foxglove_bridge/include/ros2_babel_fish/detail/babel_fish_service.hpp b/ros2_foxglove_bridge/include/ros2_babel_fish/detail/babel_fish_service.hpp index c843f1f..f926c7d 100644 --- a/ros2_foxglove_bridge/include/ros2_babel_fish/detail/babel_fish_service.hpp +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/detail/babel_fish_service.hpp @@ -1,53 +1,51 @@ // Copyright (c) 2021 Stefan Fabian. All rights reserved. -// Licensed under the MIT license. See LICENSE file in the project root for full license information. +// Licensed under the MIT license. See LICENSE file in the project root for full license +// information. #ifndef ROS2_BABEL_FISH_BABEL_FISH_SERVICE_HPP #define ROS2_BABEL_FISH_BABEL_FISH_SERVICE_HPP +#include + #include "ros2_babel_fish/detail/any_service_callback.hpp" #include "ros2_babel_fish/idl/type_support.hpp" -#include "ros2_babel_fish/idl/type_support.hpp" #include "ros2_babel_fish/messages/compound_message.hpp" -#include -namespace ros2_babel_fish -{ -namespace impl -{ +namespace ros2_babel_fish { +namespace impl { struct BabelFishService { using Request = CompoundMessage; using Response = CompoundMessage; }; -} // namespace impl +} // namespace impl -class BabelFishService : public rclcpp::ServiceBase, std::enable_shared_from_this -{ +class BabelFishService : public rclcpp::ServiceBase, + std::enable_shared_from_this { public: - RCLCPP_SMART_PTR_DEFINITIONS( BabelFishService ) + RCLCPP_SMART_PTR_DEFINITIONS(BabelFishService) //! Do not call directly, this is private API and might change. Use BabelFish::create_service. - BabelFishService( std::shared_ptr node, const std::string &service_name, - ServiceTypeSupport::ConstSharedPtr type_support, - AnyServiceCallback callback, - rcl_service_options_t options ); + BabelFishService(std::shared_ptr node, const std::string& service_name, + ServiceTypeSupport::ConstSharedPtr type_support, AnyServiceCallback callback, + rcl_service_options_t options); - bool take_request( CompoundMessage &request_out, rmw_request_id_t &request_id_out ); + bool take_request(CompoundMessage& request_out, rmw_request_id_t& request_id_out); - void send_response( rmw_request_id_t &request_id, CompoundMessage &response ); + void send_response(rmw_request_id_t& request_id, CompoundMessage& response); std::shared_ptr create_request() override; std::shared_ptr create_request_header() override; - void handle_request( std::shared_ptr request_header, - std::shared_ptr request ) override; + void handle_request(std::shared_ptr request_header, + std::shared_ptr request) override; private: - RCLCPP_DISABLE_COPY( BabelFishService ) + RCLCPP_DISABLE_COPY(BabelFishService) ServiceTypeSupport::ConstSharedPtr type_support_; AnyServiceCallback callback_; }; -} // namespace ros2_babel_fish +} // namespace ros2_babel_fish -#endif // ROS2_BABEL_FISH_BABEL_FISH_SERVICE_HPP +#endif // ROS2_BABEL_FISH_BABEL_FISH_SERVICE_HPP diff --git a/ros2_foxglove_bridge/include/ros2_babel_fish/detail/babel_fish_service_client.hpp b/ros2_foxglove_bridge/include/ros2_babel_fish/detail/babel_fish_service_client.hpp index f9ab434..63c6f4c 100644 --- a/ros2_foxglove_bridge/include/ros2_babel_fish/detail/babel_fish_service_client.hpp +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/detail/babel_fish_service_client.hpp @@ -1,5 +1,6 @@ // Copyright (c) 2021 Stefan Fabian. All rights reserved. -// Licensed under the MIT license. See LICENSE file in the project root for full license information. +// Licensed under the MIT license. See LICENSE file in the project root for full license +// information. #ifndef ROS2_BABEL_FISH_BABEL_FISH_SERVICE_CLIENT_HPP #define ROS2_BABEL_FISH_BABEL_FISH_SERVICE_CLIENT_HPP @@ -8,11 +9,9 @@ #include #include -namespace ros2_babel_fish -{ +namespace ros2_babel_fish { -class BabelFishServiceClient : public rclcpp::ClientBase -{ +class BabelFishServiceClient : public rclcpp::ClientBase { public: using SharedRequest = CompoundMessage::SharedPtr; using SharedResponse = CompoundMessage::SharedPtr; @@ -23,72 +22,70 @@ class BabelFishServiceClient : public rclcpp::ClientBase using SharedPromiseWithRequest = std::shared_ptr; using SharedFuture = std::shared_future; using SharedFutureWithRequest = std::shared_future>; - using CallbackType = std::function; - using CallbackWithRequestType = std::function; + using CallbackType = std::function; + using CallbackWithRequestType = std::function; - RCLCPP_SMART_PTR_DEFINITIONS( BabelFishServiceClient ) + RCLCPP_SMART_PTR_DEFINITIONS(BabelFishServiceClient) - BabelFishServiceClient( rclcpp::node_interfaces::NodeBaseInterface *node_base, - rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, - const std::string &service_name, - ServiceTypeSupport::ConstSharedPtr type_support, - rcl_client_options_t client_options ); + BabelFishServiceClient(rclcpp::node_interfaces::NodeBaseInterface* node_base, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, + const std::string& service_name, + ServiceTypeSupport::ConstSharedPtr type_support, + rcl_client_options_t client_options); - bool take_response( CompoundMessage &response_out, rmw_request_id_t &request_header_out ); + bool take_response(CompoundMessage& response_out, rmw_request_id_t& request_header_out); std::shared_ptr create_response() override; std::shared_ptr create_request_header() override; - SharedFuture async_send_request( SharedRequest request ); + SharedFuture async_send_request(SharedRequest request); - template::value>::type * = nullptr> - SharedFuture async_send_request( const SharedRequest &request, CallbackT &&cb ) - { - std::lock_guard lock( pending_requests_mutex_ ); + template ::value>::type* = nullptr> + SharedFuture async_send_request(const SharedRequest& request, CallbackT&& cb) { + std::lock_guard lock(pending_requests_mutex_); int64_t sequence_number; - rcl_ret_t ret = rcl_send_request( get_client_handle().get(), - request->type_erased_message().get(), &sequence_number ); - if ( RCL_RET_OK != ret ) { - rclcpp::exceptions::throw_from_rcl_error( ret, "failed to send request_template" ); + rcl_ret_t ret = rcl_send_request(get_client_handle().get(), + request->type_erased_message().get(), &sequence_number); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request_template"); } SharedPromise call_promise = std::make_shared(); - SharedFuture f( call_promise->get_future() ); + SharedFuture f(call_promise->get_future()); pending_requests_[sequence_number] = - std::make_tuple( call_promise, std::forward( cb ), f ); + std::make_tuple(call_promise, std::forward(cb), f); return f; } - template::value>::type * = nullptr> - SharedFutureWithRequest async_send_request( SharedRequest request, CallbackT &&cb ) - { + template ::value>::type* = nullptr> + SharedFutureWithRequest async_send_request(SharedRequest request, CallbackT&& cb) { SharedPromiseWithRequest promise = std::make_shared(); - SharedFutureWithRequest future_with_request( promise->get_future() ); + SharedFutureWithRequest future_with_request(promise->get_future()); - auto wrapping_cb = [future_with_request, promise, request, &cb]( const SharedFuture &future ) { + auto wrapping_cb = [future_with_request, promise, request, &cb](const SharedFuture& future) { auto response = future.get(); - promise->set_value( std::make_pair( request, response ) ); - cb( future_with_request ); + promise->set_value(std::make_pair(request, response)); + cb(future_with_request); }; - async_send_request( request, wrapping_cb ); + async_send_request(request, wrapping_cb); return future_with_request; } - void handle_response( std::shared_ptr request_header, - std::shared_ptr response ) override; + void handle_response(std::shared_ptr request_header, + std::shared_ptr response) override; private: - RCLCPP_DISABLE_COPY( BabelFishServiceClient ) + RCLCPP_DISABLE_COPY(BabelFishServiceClient) std::map> pending_requests_; ServiceTypeSupport::ConstSharedPtr type_support_; std::mutex pending_requests_mutex_; }; -} // namespace ros2_babel_fish +} // namespace ros2_babel_fish -#endif // ROS2_BABEL_FISH_BABEL_FISH_SERVICE_CLIENT_HPP +#endif // ROS2_BABEL_FISH_BABEL_FISH_SERVICE_CLIENT_HPP diff --git a/ros2_foxglove_bridge/include/ros2_babel_fish/detail/babel_fish_subscription.hpp b/ros2_foxglove_bridge/include/ros2_babel_fish/detail/babel_fish_subscription.hpp index 91a9fb2..9d49118 100644 --- a/ros2_foxglove_bridge/include/ros2_babel_fish/detail/babel_fish_subscription.hpp +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/detail/babel_fish_subscription.hpp @@ -1,31 +1,29 @@ // Copyright (c) 2021 Stefan Fabian. All rights reserved. -// Licensed under the MIT license. See LICENSE file in the project root for full license information. +// Licensed under the MIT license. See LICENSE file in the project root for full license +// information. #ifndef ROS2_BABEL_FISH_BABEL_FISH_SUBSCRIPTION_HPP #define ROS2_BABEL_FISH_BABEL_FISH_SUBSCRIPTION_HPP -#include - #include #include #include #include +#include -namespace ros2_babel_fish -{ +namespace ros2_babel_fish { class BabelFish; -class BabelFishSubscription : public rclcpp::SubscriptionBase -{ +class BabelFishSubscription : public rclcpp::SubscriptionBase { public: - RCLCPP_SMART_PTR_DEFINITIONS( BabelFishSubscription ) + RCLCPP_SMART_PTR_DEFINITIONS(BabelFishSubscription) BabelFishSubscription( - rclcpp::node_interfaces::NodeBaseInterface *node, - MessageTypeSupport::ConstSharedPtr type_support, const std::string &topic_name, - const rclcpp::QoS &qos, - rclcpp::AnySubscriptionCallback> callback, - const rclcpp::SubscriptionOptionsWithAllocator> &options ); + rclcpp::node_interfaces::NodeBaseInterface* node, + MessageTypeSupport::ConstSharedPtr type_support, const std::string& topic_name, + const rclcpp::QoS& qos, + rclcpp::AnySubscriptionCallback> callback, + const rclcpp::SubscriptionOptionsWithAllocator>& options); ~BabelFishSubscription() override; @@ -33,30 +31,32 @@ class BabelFishSubscription : public rclcpp::SubscriptionBase std::shared_ptr create_serialized_message() override; - void handle_message( std::shared_ptr &message, - const rclcpp::MessageInfo &message_info ) override; + void handle_message(std::shared_ptr& message, + const rclcpp::MessageInfo& message_info) override; - void handle_serialized_message( const std::shared_ptr &serialized_message, - const rclcpp::MessageInfo &message_info ) override; + void handle_serialized_message( + const std::shared_ptr& serialized_message, + const rclcpp::MessageInfo& message_info) override; - void handle_loaned_message( void *loaned_message, const rclcpp::MessageInfo &message_info ) override; + void handle_loaned_message(void* loaned_message, + const rclcpp::MessageInfo& message_info) override; - void return_message( std::shared_ptr &message ) override; + void return_message(std::shared_ptr& message) override; - void return_serialized_message( std::shared_ptr &message ) override; + void return_serialized_message(std::shared_ptr& message) override; - bool take( CompoundMessage &message_out, rclcpp::MessageInfo &message_info_out ); + bool take(CompoundMessage& message_out, rclcpp::MessageInfo& message_info_out); MessageTypeSupport::ConstSharedPtr get_message_type_support() const; std::string get_message_type() const; private: - RCLCPP_DISABLE_COPY( BabelFishSubscription ) + RCLCPP_DISABLE_COPY(BabelFishSubscription) MessageTypeSupport::ConstSharedPtr type_support_; rclcpp::AnySubscriptionCallback> callback_; }; -} // namespace ros2_babel_fish +} // namespace ros2_babel_fish -#endif // ROS2_BABEL_FISH_BABEL_FISH_SUBSCRIPTION_HPP +#endif // ROS2_BABEL_FISH_BABEL_FISH_SUBSCRIPTION_HPP diff --git a/ros2_foxglove_bridge/include/ros2_babel_fish/detail/topic.hpp b/ros2_foxglove_bridge/include/ros2_babel_fish/detail/topic.hpp index eb441fb..e75df35 100644 --- a/ros2_foxglove_bridge/include/ros2_babel_fish/detail/topic.hpp +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/detail/topic.hpp @@ -9,42 +9,37 @@ #include #include -namespace rclcpp -{ +namespace rclcpp { class Node; } -namespace ros2_babel_fish -{ -namespace impl -{ -bool wait_for_topic_nanoseconds( rclcpp::Node &node, const std::string &topic, - std::chrono::nanoseconds timeout ); +namespace ros2_babel_fish { +namespace impl { +bool wait_for_topic_nanoseconds(rclcpp::Node& node, const std::string& topic, + std::chrono::nanoseconds timeout); -bool wait_for_topic_and_type_nanoseconds( rclcpp::Node &node, const std::string &topic, - std::vector &types, - std::chrono::nanoseconds timeout ); -} // namespace impl +bool wait_for_topic_and_type_nanoseconds(rclcpp::Node& node, const std::string& topic, + std::vector& types, + std::chrono::nanoseconds timeout); +} // namespace impl -template +template bool wait_for_topic( - rclcpp::Node &node, const std::string &topic, - std::chrono::duration timeout = std::chrono::duration( -1 ) ) -{ + rclcpp::Node& node, const std::string& topic, + std::chrono::duration timeout = std::chrono::duration(-1)) { return impl::wait_for_topic_nanoseconds( - node, topic, std::chrono::duration_cast( timeout ) ); + node, topic, std::chrono::duration_cast(timeout)); } -template +template bool wait_for_topic_and_type( - rclcpp::Node &node, const std::string &topic, std::vector &types, - std::chrono::duration timeout = std::chrono::duration( -1 ) ) -{ + rclcpp::Node& node, const std::string& topic, std::vector& types, + std::chrono::duration timeout = std::chrono::duration(-1)) { return impl::wait_for_topic_and_type_nanoseconds( - node, topic, types, std::chrono::duration_cast( timeout ) ); + node, topic, types, std::chrono::duration_cast(timeout)); } -std::string resolve_topic( const rclcpp::Node &node, const std::string &topic ); -} // namespace ros2_babel_fish +std::string resolve_topic(const rclcpp::Node& node, const std::string& topic); +} // namespace ros2_babel_fish -#endif // ROS2_BABEL_FISH_TOPIC_HPP +#endif // ROS2_BABEL_FISH_TOPIC_HPP diff --git a/ros2_foxglove_bridge/include/ros2_babel_fish/exceptions/babel_fish_exception.hpp b/ros2_foxglove_bridge/include/ros2_babel_fish/exceptions/babel_fish_exception.hpp index 4db90e4..1d81a84 100644 --- a/ros2_foxglove_bridge/include/ros2_babel_fish/exceptions/babel_fish_exception.hpp +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/exceptions/babel_fish_exception.hpp @@ -1,19 +1,19 @@ // Copyright (c) 2021 Stefan Fabian. All rights reserved. -// Licensed under the MIT license. See LICENSE file in the project root for full license information. +// Licensed under the MIT license. See LICENSE file in the project root for full license +// information. #ifndef ROS2_BABEL_FISH_BABEL_FISH_EXCEPTION_HPP #define ROS2_BABEL_FISH_BABEL_FISH_EXCEPTION_HPP #include -namespace ros2_babel_fish -{ +namespace ros2_babel_fish { -class BabelFishException : public std::runtime_error -{ +class BabelFishException : public std::runtime_error { public: - explicit BabelFishException( const std::string &msg ) : std::runtime_error( msg ) { } + explicit BabelFishException(const std::string& msg) + : std::runtime_error(msg) {} }; -} // namespace ros2_babel_fish +} // namespace ros2_babel_fish -#endif // ROS2_BABEL_FISH_BABEL_FISH_EXCEPTION_HPP +#endif // ROS2_BABEL_FISH_BABEL_FISH_EXCEPTION_HPP diff --git a/ros2_foxglove_bridge/include/ros2_babel_fish/idl/providers/local_type_support_provider.hpp b/ros2_foxglove_bridge/include/ros2_babel_fish/idl/providers/local_type_support_provider.hpp index 4af6ee5..f2549ac 100644 --- a/ros2_foxglove_bridge/include/ros2_babel_fish/idl/providers/local_type_support_provider.hpp +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/idl/providers/local_type_support_provider.hpp @@ -1,29 +1,31 @@ // Copyright (c) 2021 Stefan Fabian. All rights reserved. -// Licensed under the MIT license. See LICENSE file in the project root for full license information. +// Licensed under the MIT license. See LICENSE file in the project root for full license +// information. #ifndef ROS2_BABEL_FISH_INTEGRATED_DESCRIPTION_PROVIDER_H #define ROS2_BABEL_FISH_INTEGRATED_DESCRIPTION_PROVIDER_H #include "ros2_babel_fish/idl/type_support_provider.hpp" -namespace ros2_babel_fish -{ +namespace ros2_babel_fish { /** * @brief Looks up message libraries that are available locally on the machine. */ -class LocalTypeSupportProvider : public TypeSupportProvider -{ +class LocalTypeSupportProvider : public TypeSupportProvider { public: LocalTypeSupportProvider(); protected: - MessageTypeSupport::ConstSharedPtr getMessageTypeSupportImpl( const std::string &type ) const override; + MessageTypeSupport::ConstSharedPtr getMessageTypeSupportImpl( + const std::string& type) const override; - ServiceTypeSupport::ConstSharedPtr getServiceTypeSupportImpl( const std::string &type ) const override; + ServiceTypeSupport::ConstSharedPtr getServiceTypeSupportImpl( + const std::string& type) const override; - ActionTypeSupport::ConstSharedPtr getActionTypeSupportImpl( const std::string &type ) const override; + ActionTypeSupport::ConstSharedPtr getActionTypeSupportImpl( + const std::string& type) const override; }; -} // namespace ros2_babel_fish +} // namespace ros2_babel_fish -#endif // ROS2_BABEL_FISH_INTEGRATED_DESCRIPTION_PROVIDER_H +#endif // ROS2_BABEL_FISH_INTEGRATED_DESCRIPTION_PROVIDER_H diff --git a/ros2_foxglove_bridge/include/ros2_babel_fish/idl/serialization.hpp b/ros2_foxglove_bridge/include/ros2_babel_fish/idl/serialization.hpp index cca430b..47475da 100644 --- a/ros2_foxglove_bridge/include/ros2_babel_fish/idl/serialization.hpp +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/idl/serialization.hpp @@ -5,21 +5,20 @@ #ifndef ROS2_BABEL_FISH_SERIALIZATION_HPP #define ROS2_BABEL_FISH_SERIALIZATION_HPP +#include + #include "ros2_babel_fish/idl/type_support.hpp" #include "ros2_babel_fish/messages/message.hpp" -#include - -namespace ros2_babel_fish -{ -std::shared_ptr -createContainer( const rosidl_typesupport_introspection_cpp::MessageMembers &members, - rosidl_runtime_cpp::MessageInitialization initialization = - rosidl_runtime_cpp::MessageInitialization::ALL ); +namespace ros2_babel_fish { +std::shared_ptr createContainer( + const rosidl_typesupport_introspection_cpp::MessageMembers& members, + rosidl_runtime_cpp::MessageInitialization initialization = + rosidl_runtime_cpp::MessageInitialization::ALL); -std::shared_ptr createContainer( const MessageMembersIntrospection &members, - rosidl_runtime_cpp::MessageInitialization initialization = - rosidl_runtime_cpp::MessageInitialization::ALL ); -} // namespace ros2_babel_fish +std::shared_ptr createContainer(const MessageMembersIntrospection& members, + rosidl_runtime_cpp::MessageInitialization initialization = + rosidl_runtime_cpp::MessageInitialization::ALL); +} // namespace ros2_babel_fish -#endif // ROS2_BABEL_FISH_SERIALIZATION_HPP +#endif // ROS2_BABEL_FISH_SERIALIZATION_HPP diff --git a/ros2_foxglove_bridge/include/ros2_babel_fish/idl/type_support.hpp b/ros2_foxglove_bridge/include/ros2_babel_fish/idl/type_support.hpp index 392c8f1..03c20d5 100644 --- a/ros2_foxglove_bridge/include/ros2_babel_fish/idl/type_support.hpp +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/idl/type_support.hpp @@ -1,21 +1,21 @@ // Copyright (c) 2021 Stefan Fabian. All rights reserved. -// Licensed under the MIT license. See LICENSE file in the project root for full license information. +// Licensed under the MIT license. See LICENSE file in the project root for full license +// information. #ifndef ROS2_BABEL_FISH_MESSAGE_DESCRIPTION_H #define ROS2_BABEL_FISH_MESSAGE_DESCRIPTION_H +#include +#include +#include + #include #include #include #include #include -#include -#include -#include - -namespace ros2_babel_fish -{ +namespace ros2_babel_fish { struct MessageTypeSupport { using SharedPtr = std::shared_ptr; @@ -23,61 +23,60 @@ struct MessageTypeSupport { std::string name; - //! In case the type_suport_handle's memory is handled elsewhere, this can be used to make sure the memory stays valid. + //! In case the type_suport_handle's memory is handled elsewhere, this can be used to make sure + //! the memory stays valid. std::shared_ptr type_support_library; //! Needed to create subscribers. rosidl_message_type_support_t type_support_handle; //! Same as above. std::shared_ptr introspection_type_support_library; - //! Needed to parse messages. Check the message_template for null to check if this handle is valid. + //! Needed to parse messages. Check the message_template for null to check if this handle is + //! valid. rosidl_message_type_support_t introspection_type_support_handle; }; struct MessageMemberIntrospection { - MessageMemberIntrospection( const rosidl_typesupport_introspection_cpp::MessageMember *member, - std::shared_ptr library ) - : library( std::move( library ) ), value( member ) - { - } + MessageMemberIntrospection(const rosidl_typesupport_introspection_cpp::MessageMember* member, + std::shared_ptr library) + : library(std::move(library)) + , value(member) {} - const rosidl_typesupport_introspection_cpp::MessageMember *operator->() const { return value; } + const rosidl_typesupport_introspection_cpp::MessageMember* operator->() const { + return value; + } std::shared_ptr library; - const rosidl_typesupport_introspection_cpp::MessageMember *value; + const rosidl_typesupport_introspection_cpp::MessageMember* value; }; struct MessageMembersIntrospection { - /* implicit */ MessageMembersIntrospection( const MessageTypeSupport &type_support ) // NOLINT - : library( type_support.introspection_type_support_library ), - value( static_cast( - type_support.introspection_type_support_handle.data ) ) - { - } - - /* implicit */ MessageMembersIntrospection( const MessageMemberIntrospection &member ) // NOLINT - : library( member.library ), - value( static_cast( - member.value->members_->data ) ) - { + /* implicit */ MessageMembersIntrospection(const MessageTypeSupport& type_support) // NOLINT + : library(type_support.introspection_type_support_library) + , value(static_cast( + type_support.introspection_type_support_handle.data)) {} + + /* implicit */ MessageMembersIntrospection(const MessageMemberIntrospection& member) // NOLINT + : library(member.library) + , value(static_cast( + member.value->members_->data)) {} + + MessageMembersIntrospection(const rosidl_typesupport_introspection_cpp::MessageMembers* members, + std::shared_ptr library) + : library(std::move(library)) + , value(members) {} + + const rosidl_typesupport_introspection_cpp::MessageMembers* operator->() const { + return value; } - MessageMembersIntrospection( const rosidl_typesupport_introspection_cpp::MessageMembers *members, - std::shared_ptr library ) - : library( std::move( library ) ), value( members ) - { - } - - const rosidl_typesupport_introspection_cpp::MessageMembers *operator->() const { return value; } - - MessageMemberIntrospection getMember( size_t index ) const - { - assert( index < value->member_count_ ); - return MessageMemberIntrospection( &value->members_[index], library ); + MessageMemberIntrospection getMember(size_t index) const { + assert(index < value->member_count_); + return MessageMemberIntrospection(&value->members_[index], library); } std::shared_ptr library; - const rosidl_typesupport_introspection_cpp::MessageMembers *value; + const rosidl_typesupport_introspection_cpp::MessageMembers* value; }; struct ServiceTypeSupport { @@ -86,28 +85,28 @@ struct ServiceTypeSupport { std::string name; - //! In case the type_suport_handle's memory is handled elsewhere, this can be used to make sure the memory stays valid. + //! In case the type_suport_handle's memory is handled elsewhere, this can be used to make sure + //! the memory stays valid. std::shared_ptr type_support_library; //! Needed to create requests / responses and advertise services rosidl_service_type_support_t type_support_handle; //! Same as above. std::shared_ptr introspection_type_support_library; - //! Needed to parse messages. Check the message_template for null to check if this handle is valid. + //! Needed to parse messages. Check the message_template for null to check if this handle is + //! valid. rosidl_service_type_support_t introspection_type_support_handle; - MessageMembersIntrospection request() const - { - const auto *service = static_cast( - introspection_type_support_handle.data ); - return { service->request_members_, introspection_type_support_library }; + MessageMembersIntrospection request() const { + const auto* service = static_cast( + introspection_type_support_handle.data); + return {service->request_members_, introspection_type_support_library}; } - MessageMembersIntrospection response() const - { - const auto *service = static_cast( - introspection_type_support_handle.data ); - return { service->response_members_, introspection_type_support_library }; + MessageMembersIntrospection response() const { + const auto* service = static_cast( + introspection_type_support_handle.data); + return {service->response_members_, introspection_type_support_library}; } }; @@ -117,14 +116,16 @@ struct ActionTypeSupport { std::string name; - //! In case the type_suport_handle's memory is handled elsewhere, this can be used to make sure the memory stays valid. + //! In case the type_suport_handle's memory is handled elsewhere, this can be used to make sure + //! the memory stays valid. std::shared_ptr type_support_library; //! Needed to create subscribers. rosidl_action_type_support_t type_support_handle; //! Same as above. std::shared_ptr introspection_type_support_library; - //! Needed to parse messages. Check the message_template for null to check if this handle is valid. + //! Needed to parse messages. Check the message_template for null to check if this handle is + //! valid. rosidl_action_type_support_t introspection_type_support_handle; ServiceTypeSupport::ConstSharedPtr goal_service_type_support; @@ -134,6 +135,6 @@ struct ActionTypeSupport { MessageTypeSupport::ConstSharedPtr feedback_message_type_support; MessageTypeSupport::ConstSharedPtr status_message_type_support; }; -} // namespace ros2_babel_fish +} // namespace ros2_babel_fish -#endif // ROS2_BABEL_FISH_MESSAGE_DESCRIPTION_H +#endif // ROS2_BABEL_FISH_MESSAGE_DESCRIPTION_H diff --git a/ros2_foxglove_bridge/include/ros2_babel_fish/idl/type_support_provider.hpp b/ros2_foxglove_bridge/include/ros2_babel_fish/idl/type_support_provider.hpp index d0ec328..1af2585 100644 --- a/ros2_foxglove_bridge/include/ros2_babel_fish/idl/type_support_provider.hpp +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/idl/type_support_provider.hpp @@ -1,61 +1,63 @@ // Copyright (c) 2021 Stefan Fabian. All rights reserved. -// Licensed under the MIT license. See LICENSE file in the project root for full license information. +// Licensed under the MIT license. See LICENSE file in the project root for full license +// information. #ifndef ROS2_BABEL_FISH_DESCRIPTION_PROVIDER_H #define ROS2_BABEL_FISH_DESCRIPTION_PROVIDER_H -#include "ros2_babel_fish/idl/type_support.hpp" - #include -namespace ros2_babel_fish -{ +#include "ros2_babel_fish/idl/type_support.hpp" + +namespace ros2_babel_fish { -class TypeSupportProvider -{ +class TypeSupportProvider { public: using SharedPtr = std::shared_ptr; using ConstSharedPtr = std::shared_ptr; TypeSupportProvider(); - MessageTypeSupport::ConstSharedPtr getMessageTypeSupport( const std::string &type ) const; + MessageTypeSupport::ConstSharedPtr getMessageTypeSupport(const std::string& type) const; - ServiceTypeSupport::ConstSharedPtr getServiceTypeSupport( const std::string &type ) const; + ServiceTypeSupport::ConstSharedPtr getServiceTypeSupport(const std::string& type) const; - ActionTypeSupport::ConstSharedPtr getActionTypeSupport( const std::string &type ) const; + ActionTypeSupport::ConstSharedPtr getActionTypeSupport(const std::string& type) const; protected: - //! Implementations should call registerMessage if the type support can be cached which is usually the case. - virtual MessageTypeSupport::ConstSharedPtr - getMessageTypeSupportImpl( const std::string &type ) const = 0; + //! Implementations should call registerMessage if the type support can be cached which is usually + //! the case. + virtual MessageTypeSupport::ConstSharedPtr getMessageTypeSupportImpl( + const std::string& type) const = 0; - virtual ServiceTypeSupport::ConstSharedPtr - getServiceTypeSupportImpl( const std::string &type ) const = 0; + virtual ServiceTypeSupport::ConstSharedPtr getServiceTypeSupportImpl( + const std::string& type) const = 0; - virtual ActionTypeSupport::ConstSharedPtr - getActionTypeSupportImpl( const std::string &type ) const = 0; + virtual ActionTypeSupport::ConstSharedPtr getActionTypeSupportImpl( + const std::string& type) const = 0; - MessageTypeSupport::ConstSharedPtr - registerMessage( const std::string &name, const std::shared_ptr &type_support_library, - rosidl_message_type_support_t type_support, - const std::shared_ptr &introspection_type_support_library, - rosidl_message_type_support_t introspection_type_support ) const; + MessageTypeSupport::ConstSharedPtr registerMessage( + const std::string& name, const std::shared_ptr& type_support_library, + rosidl_message_type_support_t type_support, + const std::shared_ptr& introspection_type_support_library, + rosidl_message_type_support_t introspection_type_support) const; - ServiceTypeSupport::ConstSharedPtr - registerService( const std::string &name, const std::shared_ptr &type_support_library, - rosidl_service_type_support_t type_support, - const std::shared_ptr &introspection_type_support_library, - rosidl_service_type_support_t introspection_type_support ) const; + ServiceTypeSupport::ConstSharedPtr registerService( + const std::string& name, const std::shared_ptr& type_support_library, + rosidl_service_type_support_t type_support, + const std::shared_ptr& introspection_type_support_library, + rosidl_service_type_support_t introspection_type_support) const; - ActionTypeSupport::ConstSharedPtr - registerAction( const std::string &name, ActionTypeSupport::ConstSharedPtr type_support ) const; + ActionTypeSupport::ConstSharedPtr registerAction( + const std::string& name, ActionTypeSupport::ConstSharedPtr type_support) const; private: - mutable std::unordered_map message_type_supports_; - mutable std::unordered_map service_type_supports_; + mutable std::unordered_map + message_type_supports_; + mutable std::unordered_map + service_type_supports_; mutable std::unordered_map action_type_supports_; }; -} // namespace ros2_babel_fish +} // namespace ros2_babel_fish -#endif // ROS2_BABEL_FISH_DESCRIPTION_PROVIDER_H +#endif // ROS2_BABEL_FISH_DESCRIPTION_PROVIDER_H diff --git a/ros2_foxglove_bridge/include/ros2_babel_fish/macros.hpp b/ros2_foxglove_bridge/include/ros2_babel_fish/macros.hpp index 169df00..ba60b29 100644 --- a/ros2_foxglove_bridge/include/ros2_babel_fish/macros.hpp +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/macros.hpp @@ -1,377 +1,386 @@ // Copyright (c) 2020 Stefan Fabian. All rights reserved. -// Licensed under the MIT license. See LICENSE file in the project root for full license information. +// Licensed under the MIT license. See LICENSE file in the project root for full license +// information. #ifndef ROS2_BABEL_FISH_MACROS_HPP #define ROS2_BABEL_FISH_MACROS_HPP #include "ros2_babel_fish/messages/message_type_traits.hpp" -#define RBF2_TEMPLATE_CALL( function, type, ... ) \ - do { \ - switch ( type ) { \ - case MessageTypes::Compound: \ - function<::ros2_babel_fish::message_type_traits::value_type::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::Array: \ - function<::ros2_babel_fish::message_type_traits::value_type::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::Bool: \ - function<::ros2_babel_fish::message_type_traits::value_type::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::Octet: \ - function<::ros2_babel_fish::message_type_traits::value_type::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::UInt8: \ - function<::ros2_babel_fish::message_type_traits::value_type::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::UInt16: \ - function<::ros2_babel_fish::message_type_traits::value_type::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::UInt32: \ - function<::ros2_babel_fish::message_type_traits::value_type::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::UInt64: \ - function<::ros2_babel_fish::message_type_traits::value_type::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::Int8: \ - function<::ros2_babel_fish::message_type_traits::value_type::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::Int16: \ - function<::ros2_babel_fish::message_type_traits::value_type::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::Int32: \ - function<::ros2_babel_fish::message_type_traits::value_type::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::Int64: \ - function<::ros2_babel_fish::message_type_traits::value_type::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::Float: \ - function<::ros2_babel_fish::message_type_traits::value_type::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::Double: \ - function<::ros2_babel_fish::message_type_traits::value_type::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::LongDouble: \ - function<::ros2_babel_fish::message_type_traits::value_type::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::Char: \ - function<::ros2_babel_fish::message_type_traits::value_type::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::WChar: \ - function<::ros2_babel_fish::message_type_traits::value_type::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::String: \ - function<::ros2_babel_fish::message_type_traits::value_type::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::WString: \ - function<::ros2_babel_fish::message_type_traits::value_type::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::None: \ - break; /* Do nothing except tell the compiler we know about this type. */ \ - } \ - } while ( false ) +#define RBF2_TEMPLATE_CALL(function, type, ...) \ + do { \ + switch (type) { \ + case MessageTypes::Compound: \ + function< \ + ::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::Array: \ + function<::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::Bool: \ + function<::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::Octet: \ + function<::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::UInt8: \ + function<::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::UInt16: \ + function<::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::UInt32: \ + function<::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::UInt64: \ + function<::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::Int8: \ + function<::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::Int16: \ + function<::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::Int32: \ + function<::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::Int64: \ + function<::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::Float: \ + function<::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::Double: \ + function<::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::LongDouble: \ + function< \ + ::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::Char: \ + function<::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::WChar: \ + function<::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::String: \ + function<::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::WString: \ + function< \ + ::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::None: \ + break; /* Do nothing except tell the compiler we know about this type. */ \ + } \ + } while (false) -#define RBF2_TEMPLATE_CALL_VALUE_TYPES( function, type, ... ) \ - do { \ - switch ( type ) { \ - case MessageTypes::Bool: \ - function<::ros2_babel_fish::message_type_traits::value_type::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::Octet: \ - function<::ros2_babel_fish::message_type_traits::value_type::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::UInt8: \ - function<::ros2_babel_fish::message_type_traits::value_type::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::UInt16: \ - function<::ros2_babel_fish::message_type_traits::value_type::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::UInt32: \ - function<::ros2_babel_fish::message_type_traits::value_type::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::UInt64: \ - function<::ros2_babel_fish::message_type_traits::value_type::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::Int8: \ - function<::ros2_babel_fish::message_type_traits::value_type::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::Int16: \ - function<::ros2_babel_fish::message_type_traits::value_type::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::Int32: \ - function<::ros2_babel_fish::message_type_traits::value_type::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::Int64: \ - function<::ros2_babel_fish::message_type_traits::value_type::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::Float: \ - function<::ros2_babel_fish::message_type_traits::value_type::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::Double: \ - function<::ros2_babel_fish::message_type_traits::value_type::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::LongDouble: \ - function<::ros2_babel_fish::message_type_traits::value_type::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::Char: \ - function<::ros2_babel_fish::message_type_traits::value_type::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::WChar: \ - function<::ros2_babel_fish::message_type_traits::value_type::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::String: \ - function<::ros2_babel_fish::message_type_traits::value_type::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::WString: \ - function<::ros2_babel_fish::message_type_traits::value_type::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::Array: \ - case MessageTypes::Compound: \ - case MessageTypes::None: \ - break; /* Do nothing except tell the compiler we know about these types. */ \ - } \ - } while ( false ) +#define RBF2_TEMPLATE_CALL_VALUE_TYPES(function, type, ...) \ + do { \ + switch (type) { \ + case MessageTypes::Bool: \ + function<::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::Octet: \ + function<::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::UInt8: \ + function<::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::UInt16: \ + function<::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::UInt32: \ + function<::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::UInt64: \ + function<::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::Int8: \ + function<::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::Int16: \ + function<::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::Int32: \ + function<::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::Int64: \ + function<::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::Float: \ + function<::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::Double: \ + function<::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::LongDouble: \ + function< \ + ::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::Char: \ + function<::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::WChar: \ + function<::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::String: \ + function<::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::WString: \ + function< \ + ::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::Array: \ + case MessageTypes::Compound: \ + case MessageTypes::None: \ + break; /* Do nothing except tell the compiler we know about these types. */ \ + } \ + } while (false) -#define RBF2_TEMPLATE_CALL_SIMPLE_VALUE_TYPES( function, type, ... ) \ - do { \ - switch ( type ) { \ - case MessageTypes::Bool: \ - function::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::Octet: \ - function::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::UInt8: \ - function::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::UInt16: \ - function::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::UInt32: \ - function::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::UInt64: \ - function::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::Int8: \ - function::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::Int16: \ - function::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::Int32: \ - function::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::Int64: \ - function::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::Float: \ - function::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::Double: \ - function::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::LongDouble: \ - function::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::Char: \ - function::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::WChar: \ - function::value>( \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::String: \ - case MessageTypes::WString: \ - case MessageTypes::Array: \ - case MessageTypes::Compound: \ - case MessageTypes::None: \ - break; /* Do nothing except tell the compiler we know about these types. */ \ - } \ - } while ( false ) +#define RBF2_TEMPLATE_CALL_SIMPLE_VALUE_TYPES(function, type, ...) \ + do { \ + switch (type) { \ + case MessageTypes::Bool: \ + function< \ + typename ::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::Octet: \ + function::value>(__VA_ARGS__); \ + break; \ + case MessageTypes::UInt8: \ + function::value>(__VA_ARGS__); \ + break; \ + case MessageTypes::UInt16: \ + function::value>(__VA_ARGS__); \ + break; \ + case MessageTypes::UInt32: \ + function::value>(__VA_ARGS__); \ + break; \ + case MessageTypes::UInt64: \ + function::value>(__VA_ARGS__); \ + break; \ + case MessageTypes::Int8: \ + function< \ + typename ::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::Int16: \ + function::value>(__VA_ARGS__); \ + break; \ + case MessageTypes::Int32: \ + function::value>(__VA_ARGS__); \ + break; \ + case MessageTypes::Int64: \ + function::value>(__VA_ARGS__); \ + break; \ + case MessageTypes::Float: \ + function::value>(__VA_ARGS__); \ + break; \ + case MessageTypes::Double: \ + function::value>(__VA_ARGS__); \ + break; \ + case MessageTypes::LongDouble: \ + function::value>(__VA_ARGS__); \ + break; \ + case MessageTypes::Char: \ + function< \ + typename ::ros2_babel_fish::message_type_traits::value_type::value>( \ + __VA_ARGS__); \ + break; \ + case MessageTypes::WChar: \ + function::value>(__VA_ARGS__); \ + break; \ + case MessageTypes::String: \ + case MessageTypes::WString: \ + case MessageTypes::Array: \ + case MessageTypes::Compound: \ + case MessageTypes::None: \ + break; /* Do nothing except tell the compiler we know about these types. */ \ + } \ + } while (false) -#define _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( FUNCTION, ... ) FUNCTION( __VA_ARGS__ ) +#define _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION(FUNCTION, ...) FUNCTION(__VA_ARGS__) -#define _RBF2_TEMPLATE_CALL_ARRAY_TYPES( FUNCTION, ARRAY, BOUNDED, FIXEDLENGTH, ... ) \ - switch ( ARRAY.elementType() ) { \ - case MessageTypes::Bool: \ - _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( \ - FUNCTION, \ - (ARRAY.as::value, \ - BOUNDED, FIXEDLENGTH>>())__VA_OPT__(, ) __VA_ARGS__ ); \ - break; \ - case MessageTypes::Octet: \ - _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( \ - FUNCTION, \ - (ARRAY.as::value, \ - BOUNDED, FIXEDLENGTH>>())__VA_OPT__(, ) __VA_ARGS__ ); \ - break; \ - case MessageTypes::UInt8: \ - _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( \ - FUNCTION, \ - (ARRAY.as::value, \ - BOUNDED, FIXEDLENGTH>>())__VA_OPT__(, ) __VA_ARGS__ ); \ - break; \ - case MessageTypes::UInt16: \ - _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( \ - FUNCTION, \ - (ARRAY.as::value, \ - BOUNDED, FIXEDLENGTH>>())__VA_OPT__(, ) __VA_ARGS__ ); \ - break; \ - case MessageTypes::UInt32: \ - _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( \ - FUNCTION, \ - (ARRAY.as::value, \ - BOUNDED, FIXEDLENGTH>>())__VA_OPT__(, ) __VA_ARGS__ ); \ - break; \ - case MessageTypes::UInt64: \ - _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( \ - FUNCTION, \ - (ARRAY.as::value, \ - BOUNDED, FIXEDLENGTH>>())__VA_OPT__(, ) __VA_ARGS__ ); \ - break; \ - case MessageTypes::Int8: \ - _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( \ - FUNCTION, \ - (ARRAY.as::value, \ - BOUNDED, FIXEDLENGTH>>())__VA_OPT__(, ) __VA_ARGS__ ); \ - break; \ - case MessageTypes::Int16: \ - _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( \ - FUNCTION, \ - (ARRAY.as::value, \ - BOUNDED, FIXEDLENGTH>>())__VA_OPT__(, ) __VA_ARGS__ ); \ - break; \ - case MessageTypes::Int32: \ - _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( \ - FUNCTION, \ - (ARRAY.as::value, \ - BOUNDED, FIXEDLENGTH>>())__VA_OPT__(, ) __VA_ARGS__ ); \ - break; \ - case MessageTypes::Int64: \ - _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( \ - FUNCTION, \ - (ARRAY.as::value, \ - BOUNDED, FIXEDLENGTH>>())__VA_OPT__(, ) __VA_ARGS__ ); \ - break; \ - case MessageTypes::Float: \ - _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( \ - FUNCTION, \ - (ARRAY.as::value, \ - BOUNDED, FIXEDLENGTH>>())__VA_OPT__(, ) __VA_ARGS__ ); \ - break; \ - case MessageTypes::Double: \ - _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( \ - FUNCTION, \ - (ARRAY.as::value, \ - BOUNDED, FIXEDLENGTH>>())__VA_OPT__(, ) __VA_ARGS__ ); \ - break; \ - case MessageTypes::LongDouble: \ - _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( \ - FUNCTION, \ - (ARRAY.as::value, \ - BOUNDED, FIXEDLENGTH>>())__VA_OPT__(, ) __VA_ARGS__ ); \ - break; \ - case MessageTypes::Char: \ - _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( \ - FUNCTION, \ - (ARRAY.as::value, \ - BOUNDED, FIXEDLENGTH>>())__VA_OPT__(, ) __VA_ARGS__ ); \ - break; \ - case MessageTypes::WChar: \ - _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( \ - FUNCTION, \ - (ARRAY.as::value, \ - BOUNDED, FIXEDLENGTH>>())__VA_OPT__(, ) __VA_ARGS__ ); \ - break; \ - case MessageTypes::String: \ - _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( \ - FUNCTION, \ - (ARRAY.as::value, \ - BOUNDED, FIXEDLENGTH>>())__VA_OPT__(, ) __VA_ARGS__ ); \ - break; \ - case MessageTypes::WString: \ - _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( \ - FUNCTION, \ - (ARRAY.as::value, \ - BOUNDED, FIXEDLENGTH>>())__VA_OPT__(, ) __VA_ARGS__ ); \ - break; \ - case MessageTypes::Compound: \ - _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( \ - FUNCTION, \ - (ARRAY.as<::ros2_babel_fish::CompoundArrayMessage_>())__VA_OPT__(, ) \ - __VA_ARGS__ ); \ - break; \ - case MessageTypes::Array: \ - case MessageTypes::None: \ - break; /* Do nothing except tell the compiler we know about these types. */ \ +#define _RBF2_TEMPLATE_CALL_ARRAY_TYPES(FUNCTION, ARRAY, BOUNDED, FIXEDLENGTH, ...) \ + switch (ARRAY.elementType()) { \ + case MessageTypes::Bool: \ + _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( \ + FUNCTION, \ + (ARRAY.as::value, \ + BOUNDED, FIXEDLENGTH>>())__VA_OPT__(, ) __VA_ARGS__); \ + break; \ + case MessageTypes::Octet: \ + _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( \ + FUNCTION, (ARRAY.as::value, \ + BOUNDED, FIXEDLENGTH>>())__VA_OPT__(, ) __VA_ARGS__); \ + break; \ + case MessageTypes::UInt8: \ + _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( \ + FUNCTION, (ARRAY.as::value, \ + BOUNDED, FIXEDLENGTH>>())__VA_OPT__(, ) __VA_ARGS__); \ + break; \ + case MessageTypes::UInt16: \ + _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( \ + FUNCTION, (ARRAY.as::value, \ + BOUNDED, FIXEDLENGTH>>())__VA_OPT__(, ) __VA_ARGS__); \ + break; \ + case MessageTypes::UInt32: \ + _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( \ + FUNCTION, (ARRAY.as::value, \ + BOUNDED, FIXEDLENGTH>>())__VA_OPT__(, ) __VA_ARGS__); \ + break; \ + case MessageTypes::UInt64: \ + _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( \ + FUNCTION, (ARRAY.as::value, \ + BOUNDED, FIXEDLENGTH>>())__VA_OPT__(, ) __VA_ARGS__); \ + break; \ + case MessageTypes::Int8: \ + _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( \ + FUNCTION, \ + (ARRAY.as::value, \ + BOUNDED, FIXEDLENGTH>>())__VA_OPT__(, ) __VA_ARGS__); \ + break; \ + case MessageTypes::Int16: \ + _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( \ + FUNCTION, (ARRAY.as::value, \ + BOUNDED, FIXEDLENGTH>>())__VA_OPT__(, ) __VA_ARGS__); \ + break; \ + case MessageTypes::Int32: \ + _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( \ + FUNCTION, (ARRAY.as::value, \ + BOUNDED, FIXEDLENGTH>>())__VA_OPT__(, ) __VA_ARGS__); \ + break; \ + case MessageTypes::Int64: \ + _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( \ + FUNCTION, (ARRAY.as::value, \ + BOUNDED, FIXEDLENGTH>>())__VA_OPT__(, ) __VA_ARGS__); \ + break; \ + case MessageTypes::Float: \ + _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( \ + FUNCTION, (ARRAY.as::value, \ + BOUNDED, FIXEDLENGTH>>())__VA_OPT__(, ) __VA_ARGS__); \ + break; \ + case MessageTypes::Double: \ + _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( \ + FUNCTION, (ARRAY.as::value, \ + BOUNDED, FIXEDLENGTH>>())__VA_OPT__(, ) __VA_ARGS__); \ + break; \ + case MessageTypes::LongDouble: \ + _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( \ + FUNCTION, (ARRAY.as::value, \ + BOUNDED, FIXEDLENGTH>>())__VA_OPT__(, ) __VA_ARGS__); \ + break; \ + case MessageTypes::Char: \ + _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( \ + FUNCTION, \ + (ARRAY.as::value, \ + BOUNDED, FIXEDLENGTH>>())__VA_OPT__(, ) __VA_ARGS__); \ + break; \ + case MessageTypes::WChar: \ + _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( \ + FUNCTION, (ARRAY.as::value, \ + BOUNDED, FIXEDLENGTH>>())__VA_OPT__(, ) __VA_ARGS__); \ + break; \ + case MessageTypes::String: \ + _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( \ + FUNCTION, (ARRAY.as::value, \ + BOUNDED, FIXEDLENGTH>>())__VA_OPT__(, ) __VA_ARGS__); \ + break; \ + case MessageTypes::WString: \ + _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( \ + FUNCTION, (ARRAY.as::value, \ + BOUNDED, FIXEDLENGTH>>())__VA_OPT__(, ) __VA_ARGS__); \ + break; \ + case MessageTypes::Compound: \ + _RBF2_TEMPLATE_CALL_ARRAY_FUNCTION( \ + FUNCTION, \ + (ARRAY.as<::ros2_babel_fish::CompoundArrayMessage_>())__VA_OPT__(, ) \ + __VA_ARGS__); \ + break; \ + case MessageTypes::Array: \ + case MessageTypes::None: \ + break; /* Do nothing except tell the compiler we know about these types. */ \ } /*! @@ -379,21 +388,22 @@ * fn( array, ... ) * where array can be ::ros2_babel_fish::ArrayMessage_ * or ::ros2_babel_fish::CompoundArrayMessage_ - * and BOUNDED, FIXED_LENGTH are booleans indicating whether the array is bounded or fixed_length (which implies bounded). + * and BOUNDED, FIXED_LENGTH are booleans indicating whether the array is bounded or fixed_length + * (which implies bounded). */ -#define RBF2_TEMPLATE_CALL_ARRAY_TYPES( FUNCTION, ARRAY, ... ) \ - do { \ - static_assert( \ - ::std::is_same<::ros2_babel_fish::ArrayMessageBase, \ - std::remove_const::type>::type>::value, \ - "Second argument to macro needs to be of type ArrayMessageBase!" ); \ - if ( ( ARRAY ).isFixedSize() ) { \ - _RBF2_TEMPLATE_CALL_ARRAY_TYPES( FUNCTION, ( ARRAY ), true, true, __VA_ARGS__ ) \ - } else if ( ( ARRAY ).isBounded() ) { \ - _RBF2_TEMPLATE_CALL_ARRAY_TYPES( FUNCTION, ( ARRAY ), true, false, __VA_ARGS__ ) \ - } else { \ - _RBF2_TEMPLATE_CALL_ARRAY_TYPES( FUNCTION, ( ARRAY ), false, false, __VA_ARGS__ ) \ - } \ - } while ( false ) +#define RBF2_TEMPLATE_CALL_ARRAY_TYPES(FUNCTION, ARRAY, ...) \ + do { \ + static_assert(::std::is_same< \ + ::ros2_babel_fish::ArrayMessageBase, \ + std::remove_const::type>::type>::value, \ + "Second argument to macro needs to be of type ArrayMessageBase!"); \ + if ((ARRAY).isFixedSize()) { \ + _RBF2_TEMPLATE_CALL_ARRAY_TYPES(FUNCTION, (ARRAY), true, true, __VA_ARGS__) \ + } else if ((ARRAY).isBounded()) { \ + _RBF2_TEMPLATE_CALL_ARRAY_TYPES(FUNCTION, (ARRAY), true, false, __VA_ARGS__) \ + } else { \ + _RBF2_TEMPLATE_CALL_ARRAY_TYPES(FUNCTION, (ARRAY), false, false, __VA_ARGS__) \ + } \ + } while (false) -#endif // ROS2_BABEL_FISH_MACROS_HPP +#endif // ROS2_BABEL_FISH_MACROS_HPP diff --git a/ros2_foxglove_bridge/include/ros2_babel_fish/messages/array_message.hpp b/ros2_foxglove_bridge/include/ros2_babel_fish/messages/array_message.hpp index 7e8ba6d..7a950f6 100644 --- a/ros2_foxglove_bridge/include/ros2_babel_fish/messages/array_message.hpp +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/messages/array_message.hpp @@ -1,510 +1,493 @@ // Copyright (c) 2021 Stefan Fabian. All rights reserved. -// Licensed under the MIT license. See LICENSE file in the project root for full license information. +// Licensed under the MIT license. See LICENSE file in the project root for full license +// information. #ifndef ROS2_BABEL_FISH_ARRAY_MESSAGE_HPP #define ROS2_BABEL_FISH_ARRAY_MESSAGE_HPP -#include "message_type_traits.hpp" -#include "ros2_babel_fish/exceptions/babel_fish_exception.hpp" -#include "ros2_babel_fish/messages/compound_message.hpp" +#include #include #include -#include +#include "message_type_traits.hpp" +#include "ros2_babel_fish/exceptions/babel_fish_exception.hpp" +#include "ros2_babel_fish/messages/compound_message.hpp" -namespace ros2_babel_fish -{ -class ArrayMessageBase : public Message -{ +namespace ros2_babel_fish { +class ArrayMessageBase : public Message { public: - RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE( ArrayMessageBase ) + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ArrayMessageBase) - ArrayMessageBase( MessageMemberIntrospection member, std::shared_ptr data ) - : Message( MessageTypes::Array, std::move( data ) ), member_( std::move( member ) ) - { - } + ArrayMessageBase(MessageMemberIntrospection member, std::shared_ptr data) + : Message(MessageTypes::Array, std::move(data)) + , member_(std::move(member)) {} - bool isFixedSize() const { return member_->array_size_ != 0 && !member_->is_upper_bound_; } + bool isFixedSize() const { + return member_->array_size_ != 0 && !member_->is_upper_bound_; + } - bool isBounded() const { return member_->is_upper_bound_; } + bool isBounded() const { + return member_->is_upper_bound_; + } - MessageType elementType() const { return MessageType( member_->type_id_ ); } + MessageType elementType() const { + return MessageType(member_->type_id_); + } - virtual size_t size() const { return member_->size_function( data_.get() ); } + virtual size_t size() const { + return member_->size_function(data_.get()); + } - //! @return The maximum size of the array if it is bounded or fixed size, otherwise, it will return 0. - size_t maxSize() const { return member_->array_size_; } + //! @return The maximum size of the array if it is bounded or fixed size, otherwise, it will + //! return 0. + size_t maxSize() const { + return member_->array_size_; + } - ArrayMessageBase &operator=( const ArrayMessageBase &other ) - { - if ( this == &other ) - return *this; - if ( elementType() != other.elementType() ) - throw BabelFishException( std::string( "Incompatible array types! (" ) + - std::to_string( elementType() ) + " vs " + - std::to_string( other.elementType() ) + ")" ); - _assign( other ); + ArrayMessageBase& operator=(const ArrayMessageBase& other) { + if (this == &other) return *this; + if (elementType() != other.elementType()) + throw BabelFishException(std::string("Incompatible array types! (") + + std::to_string(elementType()) + " vs " + + std::to_string(other.elementType()) + ")"); + _assign(other); return *this; } - MessageMemberIntrospection elementIntrospection() const { return member_; } + MessageMemberIntrospection elementIntrospection() const { + return member_; + } protected: // Disable copy construction except for subclasses - ArrayMessageBase( const ArrayMessageBase &other ) : Message( other ), member_( other.member_ ) { } + ArrayMessageBase(const ArrayMessageBase& other) + : Message(other) + , member_(other.member_) {} - void _assign( const Message &other ) override - { - if ( other.type() != MessageTypes::Array ) - throw BabelFishException( "Tried to assign non-array message to array message!" ); - *this = static_cast( other ); + void _assign(const Message& other) override { + if (other.type() != MessageTypes::Array) + throw BabelFishException("Tried to assign non-array message to array message!"); + *this = static_cast(other); } - virtual void _assign( const ArrayMessageBase &other ) = 0; + virtual void _assign(const ArrayMessageBase& other) = 0; MessageMemberIntrospection member_; }; -template -class ArrayMessage_ final : public ArrayMessageBase -{ +template +class ArrayMessage_ final : public ArrayMessageBase { protected: typedef typename message_type_traits::array_type::Reference Reference; typedef typename message_type_traits::array_type::ReturnType ReturnType; - typedef typename message_type_traits::array_type::ConstReturnType ConstReturnType; + typedef + typename message_type_traits::array_type::ConstReturnType ConstReturnType; typedef typename message_type_traits::array_type::ArgumentType ArgumentType; - static_assert( ( FIXED_LENGTH && BOUNDED ) || !FIXED_LENGTH, - "Fixed length can only be true if bounded is also true!" ); + static_assert((FIXED_LENGTH && BOUNDED) || !FIXED_LENGTH, + "Fixed length can only be true if bounded is also true!"); public: - RCLCPP_SMART_PTR_DEFINITIONS( ArrayMessage_ ) + RCLCPP_SMART_PTR_DEFINITIONS(ArrayMessage_) - ArrayMessage_( const ArrayMessage_ & ) = delete; + ArrayMessage_(const ArrayMessage_&) = delete; - explicit ArrayMessage_( MessageMemberIntrospection member, std::shared_ptr data ) - : ArrayMessageBase( std::move( member ), std::move( data ) ) - { - } + explicit ArrayMessage_(MessageMemberIntrospection member, std::shared_ptr data) + : ArrayMessageBase(std::move(member), std::move(data)) {} - ~ArrayMessage_() override { } + ~ArrayMessage_() override {} - template + template typename std::enable_if::value, Reference>::type - operator[]( size_t index ) - { + operator[](size_t index) { using Container = - typename std::conditional, std::vector>::type; - if ( member_->get_function == nullptr ) { - if ( index >= size() ) - throw std::out_of_range( "Index was out of range of array!" ); - return ( *reinterpret_cast( data_.get() ) )[index]; + typename std::conditional, std::vector>::type; + if (member_->get_function == nullptr) { + if (index >= size()) throw std::out_of_range("Index was out of range of array!"); + return (*reinterpret_cast(data_.get()))[index]; } - return *reinterpret_cast( member_->get_function( data_.get(), index ) ); + return *reinterpret_cast(member_->get_function(data_.get(), index)); } - template + template typename std::enable_if::value, Reference>::type - operator[]( size_t index ) - { + operator[](size_t index) { // Need to specialize for bool because std::vector is specialized and uses one bit per // bool, hence, you can not return a bool reference but need to use std::_Bit_reference - if ( index >= size() ) - throw std::out_of_range( "Index was out of range of array!" ); - return ( *reinterpret_cast *>( data_.get() ) )[index]; + if (index >= size()) throw std::out_of_range("Index was out of range of array!"); + return (*reinterpret_cast*>(data_.get()))[index]; } - ConstReturnType operator[]( size_t index ) const - { + ConstReturnType operator[](size_t index) const { using Container = - typename std::conditional, std::vector>::type; - if ( member_->get_function == nullptr ) { - if ( index >= size() ) - throw std::out_of_range( "Index was out of range of array!" ); - return ( *reinterpret_cast( data_.get() ) )[index]; + typename std::conditional, std::vector>::type; + if (member_->get_function == nullptr) { + if (index >= size()) throw std::out_of_range("Index was out of range of array!"); + return (*reinterpret_cast(data_.get()))[index]; } - return *reinterpret_cast( - member_->get_const_function( data_.get(), index ) ); + return *reinterpret_cast( + member_->get_const_function(data_.get(), index)); } - Reference at( size_t index ) { return operator[]( index ); } + Reference at(size_t index) { + return operator[](index); + } - ConstReturnType at( size_t index ) const { return operator[]( index ); } + ConstReturnType at(size_t index) const { + return operator[](index); + } /*! * @param index The index at which the array element is set/overwritten - * @param value The value with which the array element is overwritten, has to be the same as the element type. + * @param value The value with which the array element is overwritten, has to be the same as the + * element type. */ - void assign( size_t index, ArgumentType value ) { ( *this )[index] = value; } + void assign(size_t index, ArgumentType value) { + (*this)[index] = value; + } //! Method only for fixed length arrays to fill the array with the given value. - template - typename std::enable_if::type fill( ArgumentType &value ) - { - for ( size_t i = 0; i < maxSize(); ++i ) assign( i, value ); + template + typename std::enable_if::type fill(ArgumentType& value) { + for (size_t i = 0; i < maxSize(); ++i) assign(i, value); } //! Alias for assign - void replace( size_t index, ArgumentType value ) { assign( index, value ); } + void replace(size_t index, ArgumentType value) { + assign(index, value); + } - void push_back( ArgumentType value ) - { - if ( BOUNDED ) { - if ( FIXED_LENGTH ) - throw std::length_error( "Can not push back on fixed length array!" ); + void push_back(ArgumentType value) { + if (BOUNDED) { + if (FIXED_LENGTH) throw std::length_error("Can not push back on fixed length array!"); // This means it is upper bound, otherwise the method would not be enabled / available - if ( member_->array_size_ <= member_->size_function( data_.get() ) ) - throw std::length_error( "Exceeded upper bound!" ); + if (member_->array_size_ <= member_->size_function(data_.get())) + throw std::length_error("Exceeded upper bound!"); } - reinterpret_cast *>( data_.get() )->push_back( value ); + reinterpret_cast*>(data_.get())->push_back(value); } //! Alias for push_back - void append( ArgumentType value ) { push_back( value ); } + void append(ArgumentType value) { + push_back(value); + } - void pop_back() - { - if ( size() == 0 ) - return; - if ( FIXED_LENGTH ) - throw std::length_error( "Can not pop_back fixed length array!" ); - resize( size() - 1 ); + void pop_back() { + if (size() == 0) return; + if (FIXED_LENGTH) throw std::length_error("Can not pop_back fixed length array!"); + resize(size() - 1); } - void resize( size_t length ) - { - if ( BOUNDED ) { - if ( FIXED_LENGTH ) - throw std::length_error( "Can not resize fixed length array!" ); + void resize(size_t length) { + if (BOUNDED) { + if (FIXED_LENGTH) throw std::length_error("Can not resize fixed length array!"); // This means it is upper bound, otherwise the method would not be enabled / available - if ( member_->array_size_ < length ) - throw std::length_error( "Exceeded upper bound!" ); + if (member_->array_size_ < length) throw std::length_error("Exceeded upper bound!"); } - if ( member_->resize_function == nullptr ) - reinterpret_cast *>( data_.get() )->resize( length ); + if (member_->resize_function == nullptr) + reinterpret_cast*>(data_.get())->resize(length); else - member_->resize_function( data_.get(), length ); + member_->resize_function(data_.get(), length); } - size_t size() const override - { - if ( FIXED_LENGTH ) - return member_->array_size_; + size_t size() const override { + if (FIXED_LENGTH) return member_->array_size_; - if ( member_->size_function == nullptr ) - return reinterpret_cast *>( data_.get() )->size(); - return member_->size_function( data_.get() ); + if (member_->size_function == nullptr) + return reinterpret_cast*>(data_.get())->size(); + return member_->size_function(data_.get()); } - void clear() - { - if ( FIXED_LENGTH ) { - throw BabelFishException( "Can not clear fixed length array!" ); + void clear() { + if (FIXED_LENGTH) { + throw BabelFishException("Can not clear fixed length array!"); } else { - resize( 0 ); + resize(0); } } protected: - void _assign( const ArrayMessageBase &other ) override - { - if ( other.isBounded() ) { - if ( other.isFixedSize() ) { - _assignImpl( other ); + void _assign(const ArrayMessageBase& other) override { + if (other.isBounded()) { + if (other.isFixedSize()) { + _assignImpl(other); return; } - _assignImpl( other ); + _assignImpl(other); return; } - _assignImpl( other ); + _assignImpl(other); } - template - void _assignImpl( const ArrayMessageBase &other ) - { - auto &other_typed = dynamic_cast &>( other ); - if ( BOUNDED && other.size() > maxSize() ) { + template + void _assignImpl(const ArrayMessageBase& other) { + auto& other_typed = dynamic_cast&>(other); + if (BOUNDED && other.size() > maxSize()) { throw std::out_of_range( - "Can not assign ArrayMessage as it exceeds the maximum size of this ArrayMessage!" ); + "Can not assign ArrayMessage as it exceeds the maximum size of this ArrayMessage!"); } - if ( !FIXED_LENGTH ) - resize( other.size() ); - for ( size_t index = 0; index < other.size(); ++index ) at( index ) = other_typed.at( index ); + if (!FIXED_LENGTH) resize(other.size()); + for (size_t index = 0; index < other.size(); ++index) at(index) = other_typed.at(index); } - bool _isMessageEqual( const Message &o ) const override - { - const auto &other = o.as(); - if ( other.isBounded() ) { - if ( other.isFixedSize() ) { - return _isMessageEqualImpl( other ); + bool _isMessageEqual(const Message& o) const override { + const auto& other = o.as(); + if (other.isBounded()) { + if (other.isFixedSize()) { + return _isMessageEqualImpl(other); } - return _isMessageEqualImpl( other ); + return _isMessageEqualImpl(other); } - return _isMessageEqualImpl( other ); - } - - template - bool _isMessageEqualImpl( const ArrayMessageBase &other ) const - { - auto &other_typed = dynamic_cast &>( other ); - if ( size() != other.size() ) - return false; - for ( size_t index = 0; index < size(); ++index ) { - if ( at( index ) != other_typed.at( index ) ) - return false; + return _isMessageEqualImpl(other); + } + + template + bool _isMessageEqualImpl(const ArrayMessageBase& other) const { + auto& other_typed = dynamic_cast&>(other); + if (size() != other.size()) return false; + for (size_t index = 0; index < size(); ++index) { + if (at(index) != other_typed.at(index)) return false; } return true; } }; -template +template using ArrayMessage = ArrayMessage_; -template +template using FixedLengthArrayMessage = ArrayMessage_; -template +template using BoundedArrayMessage = ArrayMessage_; //! Specialization for CompoundMessage -template -class CompoundArrayMessage_ final : public ArrayMessageBase -{ - static_assert( ( FIXED_LENGTH && BOUNDED ) || !FIXED_LENGTH, - "Fixed length can only be true if bounded is also true!" ); +template +class CompoundArrayMessage_ final : public ArrayMessageBase { + static_assert((FIXED_LENGTH && BOUNDED) || !FIXED_LENGTH, + "Fixed length can only be true if bounded is also true!"); public: - RCLCPP_SMART_PTR_DEFINITIONS( CompoundArrayMessage_ ) + RCLCPP_SMART_PTR_DEFINITIONS(CompoundArrayMessage_) /*! - * Creates a compound array, i.e., an array of compound messages in contrast to arrays of primitives such as int, bool etc. - * If length != 0, the array is initialized with the given number of empty messages created according to the given - * MessageTemplate. + * Creates a compound array, i.e., an array of compound messages in contrast to arrays of + * primitives such as int, bool etc. If length != 0, the array is initialized with the given + * number of empty messages created according to the given MessageTemplate. * * @param msg_template The template for the CompoundMessage * @param length The length of the array. - * @param fixed_length Whether the array has fixed length or elements can be added / removed dynamically. - * @param init Initialize the elements if length is greater than 0, if false, all elements in the container will be nullptr. + * @param fixed_length Whether the array has fixed length or elements can be added / removed + * dynamically. + * @param init Initialize the elements if length is greater than 0, if false, all elements in the + * container will be nullptr. */ - explicit CompoundArrayMessage_( MessageMemberIntrospection member, std::shared_ptr data ) - : ArrayMessageBase( std::move( member ), std::move( data ) ) - { - values_.resize( member_->size_function( data_.get() ) ); + explicit CompoundArrayMessage_(MessageMemberIntrospection member, std::shared_ptr data) + : ArrayMessageBase(std::move(member), std::move(data)) { + values_.resize(member_->size_function(data_.get())); } - ~CompoundArrayMessage_() override { } + ~CompoundArrayMessage_() override {} - CompoundMessage &operator[]( size_t index ) { return getImplementation( index ); } + CompoundMessage& operator[](size_t index) { + return getImplementation(index); + } - const CompoundMessage &operator[]( size_t index ) const { return getImplementation( index ); } + const CompoundMessage& operator[](size_t index) const { + return getImplementation(index); + } - CompoundMessage &at( size_t index ) { return getImplementation( index ); } + CompoundMessage& at(size_t index) { + return getImplementation(index); + } - const CompoundMessage &at( size_t index ) const { return getImplementation( index ); } + const CompoundMessage& at(size_t index) const { + return getImplementation(index); + } /*! * @param index The index at which the array element is set/overwritten - * @param value The value with which the array element is overwritten, has to be the same as the element type. + * @param value The value with which the array element is overwritten, has to be the same as the + * element type. */ - virtual void assign( size_t index, const CompoundMessage &value ) - { - getImplementation( index ) = value; + virtual void assign(size_t index, const CompoundMessage& value) { + getImplementation(index) = value; } //! Alias for _assign - void replace( size_t index, const CompoundMessage &value ) { assign( index, value ); } - - void push_back( const CompoundMessage &value ) - { - if ( BOUNDED ) { - if ( FIXED_LENGTH ) - throw std::length_error( "Can not push back on fixed length array!" ); - if ( member_->array_size_ <= member_->size_function( data_.get() ) ) - throw std::length_error( "Exceeded upper bound!" ); + void replace(size_t index, const CompoundMessage& value) { + assign(index, value); + } + + void push_back(const CompoundMessage& value) { + if (BOUNDED) { + if (FIXED_LENGTH) throw std::length_error("Can not push back on fixed length array!"); + if (member_->array_size_ <= member_->size_function(data_.get())) + throw std::length_error("Exceeded upper bound!"); } const size_t index = size(); - resize( index + 1 ); - assign( index, value ); + resize(index + 1); + assign(index, value); } //! Alias for push_back - void append( const CompoundMessage &value ) { push_back( value ); } + void append(const CompoundMessage& value) { + push_back(value); + } //! Creates a new CompoundMessage, appends it and returns a reference to it. - CompoundMessage &appendEmpty() - { + CompoundMessage& appendEmpty() { size_t index = size(); - resize( index + 1 ); - return getImplementation( index ); + resize(index + 1); + return getImplementation(index); } - void pop_back() - { - if ( size() == 0 ) - return; - if ( FIXED_LENGTH ) - throw std::length_error( "Can not pop_back fixed length array!" ); - resize( size() - 1 ); + void pop_back() { + if (size() == 0) return; + if (FIXED_LENGTH) throw std::length_error("Can not pop_back fixed length array!"); + resize(size() - 1); } - void resize( size_t length ) - { - if ( length == values_.size() ) - return; - if ( BOUNDED ) { - if ( FIXED_LENGTH ) - throw std::length_error( "Can not resize fixed length array!" ); - if ( member_->array_size_ < length ) - throw std::length_error( "Exceeded upper bound!" ); + void resize(size_t length) { + if (length == values_.size()) return; + if (BOUNDED) { + if (FIXED_LENGTH) throw std::length_error("Can not resize fixed length array!"); + if (member_->array_size_ < length) throw std::length_error("Exceeded upper bound!"); } - member_->resize_function( data_.get(), length ); - values_.resize( length ); + member_->resize_function(data_.get(), length); + values_.resize(length); // Container may have reallocated -> content could be moved - for ( size_t i = 0; i < values_.size(); ++i ) { - if ( values_[i] == nullptr ) - continue; - void *p = member_->get_function( data_.get(), i ); - if ( p == values_[i]->data_.get() ) - return; // Content was not moved - std::shared_ptr data( p, [parent = data_]( void * ) { (void)parent; } ); - values_[i]->move( data ); + for (size_t i = 0; i < values_.size(); ++i) { + if (values_[i] == nullptr) continue; + void* p = member_->get_function(data_.get(), i); + if (p == values_[i]->data_.get()) return; // Content was not moved + std::shared_ptr data(p, [parent = data_](void*) { + (void)parent; + }); + values_[i]->move(data); } } - std::vector values() - { + std::vector values() { ensureInitialized(); return values_; } - std::vector values() const - { + std::vector values() const { ensureInitialized(); - return { values_.begin(), values_.end() }; + return {values_.begin(), values_.end()}; } - template - CompoundArrayMessage_ &operator=( const CompoundArrayMessage_ &other ) - { - if ( this == &other ) - return *this; - _assignImpl( other ); + template + CompoundArrayMessage_& operator=( + const CompoundArrayMessage_& other) { + if (this == &other) return *this; + _assignImpl(other); return *this; } - void clear() - { - if ( FIXED_LENGTH ) { - throw BabelFishException( "Can not clear fixed length array!" ); + void clear() { + if (FIXED_LENGTH) { + throw BabelFishException("Can not clear fixed length array!"); } else { - member_->resize_function( data_.get(), 0 ); + member_->resize_function(data_.get(), 0); values_.clear(); } } protected: - void onMoved() override - { - for ( size_t i = 0; i < values_.size(); ++i ) { - if ( values_[i] == nullptr ) - continue; - void *p = member_->get_function( data_.get(), i ); - if ( p == values_[i]->data_.get() ) - continue; // No need to move - std::shared_ptr data( p, [parent = data_]( void * ) { (void)parent; } ); - values_[i]->move( data ); + void onMoved() override { + for (size_t i = 0; i < values_.size(); ++i) { + if (values_[i] == nullptr) continue; + void* p = member_->get_function(data_.get(), i); + if (p == values_[i]->data_.get()) continue; // No need to move + std::shared_ptr data(p, [parent = data_](void*) { + (void)parent; + }); + values_[i]->move(data); } } private: - void ensureInitialized() const - { - for ( int index = 0; index < values_.size(); ++index ) { - if ( values_[index] != nullptr ) - continue; - - void *p = member_->get_function( data_.get(), index ); - std::shared_ptr data( p, [parent = data_]( void * ) { (void)parent; } ); - values_[index] = CompoundMessage::make_shared( member_, std::move( data ) ); + void ensureInitialized() const { + for (int index = 0; index < values_.size(); ++index) { + if (values_[index] != nullptr) continue; + + void* p = member_->get_function(data_.get(), index); + std::shared_ptr data(p, [parent = data_](void*) { + (void)parent; + }); + values_[index] = CompoundMessage::make_shared(member_, std::move(data)); } } - void ensureInitialized( size_t index ) const - { - if ( index >= values_.size() ) { - size_t size = member_->size_function( data_.get() ); - if ( index >= size ) - throw std::out_of_range( "Index was out of range of compound array!" ); - values_.resize( size ); + void ensureInitialized(size_t index) const { + if (index >= values_.size()) { + size_t size = member_->size_function(data_.get()); + if (index >= size) throw std::out_of_range("Index was out of range of compound array!"); + values_.resize(size); } - if ( values_[index] == nullptr ) { - void *p = member_->get_function( data_.get(), index ); - std::shared_ptr data( p, [parent = data_]( void * ) { (void)parent; } ); - values_[index] = CompoundMessage::make_shared( member_, std::move( data ) ); + if (values_[index] == nullptr) { + void* p = member_->get_function(data_.get(), index); + std::shared_ptr data(p, [parent = data_](void*) { + (void)parent; + }); + values_[index] = CompoundMessage::make_shared(member_, std::move(data)); } } - CompoundMessage &getImplementation( size_t index ) - { - ensureInitialized( index ); + CompoundMessage& getImplementation(size_t index) { + ensureInitialized(index); return *values_[index]; } - const CompoundMessage &getImplementation( size_t index ) const - { - ensureInitialized( index ); + const CompoundMessage& getImplementation(size_t index) const { + ensureInitialized(index); return *values_[index]; } - void _assign( const ArrayMessageBase &other ) override - { - if ( other.isBounded() ) { - if ( other.isFixedSize() ) { - _assignImpl( other ); + void _assign(const ArrayMessageBase& other) override { + if (other.isBounded()) { + if (other.isFixedSize()) { + _assignImpl(other); return; } - _assignImpl( other ); + _assignImpl(other); return; } - _assignImpl( other ); + _assignImpl(other); } - template - void _assignImpl( const ArrayMessageBase &other ) - { - auto &other_typed = static_cast &>( other ); - if ( BOUNDED && other.size() > maxSize() ) { - throw std::out_of_range( "Can not assign CompoundArrayMessage as it exceeds the maximum size " - "of this CompoundArrayMessage!" ); + template + void _assignImpl(const ArrayMessageBase& other) { + auto& other_typed = static_cast&>(other); + if (BOUNDED && other.size() > maxSize()) { + throw std::out_of_range( + "Can not assign CompoundArrayMessage as it exceeds the maximum size " + "of this CompoundArrayMessage!"); } - if ( !FIXED_LENGTH ) - resize( other.size() ); - for ( size_t index = 0; index < other.size(); ++index ) at( index ) = other_typed.at( index ); + if (!FIXED_LENGTH) resize(other.size()); + for (size_t index = 0; index < other.size(); ++index) at(index) = other_typed.at(index); } - bool _isMessageEqual( const Message &o ) const override - { - const auto &other = o.as(); - if ( other.isBounded() ) { - if ( other.isFixedSize() ) { - return _isMessageEqualImpl( other ); + bool _isMessageEqual(const Message& o) const override { + const auto& other = o.as(); + if (other.isBounded()) { + if (other.isFixedSize()) { + return _isMessageEqualImpl(other); } - return _isMessageEqualImpl( other ); + return _isMessageEqualImpl(other); } - return _isMessageEqualImpl( other ); - } - - template - bool _isMessageEqualImpl( const ArrayMessageBase &other ) const - { - auto &other_typed = dynamic_cast &>( other ); - if ( size() != other.size() ) - return false; - for ( size_t index = 0; index < size(); ++index ) { - if ( at( index ) != other_typed.at( index ) ) - return false; + return _isMessageEqualImpl(other); + } + + template + bool _isMessageEqualImpl(const ArrayMessageBase& other) const { + auto& other_typed = dynamic_cast&>(other); + if (size() != other.size()) return false; + for (size_t index = 0; index < size(); ++index) { + if (at(index) != other_typed.at(index)) return false; } return true; } @@ -517,6 +500,6 @@ using CompoundArrayMessage = CompoundArrayMessage_; using FixedLengthCompoundArrayMessage = CompoundArrayMessage_; using BoundedCompoundArrayMessage = CompoundArrayMessage_; -} // namespace ros2_babel_fish +} // namespace ros2_babel_fish -#endif // ROS2_BABEL_FISH_ARRAY_MESSAGE_HPP +#endif // ROS2_BABEL_FISH_ARRAY_MESSAGE_HPP diff --git a/ros2_foxglove_bridge/include/ros2_babel_fish/messages/compound_message.hpp b/ros2_foxglove_bridge/include/ros2_babel_fish/messages/compound_message.hpp index 2dde1ef..d1c86bb 100644 --- a/ros2_foxglove_bridge/include/ros2_babel_fish/messages/compound_message.hpp +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/messages/compound_message.hpp @@ -1,37 +1,37 @@ // Copyright (c) 2021 Stefan Fabian. All rights reserved. -// Licensed under the MIT license. See LICENSE file in the project root for full license information. +// Licensed under the MIT license. See LICENSE file in the project root for full license +// information. #ifndef ROS2_BABEL_FISH_COMPOUND_MESSAGE_HPP #define ROS2_BABEL_FISH_COMPOUND_MESSAGE_HPP -#include "message.hpp" -#include "ros2_babel_fish/idl/type_support.hpp" +#include + #include -#include +#include "message.hpp" +#include "ros2_babel_fish/idl/type_support.hpp" -namespace ros2_babel_fish -{ +namespace ros2_babel_fish { -class CompoundMessage final : public Message -{ +class CompoundMessage final : public Message { public: - RCLCPP_SMART_PTR_DEFINITIONS( CompoundMessage ) + RCLCPP_SMART_PTR_DEFINITIONS(CompoundMessage) //! Creates an invalid instance of a compound message CompoundMessage(); - CompoundMessage( MessageMembersIntrospection members, std::shared_ptr data ); + CompoundMessage(MessageMembersIntrospection members, std::shared_ptr data); - explicit CompoundMessage( MessageMembersIntrospection members, - rosidl_runtime_cpp::MessageInitialization init = - rosidl_runtime_cpp::MessageInitialization::ALL ); + explicit CompoundMessage(MessageMembersIntrospection members, + rosidl_runtime_cpp::MessageInitialization init = + rosidl_runtime_cpp::MessageInitialization::ALL); //! Creates a copy of the CompoundMessage pointing at the same message. //! Use clone() to get an independent copy of this message. - CompoundMessage( const CompoundMessage &other ); + CompoundMessage(const CompoundMessage& other); - CompoundMessage( CompoundMessage && ) = default; + CompoundMessage(CompoundMessage&&) = default; ~CompoundMessage() override = default; @@ -41,27 +41,29 @@ class CompoundMessage final : public Message //! Name of the message, e.g., geometry_msgs/msg/Pose. std::string name() const; - bool containsKey( const std::string &key ) const; + bool containsKey(const std::string& key) const; std::vector keys() const; - std::string keyAt( size_t index ); + std::string keyAt(size_t index); - uint32_t memberCount() const { return members_->member_count_; } + uint32_t memberCount() const { + return members_->member_count_; + } - CompoundMessage &operator=( const Message &other ); + CompoundMessage& operator=(const Message& other); - CompoundMessage &operator=( const builtin_interfaces::msg::Time &value ); + CompoundMessage& operator=(const builtin_interfaces::msg::Time& value); - CompoundMessage &operator=( const builtin_interfaces::msg::Duration &value ); + CompoundMessage& operator=(const builtin_interfaces::msg::Duration& value); - CompoundMessage &operator=( const rclcpp::Time &value ); + CompoundMessage& operator=(const rclcpp::Time& value); - CompoundMessage &operator=( const rclcpp::Duration &value ); + CompoundMessage& operator=(const rclcpp::Duration& value); - CompoundMessage &operator=( const CompoundMessage &other ); + CompoundMessage& operator=(const CompoundMessage& other); - CompoundMessage &operator=( CompoundMessage &&other ) noexcept; + CompoundMessage& operator=(CompoundMessage&& other) noexcept; /*! * Accesses submessage with the given key. @@ -70,9 +72,9 @@ class CompoundMessage final : public Message * @param key The name of the member you want to access. * @return The value of the member with the given name. */ - Message &operator[]( const std::string &key ) override; + Message& operator[](const std::string& key) override; - const Message &operator[]( const std::string &key ) const override; + const Message& operator[](const std::string& key) const override; std::vector values(); @@ -92,18 +94,18 @@ class CompoundMessage final : public Message void initValues() const; - Message::SharedPtr operator[]( size_t index ); + Message::SharedPtr operator[](size_t index); - Message::ConstSharedPtr operator[]( size_t index ) const; + Message::ConstSharedPtr operator[](size_t index) const; - bool _isMessageEqual( const Message &other ) const override; + bool _isMessageEqual(const Message& other) const override; - void _assign( const Message &other ) override; + void _assign(const Message& other) override; MessageMembersIntrospection members_; mutable std::vector values_; mutable bool initialized_values_ = false; }; -} // namespace ros2_babel_fish +} // namespace ros2_babel_fish -#endif // ROS2_BABEL_FISH_COMPOUND_MESSAGE_HPP +#endif // ROS2_BABEL_FISH_COMPOUND_MESSAGE_HPP diff --git a/ros2_foxglove_bridge/include/ros2_babel_fish/messages/message.hpp b/ros2_foxglove_bridge/include/ros2_babel_fish/messages/message.hpp index fa48db4..44e2376 100644 --- a/ros2_foxglove_bridge/include/ros2_babel_fish/messages/message.hpp +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/messages/message.hpp @@ -1,37 +1,38 @@ // Copyright (c) 2021 Stefan Fabian. All rights reserved. -// Licensed under the MIT license. See LICENSE file in the project root for full license information. +// Licensed under the MIT license. See LICENSE file in the project root for full license +// information. #ifndef ROS2_BABEL_FISH_MESSAGE_HPP #define ROS2_BABEL_FISH_MESSAGE_HPP -#include "ros2_babel_fish/exceptions/babel_fish_exception.hpp" -#include "ros2_babel_fish/macros.hpp" +#include #include #include #include #include -#include +#include "ros2_babel_fish/exceptions/babel_fish_exception.hpp" +#include "ros2_babel_fish/macros.hpp" -namespace ros2_babel_fish -{ +namespace ros2_babel_fish { /*! * Message representation used by BabelFish. * Wraps the memory of an actual type erased ROS2 message. * Changes will affect the message that can be sent using the ROS runtime. */ -class Message -{ +class Message { public: - RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE( Message ) + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Message) - explicit Message( MessageType type, std::shared_ptr data ); + explicit Message(MessageType type, std::shared_ptr data); virtual ~Message(); - MessageType type() const { return type_; } + MessageType type() const { + return type_; + } /** * Convenience method to obtain the content of a ValueMessage as the given type. @@ -45,12 +46,10 @@ class Message * @throws BabelFishException If the type of the ValueMessage can not be casted to a different * type which is the case for bool, std::string, ros::Time and ros::Duration */ - template - T value() const - { - auto result = std::dynamic_pointer_cast( data_ ); - if ( !result ) - throw BabelFishException( "Invalid cast!" ); + template + T value() const { + auto result = std::dynamic_pointer_cast(data_); + if (!result) throw BabelFishException("Invalid cast!"); return *result; } @@ -66,29 +65,27 @@ class Message * * @throws BabelFishException If the message can not be casted to the target type */ - template - T &as() - { - T *result = dynamic_cast( this ); - if ( result == nullptr ) - throw BabelFishException( "Tried to cast message to incompatible type!" ); + template + T& as() { + T* result = dynamic_cast(this); + if (result == nullptr) throw BabelFishException("Tried to cast message to incompatible type!"); return *result; } //! @copydoc Message::as() - template - const T &as() const - { - const T *result = dynamic_cast( this ); - if ( result == nullptr ) - throw BabelFishException( "Tried to cast message to incompatible type!" ); + template + const T& as() const { + const T* result = dynamic_cast(this); + if (result == nullptr) throw BabelFishException("Tried to cast message to incompatible type!"); return *result; } - //! @return True if this message is a builtin_interfaces/Time message and can be cast to rclcpp::Time using the value method. + //! @return True if this message is a builtin_interfaces/Time message and can be cast to + //! rclcpp::Time using the value method. bool isTime() const; - //! @return True if this message is a builtin_interfaces/Duration message and can be cast to rclcpp::Duration using the value method. + //! @return True if this message is a builtin_interfaces/Duration message and can be cast to + //! rclcpp::Duration using the value method. bool isDuration() const; /** @@ -98,98 +95,103 @@ class Message * * @throws BabelFishException If child access by key is not supported. */ - virtual Message &operator[]( const std::string &key ); + virtual Message& operator[](const std::string& key); //!@copydoc Message::operator[](const std::string&) - virtual const Message &operator[]( const std::string &key ) const; + virtual const Message& operator[](const std::string& key) const; /** * @defgroup Convenience methods for ValueMessage * @brief Will try to set the value of ValueMessage to the given value. - * Incompatible types are checked at runtime. If the type of ValueMessage can not fit the passed value prints an error. + * Incompatible types are checked at runtime. If the type of ValueMessage can not fit the passed + * value prints an error. * - * @throws BabelFishException If bool is assigned to non-boolean ValueMessage or non-boolean value to bool ValueMessage - * @throws BabelFishException If ros::Time / ros::Duration value is set to a different type of ValueMessage. + * @throws BabelFishException If bool is assigned to non-boolean ValueMessage or non-boolean value + * to bool ValueMessage + * @throws BabelFishException If ros::Time / ros::Duration value is set to a different type of + * ValueMessage. * @{ */ - Message &operator=( bool value ); + Message& operator=(bool value); - Message &operator=( uint8_t value ); + Message& operator=(uint8_t value); - Message &operator=( uint16_t value ); + Message& operator=(uint16_t value); - Message &operator=( uint32_t value ); + Message& operator=(uint32_t value); - Message &operator=( uint64_t value ); + Message& operator=(uint64_t value); - Message &operator=( int8_t value ); + Message& operator=(int8_t value); - Message &operator=( int16_t value ); + Message& operator=(int16_t value); - Message &operator=( int32_t value ); + Message& operator=(int32_t value); - Message &operator=( int64_t value ); + Message& operator=(int64_t value); - Message &operator=( float value ); + Message& operator=(float value); - Message &operator=( double value ); + Message& operator=(double value); - Message &operator=( long double value ); + Message& operator=(long double value); - Message &operator=( const char *value ); + Message& operator=(const char* value); - Message &operator=( const std::string &value ); + Message& operator=(const std::string& value); - Message &operator=( const std::wstring &value ); + Message& operator=(const std::wstring& value); - Message &operator=( const builtin_interfaces::msg::Time &value ); + Message& operator=(const builtin_interfaces::msg::Time& value); - Message &operator=( const builtin_interfaces::msg::Duration &value ); + Message& operator=(const builtin_interfaces::msg::Duration& value); - Message &operator=( const rclcpp::Time &value ); + Message& operator=(const rclcpp::Time& value); - Message &operator=( const rclcpp::Duration &value ); + Message& operator=(const rclcpp::Duration& value); - Message &operator=( const Message &other ); + Message& operator=(const Message& other); /**@}*/ - template - bool operator==( const T &other ) const; + template + bool operator==(const T& other) const; - bool operator==( const Message &other ) const; + bool operator==(const Message& other) const; - bool operator==( const char *c ) const; + bool operator==(const char* c) const; - bool operator==( const wchar_t *c ) const; + bool operator==(const wchar_t* c) const; - template - bool operator!=( const T &other ) const; + template + bool operator!=(const T& other) const; protected: - void move( std::shared_ptr new_data ) - { - data_ = std::move( new_data ); + void move(std::shared_ptr new_data) { + data_ = std::move(new_data); onMoved(); } - virtual void onMoved() { } + virtual void onMoved() {} - template - void checkValueEqual( const U &other, bool &result ) const; + template + void checkValueEqual(const U& other, bool& result) const; // Disable copy construction except for subclasses - Message( const Message &other ) : data_( other.data_ ), type_( other.type_ ) { } + Message(const Message& other) + : data_(other.data_) + , type_(other.type_) {} - virtual bool _isMessageEqual( const Message &other ) const = 0; + virtual bool _isMessageEqual(const Message& other) const = 0; - virtual void _assign( const Message &other ) = 0; + virtual void _assign(const Message& other) = 0; - unsigned char *data_ptr() { return reinterpret_cast( data_.get() ); } + unsigned char* data_ptr() { + return reinterpret_cast(data_.get()); + } - const unsigned char *data_ptr() const - { - return reinterpret_cast( data_.get() ); + const unsigned char* data_ptr() const { + return reinterpret_cast(data_.get()); } std::shared_ptr data_; @@ -197,230 +199,217 @@ class Message friend class CompoundMessage; - template + template friend class CompoundArrayMessage_; }; -template<> +template <> bool Message::value() const; -template<> +template <> uint8_t Message::value() const; -template<> +template <> uint16_t Message::value() const; -template<> +template <> uint32_t Message::value() const; -template<> +template <> uint64_t Message::value() const; -template<> +template <> int8_t Message::value() const; -template<> +template <> int16_t Message::value() const; -template<> +template <> int32_t Message::value() const; -template<> +template <> int64_t Message::value() const; -template<> +template <> float Message::value() const; -template<> +template <> double Message::value() const; -template<> +template <> long double Message::value() const; -template<> +template <> std::string Message::value() const; -template<> +template <> char16_t Message::value() const; -template<> +template <> std::wstring Message::value() const; // These are now compound messages instead of value messages -template<> +template <> rclcpp::Time Message::value() const; -template<> +template <> rclcpp::Duration Message::value() const; // Equality comparison needs to come after value specializations -namespace impl -{ -template +namespace impl { +template struct PassThrough { typedef X type; }; // Make sure we only compile possible comparisons. -// Looks more complicated than it is but the base implementation just makes sure we don't compare signed and unsigned -template +// Looks more complicated than it is but the base implementation just makes sure we don't compare +// signed and unsigned +template struct EqHelper { - template + template using is_same_sign = - std::integral_constant::value == std::is_signed::value>; + std::integral_constant::value == std::is_signed::value>; - template + template static typename std::enable_if::value && is_same_sign::value, void>::type - equal( const Message *m, const U &other, bool &result ) - { + equal(const Message* m, const U& other, bool& result) { result = m->template value() == other; } - template + template static typename std::enable_if< - std::is_arithmetic::value && !is_same_sign::value && std::is_signed::value, void>::type - equal( const Message *m, const U &other, bool &result ) - { + std::is_arithmetic::value && !is_same_sign::value && std::is_signed::value, void>::type + equal(const Message* m, const U& other, bool& result) { // Different sign and U signed so T is unsigned - if ( other < 0 ) { - result = false; // T can't be negative so no need to check + if (other < 0) { + result = false; // T can't be negative so no need to check return; } - // We always cast unsigned to signed since the other way is not always possible (e.g. float/double). + // We always cast unsigned to signed since the other way is not always possible (e.g. + // float/double). using SignedT = typename std::common_type::type>::type; - const T &val = m->template value(); - // val has to be able to fit into the SignedT type, this may have inaccuracies with some floating point types/values. - result = val <= static_cast( std::numeric_limits::max() ) && - static_cast( val ) == other; + const T& val = m->template value(); + // val has to be able to fit into the SignedT type, this may have inaccuracies with some + // floating point types/values. + result = val <= static_cast(std::numeric_limits::max()) && + static_cast(val) == other; } - template - static typename std::enable_if< - std::is_arithmetic::value && !is_same_sign::value && !std::is_signed::value, void>::type - equal( const Message *m, const U &other, bool &result ) - { + template + static typename std::enable_if::value && !is_same_sign::value && + !std::is_signed::value, + void>::type + equal(const Message* m, const U& other, bool& result) { // Different sign and U unsigned so T is signed using SignedU = typename std::make_signed::type; - if ( other > static_cast( std::numeric_limits::max() ) ) + if (other > static_cast(std::numeric_limits::max())) result = false; else - result = m->template value() == static_cast( other ); + result = m->template value() == static_cast(other); } - template - static typename std::enable_if::value, void>::type - equal( const Message *, const U &, bool &result ) - { + template + static typename std::enable_if::value, void>::type equal(const Message*, + const U&, + bool& result) { result = false; } }; -template<> +template <> struct EqHelper { - template - static typename std::enable_if::value, void>::type - equal( const Message *m, const U &other, bool &result ) - { + template + static typename std::enable_if::value, void>::type equal( + const Message* m, const U& other, bool& result) { result = m->template value() == other; } - template - static typename std::enable_if::value, void>::type - equal( const Message *, const U &, bool &result ) - { + template + static typename std::enable_if::value, void>::type equal( + const Message*, const U&, bool& result) { result = false; } }; -template<> +template <> struct EqHelper { - template - static typename std::enable_if::value, void>::type - equal( const Message *m, const U &other, bool &result ) - { + template + static typename std::enable_if::value, void>::type equal( + const Message* m, const U& other, bool& result) { result = m->template value() == other; } - template - static typename std::enable_if::value, void>::type - equal( const Message *, const U &, bool &result ) - { + template + static typename std::enable_if::value, void>::type equal( + const Message*, const U&, bool& result) { result = false; } }; -template<> +template <> struct EqHelper { - template - static typename std::enable_if::value, void>::type - equal( const Message *m, const U &other, bool &result ) - { + template + static typename std::enable_if::value, void>::type equal( + const Message* m, const U& other, bool& result) { result = m->template value() == other; } - template - static typename std::enable_if::value, void>::type - equal( const Message *, const U &, bool &result ) - { + template + static typename std::enable_if::value, void>::type equal( + const Message*, const U&, bool& result) { result = false; } }; -template<> +template <> struct EqHelper { - template - static typename std::enable_if::value, void>::type - equal( const Message *m, const U &other, bool &result ) - { + template + static typename std::enable_if::value, void>::type equal( + const Message* m, const U& other, bool& result) { result = *m == other; } - template - static typename std::enable_if::value, void>::type - equal( const Message *m, const U &other, bool &result ) - { + template + static typename std::enable_if::value, void>::type equal( + const Message* m, const U& other, bool& result) { result = m->template value() == other; } }; -template<> +template <> struct EqHelper { - template - static typename std::enable_if::value, void>::type - equal( const Message *m, const U &other, bool &result ) - { + template + static typename std::enable_if::value, void>::type equal( + const Message* m, const U& other, bool& result) { result = *m == other; } - template - static typename std::enable_if::value, void>::type - equal( const Message *, const U &, bool &result ) - { + template + static typename std::enable_if::value, void>::type equal( + const Message*, const U&, bool& result) { result = false; } }; -} // namespace impl +} // namespace impl -template -void Message::checkValueEqual( const U &other, bool &result ) const -{ - impl::EqHelper::equal( this, other, result ); +template +void Message::checkValueEqual(const U& other, bool& result) const { + impl::EqHelper::equal(this, other, result); } -template -bool Message::operator==( const T &other ) const -{ +template +bool Message::operator==(const T& other) const { bool result = false; - RBF2_TEMPLATE_CALL( checkValueEqual, type_, other, result ); + RBF2_TEMPLATE_CALL(checkValueEqual, type_, other, result); return result; } -template -bool Message::operator!=( const T &other ) const -{ - return !( *this == other ); +template +bool Message::operator!=(const T& other) const { + return !(*this == other); } -} // namespace ros2_babel_fish +} // namespace ros2_babel_fish -#endif // ROS2_BABEL_FISH_MESSAGE_HPP +#endif // ROS2_BABEL_FISH_MESSAGE_HPP diff --git a/ros2_foxglove_bridge/include/ros2_babel_fish/messages/message_type_traits.hpp b/ros2_foxglove_bridge/include/ros2_babel_fish/messages/message_type_traits.hpp index a7ad371..c825021 100644 --- a/ros2_foxglove_bridge/include/ros2_babel_fish/messages/message_type_traits.hpp +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/messages/message_type_traits.hpp @@ -5,12 +5,12 @@ #ifndef ROS2_BABEL_FISH_MESSAGE_TYPE_TRAITS_HPP #define ROS2_BABEL_FISH_MESSAGE_TYPE_TRAITS_HPP -#include "ros2_babel_fish/messages/message_types.hpp" #include #include -namespace ros2_babel_fish -{ +#include "ros2_babel_fish/messages/message_types.hpp" + +namespace ros2_babel_fish { // Predeclare message classes class Message; @@ -18,131 +18,130 @@ class ArrayMessageBase; class CompoundMessage; -namespace message_type_traits -{ -template +namespace message_type_traits { +template struct message_type { static constexpr MessageType value = MessageTypes::None; }; -template +template struct member_type { typedef void value; }; -template +template struct value_type { typedef void value; }; -inline bool isValueType( MessageType type ) -{ - return type != MessageTypes::Compound && type != MessageTypes::Array && type != MessageTypes::None; +inline bool isValueType(MessageType type) { + return type != MessageTypes::Compound && type != MessageTypes::Array && + type != MessageTypes::None; } -#define DECLARE_MESSAGE_TYPE_FOR_TYPE( __message_type, __type ) \ - template<> \ - struct message_type<__type> { \ - static constexpr MessageType value = __message_type; \ +#define DECLARE_MESSAGE_TYPE_FOR_TYPE(__message_type, __type) \ + template <> \ + struct message_type<__type> { \ + static constexpr MessageType value = __message_type; \ } -DECLARE_MESSAGE_TYPE_FOR_TYPE( MessageTypes::Bool, bool ); -DECLARE_MESSAGE_TYPE_FOR_TYPE( MessageTypes::UInt8, uint8_t ); -DECLARE_MESSAGE_TYPE_FOR_TYPE( MessageTypes::UInt16, uint16_t ); -DECLARE_MESSAGE_TYPE_FOR_TYPE( MessageTypes::UInt32, uint32_t ); -DECLARE_MESSAGE_TYPE_FOR_TYPE( MessageTypes::UInt64, uint64_t ); -DECLARE_MESSAGE_TYPE_FOR_TYPE( MessageTypes::Int8, int8_t ); -DECLARE_MESSAGE_TYPE_FOR_TYPE( MessageTypes::Int16, int16_t ); -DECLARE_MESSAGE_TYPE_FOR_TYPE( MessageTypes::Int32, int32_t ); -DECLARE_MESSAGE_TYPE_FOR_TYPE( MessageTypes::Int64, int64_t ); -DECLARE_MESSAGE_TYPE_FOR_TYPE( MessageTypes::Float, float ); -DECLARE_MESSAGE_TYPE_FOR_TYPE( MessageTypes::Double, double ); -DECLARE_MESSAGE_TYPE_FOR_TYPE( MessageTypes::LongDouble, long double ); -DECLARE_MESSAGE_TYPE_FOR_TYPE( MessageTypes::WChar, char16_t ); -DECLARE_MESSAGE_TYPE_FOR_TYPE( MessageTypes::String, std::string ); -DECLARE_MESSAGE_TYPE_FOR_TYPE( MessageTypes::WString, std::wstring ); +DECLARE_MESSAGE_TYPE_FOR_TYPE(MessageTypes::Bool, bool); +DECLARE_MESSAGE_TYPE_FOR_TYPE(MessageTypes::UInt8, uint8_t); +DECLARE_MESSAGE_TYPE_FOR_TYPE(MessageTypes::UInt16, uint16_t); +DECLARE_MESSAGE_TYPE_FOR_TYPE(MessageTypes::UInt32, uint32_t); +DECLARE_MESSAGE_TYPE_FOR_TYPE(MessageTypes::UInt64, uint64_t); +DECLARE_MESSAGE_TYPE_FOR_TYPE(MessageTypes::Int8, int8_t); +DECLARE_MESSAGE_TYPE_FOR_TYPE(MessageTypes::Int16, int16_t); +DECLARE_MESSAGE_TYPE_FOR_TYPE(MessageTypes::Int32, int32_t); +DECLARE_MESSAGE_TYPE_FOR_TYPE(MessageTypes::Int64, int64_t); +DECLARE_MESSAGE_TYPE_FOR_TYPE(MessageTypes::Float, float); +DECLARE_MESSAGE_TYPE_FOR_TYPE(MessageTypes::Double, double); +DECLARE_MESSAGE_TYPE_FOR_TYPE(MessageTypes::LongDouble, long double); +DECLARE_MESSAGE_TYPE_FOR_TYPE(MessageTypes::WChar, char16_t); +DECLARE_MESSAGE_TYPE_FOR_TYPE(MessageTypes::String, std::string); +DECLARE_MESSAGE_TYPE_FOR_TYPE(MessageTypes::WString, std::wstring); #undef DECLARE_MESSAGE_TYPE_FOR_TYPE -#define DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE( __message_type, __type ) \ - template<> \ - struct member_type<__message_type> { \ - typedef __type value; \ +#define DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE(__message_type, __type) \ + template <> \ + struct member_type<__message_type> { \ + typedef __type value; \ } -DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE( MessageTypes::Bool, bool ); -DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE( MessageTypes::Octet, unsigned char ); -DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE( MessageTypes::UInt8, uint8_t ); -DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE( MessageTypes::UInt16, uint16_t ); -DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE( MessageTypes::UInt32, uint32_t ); -DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE( MessageTypes::UInt64, uint64_t ); -DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE( MessageTypes::Int8, int8_t ); -DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE( MessageTypes::Int16, int16_t ); -DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE( MessageTypes::Int32, int32_t ); -DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE( MessageTypes::Int64, int64_t ); -DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE( MessageTypes::Float, float ); -DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE( MessageTypes::Double, double ); -DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE( MessageTypes::LongDouble, long double ); -DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE( MessageTypes::Char, unsigned char ); -DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE( MessageTypes::WChar, char16_t ); -DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE( MessageTypes::String, std::string ); -DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE( MessageTypes::WString, std::wstring ); -DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE( MessageTypes::Compound, CompoundMessage ); -DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE( MessageTypes::Array, ArrayMessageBase ); -DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE( MessageTypes::None, void ); +DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE(MessageTypes::Bool, bool); +DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE(MessageTypes::Octet, unsigned char); +DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE(MessageTypes::UInt8, uint8_t); +DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE(MessageTypes::UInt16, uint16_t); +DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE(MessageTypes::UInt32, uint32_t); +DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE(MessageTypes::UInt64, uint64_t); +DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE(MessageTypes::Int8, int8_t); +DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE(MessageTypes::Int16, int16_t); +DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE(MessageTypes::Int32, int32_t); +DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE(MessageTypes::Int64, int64_t); +DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE(MessageTypes::Float, float); +DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE(MessageTypes::Double, double); +DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE(MessageTypes::LongDouble, long double); +DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE(MessageTypes::Char, unsigned char); +DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE(MessageTypes::WChar, char16_t); +DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE(MessageTypes::String, std::string); +DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE(MessageTypes::WString, std::wstring); +DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE(MessageTypes::Compound, CompoundMessage); +DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE(MessageTypes::Array, ArrayMessageBase); +DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE(MessageTypes::None, void); #undef DECLARE_MEMBER_TYPE_FOR_MESSAGE_TYPE -#define DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE( __message_type, __type ) \ - template<> \ - struct value_type<__message_type> { \ - typedef __type value; \ +#define DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE(__message_type, __type) \ + template <> \ + struct value_type<__message_type> { \ + typedef __type value; \ } -DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE( MessageTypes::Bool, bool ); -DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE( MessageTypes::Octet, unsigned char ); -DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE( MessageTypes::UInt8, uint8_t ); -DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE( MessageTypes::UInt16, uint16_t ); -DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE( MessageTypes::UInt32, uint32_t ); -DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE( MessageTypes::UInt64, uint64_t ); -DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE( MessageTypes::Int8, int8_t ); -DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE( MessageTypes::Int16, int16_t ); -DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE( MessageTypes::Int32, int32_t ); -DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE( MessageTypes::Int64, int64_t ); -DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE( MessageTypes::Float, float ); -DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE( MessageTypes::Double, double ); -DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE( MessageTypes::LongDouble, long double ); -DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE( MessageTypes::Char, unsigned char ); -DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE( MessageTypes::WChar, char16_t ); -DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE( MessageTypes::String, std::string ); -DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE( MessageTypes::WString, std::wstring ); -DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE( MessageTypes::Compound, CompoundMessage ); -DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE( MessageTypes::Array, ArrayMessageBase ); +DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE(MessageTypes::Bool, bool); +DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE(MessageTypes::Octet, unsigned char); +DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE(MessageTypes::UInt8, uint8_t); +DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE(MessageTypes::UInt16, uint16_t); +DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE(MessageTypes::UInt32, uint32_t); +DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE(MessageTypes::UInt64, uint64_t); +DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE(MessageTypes::Int8, int8_t); +DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE(MessageTypes::Int16, int16_t); +DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE(MessageTypes::Int32, int32_t); +DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE(MessageTypes::Int64, int64_t); +DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE(MessageTypes::Float, float); +DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE(MessageTypes::Double, double); +DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE(MessageTypes::LongDouble, long double); +DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE(MessageTypes::Char, unsigned char); +DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE(MessageTypes::WChar, char16_t); +DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE(MessageTypes::String, std::string); +DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE(MessageTypes::WString, std::wstring); +DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE(MessageTypes::Compound, CompoundMessage); +DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE(MessageTypes::Array, ArrayMessageBase); #undef DECLARE_VALUE_TYPE_FOR_MESSAGE_TYPE -template +template struct array_type { - typedef T &Reference; + typedef T& Reference; typedef T ReturnType; typedef const T ConstReturnType; - typedef const T &ArgumentType; + typedef const T& ArgumentType; }; -template<> +template <> struct array_type { typedef std::vector::reference Reference; typedef bool ReturnType; typedef bool ConstReturnType; typedef bool ArgumentType; }; -template<> +template <> struct array_type { typedef void Reference; typedef void ReturnType; typedef const void ConstReturnType; typedef void ArgumentType; }; -template<> +template <> struct array_type { typedef void Reference; typedef void ReturnType; typedef const void ConstReturnType; typedef void ArgumentType; }; -} // namespace message_type_traits -} // namespace ros2_babel_fish +} // namespace message_type_traits +} // namespace ros2_babel_fish -#endif // ROS2_BABEL_FISH_MESSAGE_TYPE_TRAITS_HPP +#endif // ROS2_BABEL_FISH_MESSAGE_TYPE_TRAITS_HPP diff --git a/ros2_foxglove_bridge/include/ros2_babel_fish/messages/message_types.hpp b/ros2_foxglove_bridge/include/ros2_babel_fish/messages/message_types.hpp index 2f604b8..ae19ff6 100644 --- a/ros2_foxglove_bridge/include/ros2_babel_fish/messages/message_types.hpp +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/messages/message_types.hpp @@ -1,16 +1,15 @@ // Copyright (c) 2021 Stefan Fabian. All rights reserved. -// Licensed under the MIT license. See LICENSE file in the project root for full license information. +// Licensed under the MIT license. See LICENSE file in the project root for full license +// information. #ifndef ROS2_BABEL_FISH_MESSAGE_TYPES_HPP #define ROS2_BABEL_FISH_MESSAGE_TYPES_HPP #include -namespace ros2_babel_fish -{ +namespace ros2_babel_fish { -namespace MessageTypes -{ +namespace MessageTypes { enum MessageType : uint8_t { None = 0x00000, Float = ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT, @@ -35,6 +34,6 @@ enum MessageType : uint8_t { }; } typedef MessageTypes::MessageType MessageType; -} // namespace ros2_babel_fish +} // namespace ros2_babel_fish -#endif // ROS2_BABEL_FISH_MESSAGE_TYPES_HPP +#endif // ROS2_BABEL_FISH_MESSAGE_TYPES_HPP diff --git a/ros2_foxglove_bridge/include/ros2_babel_fish/messages/value_message.hpp b/ros2_foxglove_bridge/include/ros2_babel_fish/messages/value_message.hpp index eb48f73..d5b2cd7 100644 --- a/ros2_foxglove_bridge/include/ros2_babel_fish/messages/value_message.hpp +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/messages/value_message.hpp @@ -1,92 +1,83 @@ // Copyright (c) 2021 Stefan Fabian. All rights reserved. -// Licensed under the MIT license. See LICENSE file in the project root for full license information. +// Licensed under the MIT license. See LICENSE file in the project root for full license +// information. #ifndef ROS2_BABEL_FISH_VALUE_MESSAGE_HPP #define ROS2_BABEL_FISH_VALUE_MESSAGE_HPP +#include + #include "message_type_traits.hpp" #include "ros2_babel_fish/exceptions/babel_fish_exception.hpp" #include "ros2_babel_fish/macros.hpp" #include "ros2_babel_fish/messages/message.hpp" -#include - -namespace ros2_babel_fish -{ +namespace ros2_babel_fish { -template -class ValueMessage final : public Message -{ +template +class ValueMessage final : public Message { static constexpr MessageType _type = message_type_traits::message_type::value; - static_assert( _type != MessageTypes::None, "Invalid type parameter for ValueMessage!" ); + static_assert(_type != MessageTypes::None, "Invalid type parameter for ValueMessage!"); public: - RCLCPP_SMART_PTR_DEFINITIONS( ValueMessage ) + RCLCPP_SMART_PTR_DEFINITIONS(ValueMessage) - explicit ValueMessage( MessageMemberIntrospection member, std::shared_ptr data ) - : Message( _type, std::move( data ) ), member_( std::move( member ) ) - { - } + explicit ValueMessage(MessageMemberIntrospection member, std::shared_ptr data) + : Message(_type, std::move(data)) + , member_(std::move(member)) {} - T getValue() const { return *reinterpret_cast( this->data_ptr() + member_->offset_ ); } + T getValue() const { + return *reinterpret_cast(this->data_ptr() + member_->offset_); + } - void setValue( T value ) - { - *reinterpret_cast( this->data_ptr() + member_->offset_ ) = value; + void setValue(T value) { + *reinterpret_cast(this->data_ptr() + member_->offset_) = value; } - ValueMessage &operator=( const T &value ) - { - setValue( value ); + ValueMessage& operator=(const T& value) { + setValue(value); return *this; } - ValueMessage &operator=( const ValueMessage &other ) - { - if ( this == &other ) - return *this; - setValue( other.getValue() ); + ValueMessage& operator=(const ValueMessage& other) { + if (this == &other) return *this; + setValue(other.getValue()); return *this; } protected: - template - void assignValue( const Message &other ) - { - Message::operator=( other.value() ); + template + void assignValue(const Message& other) { + Message::operator=(other.value()); } - void _assign( const Message &other ) override - { - if ( !message_type_traits::isValueType( other.type() ) ) - throw BabelFishException( "Tried to assign non-value message to value message!" ); - RBF2_TEMPLATE_CALL_VALUE_TYPES( assignValue, other.type(), other ); + void _assign(const Message& other) override { + if (!message_type_traits::isValueType(other.type())) + throw BabelFishException("Tried to assign non-value message to value message!"); + RBF2_TEMPLATE_CALL_VALUE_TYPES(assignValue, other.type(), other); } - bool _isMessageEqual( const Message &o ) const override - { - const auto &other = o.as>(); + bool _isMessageEqual(const Message& o) const override { + const auto& other = o.as>(); return getValue() == other.getValue(); } MessageMemberIntrospection member_; }; -template<> -inline void ValueMessage::setValue( std::string value ) -{ - if ( member_->string_upper_bound_ != 0 && value.length() > member_->string_upper_bound_ ) - throw std::length_error( "Exceeded string upper bound!" ); - *reinterpret_cast( data_ptr() + member_->offset_ ) = std::move( value ); +template <> +inline void ValueMessage::setValue(std::string value) { + if (member_->string_upper_bound_ != 0 && value.length() > member_->string_upper_bound_) + throw std::length_error("Exceeded string upper bound!"); + *reinterpret_cast(data_ptr() + member_->offset_) = std::move(value); } -template<> -inline void ValueMessage::setValue( std::wstring value ) -{ - if ( member_->string_upper_bound_ != 0 && value.length() > member_->string_upper_bound_ ) - throw std::length_error( "Exceeded string upper bound!" ); - *reinterpret_cast( data_ptr() + member_->offset_ ) = std::move( value ); +template <> +inline void ValueMessage::setValue(std::wstring value) { + if (member_->string_upper_bound_ != 0 && value.length() > member_->string_upper_bound_) + throw std::length_error("Exceeded string upper bound!"); + *reinterpret_cast(data_ptr() + member_->offset_) = std::move(value); } -} // namespace ros2_babel_fish +} // namespace ros2_babel_fish -#endif // ROS2_BABEL_FISH_VALUE_MESSAGE_HPP +#endif // ROS2_BABEL_FISH_VALUE_MESSAGE_HPP diff --git a/ros2_foxglove_bridge/include/ros2_babel_fish/method_invoke_helpers.hpp b/ros2_foxglove_bridge/include/ros2_babel_fish/method_invoke_helpers.hpp index a36d85c..257f64a 100644 --- a/ros2_foxglove_bridge/include/ros2_babel_fish/method_invoke_helpers.hpp +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/method_invoke_helpers.hpp @@ -5,13 +5,12 @@ #ifndef ROS2_BABEL_FISH_METHOD_INVOKE_HELPERS_HPP #define ROS2_BABEL_FISH_METHOD_INVOKE_HELPERS_HPP +#include + #include "ros2_babel_fish/messages/array_message.hpp" #include "ros2_babel_fish/messages/compound_message.hpp" -#include - -namespace ros2_babel_fish -{ +namespace ros2_babel_fish { // Define invoke function with a "polyfill" for CPP versions below C++17 // Macro is unset at the end of the header #if __cplusplus >= 201700L @@ -20,42 +19,38 @@ namespace ros2_babel_fish #else -namespace impl -{ -template>{}, int> = 0> -constexpr decltype( auto ) -invoke( Fn &&f, - Args &&...args ) noexcept( noexcept( std::mem_fn( f )( std::forward( args )... ) ) ) -{ - return std::mem_fn( f )( std::forward( args )... ); +namespace impl { +template >{}, int> = 0> +constexpr decltype(auto) invoke(Fn&& f, Args&&... args) noexcept( + noexcept(std::mem_fn(f)(std::forward(args)...))) { + return std::mem_fn(f)(std::forward(args)...); } -template>{}, int> = 0> -constexpr decltype( auto ) invoke( Fn &&f, Args &&...args ) noexcept( - noexcept( std::forward( f )( std::forward( args )... ) ) ) -{ - return std::forward( f )( std::forward( args )... ); +template >{}, int> = 0> +constexpr decltype(auto) invoke(Fn&& f, Args&&... args) noexcept( + noexcept(std::forward(f)(std::forward(args)...))) { + return std::forward(f)(std::forward(args)...); } -} // namespace impl +} // namespace impl #define RBF2_INVOKE_FN ::ros2_babel_fish::impl::invoke #endif /*! - * @brief Invokes the given functor with the given message casted to the actual type of the given message and the - * optionally passed additional arguments. - * For array messages the method is invoked with the given message casted to ArrayMessageBase. - * This can be used with invoke_for_array_message to call for the concrete array message type. + * @brief Invokes the given functor with the given message casted to the actual type of the given + * message and the optionally passed additional arguments. For array messages the method is invoked + * with the given message casted to ArrayMessageBase. This can be used with invoke_for_array_message + * to call for the concrete array message type. * @return The return value of the invoked method. */ -template -auto invoke_for_message( Message &msg, Callable &&f, Args &&...args ); +template +auto invoke_for_message(Message& msg, Callable&& f, Args&&... args); //! @copydoc invoke_for_message -template -auto invoke_for_message( const Message &msg, Callable &&f, Args &&...args ); +template +auto invoke_for_message(const Message& msg, Callable&& f, Args&&... args); /*! * @brief Invokes the given functor with the given value message casted to the actual type of the @@ -64,12 +59,12 @@ auto invoke_for_message( const Message &msg, Callable &&f, Args &&...args ); * @return The return value of the invoked method. * @throws BabelFishException If the message is not a ValueMessage. */ -template -auto invoke_for_value_message( Message &msg, Callable &&f, Args &&...args ); +template +auto invoke_for_value_message(Message& msg, Callable&& f, Args&&... args); //! @copydoc invoke_for_value_message -template -auto invoke_for_value_message( const Message &msg, Callable &&f, Args &&...args ); +template +auto invoke_for_value_message(const Message& msg, Callable&& f, Args&&... args); /*! * @brief Invokes the given functor with the given message casted to the actual type of the given @@ -77,489 +72,480 @@ auto invoke_for_value_message( const Message &msg, Callable &&f, Args &&...args * * @return The return value of the invoked method. */ -template -auto invoke_for_array_message( ArrayMessageBase &msg, Callable &&f, Args &&...args ); +template +auto invoke_for_array_message(ArrayMessageBase& msg, Callable&& f, Args&&... args); //! @copydoc invoke_for_array_message -template -auto invoke_for_array_message( const ArrayMessageBase &msg, Callable &&f, Args &&...args ); +template +auto invoke_for_array_message(const ArrayMessageBase& msg, Callable&& f, Args&&... args); // ============================================== // =============== IMPLEMENTATION =============== // ============================================== -template -auto invoke_for_message( Message &msg, Callable &&f, Args &&...args ) -{ +template +auto invoke_for_message(Message& msg, Callable&& f, Args&&... args) { using namespace ::ros2_babel_fish::message_type_traits; using ::ros2_babel_fish::ValueMessage; - switch ( msg.type() ) { - case MessageTypes::Float: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::Double: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::LongDouble: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::Char: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::WChar: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::Bool: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::Octet: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::UInt8: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::Int8: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::UInt16: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::Int16: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::UInt32: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::Int32: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::UInt64: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::Int64: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::String: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::WString: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::Compound: - return RBF2_INVOKE_FN( f, msg.as(), std::forward( args )... ); - case MessageTypes::Array: - return RBF2_INVOKE_FN( f, msg.as(), std::forward( args )... ); - case MessageTypes::None: - default: - throw BabelFishException( "invoke_for_message called with invalid message!" ); + switch (msg.type()) { + case MessageTypes::Float: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::Double: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::LongDouble: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::Char: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::WChar: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::Bool: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::Octet: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::UInt8: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::Int8: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::UInt16: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::Int16: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::UInt32: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::Int32: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::UInt64: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::Int64: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::String: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::WString: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::Compound: + return RBF2_INVOKE_FN(f, msg.as(), std::forward(args)...); + case MessageTypes::Array: + return RBF2_INVOKE_FN(f, msg.as(), std::forward(args)...); + case MessageTypes::None: + default: + throw BabelFishException("invoke_for_message called with invalid message!"); } } -template -auto invoke_for_message( const Message &msg, Callable &&f, Args &&...args ) -{ +template +auto invoke_for_message(const Message& msg, Callable&& f, Args&&... args) { using namespace ::ros2_babel_fish::message_type_traits; using ::ros2_babel_fish::ValueMessage; - switch ( msg.type() ) { - case MessageTypes::Float: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::Double: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::LongDouble: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::Char: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::WChar: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::Bool: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::Octet: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::UInt8: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::Int8: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::UInt16: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::Int16: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::UInt32: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::Int32: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::UInt64: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::Int64: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::String: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::WString: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::Compound: - return RBF2_INVOKE_FN( f, msg.as(), std::forward( args )... ); - case MessageTypes::Array: - return RBF2_INVOKE_FN( f, msg.as(), std::forward( args )... ); - case MessageTypes::None: - default: - throw BabelFishException( "invoke_for_message called with invalid message!" ); + switch (msg.type()) { + case MessageTypes::Float: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::Double: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::LongDouble: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::Char: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::WChar: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::Bool: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::Octet: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::UInt8: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::Int8: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::UInt16: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::Int16: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::UInt32: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::Int32: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::UInt64: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::Int64: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::String: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::WString: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::Compound: + return RBF2_INVOKE_FN(f, msg.as(), std::forward(args)...); + case MessageTypes::Array: + return RBF2_INVOKE_FN(f, msg.as(), std::forward(args)...); + case MessageTypes::None: + default: + throw BabelFishException("invoke_for_message called with invalid message!"); } } -template -auto invoke_for_value_message( Message &msg, Callable &&f, Args &&...args ) -{ +template +auto invoke_for_value_message(Message& msg, Callable&& f, Args&&... args) { using namespace ::ros2_babel_fish::message_type_traits; using ::ros2_babel_fish::ValueMessage; - switch ( msg.type() ) { - case MessageTypes::Float: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::Double: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::LongDouble: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::Char: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::WChar: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::Bool: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::Octet: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::UInt8: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::Int8: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::UInt16: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::Int16: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::UInt32: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::Int32: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::UInt64: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::Int64: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::String: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::WString: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::Compound: - case MessageTypes::Array: - throw BabelFishException( - "invoke_for_value_message called with message that is not a ValueMessage!" ); - case MessageTypes::None: - default: - throw BabelFishException( "invoke_for_value_message called with invalid message!" ); + switch (msg.type()) { + case MessageTypes::Float: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::Double: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::LongDouble: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::Char: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::WChar: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::Bool: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::Octet: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::UInt8: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::Int8: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::UInt16: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::Int16: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::UInt32: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::Int32: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::UInt64: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::Int64: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::String: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::WString: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::Compound: + case MessageTypes::Array: + throw BabelFishException( + "invoke_for_value_message called with message that is not a ValueMessage!"); + case MessageTypes::None: + default: + throw BabelFishException("invoke_for_value_message called with invalid message!"); } } -template -auto invoke_for_value_message( const Message &msg, Callable &&f, Args &&...args ) -{ +template +auto invoke_for_value_message(const Message& msg, Callable&& f, Args&&... args) { using namespace ::ros2_babel_fish::message_type_traits; using ::ros2_babel_fish::ValueMessage; - switch ( msg.type() ) { - case MessageTypes::Float: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::Double: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::LongDouble: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::Char: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::WChar: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::Bool: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::Octet: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::UInt8: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::Int8: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::UInt16: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::Int16: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::UInt32: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::Int32: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::UInt64: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::Int64: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::String: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::WString: - return RBF2_INVOKE_FN( f, msg.as::value>>(), - std::forward( args )... ); - case MessageTypes::Compound: - case MessageTypes::Array: - throw BabelFishException( - "invoke_for_value_message called with message that is not a ValueMessage!" ); - case MessageTypes::None: - default: - throw BabelFishException( "invoke_for_value_message called with invalid message!" ); + switch (msg.type()) { + case MessageTypes::Float: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::Double: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::LongDouble: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::Char: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::WChar: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::Bool: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::Octet: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::UInt8: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::Int8: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::UInt16: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::Int16: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::UInt32: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::Int32: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::UInt64: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::Int64: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::String: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::WString: + return RBF2_INVOKE_FN(f, msg.as::value>>(), + std::forward(args)...); + case MessageTypes::Compound: + case MessageTypes::Array: + throw BabelFishException( + "invoke_for_value_message called with message that is not a ValueMessage!"); + case MessageTypes::None: + default: + throw BabelFishException("invoke_for_value_message called with invalid message!"); } } -namespace impl -{ -template -auto call_for_array_message( ArrayMessageBase &msg, Callable &&f, Args &&...args ) -{ +namespace impl { +template +auto call_for_array_message(ArrayMessageBase& msg, Callable&& f, Args&&... args) { using namespace ::ros2_babel_fish::message_type_traits; using ::ros2_babel_fish::ArrayMessage_; using ::ros2_babel_fish::CompoundArrayMessage_; - switch ( msg.elementType() ) { - case MessageTypes::Float: - return RBF2_INVOKE_FN( + switch (msg.elementType()) { + case MessageTypes::Float: + return RBF2_INVOKE_FN( f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), - std::forward( args )... ); - case MessageTypes::Double: - return RBF2_INVOKE_FN( + std::forward(args)...); + case MessageTypes::Double: + return RBF2_INVOKE_FN( f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), - std::forward( args )... ); - case MessageTypes::LongDouble: - return RBF2_INVOKE_FN( + std::forward(args)...); + case MessageTypes::LongDouble: + return RBF2_INVOKE_FN( f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), - std::forward( args )... ); - case MessageTypes::Char: - return RBF2_INVOKE_FN( + std::forward(args)...); + case MessageTypes::Char: + return RBF2_INVOKE_FN( f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), - std::forward( args )... ); - case MessageTypes::WChar: - return RBF2_INVOKE_FN( + std::forward(args)...); + case MessageTypes::WChar: + return RBF2_INVOKE_FN( f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), - std::forward( args )... ); - case MessageTypes::Bool: - return RBF2_INVOKE_FN( + std::forward(args)...); + case MessageTypes::Bool: + return RBF2_INVOKE_FN( f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), - std::forward( args )... ); - case MessageTypes::Octet: - return RBF2_INVOKE_FN( + std::forward(args)...); + case MessageTypes::Octet: + return RBF2_INVOKE_FN( f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), - std::forward( args )... ); - case MessageTypes::UInt8: - return RBF2_INVOKE_FN( + std::forward(args)...); + case MessageTypes::UInt8: + return RBF2_INVOKE_FN( f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), - std::forward( args )... ); - case MessageTypes::Int8: - return RBF2_INVOKE_FN( + std::forward(args)...); + case MessageTypes::Int8: + return RBF2_INVOKE_FN( f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), - std::forward( args )... ); - case MessageTypes::UInt16: - return RBF2_INVOKE_FN( + std::forward(args)...); + case MessageTypes::UInt16: + return RBF2_INVOKE_FN( f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), - std::forward( args )... ); - case MessageTypes::Int16: - return RBF2_INVOKE_FN( + std::forward(args)...); + case MessageTypes::Int16: + return RBF2_INVOKE_FN( f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), - std::forward( args )... ); - case MessageTypes::UInt32: - return RBF2_INVOKE_FN( + std::forward(args)...); + case MessageTypes::UInt32: + return RBF2_INVOKE_FN( f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), - std::forward( args )... ); - case MessageTypes::Int32: - return RBF2_INVOKE_FN( + std::forward(args)...); + case MessageTypes::Int32: + return RBF2_INVOKE_FN( f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), - std::forward( args )... ); - case MessageTypes::UInt64: - return RBF2_INVOKE_FN( + std::forward(args)...); + case MessageTypes::UInt64: + return RBF2_INVOKE_FN( f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), - std::forward( args )... ); - case MessageTypes::Int64: - return RBF2_INVOKE_FN( + std::forward(args)...); + case MessageTypes::Int64: + return RBF2_INVOKE_FN( f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), - std::forward( args )... ); - case MessageTypes::String: - return RBF2_INVOKE_FN( + std::forward(args)...); + case MessageTypes::String: + return RBF2_INVOKE_FN( f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), - std::forward( args )... ); - case MessageTypes::WString: - return RBF2_INVOKE_FN( + std::forward(args)...); + case MessageTypes::WString: + return RBF2_INVOKE_FN( f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), - std::forward( args )... ); - case MessageTypes::Compound: - return RBF2_INVOKE_FN( f, msg.as>(), - std::forward( args )... ); - case MessageTypes::Array: - throw BabelFishException( "Arrays of arrays are not supported in ROS2!" ); - case MessageTypes::None: - default: - throw BabelFishException( "invoke_for_array_message called with invalid message!" ); + std::forward(args)...); + case MessageTypes::Compound: + return RBF2_INVOKE_FN(f, msg.as>(), + std::forward(args)...); + case MessageTypes::Array: + throw BabelFishException("Arrays of arrays are not supported in ROS2!"); + case MessageTypes::None: + default: + throw BabelFishException("invoke_for_array_message called with invalid message!"); } } -template -auto call_for_array_message( const ArrayMessageBase &msg, Callable &&f, Args &&...args ) -{ +template +auto call_for_array_message(const ArrayMessageBase& msg, Callable&& f, Args&&... args) { using namespace ::ros2_babel_fish::message_type_traits; using ::ros2_babel_fish::ArrayMessage_; using ::ros2_babel_fish::CompoundArrayMessage_; - switch ( msg.elementType() ) { - case MessageTypes::Float: - return RBF2_INVOKE_FN( + switch (msg.elementType()) { + case MessageTypes::Float: + return RBF2_INVOKE_FN( f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), - std::forward( args )... ); - case MessageTypes::Double: - return RBF2_INVOKE_FN( + std::forward(args)...); + case MessageTypes::Double: + return RBF2_INVOKE_FN( f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), - std::forward( args )... ); - case MessageTypes::LongDouble: - return RBF2_INVOKE_FN( + std::forward(args)...); + case MessageTypes::LongDouble: + return RBF2_INVOKE_FN( f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), - std::forward( args )... ); - case MessageTypes::Char: - return RBF2_INVOKE_FN( + std::forward(args)...); + case MessageTypes::Char: + return RBF2_INVOKE_FN( f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), - std::forward( args )... ); - case MessageTypes::WChar: - return RBF2_INVOKE_FN( + std::forward(args)...); + case MessageTypes::WChar: + return RBF2_INVOKE_FN( f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), - std::forward( args )... ); - case MessageTypes::Bool: - return RBF2_INVOKE_FN( + std::forward(args)...); + case MessageTypes::Bool: + return RBF2_INVOKE_FN( f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), - std::forward( args )... ); - case MessageTypes::Octet: - return RBF2_INVOKE_FN( + std::forward(args)...); + case MessageTypes::Octet: + return RBF2_INVOKE_FN( f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), - std::forward( args )... ); - case MessageTypes::UInt8: - return RBF2_INVOKE_FN( + std::forward(args)...); + case MessageTypes::UInt8: + return RBF2_INVOKE_FN( f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), - std::forward( args )... ); - case MessageTypes::Int8: - return RBF2_INVOKE_FN( + std::forward(args)...); + case MessageTypes::Int8: + return RBF2_INVOKE_FN( f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), - std::forward( args )... ); - case MessageTypes::UInt16: - return RBF2_INVOKE_FN( + std::forward(args)...); + case MessageTypes::UInt16: + return RBF2_INVOKE_FN( f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), - std::forward( args )... ); - case MessageTypes::Int16: - return RBF2_INVOKE_FN( + std::forward(args)...); + case MessageTypes::Int16: + return RBF2_INVOKE_FN( f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), - std::forward( args )... ); - case MessageTypes::UInt32: - return RBF2_INVOKE_FN( + std::forward(args)...); + case MessageTypes::UInt32: + return RBF2_INVOKE_FN( f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), - std::forward( args )... ); - case MessageTypes::Int32: - return RBF2_INVOKE_FN( + std::forward(args)...); + case MessageTypes::Int32: + return RBF2_INVOKE_FN( f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), - std::forward( args )... ); - case MessageTypes::UInt64: - return RBF2_INVOKE_FN( + std::forward(args)...); + case MessageTypes::UInt64: + return RBF2_INVOKE_FN( f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), - std::forward( args )... ); - case MessageTypes::Int64: - return RBF2_INVOKE_FN( + std::forward(args)...); + case MessageTypes::Int64: + return RBF2_INVOKE_FN( f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), - std::forward( args )... ); - case MessageTypes::String: - return RBF2_INVOKE_FN( + std::forward(args)...); + case MessageTypes::String: + return RBF2_INVOKE_FN( f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), - std::forward( args )... ); - case MessageTypes::WString: - return RBF2_INVOKE_FN( + std::forward(args)...); + case MessageTypes::WString: + return RBF2_INVOKE_FN( f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), - std::forward( args )... ); - case MessageTypes::Compound: - return RBF2_INVOKE_FN( f, msg.as>(), - std::forward( args )... ); - case MessageTypes::Array: - throw BabelFishException( "Arrays of arrays are not supported in ROS2!" ); - case MessageTypes::None: - default: - throw BabelFishException( "invoke_for_array_message called with invalid message!" ); + std::forward(args)...); + case MessageTypes::Compound: + return RBF2_INVOKE_FN(f, msg.as>(), + std::forward(args)...); + case MessageTypes::Array: + throw BabelFishException("Arrays of arrays are not supported in ROS2!"); + case MessageTypes::None: + default: + throw BabelFishException("invoke_for_array_message called with invalid message!"); } } -} // namespace impl +} // namespace impl -template -auto invoke_for_array_message( ArrayMessageBase &msg, Callable &&f, Args &&...args ) -{ - if ( msg.isFixedSize() ) - return impl::call_for_array_message( msg, std::forward( f ), - std::forward( args )... ); - if ( msg.isBounded() ) - return impl::call_for_array_message( msg, std::forward( f ), - std::forward( args )... ); - return impl::call_for_array_message( msg, std::forward( f ), - std::forward( args )... ); +template +auto invoke_for_array_message(ArrayMessageBase& msg, Callable&& f, Args&&... args) { + if (msg.isFixedSize()) + return impl::call_for_array_message(msg, std::forward(f), + std::forward(args)...); + if (msg.isBounded()) + return impl::call_for_array_message(msg, std::forward(f), + std::forward(args)...); + return impl::call_for_array_message(msg, std::forward(f), + std::forward(args)...); } -template -auto invoke_for_array_message( const ArrayMessageBase &msg, Callable &&f, Args &&...args ) -{ - if ( msg.isFixedSize() ) - return impl::call_for_array_message( msg, std::forward( f ), - std::forward( args )... ); - if ( msg.isBounded() ) - return impl::call_for_array_message( msg, std::forward( f ), - std::forward( args )... ); - return impl::call_for_array_message( msg, std::forward( f ), - std::forward( args )... ); +template +auto invoke_for_array_message(const ArrayMessageBase& msg, Callable&& f, Args&&... args) { + if (msg.isFixedSize()) + return impl::call_for_array_message(msg, std::forward(f), + std::forward(args)...); + if (msg.isBounded()) + return impl::call_for_array_message(msg, std::forward(f), + std::forward(args)...); + return impl::call_for_array_message(msg, std::forward(f), + std::forward(args)...); } -} // namespace ros2_babel_fish +} // namespace ros2_babel_fish #undef RBF2_INVOKE_FN -#endif // ROS2_BABEL_FISH_METHOD_INVOKE_HELPERS_HPP +#endif // ROS2_BABEL_FISH_METHOD_INVOKE_HELPERS_HPP