diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index f450706..d7410ef 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -10,15 +10,16 @@ ENV CC=clang ENV CXX=clang++ # Set environment and working directory -ENV ROS_WS /ros2_ws +ENV ROS_WS /ros_ws WORKDIR $ROS_WS # Install system dependencies RUN apt-get update && apt-get install -y --no-install-recommends \ bash-completion \ build-essential \ - clang \ - clang-format \ + clang-15 \ + clangd-15 \ + clang-format-15 \ curl \ gdb \ git \ @@ -33,6 +34,11 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ sudo \ && rm -rf /var/lib/apt/lists/* +# Set clang*-15 as the default +RUN update-alternatives --install /usr/bin/clang clang /usr/bin/clang-15 100 +RUN update-alternatives --install /usr/bin/clangd clangd /usr/bin/clangd-15 100 +RUN update-alternatives --install /usr/bin/clang-format clang-format /usr/bin/clang-format-15 100 + # Add package.xml so we can install package dependencies COPY package.xml src/ros-foxglove-bridge/ @@ -60,11 +66,14 @@ RUN groupadd --gid $USER_GID $USERNAME \ && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME \ && chmod 0440 /etc/sudoers.d/$USERNAME +# Make the workspace directory writable by the non-root user +RUN chown -R $USERNAME:$USERNAME $ROS_WS + USER $USERNAME # Add aliases to .bashrc RUN echo $'\ -alias ros2_build_debug="source /opt/ros/humble/setup.bash && colcon build --cmake-args -DCMAKE_BUILD_TYPE=Debug"\n\ -alias ros2_build_release="source /opt/ros/humble/setup.bash && colcon build --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo"\n\ +alias ros2_build_debug="source /opt/ros/$ROS_DISTRO/setup.bash && colcon build --cmake-args -DCMAKE_BUILD_TYPE=Debug -DCMAKE_EXPORT_COMPILE_COMMANDS:BOOL=ON"\n\ +alias ros2_build_release="source /opt/ros/$ROS_DISTRO/setup.bash && colcon build --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_EXPORT_COMPILE_COMMANDS:BOOL=ON"\n\ alias ros2_foxglove_bridge="source /ros_ws/install_ros2/setup.bash && ros2 run foxglove_bridge foxglove_bridge --ros-args --log-level debug --log-level rcl:=INFO"\n\ ' >> ~/.bashrc diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 6b6f288..0629c23 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -4,6 +4,11 @@ "workspaceFolder": "/ros_ws/src/ros-foxglove-bridge", "workspaceMount": "source=${localWorkspaceFolder},target=/ros_ws/src/ros-foxglove-bridge,type=bind,consistency=cached", "dockerFile": "./Dockerfile", + "build": { + "args": { + "ROS_DISTRIBUTION": "humble" + } + }, "context": "..", "forwardPorts": [ 8765 @@ -11,8 +16,7 @@ "customizations": { "vscode": { "extensions": [ - "ms-vscode.cpptools", - "xaver.clang-format", + "llvm-vs-code-extensions.vscode-clangd", "twxs.cmake" ], "settings": { diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json deleted file mode 100644 index 23dc80f..0000000 --- a/.vscode/c_cpp_properties.json +++ /dev/null @@ -1,22 +0,0 @@ -// -*- jsonc -*- -{ - "configurations": [ - { - "name": "Linux", - "includePath": [ - "${workspaceFolder}/**", - "/opt/ros/humble/include/**", - "/usr/include/**" - ], - "browse": { - "path": [ - "${workspaceFolder}", - "/opt/ros/humble/include", - "/usr/include" - ] - }, - "cppStandard": "c++17" - } - ], - "version": 4 -} diff --git a/.vscode/settings.json b/.vscode/settings.json index 4d9c9fb..60acb5d 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,7 +1,7 @@ // -*- jsonc -*- { - "C_Cpp.autoAddFileAssociations": false, - "editor.defaultFormatter": "xaver.clang-format", + "clangd.arguments": ["--compile-commands-dir=/ros_ws/build_ros2"], + "editor.defaultFormatter": "llvm-vs-code-extensions.vscode-clangd", "editor.formatOnSave": true, "files.eol": "\n", "files.insertFinalNewline": true, diff --git a/CMakeLists.txt b/CMakeLists.txt index 7b050b1..f5df304 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -142,12 +142,19 @@ elseif("$ENV{ROS_VERSION}" STREQUAL "2") set(ROS_BUILD_TYPE "ament_cmake") find_package(ament_cmake REQUIRED) - find_package(rosgraph_msgs REQUIRED) + find_package(ament_index_cpp REQUIRED) find_package(rclcpp REQUIRED) + find_package(rclcpp_action REQUIRED) find_package(rclcpp_components REQUIRED) + find_package(rcpputils REQUIRED) find_package(resource_retriever REQUIRED) + find_package(rosgraph_msgs REQUIRED) + find_package(rosidl_runtime_cpp REQUIRED) + find_package(rosidl_typesupport_cpp REQUIRED) + find_package(rosidl_typesupport_introspection_cpp REQUIRED) add_library(foxglove_bridge_component SHARED + ros2_foxglove_bridge/src/json_to_ros.cpp ros2_foxglove_bridge/src/message_definition_cache.cpp ros2_foxglove_bridge/src/param_utils.cpp ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp @@ -160,8 +167,49 @@ elseif("$ENV{ROS_VERSION}" STREQUAL "2") $ $ ) + + # For ROS 2 Humble only, build and link the vendored ros2_babel_fish library + if("$ENV{ROS_DISTRO}" STREQUAL "humble") + message(STATUS "Building vendored ros2_babel_fish") + set(ROS2_BABEL_FISH_DIR ${CMAKE_CURRENT_SOURCE_DIR}/ros2_foxglove_bridge/src/ros2_babel_fish) + set(ROS2_BABEL_FISH_SOURCES + ${ROS2_BABEL_FISH_DIR}/detail/babel_fish_action_client.cpp + ${ROS2_BABEL_FISH_DIR}/detail/babel_fish_publisher.cpp + ${ROS2_BABEL_FISH_DIR}/detail/babel_fish_service.cpp + ${ROS2_BABEL_FISH_DIR}/detail/babel_fish_service_client.cpp + ${ROS2_BABEL_FISH_DIR}/detail/babel_fish_subscription.cpp + ${ROS2_BABEL_FISH_DIR}/idl/providers/local_type_support_provider.cpp + ${ROS2_BABEL_FISH_DIR}/idl/serialization.cpp + ${ROS2_BABEL_FISH_DIR}/idl/type_support_provider.cpp + ${ROS2_BABEL_FISH_DIR}/messages/array_message.cpp + ${ROS2_BABEL_FISH_DIR}/messages/compound_message.cpp + ${ROS2_BABEL_FISH_DIR}/messages/message.cpp + ${ROS2_BABEL_FISH_DIR}/babel_fish.cpp + ${ROS2_BABEL_FISH_DIR}/detail/topic.cpp + ) + + add_library(ros2_babel_fish STATIC ${ROS2_BABEL_FISH_SOURCES}) + set_target_properties(ros2_babel_fish PROPERTIES POSITION_INDEPENDENT_CODE ON) + target_include_directories(ros2_babel_fish PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/ros2_foxglove_bridge/include) + target_compile_definitions(ros2_babel_fish PUBLIC RCLCPP_VERSION_MAJOR=${rclcpp_VERSION_MAJOR}) + ament_target_dependencies(ros2_babel_fish + ament_index_cpp + rclcpp + rclcpp_action + rcpputils + rosidl_runtime_cpp + rosidl_typesupport_cpp + rosidl_typesupport_introspection_cpp + ) + + target_link_libraries(foxglove_bridge_component foxglove_bridge_base ros2_babel_fish) + target_compile_definitions(foxglove_bridge_component PUBLIC ENABLE_JSON_MESSAGES=1) + elseif(${rclcpp_VERSION_MAJOR} GREATER 20) + # NOTE: For ROS 2 Iron and later, we can use native rclcpp for dynamic message serialization + # ... + endif() + ament_target_dependencies(foxglove_bridge_component ament_index_cpp rclcpp rclcpp_components resource_retriever rosgraph_msgs) - target_link_libraries(foxglove_bridge_component foxglove_bridge_base) rclcpp_components_register_nodes(foxglove_bridge_component "foxglove_bridge::FoxgloveBridge") enable_strict_compiler_warnings(foxglove_bridge_component) add_executable(foxglove_bridge @@ -231,6 +279,13 @@ elseif(ROS_BUILD_TYPE STREQUAL "ament_cmake") target_link_libraries(base64_test foxglove_bridge_base) enable_strict_compiler_warnings(base64_test) + if("$ENV{ROS_DISTRO}" STREQUAL "humble") + ament_add_gtest(json_to_ros_test ros2_foxglove_bridge/tests/json_to_ros_test.cpp) + ament_target_dependencies(json_to_ros_test rclcpp rclcpp_components std_msgs) + target_link_libraries(json_to_ros_test foxglove_bridge_component) + enable_strict_compiler_warnings(json_to_ros_test) + endif() + ament_add_gtest(smoke_test ros2_foxglove_bridge/tests/smoke_test.cpp) ament_target_dependencies(smoke_test rclcpp rclcpp_components std_msgs std_srvs) target_link_libraries(smoke_test foxglove_bridge_base) diff --git a/Dockerfile.ros1 b/Dockerfile.ros1 index e3ea1ce..7a9b1b2 100644 --- a/Dockerfile.ros1 +++ b/Dockerfile.ros1 @@ -10,7 +10,7 @@ ENV CC=clang ENV CXX=clang++ # Set environment and working directory -ENV ROS_WS /ros1_ws +ENV ROS_WS /ros_ws WORKDIR $ROS_WS # Add package.xml so we can install package dependencies. diff --git a/Dockerfile.ros2 b/Dockerfile.ros2 index 29dc30b..abd8641 100644 --- a/Dockerfile.ros2 +++ b/Dockerfile.ros2 @@ -10,7 +10,7 @@ ENV CC=clang ENV CXX=clang++ # Set environment and working directory -ENV ROS_WS /ros2_ws +ENV ROS_WS /ros_ws WORKDIR $ROS_WS # Install system dependencies diff --git a/foxglove_bridge_base/include/foxglove_bridge/websocket_server.hpp b/foxglove_bridge_base/include/foxglove_bridge/websocket_server.hpp index a642dee..bac5ddd 100644 --- a/foxglove_bridge_base/include/foxglove_bridge/websocket_server.hpp +++ b/foxglove_bridge_base/include/foxglove_bridge/websocket_server.hpp @@ -759,10 +759,11 @@ inline void Server::handleBinaryMessage(ConnHandle hdl, Mes length, data}; _handlers.clientMessageHandler(clientMessage, hdl); - } catch (const ServiceError& e) { + } catch (ServiceError const& e) { sendStatusAndLogMsg(hdl, StatusLevel::Error, e.what()); - } catch (...) { - sendStatusAndLogMsg(hdl, StatusLevel::Error, "callService: Failed to execute handler"); + } catch (std::exception const& e) { + sendStatusAndLogMsg(hdl, StatusLevel::Error, + std::string{"Failed to execute client message handler: "} + e.what()); } } break; case ClientBinaryOpcode::SERVICE_CALL_REQUEST: { diff --git a/ros2_foxglove_bridge/include/foxglove_bridge/json_to_ros.hpp b/ros2_foxglove_bridge/include/foxglove_bridge/json_to_ros.hpp new file mode 100644 index 0000000..09a3fd6 --- /dev/null +++ b/ros2_foxglove_bridge/include/foxglove_bridge/json_to_ros.hpp @@ -0,0 +1,21 @@ +#pragma once + +#include + +#include + +namespace foxglove_bridge { + +/** + * Convert a JSON-serialized message with a given named schema to a ROS message + * using ros2_babel_fish. The output message is allocated as a shared pointer + * and assigned to the outputMessage argument. The return value is an optional + * exception, which if set indicates that an error occurred during the + * conversion and `outputMessage` is not valid. + */ +std::optional jsonMessageToRos( + const std::string_view jsonMessage, const std::string& schemaName, + ros2_babel_fish::BabelFish::SharedPtr babelFish, + ros2_babel_fish::CompoundMessage::SharedPtr& outputMessage); + +} // namespace foxglove_bridge diff --git a/ros2_foxglove_bridge/include/foxglove_bridge/ros2_foxglove_bridge.hpp b/ros2_foxglove_bridge/include/foxglove_bridge/ros2_foxglove_bridge.hpp index 413e104..dd29c84 100644 --- a/ros2_foxglove_bridge/include/foxglove_bridge/ros2_foxglove_bridge.hpp +++ b/ros2_foxglove_bridge/include/foxglove_bridge/ros2_foxglove_bridge.hpp @@ -20,6 +20,10 @@ #include #include +#ifdef ENABLE_JSON_MESSAGES +#include +#endif + namespace foxglove_bridge { using ConnectionHandle = websocketpp::connection_hdl; @@ -82,6 +86,9 @@ class FoxgloveBridge : public rclcpp::Node { std::atomic _subscribeGraphUpdates = false; bool _includeHidden = false; std::unique_ptr _fetchAssetQueue; +#ifdef ENABLE_JSON_MESSAGES + ros2_babel_fish::BabelFish::SharedPtr _babelFish; +#endif void subscribeConnectionGraph(bool subscribe); diff --git a/ros2_foxglove_bridge/include/ros2_babel_fish/babel_fish.hpp b/ros2_foxglove_bridge/include/ros2_babel_fish/babel_fish.hpp new file mode 100644 index 0000000..2ad3182 --- /dev/null +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/babel_fish.hpp @@ -0,0 +1,178 @@ +// Copyright (c) 2021 Stefan Fabian. All rights reserved. +// 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" +#include "ros2_babel_fish/detail/babel_fish_service_client.hpp" +#include "ros2_babel_fish/detail/babel_fish_subscription.hpp" +#include "ros2_babel_fish/idl/type_support_provider.hpp" +#include "ros2_babel_fish/messages/array_message.hpp" +#include "ros2_babel_fish/messages/compound_message.hpp" +#include "ros2_babel_fish/messages/value_message.hpp" + +namespace ros2_babel_fish { + +/*! + * Allows communication using message types that are not known at compile time. + */ +class BabelFish : public std::enable_shared_from_this { +public: + RCLCPP_SMART_PTR_DEFINITIONS(BabelFish) + + /*! + * Constructs an instance of BabelFish with a new instance of the default description provider. + * If you have to use multiple BabelFish instances, it is recommended to share the + * TypeSupportProvider to prevent multiple look ups of the same message. + */ + BabelFish(); + + 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)) { +#if RCLCPP_VERSION_MAJOR >= 9 + rclcpp::AnySubscriptionCallback> any_callback; +#else + rclcpp::AnySubscriptionCallback> any_callback( + 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); + } + + /*! + * 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. + */ + 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)); + + //! 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 = {}) { +#if RCLCPP_VERSION_MAJOR >= 9 + rclcpp::AnySubscriptionCallback> any_callback; +#else + rclcpp::AnySubscriptionCallback> any_callback( + 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)); + } + + /*! + * 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.ยด + * @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. + */ + 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); + } + + 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); + + 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); + + /*! + * Creates an empty message of the given type. + * @param type The message type, e.g.: "std_msgs/Header" + * @return An empty message of the given type + * + * @throws BabelFishException If the message description was not found + */ + CompoundMessage create_message(const std::string& type) const; + + //! @copydoc create_message + CompoundMessage::SharedPtr create_message_shared(const std::string& type) const; + + /*! + * Creates a service request message for the given service type. + * @param type The type of the service, e.g., rosapi/GetParam + * @return An empty service request message that can be used to call a service of the given type + * + * @throws BabelFishException If the service description was not found + */ + CompoundMessage create_service_request(const std::string& type) const; + + //! @copydoc create_service_request + CompoundMessage::SharedPtr create_service_request_shared(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; + + 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 + +#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 new file mode 100644 index 0000000..16a4b58 --- /dev/null +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/detail/any_service_callback.hpp @@ -0,0 +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. + +#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 { +class BabelFishService; +class CompoundMessage; + +/** + * Designed to be compatible with rclcpp::AnyServiceCallback compatible callbacks with equivalent + * tracing support. + */ +class AnyServiceCallback { + template ::value, int>::type = 0> + bool isNullptr(CallbackT&& callback) { + return !callback; + } + + 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!"); + } + 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!"); + } + } + + 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)); + } + TRACEPOINT(callback_end, static_cast(this)); + } + + 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 + } + +private: + using SharedPtrCallback = + std::function, std::shared_ptr)>; + using SharedPtrWithRequestHeaderCallback = + std::function, std::shared_ptr, + std::shared_ptr)>; + using SharedPtrDeferResponseCallback = + std::function, std::shared_ptr)>; + using SharedPtrDeferResponseCallbackWithServiceHandle = + std::function, std::shared_ptr, + std::shared_ptr)>; + + std::variant + callback_; +}; + +} // namespace ros2_babel_fish + +#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 new file mode 100644 index 0000000..25264d3 --- /dev/null +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/detail/babel_fish_action_client.hpp @@ -0,0 +1,167 @@ +// Copyright (c) 2021 Stefan Fabian. All rights reserved. +// 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 + +#include "ros2_babel_fish/messages/compound_message.hpp" + +namespace ros2_babel_fish { +struct ActionTypeSupport; + +namespace impl { +struct BabelFishAction { + using Feedback = CompoundMessage; + using Goal = CompoundMessage; + using Result = CompoundMessage; + + typedef struct Impl { + struct CancelGoalService { + using Request = CompoundMessage; + using Response = CompoundMessage; + }; + } Impl; +}; +} // namespace impl +} // namespace ros2_babel_fish + +namespace rclcpp_action { +template <> +class Client : public rclcpp_action::ClientBase { +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(Client) + + using GoalHandle = rclcpp_action::ClientGoalHandle; + using WrappedResult = GoalHandle::WrappedResult; + 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; + + /// Options for sending a goal. + /** + * This struct is used to pass parameters to the function `async_send_goal`. + */ + struct SendGoalOptions { + /// Function called when the goal is accepted or rejected. + /** + * Takes a single argument that is a goal handle shared pointer. + * If the goal is accepted, then the pointer points to a valid goal handle. + * If the goal is rejected, then pointer has the value `nullptr`. + */ + GoalResponseCallback goal_response_callback; + + /// Function called whenever feedback is received for the goal. + FeedbackCallback feedback_callback; + + /// Function called when the result for the goal is received. + 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()); + + ros2_babel_fish::CompoundMessage create_goal() const; + + std::shared_future async_send_goal( + const ros2_babel_fish::CompoundMessage& goal, const SendGoalOptions& options = {}); + + /// Asynchronously get the result for an active goal. + /** + * @throws exceptions::UnknownGoalHandleError If the goal unknown or already reached a terminal + * state, or if there was an error requesting the result. + * @param[in] goal_handle The goal handle for which to get the result. + * @param[in] result_callback Optional callback that is called when the result is received. + * 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); + + /// Asynchronously request a goal be canceled. + /** + * \throws exceptions::UnknownGoalHandleError If the goal is unknown or already reached a + * terminal state. + * \param[in] goal_handle The goal handle requesting to be canceled. + * \param[in] cancel_callback Optional callback that is called when the response is received. + * The callback takes one parameter: a shared pointer to the CancelResponse message. + * \return A future to a CancelResponse message that is set when the request has been + * acknowledged by an action server. + * See + * + * action_msgs/CancelGoal.srv. + */ + std::shared_future async_cancel_goal(GoalHandle::SharedPtr goal_handle, + CancelCallback cancel_callback = nullptr); + + /// Asynchronously request for all goals to be canceled. + /** + * \param[in] cancel_callback Optional callback that is called when the response is received. + * The callback takes one parameter: a shared pointer to the CancelResponse message. + * \return A future to a CancelResponse message that is set when the request has been + * acknowledged by an action server. + * See + * + * action_msgs/CancelGoal.srv. + */ + std::shared_future async_cancel_all_goals( + CancelCallback cancel_callback = nullptr); + + /// Asynchronously request all goals at or before a specified time be canceled. + /** + * \param[in] stamp The timestamp for the cancel goal request. + * \param[in] cancel_callback Optional callback that is called when the response is received. + * The callback takes one parameter: a shared pointer to the CancelResponse message. + * \return A future to a CancelResponse message that is set when the request has been + * acknowledged by an action server. + * See + * + * action_msgs/CancelGoal.srv. + */ + std::shared_future async_cancel_goals_before( + const rclcpp::Time& stamp, CancelCallback cancel_callback = nullptr); + +protected: + std::shared_ptr create_goal_response() const override; + + std::shared_ptr create_result_response() const override; + + std::shared_ptr create_cancel_response() const override; + + std::shared_ptr create_feedback_message() const 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 make_result_aware(GoalHandle::SharedPtr goal_handle); + + 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 ros2_babel_fish { + +using BabelFishActionClient = rclcpp_action::Client; + +} + +#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 new file mode 100644 index 0000000..59f3cd4 --- /dev/null +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/detail/babel_fish_action_server.hpp @@ -0,0 +1,32 @@ +// Copyright (c) 2021 Stefan Fabian. All rights reserved. +// 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 + +#include +#include +#include + +namespace ros2_babel_fish { + +class BabelFishService { +public: + 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::ServiceBase::ConstSharedPtr getService() const; + + rclcpp::ServiceBase::SharedPtr getService(); + +private: + rclcpp::ServiceBase::SharedPtr service_; +}; +} // namespace ros2_babel_fish + +#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 new file mode 100644 index 0000000..d44664c --- /dev/null +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/detail/babel_fish_publisher.hpp @@ -0,0 +1,56 @@ +// Copyright (c) 2021 Stefan Fabian. All rights reserved. +// 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 +#include +#include + +#include "ros2_babel_fish/idl/type_support.hpp" + +namespace rclcpp { +class SerializedMessage; +} + +namespace ros2_babel_fish { + +class BabelFishPublisher : public rclcpp::PublisherBase { +public: + 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); + + /// 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 publish(std::unique_ptr msg); + + virtual void publish(const CompoundMessage& msg); + + void publish(const rcl_serialized_message_t& serialized_msg); + + void publish(const rclcpp::SerializedMessage& serialized_msg); + +private: + void do_inter_process_publish(const CompoundMessage& msg); + + void do_serialized_publish(const rcl_serialized_message_t* serialized_msg); + + /// Copy of original options passed during construction. + /** + * It is important to save a copy of this so that the rmw payload which it + * may contain is kept alive for the duration of the publisher. + */ + const rclcpp::PublisherOptionsWithAllocator> options_; +}; +} // namespace ros2_babel_fish + +#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 new file mode 100644 index 0000000..f926c7d --- /dev/null +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/detail/babel_fish_service.hpp @@ -0,0 +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. + +#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/messages/compound_message.hpp" + +namespace ros2_babel_fish { +namespace impl { +struct BabelFishService { + using Request = CompoundMessage; + using Response = CompoundMessage; +}; +} // namespace impl + +class BabelFishService : public rclcpp::ServiceBase, + std::enable_shared_from_this { +public: + 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); + + bool take_request(CompoundMessage& request_out, rmw_request_id_t& request_id_out); + + 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; + +private: + RCLCPP_DISABLE_COPY(BabelFishService) + + ServiceTypeSupport::ConstSharedPtr type_support_; + AnyServiceCallback callback_; +}; +} // namespace ros2_babel_fish + +#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 new file mode 100644 index 0000000..63c6f4c --- /dev/null +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/detail/babel_fish_service_client.hpp @@ -0,0 +1,91 @@ +// Copyright (c) 2021 Stefan Fabian. All rights reserved. +// 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 + +#include +#include +#include + +namespace ros2_babel_fish { + +class BabelFishServiceClient : public rclcpp::ClientBase { +public: + using SharedRequest = CompoundMessage::SharedPtr; + using SharedResponse = CompoundMessage::SharedPtr; + + using Promise = std::promise; + using PromiseWithRequest = std::promise>; + using SharedPromise = std::shared_ptr; + using SharedPromiseWithRequest = std::shared_ptr; + using SharedFuture = std::shared_future; + using SharedFutureWithRequest = std::shared_future>; + using CallbackType = std::function; + using CallbackWithRequestType = std::function; + + 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); + + 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); + + 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"); + } + + SharedPromise call_promise = std::make_shared(); + SharedFuture f(call_promise->get_future()); + pending_requests_[sequence_number] = + std::make_tuple(call_promise, std::forward(cb), f); + return f; + } + + template ::value>::type* = nullptr> + SharedFutureWithRequest async_send_request(SharedRequest request, CallbackT&& cb) { + SharedPromiseWithRequest promise = std::make_shared(); + SharedFutureWithRequest future_with_request(promise->get_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); + }; + + async_send_request(request, wrapping_cb); + + return future_with_request; + } + + void handle_response(std::shared_ptr request_header, + std::shared_ptr response) override; + +private: + RCLCPP_DISABLE_COPY(BabelFishServiceClient) + + std::map> pending_requests_; + ServiceTypeSupport::ConstSharedPtr type_support_; + std::mutex pending_requests_mutex_; +}; +} // namespace ros2_babel_fish + +#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 new file mode 100644 index 0000000..9d49118 --- /dev/null +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/detail/babel_fish_subscription.hpp @@ -0,0 +1,62 @@ +// Copyright (c) 2021 Stefan Fabian. All rights reserved. +// 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 + +namespace ros2_babel_fish { +class BabelFish; + +class BabelFishSubscription : public rclcpp::SubscriptionBase { +public: + 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); + + ~BabelFishSubscription() override; + + std::shared_ptr create_message() override; + + std::shared_ptr create_serialized_message() 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_loaned_message(void* loaned_message, + const rclcpp::MessageInfo& message_info) override; + + void return_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); + + MessageTypeSupport::ConstSharedPtr get_message_type_support() const; + + std::string get_message_type() const; + +private: + RCLCPP_DISABLE_COPY(BabelFishSubscription) + + MessageTypeSupport::ConstSharedPtr type_support_; + rclcpp::AnySubscriptionCallback> callback_; +}; +} // namespace ros2_babel_fish + +#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 new file mode 100644 index 0000000..e75df35 --- /dev/null +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/detail/topic.hpp @@ -0,0 +1,45 @@ +// +// Created by Stefan Fabian on 27.07.21. +// + +#ifndef ROS2_BABEL_FISH_TOPIC_HPP +#define ROS2_BABEL_FISH_TOPIC_HPP + +#include +#include +#include + +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); + +bool wait_for_topic_and_type_nanoseconds(rclcpp::Node& node, const std::string& topic, + std::vector& types, + std::chrono::nanoseconds timeout); +} // namespace impl + +template +bool wait_for_topic( + 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)); +} + +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)) { + return impl::wait_for_topic_and_type_nanoseconds( + node, topic, types, std::chrono::duration_cast(timeout)); +} + +std::string resolve_topic(const rclcpp::Node& node, const std::string& topic); +} // namespace ros2_babel_fish + +#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 new file mode 100644 index 0000000..1d81a84 --- /dev/null +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/exceptions/babel_fish_exception.hpp @@ -0,0 +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. + +#ifndef ROS2_BABEL_FISH_BABEL_FISH_EXCEPTION_HPP +#define ROS2_BABEL_FISH_BABEL_FISH_EXCEPTION_HPP + +#include + +namespace ros2_babel_fish { + +class BabelFishException : public std::runtime_error { +public: + explicit BabelFishException(const std::string& msg) + : std::runtime_error(msg) {} +}; +} // namespace ros2_babel_fish + +#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 new file mode 100644 index 0000000..f2549ac --- /dev/null +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/idl/providers/local_type_support_provider.hpp @@ -0,0 +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. + +#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 { + +/** + * @brief Looks up message libraries that are available locally on the machine. + */ +class LocalTypeSupportProvider : public TypeSupportProvider { +public: + LocalTypeSupportProvider(); + +protected: + MessageTypeSupport::ConstSharedPtr getMessageTypeSupportImpl( + const std::string& type) const override; + + ServiceTypeSupport::ConstSharedPtr getServiceTypeSupportImpl( + const std::string& type) const override; + + ActionTypeSupport::ConstSharedPtr getActionTypeSupportImpl( + const std::string& type) const override; +}; +} // namespace ros2_babel_fish + +#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 new file mode 100644 index 0000000..47475da --- /dev/null +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/idl/serialization.hpp @@ -0,0 +1,24 @@ +// +// Created by stefan on 23.01.21. +// + +#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" + +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 + +#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 new file mode 100644 index 0000000..03c20d5 --- /dev/null +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/idl/type_support.hpp @@ -0,0 +1,140 @@ +// Copyright (c) 2021 Stefan Fabian. All rights reserved. +// 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 + +namespace ros2_babel_fish { + +struct MessageTypeSupport { + using SharedPtr = std::shared_ptr; + using ConstSharedPtr = std::shared_ptr; + + 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. + 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. + 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) {} + + const rosidl_typesupport_introspection_cpp::MessageMember* operator->() const { + return value; + } + + std::shared_ptr library; + 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)) {} + + 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); + } + + std::shared_ptr library; + const rosidl_typesupport_introspection_cpp::MessageMembers* value; +}; + +struct ServiceTypeSupport { + typedef std::shared_ptr SharedPtr; + typedef std::shared_ptr ConstSharedPtr; + + 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. + 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. + 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 response() const { + const auto* service = static_cast( + introspection_type_support_handle.data); + return {service->response_members_, introspection_type_support_library}; + } +}; + +struct ActionTypeSupport { + using SharedPtr = std::shared_ptr; + using ConstSharedPtr = std::shared_ptr; + + 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. + 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. + rosidl_action_type_support_t introspection_type_support_handle; + + ServiceTypeSupport::ConstSharedPtr goal_service_type_support; + ServiceTypeSupport::ConstSharedPtr cancel_service_type_support; + ServiceTypeSupport::ConstSharedPtr result_service_type_support; + //! Feedback message with generic fields wrapping the feedback message. + MessageTypeSupport::ConstSharedPtr feedback_message_type_support; + MessageTypeSupport::ConstSharedPtr status_message_type_support; +}; +} // namespace ros2_babel_fish + +#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 new file mode 100644 index 0000000..1af2585 --- /dev/null +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/idl/type_support_provider.hpp @@ -0,0 +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. + +#ifndef ROS2_BABEL_FISH_DESCRIPTION_PROVIDER_H +#define ROS2_BABEL_FISH_DESCRIPTION_PROVIDER_H + +#include + +#include "ros2_babel_fish/idl/type_support.hpp" + +namespace ros2_babel_fish { + +class TypeSupportProvider { +public: + using SharedPtr = std::shared_ptr; + using ConstSharedPtr = std::shared_ptr; + + TypeSupportProvider(); + + MessageTypeSupport::ConstSharedPtr getMessageTypeSupport(const std::string& type) const; + + ServiceTypeSupport::ConstSharedPtr getServiceTypeSupport(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; + + virtual ServiceTypeSupport::ConstSharedPtr getServiceTypeSupportImpl( + 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; + + 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; + +private: + 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 + +#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 new file mode 100644 index 0000000..ba60b29 --- /dev/null +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/macros.hpp @@ -0,0 +1,409 @@ +// Copyright (c) 2020 Stefan Fabian. All rights reserved. +// 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_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< \ + 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_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. */ \ + } + +/*! + * Call a function with the signature: + * 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). + */ +#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 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 new file mode 100644 index 0000000..7a950f6 --- /dev/null +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/messages/array_message.hpp @@ -0,0 +1,505 @@ +// Copyright (c) 2021 Stefan Fabian. All rights reserved. +// 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 + +#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 { +public: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ArrayMessageBase) + + 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 isBounded() const { + return member_->is_upper_bound_; + } + + MessageType elementType() const { + return MessageType(member_->type_id_); + } + + 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_; + } + + 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_; + } + +protected: + // Disable copy construction except for subclasses + 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); + } + + virtual void _assign(const ArrayMessageBase& other) = 0; + + MessageMemberIntrospection member_; +}; + +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::ArgumentType ArgumentType; + static_assert((FIXED_LENGTH && BOUNDED) || !FIXED_LENGTH, + "Fixed length can only be true if bounded is also true!"); + +public: + RCLCPP_SMART_PTR_DEFINITIONS(ArrayMessage_) + + ArrayMessage_(const ArrayMessage_&) = delete; + + explicit ArrayMessage_(MessageMemberIntrospection member, std::shared_ptr data) + : ArrayMessageBase(std::move(member), std::move(data)) {} + + ~ArrayMessage_() override {} + + template + typename std::enable_if::value, Reference>::type + 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]; + } + return *reinterpret_cast(member_->get_function(data_.get(), index)); + } + + template + typename std::enable_if::value, Reference>::type + 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]; + } + + 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]; + } + return *reinterpret_cast( + member_->get_const_function(data_.get(), index)); + } + + Reference at(size_t index) { + 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. + */ + 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); + } + + //! Alias for assign + 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!"); + // 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!"); + } + reinterpret_cast*>(data_.get())->push_back(value); + } + + //! Alias for push_back + 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 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_->resize_function == nullptr) + reinterpret_cast*>(data_.get())->resize(length); + else + member_->resize_function(data_.get(), length); + } + + 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()); + } + + void clear() { + if (FIXED_LENGTH) { + throw BabelFishException("Can not clear fixed length array!"); + } else { + resize(0); + } + } + +protected: + void _assign(const ArrayMessageBase& other) override { + if (other.isBounded()) { + if (other.isFixedSize()) { + _assignImpl(other); + return; + } + _assignImpl(other); + return; + } + _assignImpl(other); + } + + 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!"); + } + 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); + } + 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 true; + } +}; + +template +using ArrayMessage = ArrayMessage_; + +template +using FixedLengthArrayMessage = ArrayMessage_; + +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!"); + +public: + 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. + * + * @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. + */ + + explicit CompoundArrayMessage_(MessageMemberIntrospection member, std::shared_ptr data) + : ArrayMessageBase(std::move(member), std::move(data)) { + values_.resize(member_->size_function(data_.get())); + } + + ~CompoundArrayMessage_() override {} + + CompoundMessage& operator[](size_t index) { + return getImplementation(index); + } + + const CompoundMessage& operator[](size_t index) const { + return getImplementation(index); + } + + CompoundMessage& at(size_t index) { + 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. + */ + 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!"); + } + const size_t index = size(); + resize(index + 1); + assign(index, value); + } + + //! Alias for push_back + void append(const CompoundMessage& value) { + push_back(value); + } + + //! Creates a new CompoundMessage, appends it and returns a reference to it. + CompoundMessage& appendEmpty() { + size_t index = size(); + 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 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); + // 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); + } + } + + std::vector values() { + ensureInitialized(); + return values_; + } + + std::vector values() const { + ensureInitialized(); + return {values_.begin(), values_.end()}; + } + + 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!"); + } else { + 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); + } + } + +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(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)); + } + } + + CompoundMessage& getImplementation(size_t index) { + ensureInitialized(index); + return *values_[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); + return; + } + _assignImpl(other); + return; + } + _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!"); + } + 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); + } + 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 true; + } + + mutable std::vector values_; +}; + +using CompoundArrayMessage = CompoundArrayMessage_; + +using FixedLengthCompoundArrayMessage = CompoundArrayMessage_; + +using BoundedCompoundArrayMessage = CompoundArrayMessage_; +} // namespace ros2_babel_fish + +#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 new file mode 100644 index 0000000..d1c86bb --- /dev/null +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/messages/compound_message.hpp @@ -0,0 +1,111 @@ +// Copyright (c) 2021 Stefan Fabian. All rights reserved. +// 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 + +#include + +#include "message.hpp" +#include "ros2_babel_fish/idl/type_support.hpp" + +namespace ros2_babel_fish { + +class CompoundMessage final : public Message { +public: + RCLCPP_SMART_PTR_DEFINITIONS(CompoundMessage) + + //! Creates an invalid instance of a compound message + CompoundMessage(); + + CompoundMessage(MessageMembersIntrospection members, std::shared_ptr data); + + 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(CompoundMessage&&) = default; + + ~CompoundMessage() override = default; + + //! Datatype of the message, e.g., geometry_msgs::msg::Pose. + std::string datatype() const; + + //! Name of the message, e.g., geometry_msgs/msg/Pose. + std::string name() const; + + bool containsKey(const std::string& key) const; + + std::vector keys() const; + + std::string keyAt(size_t index); + + uint32_t memberCount() const { + return members_->member_count_; + } + + CompoundMessage& operator=(const Message& other); + + CompoundMessage& operator=(const builtin_interfaces::msg::Time& value); + + CompoundMessage& operator=(const builtin_interfaces::msg::Duration& value); + + CompoundMessage& operator=(const rclcpp::Time& value); + + CompoundMessage& operator=(const rclcpp::Duration& value); + + CompoundMessage& operator=(const CompoundMessage& other); + + CompoundMessage& operator=(CompoundMessage&& other) noexcept; + + /*! + * Accesses submessage with the given key. + * @throws std::out_of_range If key is not in message. + * @throws std::runtime_error If message was not initialized for key. + * @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; + + const Message& operator[](const std::string& key) const override; + + std::vector values(); + + std::vector values() const; + + std::shared_ptr type_erased_message() const; + + std::shared_ptr type_erased_message(); + + //! Creates a copy of this compound message + CompoundMessage clone() const; + + bool isValid() const; + +protected: + void onMoved() override; + + void initValues() const; + + Message::SharedPtr operator[](size_t index); + + Message::ConstSharedPtr operator[](size_t index) const; + + bool _isMessageEqual(const Message& other) const override; + + void _assign(const Message& other) override; + + MessageMembersIntrospection members_; + mutable std::vector values_; + mutable bool initialized_values_ = false; +}; +} // namespace ros2_babel_fish + +#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 new file mode 100644 index 0000000..44e2376 --- /dev/null +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/messages/message.hpp @@ -0,0 +1,415 @@ +// Copyright (c) 2021 Stefan Fabian. All rights reserved. +// 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 + +#include +#include +#include +#include + +#include "ros2_babel_fish/exceptions/babel_fish_exception.hpp" +#include "ros2_babel_fish/macros.hpp" + +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 { +public: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Message) + + explicit Message(MessageType type, std::shared_ptr data); + + virtual ~Message(); + + MessageType type() const { + return type_; + } + + /** + * Convenience method to obtain the content of a ValueMessage as the given type. + * A type conversion is done if the type doesn't match exactly. If the target type can not fit the + * source type, a warning is printed. + * + * @tparam T The type as which the value is retrieved + * @return The value casted to the given type T + * + * @throws BabelFishException If the message is not a ValueMessage + * @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!"); + return *result; + } + + /*! + * Convenience method that casts the message to the given type. + * Example: + * @code + * Message &msg = getMessage(); + * CompoundMessage &compound = msg.as(); + * @endcode + * @tparam T Target type + * @return Message casted to the target type as reference + * + * @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!"); + 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!"); + return *result; + } + + //! @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. + bool isDuration() const; + + /** + * Convenience method to access the child with the given key of a CompoundMessage. + * @param key The name or path of the child + * @return The child message accessed by the given key + * + * @throws BabelFishException If child access by key is not supported. + */ + virtual Message& operator[](const std::string& key); + + //!@copydoc Message::operator[](const std::string&) + 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. + * + * @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=(uint8_t value); + + Message& operator=(uint16_t value); + + Message& operator=(uint32_t value); + + Message& operator=(uint64_t value); + + Message& operator=(int8_t value); + + Message& operator=(int16_t value); + + Message& operator=(int32_t value); + + Message& operator=(int64_t value); + + Message& operator=(float value); + + Message& operator=(double value); + + Message& operator=(long double value); + + Message& operator=(const char* value); + + Message& operator=(const std::string& value); + + Message& operator=(const std::wstring& value); + + Message& operator=(const builtin_interfaces::msg::Time& value); + + Message& operator=(const builtin_interfaces::msg::Duration& value); + + Message& operator=(const rclcpp::Time& value); + + Message& operator=(const rclcpp::Duration& value); + + Message& operator=(const Message& other); + + /**@}*/ + + template + bool operator==(const T& other) const; + + bool operator==(const Message& other) const; + + bool operator==(const char* c) const; + + bool operator==(const wchar_t* c) const; + + template + bool operator!=(const T& other) const; + +protected: + void move(std::shared_ptr new_data) { + data_ = std::move(new_data); + onMoved(); + } + + virtual void onMoved() {} + + 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_) {} + + virtual bool _isMessageEqual(const Message& other) const = 0; + + virtual void _assign(const Message& other) = 0; + + unsigned char* data_ptr() { + return reinterpret_cast(data_.get()); + } + + const unsigned char* data_ptr() const { + return reinterpret_cast(data_.get()); + } + + std::shared_ptr data_; + MessageType type_; + + friend class CompoundMessage; + + template + friend class CompoundArrayMessage_; +}; + +template <> +bool Message::value() const; + +template <> +uint8_t Message::value() const; + +template <> +uint16_t Message::value() const; + +template <> +uint32_t Message::value() const; + +template <> +uint64_t Message::value() const; + +template <> +int8_t Message::value() const; + +template <> +int16_t Message::value() const; + +template <> +int32_t Message::value() const; + +template <> +int64_t Message::value() const; + +template <> +float Message::value() const; + +template <> +double Message::value() const; + +template <> +long double Message::value() const; + +template <> +std::string Message::value() const; + +template <> +char16_t Message::value() const; + +template <> +std::wstring Message::value() const; + +// These are now compound messages instead of value messages +template <> +rclcpp::Time Message::value() const; + +template <> +rclcpp::Duration Message::value() const; + +// Equality comparison needs to come after value specializations + +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 +struct EqHelper { + template + using is_same_sign = + std::integral_constant::value == std::is_signed::value>; + + template + static typename std::enable_if::value && is_same_sign::value, void>::type + equal(const Message* m, const U& other, bool& result) { + result = m->template value() == 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) { + // Different sign and U signed so T is unsigned + 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). + 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; + } + + 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())) + result = false; + else + result = m->template value() == static_cast(other); + } + + template + static typename std::enable_if::value, void>::type equal(const Message*, + const U&, + bool& result) { + result = false; + } +}; + +template <> +struct EqHelper { + 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) { + result = false; + } +}; + +template <> +struct EqHelper { + 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) { + result = false; + } +}; + +template <> +struct EqHelper { + 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) { + result = false; + } +}; + +template <> +struct EqHelper { + 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) { + result = m->template value() == other; + } +}; + +template <> +struct EqHelper { + 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) { + result = false; + } +}; +} // namespace impl + +template +void Message::checkValueEqual(const U& other, bool& result) const { + impl::EqHelper::equal(this, other, result); +} + +template +bool Message::operator==(const T& other) const { + bool result = false; + RBF2_TEMPLATE_CALL(checkValueEqual, type_, other, result); + return result; +} + +template +bool Message::operator!=(const T& other) const { + return !(*this == other); +} +} // namespace ros2_babel_fish + +#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 new file mode 100644 index 0000000..c825021 --- /dev/null +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/messages/message_type_traits.hpp @@ -0,0 +1,147 @@ +// +// Created by stefan on 23.01.21. +// + +#ifndef ROS2_BABEL_FISH_MESSAGE_TYPE_TRAITS_HPP +#define ROS2_BABEL_FISH_MESSAGE_TYPE_TRAITS_HPP + +#include +#include + +#include "ros2_babel_fish/messages/message_types.hpp" + +namespace ros2_babel_fish { +// Predeclare message classes +class Message; + +class ArrayMessageBase; + +class CompoundMessage; + +namespace message_type_traits { +template +struct message_type { + static constexpr MessageType value = MessageTypes::None; +}; + +template +struct member_type { + typedef void value; +}; + +template +struct value_type { + typedef void value; +}; + +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; \ + } +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; \ + } +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; \ + } +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 +struct array_type { + typedef T& Reference; + typedef T ReturnType; + typedef const T ConstReturnType; + typedef const T& ArgumentType; +}; +template <> +struct array_type { + typedef std::vector::reference Reference; + typedef bool ReturnType; + typedef bool ConstReturnType; + typedef bool ArgumentType; +}; +template <> +struct array_type { + typedef void Reference; + typedef void ReturnType; + typedef const void ConstReturnType; + typedef void ArgumentType; +}; +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 + +#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 new file mode 100644 index 0000000..ae19ff6 --- /dev/null +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/messages/message_types.hpp @@ -0,0 +1,39 @@ +// Copyright (c) 2021 Stefan Fabian. All rights reserved. +// 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 MessageTypes { +enum MessageType : uint8_t { + None = 0x00000, + Float = ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT, + Double = ::rosidl_typesupport_introspection_cpp::ROS_TYPE_DOUBLE, + LongDouble = ::rosidl_typesupport_introspection_cpp::ROS_TYPE_LONG_DOUBLE, + Char = ::rosidl_typesupport_introspection_cpp::ROS_TYPE_CHAR, + WChar = ::rosidl_typesupport_introspection_cpp::ROS_TYPE_WCHAR, + Bool = ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOLEAN, + Octet = ::rosidl_typesupport_introspection_cpp::ROS_TYPE_OCTET, + UInt8 = ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8, + Int8 = ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8, + UInt16 = ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16, + Int16 = ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16, + UInt32 = ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32, + Int32 = ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32, + UInt64 = ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64, + Int64 = ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64, + String = ::rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING, + WString = ::rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING, + Compound = ::rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE, + Array = 200, +}; +} +typedef MessageTypes::MessageType MessageType; +} // namespace ros2_babel_fish + +#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 new file mode 100644 index 0000000..d5b2cd7 --- /dev/null +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/messages/value_message.hpp @@ -0,0 +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. + +#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" + +namespace ros2_babel_fish { + +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!"); + +public: + RCLCPP_SMART_PTR_DEFINITIONS(ValueMessage) + + 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_); + } + + void setValue(T value) { + *reinterpret_cast(this->data_ptr() + member_->offset_) = value; + } + + ValueMessage& operator=(const T& value) { + setValue(value); + return *this; + } + + 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()); + } + + 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>(); + 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::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 + +#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 new file mode 100644 index 0000000..257f64a --- /dev/null +++ b/ros2_foxglove_bridge/include/ros2_babel_fish/method_invoke_helpers.hpp @@ -0,0 +1,551 @@ +// +// Created by Stefan Fabian on 02.08.21. +// + +#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" + +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 + +#define RBF2_INVOKE_FN std::invoke + +#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)...); +} + +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 +#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. + * @return The return value of the invoked method. + */ +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); + +/*! + * @brief Invokes the given functor with the given value message casted to the actual type of the + * given message and the optionally passed additional arguments. + * + * @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); + +//! @copydoc invoke_for_value_message +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 + * array message and the optionally passed additional arguments. + * + * @return The return value of the invoked method. + */ +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); + +// ============================================== +// =============== IMPLEMENTATION =============== +// ============================================== + +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!"); + } +} + +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!"); + } +} + +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!"); + } +} + +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!"); + } +} + +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( + f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), + 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( + f, + msg.as::value, BOUNDED, FIXED_LENGTH>>(), + 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( + f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), + 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( + f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), + 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( + f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), + 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( + f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), + 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( + f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), + 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( + f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), + 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( + 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!"); + } +} + +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( + f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), + 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( + f, + msg.as::value, BOUNDED, FIXED_LENGTH>>(), + 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( + f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), + 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( + f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), + 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( + f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), + 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( + f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), + 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( + f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), + 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( + f, msg.as::value, BOUNDED, FIXED_LENGTH>>(), + 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( + 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!"); + } +} +} // 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(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 + +#undef RBF2_INVOKE_FN + +#endif // ROS2_BABEL_FISH_METHOD_INVOKE_HELPERS_HPP diff --git a/ros2_foxglove_bridge/src/json_to_ros.cpp b/ros2_foxglove_bridge/src/json_to_ros.cpp new file mode 100644 index 0000000..55eab72 --- /dev/null +++ b/ros2_foxglove_bridge/src/json_to_ros.cpp @@ -0,0 +1,139 @@ +#ifdef ENABLE_JSON_MESSAGES + +#include "foxglove_bridge/json_to_ros.hpp" + +#include + +namespace foxglove_bridge { + +static void assignJsonToMessageField(ros2_babel_fish::Message& field, const nlohmann::json& value) { + using MessageType = ros2_babel_fish::MessageType; + + if (value.is_null()) { + return; + } + + switch (field.type()) { + case MessageType::None: + break; + case MessageType::Float: + field = value.get(); + break; + case MessageType::Double: + field = value.get(); + break; + case MessageType::LongDouble: + field = value.get(); + break; + case MessageType::Char: + field = value.get(); + break; + case MessageType::WChar: + field = value.get(); + break; + case MessageType::Bool: + field = value.get(); + break; + case MessageType::Octet: + case MessageType::UInt8: + field = value.get(); + break; + case MessageType::Int8: + field = value.get(); + break; + case MessageType::UInt16: + field = value.get(); + break; + case MessageType::Int16: + field = value.get(); + break; + case MessageType::UInt32: + field = value.get(); + break; + case MessageType::Int32: + field = value.get(); + break; + case MessageType::UInt64: + field = value.get(); + break; + case MessageType::Int64: + field = value.get(); + break; + case MessageType::String: + field = value.get(); + break; + case MessageType::WString: + field = value.get(); + break; + case MessageType::Compound: { + // Recursively convert compound messages + auto& compound = field.as(); + for (const auto& key : compound.keys()) { + assignJsonToMessageField(compound[key], value[key]); + } + break; + } + case MessageType::Array: { + // Ensure the JSON value is an array + if (!value.is_array()) { + break; + } + + auto& array = field.as(); + if (array.isFixedSize() || array.isBounded()) { + const size_t limit = std::min(array.maxSize(), value.size()); + for (size_t i = 0; i < limit; ++i) { + auto& arrayEntry = array.size() > i ? array[i] : array.appendEmpty(); + assignJsonToMessageField(arrayEntry, value[i]); + } + } else { + array.clear(); + for (const auto& jsonArrayEntry : value) { + auto& arrayEntry = array.appendEmpty(); + assignJsonToMessageField(arrayEntry, jsonArrayEntry); + } + } + break; + } + } +} + +std::optional jsonMessageToRos( + const std::string_view jsonMessage, const std::string& schemaName, + ros2_babel_fish::BabelFish::SharedPtr babelFish, + ros2_babel_fish::CompoundMessage::SharedPtr& outputMessage) { + // Decode the JSON message + nlohmann::json json; + try { + json = nlohmann::json::parse(jsonMessage); + } catch (const nlohmann::json::parse_error& e) { + return e; + } + + // Convert the JSON message to a ROS message using ros2_babel_fish + ros2_babel_fish::CompoundMessage::SharedPtr rosMsgPtr; + try { + rosMsgPtr = babelFish->create_message_shared(schemaName); + } catch (const ros2_babel_fish::BabelFishException& e) { + return e; + } + auto& rosMsg = *rosMsgPtr; + for (const auto& key : rosMsg.keys()) { + if (!json.contains(key)) { + continue; + } + + try { + assignJsonToMessageField(rosMsg[key], json[key]); + } catch (const std::exception& e) { + return e; + } + } + + outputMessage = rosMsgPtr; + return std::nullopt; +} + +} // namespace foxglove_bridge + +#endif diff --git a/ros2_foxglove_bridge/src/ros2_babel_fish/babel_fish.cpp b/ros2_foxglove_bridge/src/ros2_babel_fish/babel_fish.cpp new file mode 100644 index 0000000..e8bb0a4 --- /dev/null +++ b/ros2_foxglove_bridge/src/ros2_babel_fish/babel_fish.cpp @@ -0,0 +1,259 @@ +// Copyright (c) 2021 Stefan Fabian. All rights reserved. +// Licensed under the MIT license. See LICENSE file in the project root for full license +// information. + +#include "ros2_babel_fish/babel_fish.hpp" + +#include +#include + +#include "logging.hpp" +#include "ros2_babel_fish/detail/babel_fish_service_client.hpp" +#include "ros2_babel_fish/detail/topic.hpp" +#include "ros2_babel_fish/exceptions/babel_fish_exception.hpp" +#include "ros2_babel_fish/idl/providers/local_type_support_provider.hpp" + +namespace ros2_babel_fish { + +BabelFish::BabelFish() { + type_support_providers_.push_back(std::make_shared()); +} + +BabelFish::BabelFish(std::vector type_support_providers) + : type_support_providers_(std::move(type_support_providers)) {} + +BabelFish::~BabelFish() = default; + +BabelFishSubscription::SharedPtr BabelFish::create_subscription( + rclcpp::Node& node, const std::string& topic, const rclcpp::QoS& qos, + rclcpp::AnySubscriptionCallback> callback, + rclcpp::CallbackGroup::SharedPtr group, rclcpp::SubscriptionOptions options, + std::chrono::nanoseconds timeout) { + const std::string& resolved_topic = resolve_topic(node, topic); + std::vector types; + if (!wait_for_topic_and_type(node, resolved_topic, types, timeout)) return nullptr; + if (types.empty()) { + RBF2_ERROR("Could not subscribe to '%s'.Topic is available but has no type!", + resolved_topic.c_str()); + return nullptr; + } + + if (types.size() > 1) { + RBF2_INFO("Topic '%s' has more than one type. Selecting the first arbitrarily: '%s'.", + resolved_topic.c_str(), types[0].c_str()); + } + + MessageTypeSupport::ConstSharedPtr type_support = get_message_type_support(types[0]); + if (type_support == nullptr) { + throw BabelFishException("Failed to create a subscriber for type: " + types[0] + + ". Type not found!"); + } + try { + // TODO Add support for topic statistics? + auto subscription = + std::make_shared(node.get_node_base_interface().get(), type_support, + topic, qos, std::move(callback), std::move(options)); + node.get_node_topics_interface()->add_subscription(subscription, std::move(group)); + return subscription; + } catch (const std::runtime_error& ex) { + throw BabelFishException("Failed to create Subscription: " + std::string(ex.what())); + } +} + +BabelFishSubscription::SharedPtr BabelFish::create_subscription( + rclcpp::Node& node, const std::string& topic, const std::string& type, const rclcpp::QoS& qos, + rclcpp::AnySubscriptionCallback> callback, + rclcpp::CallbackGroup::SharedPtr group, rclcpp::SubscriptionOptions options) { + const std::string& resolved_topic = resolve_topic(node, topic); + + MessageTypeSupport::ConstSharedPtr type_support = get_message_type_support(type); + if (type_support == nullptr) { + throw BabelFishException("Failed to create a subscriber for type: " + type + + ". Type not found!"); + } + try { + auto subscription = + std::make_shared(node.get_node_base_interface().get(), type_support, + topic, qos, std::move(callback), std::move(options)); + + node.get_node_topics_interface()->add_subscription(subscription, std::move(group)); + return subscription; + } catch (const std::runtime_error& ex) { + throw BabelFishException("Failed to create Subscription: " + std::string(ex.what())); + } +} + +BabelFishPublisher::SharedPtr BabelFish::create_publisher(rclcpp::Node& node, + const std::string& topic, + const std::string& type, + const rclcpp::QoS& qos, + rclcpp::PublisherOptions options) { + // Extract the NodeTopicsInterface from the NodeT. + using rclcpp::node_interfaces::get_node_topics_interface; + auto node_topics = get_node_topics_interface(node); + + options.use_intra_process_comm = + rclcpp::IntraProcessSetting::Disable; // Currently not supported by ROS2 for serialized + // messages + MessageTypeSupport::ConstSharedPtr type_support = get_message_type_support(type); + if (type_support == nullptr) { + throw BabelFishException("Failed to create a publisher for type: " + type + + ". Type not found!"); + } + auto result = BabelFishPublisher::make_shared( + node.get_node_base_interface().get(), type_support->type_support_handle, topic, qos, options); + result->post_init_setup(node.get_node_base_interface().get(), topic, qos, options); + // Add the publisher to the node topics interface. + node_topics->add_publisher(result, options.callback_group); + return result; +} + +BabelFishService::SharedPtr BabelFish::create_service(rclcpp::Node& node, + const std::string& service_name, + const std::string& type, + AnyServiceCallback callback, + const rmw_qos_profile_t& qos_profile, + rclcpp::CallbackGroup::SharedPtr group) { + ServiceTypeSupport::ConstSharedPtr type_support = get_service_type_support(type); + if (type_support == nullptr) { + throw BabelFishException("Failed to create a service for type: " + type + ". Type not found!"); + } + std::string expanded_name = resolve_topic(node, service_name); + rcl_service_options_t options = rcl_service_get_default_options(); + options.qos = qos_profile; + auto result = + BabelFishService::make_shared(node.get_node_base_interface()->get_shared_rcl_node_handle(), + expanded_name, type_support, std::move(callback), options); + node.get_node_services_interface()->add_service(result, std::move(group)); + return result; +} + +BabelFishServiceClient::SharedPtr BabelFish::create_service_client( + rclcpp::Node& node, const std::string& service_name, const std::string& type, + const rmw_qos_profile_t& qos_profile, rclcpp::CallbackGroup::SharedPtr group) { + ServiceTypeSupport::ConstSharedPtr type_support = get_service_type_support(type); + if (type_support == nullptr) { + throw BabelFishException("Failed to create a service client for type: " + type + + ". Type not found!"); + } + rcl_client_options_t options = rcl_client_get_default_options(); + options.qos = qos_profile; + auto result = BabelFishServiceClient::make_shared(node.get_node_base_interface().get(), + node.get_node_graph_interface(), service_name, + type_support, options); + + node.get_node_services_interface()->add_client(result, std::move(group)); + return result; +} + +BabelFishActionClient::SharedPtr BabelFish::create_action_client( + rclcpp::Node& node, const std::string& name, const std::string& type, + const rcl_action_client_options_t& options, rclcpp::CallbackGroup::SharedPtr group) { + ActionTypeSupport::ConstSharedPtr type_support = get_action_type_support(type); + if (type_support == nullptr) { + throw BabelFishException("Failed to create an action client for type: " + type + + ". Type not found!"); + } + std::weak_ptr weak_node = + node.get_node_waitables_interface(); + std::weak_ptr weak_group = group; + bool group_is_null = (nullptr == group.get()); + + auto deleter = [weak_node, weak_group, group_is_null](BabelFishActionClient* ptr) { + if (nullptr == ptr) { + return; + } + auto shared_node = weak_node.lock(); + if (shared_node) { + // API expects a shared pointer, give it one with a deleter that does nothing. + std::shared_ptr fake_shared_ptr(ptr, [](BabelFishActionClient*) {}); + + if (group_is_null) { + // Was added to default group + shared_node->remove_waitable(fake_shared_ptr, nullptr); + } else { + // Was added to a specific group + auto shared_group = weak_group.lock(); + if (shared_group) { + shared_node->remove_waitable(fake_shared_ptr, shared_group); + } + } + } + delete ptr; + }; + + std::shared_ptr action_client( + new BabelFishActionClient(node.get_node_base_interface(), node.get_node_graph_interface(), + node.get_node_logging_interface(), name, type_support, options), + deleter); + + node.get_node_waitables_interface()->add_waitable(action_client, std::move(group)); + return action_client; +} + +CompoundMessage BabelFish::create_message(const std::string& type) const { + MessageTypeSupport::ConstSharedPtr type_support = get_message_type_support(type); + if (type_support == nullptr) { + throw BabelFishException("BabelFish doesn't know a message of type: " + type); + } + return CompoundMessage(*type_support); +} + +CompoundMessage::SharedPtr BabelFish::create_message_shared(const std::string& type) const { + MessageTypeSupport::ConstSharedPtr type_support = get_message_type_support(type); + if (type_support == nullptr) { + throw BabelFishException("BabelFish doesn't know a message of type: " + type); + } + return CompoundMessage::make_shared(*type_support); +} + +CompoundMessage BabelFish::create_service_request(const std::string& type) const { + const ServiceTypeSupport::ConstSharedPtr& type_support = get_service_type_support(type); + if (type_support == nullptr) { + throw BabelFishException("BabelFish doesn't know a service of type: " + type); + } + return CompoundMessage(type_support->request()); +} + +CompoundMessage::SharedPtr BabelFish::create_service_request_shared(const std::string& type) const { + const ServiceTypeSupport::ConstSharedPtr& type_support = get_service_type_support(type); + if (type_support == nullptr) { + throw BabelFishException("BabelFish doesn't know a service of type: " + type); + } + return CompoundMessage::make_shared(type_support->request()); +} + +MessageTypeSupport::ConstSharedPtr BabelFish::get_message_type_support( + const std::string& type) const { + for (const auto& provider : type_support_providers_) { + MessageTypeSupport::ConstSharedPtr result = provider->getMessageTypeSupport(type); + if (result == nullptr) continue; + return result; + } + return nullptr; +} + +ServiceTypeSupport::ConstSharedPtr BabelFish::get_service_type_support( + const std::string& type) const { + for (const auto& provider : type_support_providers_) { + ServiceTypeSupport::ConstSharedPtr result = provider->getServiceTypeSupport(type); + if (result == nullptr) continue; + return result; + } + return nullptr; +} + +ActionTypeSupport::ConstSharedPtr BabelFish::get_action_type_support( + const std::string& type) const { + for (const auto& provider : type_support_providers_) { + ActionTypeSupport::ConstSharedPtr result = provider->getActionTypeSupport(type); + if (result == nullptr) continue; + return result; + } + return nullptr; +} + +std::vector BabelFish::type_support_providers() { + return type_support_providers_; +} +} // namespace ros2_babel_fish diff --git a/ros2_foxglove_bridge/src/ros2_babel_fish/detail/babel_fish_action_client.cpp b/ros2_foxglove_bridge/src/ros2_babel_fish/detail/babel_fish_action_client.cpp new file mode 100644 index 0000000..83c6f37 --- /dev/null +++ b/ros2_foxglove_bridge/src/ros2_babel_fish/detail/babel_fish_action_client.cpp @@ -0,0 +1,252 @@ +// Copyright (c) 2021 Stefan Fabian. All rights reserved. +// Licensed under the MIT license. See LICENSE file in the project root for full license +// information. + +#include "ros2_babel_fish/detail/babel_fish_action_client.hpp" + +#include + +#include "ros2_babel_fish/idl/serialization.hpp" +#include "ros2_babel_fish/messages/array_message.hpp" +#include "ros2_babel_fish/messages/compound_message.hpp" + +using namespace ros2_babel_fish; + +namespace rclcpp_action { + +Client::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, ros2_babel_fish::ActionTypeSupport::ConstSharedPtr type_support, + const rcl_action_client_options_t& client_options) + : ClientBase(node_base, node_graph, node_logging, action_name, + &type_support->type_support_handle, client_options) + , type_support_(std::move(type_support)) {} + +CompoundMessage Client::create_goal() const { + MessageMembersIntrospection introspection = type_support_->goal_service_type_support->request(); + size_t index = + std::find_if(introspection->members_, introspection->members_ + introspection->member_count_, + [](const auto& a) { + return std::strcmp(a.name_, "goal") == 0; + }) - + introspection->members_; + return CompoundMessage(type_support_->goal_service_type_support->request().getMember(index)); +} + +std::shared_future< + rclcpp_action::ClientGoalHandle::SharedPtr> +Client::async_send_goal(const CompoundMessage& goal, + const SendGoalOptions& options) { + auto promise = std::make_shared>(); + auto future = promise->get_future(); + CompoundMessage goal_request(type_support_->goal_service_type_support->request()); + auto uuid = this->generate_goal_id(); + auto& request_uuid = goal_request["goal_id"]["uuid"].as>(); + for (size_t i = 0; i < uuid.size(); ++i) request_uuid[i] = uuid[i]; + goal_request["goal"] = goal; + this->send_goal_request( + goal_request.type_erased_message(), + [this, promise, options = options, uuid](std::shared_ptr response) { + CompoundMessage goal_response(type_support_->goal_service_type_support->response(), response); + if (!goal_response["accepted"].value()) { + promise->set_value(nullptr); + if (options.goal_response_callback) options.goal_response_callback(nullptr); + return; + } + rclcpp_action::GoalInfo goal_info; + goal_info.goal_id.uuid = uuid; + goal_info.stamp = goal_response["stamp"].value(); + std::shared_ptr goal_handle( + new GoalHandle(goal_info, options.feedback_callback, options.result_callback)); + { + std::lock_guard guard(goal_handles_mutex_); + goal_handles_[uuid] = goal_handle; + } + promise->set_value(goal_handle); + if (options.goal_response_callback) options.goal_response_callback(goal_handle); + + if (options.result_callback) this->make_result_aware(goal_handle); + }); + + // Clean old goal handles + { + std::lock_guard guard(goal_handles_mutex_); + auto it = goal_handles_.begin(); + while (it != goal_handles_.end()) { + if (!it->second.lock()) + it = goal_handles_.erase(it); + else + ++it; + } + } + return future; +} + +std::shared_future< + rclcpp_action::ClientGoalHandle::WrappedResult> +Client::async_get_result(typename GoalHandle::SharedPtr goal_handle, + ResultCallback result_callback) { + std::lock_guard lock(goal_handles_mutex_); + if (goal_handles_.find(goal_handle->get_goal_id()) == goal_handles_.end()) + throw exceptions::UnknownGoalHandleError(); + +#if RCLCPP_VERSION_MAJOR >= 9 + if (goal_handle->is_invalidated()) { + throw goal_handle->invalidate_exception_; + } +#endif + if (result_callback) goal_handle->set_result_callback(result_callback); + this->make_result_aware(goal_handle); + return goal_handle->async_get_result(); +} + +std::shared_future +Client::async_cancel_goal(GoalHandle::SharedPtr goal_handle, + CancelCallback cancel_callback) { + std::lock_guard lock(goal_handles_mutex_); + if (goal_handles_.count(goal_handle->get_goal_id()) == 0) { + throw exceptions::UnknownGoalHandleError(); + } + CompoundMessage cancel_request(type_support_->cancel_service_type_support->request()); + const auto& uuid = goal_handle->get_goal_id(); + auto& request_uuid = + cancel_request["goal_info"]["goal_id"]["uuid"].as>(); + for (size_t i = 0; i < uuid.size(); ++i) request_uuid[i] = uuid[i]; + return async_cancel(cancel_request, cancel_callback); +} + +std::shared_future +Client::async_cancel_all_goals(CancelCallback cancel_callback) { + CompoundMessage cancel_request(type_support_->cancel_service_type_support->request()); + cancel_request["goal_info"]["goal_id"]["uuid"].as>().fill(0u); + return async_cancel(cancel_request, std::move(cancel_callback)); +} + +std::shared_future +Client::async_cancel_goals_before(const rclcpp::Time& stamp, + CancelCallback cancel_callback) { + CompoundMessage cancel_request(type_support_->cancel_service_type_support->request()); + cancel_request["goal_info"]["goal_id"]["uuid"].as>().fill(0u); + cancel_request["goal_info"]["stamp"] = stamp; + return async_cancel(cancel_request, cancel_callback); +} + +std::shared_future Client::async_cancel( + CompoundMessage cancel_request, CancelCallback cancel_callback) { + // Put promise in the heap to move it around. + auto promise = std::make_shared>(); + std::shared_future future(promise->get_future()); + this->send_cancel_request( + cancel_request.type_erased_message(), + [this, cancel_callback, promise](std::shared_ptr response) mutable { + CompoundMessage cancel_response(type_support_->cancel_service_type_support->response(), + response); + promise->set_value(cancel_response); + if (cancel_callback) { + cancel_callback(cancel_response); + } + }); + return future; +} + +std::shared_ptr Client::create_goal_response() const { + return createContainer(type_support_->goal_service_type_support->response()); +} + +std::shared_ptr Client::create_result_response() const { + return createContainer(type_support_->result_service_type_support->response()); +} + +std::shared_ptr Client::create_cancel_response() const { + return createContainer(type_support_->cancel_service_type_support->response()); +} + +std::shared_ptr Client::create_feedback_message() const { + return createContainer(*type_support_->feedback_message_type_support); +} + +void Client::handle_feedback_message(std::shared_ptr message) { + CompoundMessage feedback_message(*type_support_->feedback_message_type_support, message); + GoalUUID goal_id; + const auto& feedback_goal_id = + feedback_message["goal_id"]["uuid"].as>(); + for (size_t i = 0; i < goal_id.size(); ++i) goal_id[i] = feedback_goal_id[i]; + + std::lock_guard guard(goal_handles_mutex_); + // Feedback is not for one of our goal handles + auto it = goal_handles_.find(goal_id); + if (it == goal_handles_.end()) return; + GoalHandle::SharedPtr goal_handle = it->second.lock(); + if (!goal_handle) { + // No more references to goal handle => drop it + goal_handles_.erase(it); + return; + } + goal_handle->call_feedback_callback( + goal_handle, CompoundMessage::make_shared(feedback_message["feedback"].as())); +} + +std::shared_ptr Client::create_status_message() const { + return createContainer(*type_support_->status_message_type_support); +} + +void Client::handle_status_message(std::shared_ptr message) { + CompoundMessage status_message(*type_support_->status_message_type_support, message); + + std::lock_guard guard(goal_handles_mutex_); + const auto& status_list = status_message["status_list"].as(); + for (size_t i = 0; i < status_list.size(); ++i) { + const auto& status = status_list[i]; + GoalUUID goal_id; + const auto& status_goal_id = + status["goal_info"]["goal_id"]["uuid"].as>(); + for (size_t k = 0; k < goal_id.size(); ++k) goal_id[k] = status_goal_id[k]; + + // Status is not for one of our goal handles + auto it = goal_handles_.find(goal_id); + if (it == goal_handles_.end()) return; + GoalHandle::SharedPtr goal_handle = it->second.lock(); + if (!goal_handle) { + // No more references to goal handle => drop it + goal_handles_.erase(it); + return; + } + + goal_handle->set_status(status["status"].value()); + } +} + +void Client::make_result_aware( + GoalHandle::SharedPtr goal_handle) { + // Avoid making more than one request + if (goal_handle->set_result_awareness(true)) return; + CompoundMessage goal_result_request(type_support_->goal_service_type_support->request()); + const auto& uuid = goal_handle->get_goal_id(); + auto& request_uuid = + goal_result_request["goal_id"]["uuid"].as>(); + for (size_t i = 0; i < uuid.size(); ++i) request_uuid[i] = uuid[i]; + try { + this->send_result_request( + goal_result_request.type_erased_message(), [goal_handle, this](std::shared_ptr data) { + CompoundMessage response(type_support_->result_service_type_support->response(), data); + WrappedResult wrapped_result; + wrapped_result.result = + CompoundMessage::make_shared(response["result"].as()); + wrapped_result.goal_id = goal_handle->get_goal_id(); + wrapped_result.code = static_cast(response["status"].value()); + goal_handle->set_result(wrapped_result); + std::lock_guard lock(goal_handles_mutex_); + goal_handles_.erase(goal_handle->get_goal_id()); + }); + } catch (rclcpp::exceptions::RCLError& ex) { + // Cause exception when user tries to access result +#if RCLCPP_VERSION_MAJOR >= 9 + goal_handle->invalidate(exceptions::UnawareGoalHandleError(ex.message)); +#else + goal_handle->invalidate(); +#endif + } +} +} // namespace rclcpp_action diff --git a/ros2_foxglove_bridge/src/ros2_babel_fish/detail/babel_fish_publisher.cpp b/ros2_foxglove_bridge/src/ros2_babel_fish/detail/babel_fish_publisher.cpp new file mode 100644 index 0000000..56116d0 --- /dev/null +++ b/ros2_foxglove_bridge/src/ros2_babel_fish/detail/babel_fish_publisher.cpp @@ -0,0 +1,118 @@ +// Copyright (c) 2021 Stefan Fabian. All rights reserved. +// Licensed under the MIT license. See LICENSE file in the project root for full license +// information. + +#include "ros2_babel_fish/detail/babel_fish_publisher.hpp" + +#include +#include +#include + +namespace ros2_babel_fish { + +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) + : PublisherBase(node_base, topic, type_support, + options.to_rcl_publisher_options(qos)) + , options_(options) { + if (options_.event_callbacks.deadline_callback) { + this->add_event_handler(options_.event_callbacks.deadline_callback, + RCL_PUBLISHER_OFFERED_DEADLINE_MISSED); + } + if (options_.event_callbacks.liveliness_callback) { + this->add_event_handler(options_.event_callbacks.liveliness_callback, + RCL_PUBLISHER_LIVELINESS_LOST); + } + if (options_.event_callbacks.incompatible_qos_callback) { + this->add_event_handler(options_.event_callbacks.incompatible_qos_callback, + RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS); + } else if (options_.use_default_callbacks) { + // Register default callback when not specified + try { + this->add_event_handler( + [this](rclcpp::QOSOfferedIncompatibleQoSInfo& info) { + this->default_incompatible_qos_callback(info); + }, + RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS); + } catch (rclcpp::UnsupportedEventTypeException& /*exc*/) { + // pass + } + } + // Setup continues in the post construction method, post_init_setup(). +} + +void BabelFishPublisher::post_init_setup( + rclcpp::node_interfaces::NodeBaseInterface* node_base, const std::string& topic, + const rclcpp::QoS& qos, + const rclcpp::PublisherOptionsWithAllocator>& options) { + // Topic is unused for now. + (void)topic; + (void)options; + + // If needed, setup intra process communication. + if (rclcpp::detail::resolve_use_intra_process(options_, *node_base)) { + auto context = node_base->get_context(); + // Get the intra process manager instance for this context. + auto ipm = context->get_sub_context(); + // Register the publisher with the intra process manager. + if (qos.get_rmw_qos_profile().history == RMW_QOS_POLICY_HISTORY_KEEP_ALL) { + throw std::invalid_argument( + "intraprocess communication is not allowed with keep all history qos policy"); + } + if (qos.get_rmw_qos_profile().depth == 0) { + throw std::invalid_argument( + "intraprocess communication is not allowed with a zero qos history depth value"); + } + if (qos.get_rmw_qos_profile().durability != RMW_QOS_POLICY_DURABILITY_VOLATILE) { + throw std::invalid_argument( + "intraprocess communication allowed only with volatile durability"); + } + uint64_t intra_process_publisher_id = ipm->add_publisher(this->shared_from_this()); + this->setup_intra_process(intra_process_publisher_id, ipm); + } +} + +void BabelFishPublisher::publish(std::unique_ptr msg) { + this->do_inter_process_publish(*msg); +} + +void BabelFishPublisher::publish(const CompoundMessage& msg) { + // Intra process is currently not supported. + return this->do_inter_process_publish(msg); +} + +void BabelFishPublisher::publish(const rcl_serialized_message_t& serialized_msg) { + return this->do_serialized_publish(&serialized_msg); +} + +void BabelFishPublisher::publish(const rclcpp::SerializedMessage& serialized_msg) { + return this->do_serialized_publish(&serialized_msg.get_rcl_serialized_message()); +} + +void BabelFishPublisher::do_inter_process_publish(const CompoundMessage& msg) { + auto status = rcl_publish(publisher_handle_.get(), msg.type_erased_message().get(), nullptr); + + if (RCL_RET_PUBLISHER_INVALID == status) { + rcl_reset_error(); // next call will reset error message if not context + if (rcl_publisher_is_valid_except_context(publisher_handle_.get())) { + rcl_context_t* context = rcl_publisher_get_context(publisher_handle_.get()); + if (nullptr != context && !rcl_context_is_valid(context)) { + // publisher is invalid due to context being shutdown + return; + } + } + } + if (RCL_RET_OK != status) { + rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish message"); + } +} + +void BabelFishPublisher::do_serialized_publish(const rcl_serialized_message_t* serialized_msg) { + auto status = rcl_publish_serialized_message(publisher_handle_.get(), serialized_msg, nullptr); + if (RCL_RET_OK != status) { + rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish serialized message"); + } +} +} // namespace ros2_babel_fish diff --git a/ros2_foxglove_bridge/src/ros2_babel_fish/detail/babel_fish_service.cpp b/ros2_foxglove_bridge/src/ros2_babel_fish/detail/babel_fish_service.cpp new file mode 100644 index 0000000..e376198 --- /dev/null +++ b/ros2_foxglove_bridge/src/ros2_babel_fish/detail/babel_fish_service.cpp @@ -0,0 +1,87 @@ +// Copyright (c) 2021 Stefan Fabian. All rights reserved. +// Licensed under the MIT license. See LICENSE file in the project root for full license +// information. + +#include "ros2_babel_fish/detail/babel_fish_service.hpp" + +#include + +#include + +#include "ros2_babel_fish/idl/serialization.hpp" + +namespace ros2_babel_fish { + +BabelFishService::BabelFishService(std::shared_ptr node_base, + const std::string& service_name, + ServiceTypeSupport::ConstSharedPtr type_support, + AnyServiceCallback callback, rcl_service_options_t options) + : ServiceBase(std::move(node_base)) + , type_support_(std::move(type_support)) + , callback_(std::move(callback)) { + // rcl does the static memory allocation here + service_handle_ = std::shared_ptr( + new rcl_service_t, [handle = node_handle_](rcl_service_t* service) { + if (rcl_service_fini(service, handle.get()) != RCL_RET_OK) { + RCLCPP_ERROR(rclcpp::get_node_logger(handle.get()).get_child("rclcpp"), + "Error in destruction of rcl service handle: %s", rcl_get_error_string().str); + rcl_reset_error(); + } + delete service; + }); + *service_handle_ = rcl_get_zero_initialized_service(); + + rcl_ret_t ret = + rcl_service_init(service_handle_.get(), node_handle_.get(), &type_support_->type_support_handle, + service_name.c_str(), &options); + if (ret != RCL_RET_OK) { + if (ret == RCL_RET_SERVICE_NAME_INVALID) { + auto rcl_node_handle = get_rcl_node_handle(); + // this will throw on any validation problem + rcl_reset_error(); + rclcpp::expand_topic_or_service_name(service_name, rcl_node_get_name(rcl_node_handle), + rcl_node_get_namespace(rcl_node_handle), true); + } + + rclcpp::exceptions::throw_from_rcl_error(ret, "could not create service"); + } + TRACEPOINT(rclcpp_service_callback_added, static_cast(get_service_handle().get()), + static_cast(&callback_)); +#ifndef TRACETOOLS_DISABLED + callback_.register_callback_for_tracing(); +#endif +} + +bool BabelFishService::take_request(CompoundMessage& request_out, + rmw_request_id_t& request_id_out) { + std::shared_ptr type_erased = create_request(); + if (!take_type_erased_request(type_erased.get(), request_id_out)) return false; + request_out = CompoundMessage(type_support_->request(), std::move(type_erased)); + return true; +} + +void BabelFishService::send_response(rmw_request_id_t& request_id, CompoundMessage& response) { + rcl_ret_t ret = rcl_send_response(get_service_handle().get(), &request_id, + response.type_erased_message().get()); + + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send response"); + } +} + +std::shared_ptr BabelFishService::create_request() { + return createContainer(type_support_->request()); +} + +std::shared_ptr BabelFishService::create_request_header() { + return std::shared_ptr(); +} + +void BabelFishService::handle_request(std::shared_ptr request_header, + std::shared_ptr request) { + auto typed_request = CompoundMessage::make_shared(type_support_->request(), request); + auto response = CompoundMessage::make_shared(type_support_->response()); + callback_.dispatch(this->shared_from_this(), request_header, typed_request, response); + send_response(*request_header, *response); +} +} // namespace ros2_babel_fish diff --git a/ros2_foxglove_bridge/src/ros2_babel_fish/detail/babel_fish_service_client.cpp b/ros2_foxglove_bridge/src/ros2_babel_fish/detail/babel_fish_service_client.cpp new file mode 100644 index 0000000..c475ff2 --- /dev/null +++ b/ros2_foxglove_bridge/src/ros2_babel_fish/detail/babel_fish_service_client.cpp @@ -0,0 +1,77 @@ +// Copyright (c) 2021 Stefan Fabian. All rights reserved. +// Licensed under the MIT license. See LICENSE file in the project root for full license +// information. + +#include "ros2_babel_fish/detail/babel_fish_service_client.hpp" + +#include + +#include "ros2_babel_fish/idl/serialization.hpp" + +namespace ros2_babel_fish { + +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) + : ClientBase(node_base, std::move(node_graph)) + , type_support_(std::move(type_support)) { + rcl_ret_t ret = + rcl_client_init(this->get_client_handle().get(), this->get_rcl_node_handle(), + &type_support_->type_support_handle, service_name.c_str(), &client_options); + if (ret != RCL_RET_OK) { + if (ret == RCL_RET_SERVICE_NAME_INVALID) { + auto rcl_node_handle = this->get_rcl_node_handle(); + // this will throw on any validation problem + rcl_reset_error(); + rclcpp::expand_topic_or_service_name(service_name, rcl_node_get_name(rcl_node_handle), + rcl_node_get_namespace(rcl_node_handle), true); + } + rclcpp::exceptions::throw_from_rcl_error(ret, "could not create client"); + } +} + +bool BabelFishServiceClient::take_response(CompoundMessage& response_out, + rmw_request_id_t& request_header_out) { + std::shared_ptr type_erased = create_response(); + if (type_erased == nullptr || !take_type_erased_response(type_erased.get(), request_header_out)) + return false; + response_out = CompoundMessage(type_support_->response(), std::move(type_erased)); + return true; +} + +std::shared_ptr BabelFishServiceClient::create_response() { + return createContainer(type_support_->response()); +} + +std::shared_ptr BabelFishServiceClient::create_request_header() { + return std::make_shared(); +} + +void BabelFishServiceClient::handle_response(std::shared_ptr request_header, + std::shared_ptr response) { + std::unique_lock lock(pending_requests_mutex_); + const int64_t sequence_number = request_header->sequence_number; + auto it = pending_requests_.find(sequence_number); + if (it == pending_requests_.end()) { + RCUTILS_LOG_ERROR_NAMED("rclcpp", "Received invalid sequence number. Ignoring..."); + return; + } + auto& request = it->second; + auto call_promise = std::get<0>(request); + auto callback = std::get<1>(request); + auto future = std::get<2>(request); + pending_requests_.erase(it); + // Unlock since the callback might call this recursively + lock.unlock(); + + call_promise->set_value(CompoundMessage::make_shared(type_support_->response(), response)); + callback(future); +} + +BabelFishServiceClient::SharedFuture BabelFishServiceClient::async_send_request( + SharedRequest request) { + return async_send_request(request, [](SharedFuture) {}); +} +} // namespace ros2_babel_fish diff --git a/ros2_foxglove_bridge/src/ros2_babel_fish/detail/babel_fish_subscription.cpp b/ros2_foxglove_bridge/src/ros2_babel_fish/detail/babel_fish_subscription.cpp new file mode 100644 index 0000000..5585742 --- /dev/null +++ b/ros2_foxglove_bridge/src/ros2_babel_fish/detail/babel_fish_subscription.cpp @@ -0,0 +1,175 @@ +// +// Created by stefan on 22.01.21. +// + +#include "ros2_babel_fish/detail/babel_fish_subscription.hpp" + +#include +#include + +#include "../logging.hpp" +#include "ros2_babel_fish/idl/serialization.hpp" + +namespace ros2_babel_fish { +namespace { +rcl_subscription_options_t create_subscription_options(const rclcpp::QoS& qos) { + auto options = rcl_subscription_get_default_options(); + options.qos = qos.get_rmw_qos_profile(); + return options; +} +} // namespace + +BabelFishSubscription::BabelFishSubscription( + rclcpp::node_interfaces::NodeBaseInterface* node_base, + MessageTypeSupport::ConstSharedPtr type_support, const std::string& topic_name, + const rclcpp::QoS& qos, + rclcpp::AnySubscriptionCallback> callback, + const rclcpp::SubscriptionOptionsWithAllocator>& options) + : rclcpp::SubscriptionBase(node_base, type_support->type_support_handle, topic_name, + create_subscription_options(qos), false) + , type_support_(std::move(type_support)) + , callback_(callback) { + if (options.event_callbacks.deadline_callback) { + this->add_event_handler(options.event_callbacks.deadline_callback, + RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED); + } + if (options.event_callbacks.liveliness_callback) { + this->add_event_handler(options.event_callbacks.liveliness_callback, + RCL_SUBSCRIPTION_LIVELINESS_CHANGED); + } + if (options.event_callbacks.incompatible_qos_callback) { + this->add_event_handler(options.event_callbacks.incompatible_qos_callback, + RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS); + } else if (options.use_default_callbacks) { + // Register default callback when not specified + try { + this->add_event_handler( + [this](rclcpp::QOSRequestedIncompatibleQoSInfo& info) { + this->default_incompatible_qos_callback(info); + }, + RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS); + } catch (rclcpp::UnsupportedEventTypeException& /*exc*/) { + // pass + } + } + if (options.event_callbacks.message_lost_callback) { + this->add_event_handler(options.event_callbacks.message_lost_callback, + RCL_SUBSCRIPTION_MESSAGE_LOST); + } +#if RCLCPP_VERSION_MAJOR >= 9 + if (options.event_callbacks.message_lost_callback) { + this->add_event_handler(options.event_callbacks.message_lost_callback, + RCL_SUBSCRIPTION_MESSAGE_LOST); + } +#endif + + // Setup intra process publishing if requested. + if (rclcpp::detail::resolve_use_intra_process(options, *node_base)) { + using rclcpp::detail::resolve_intra_process_buffer_type; + + // Check if the QoS is compatible with intra-process. + auto qos_profile = get_actual_qos(); + if (qos_profile.history() != rclcpp::HistoryPolicy::KeepLast) { + throw std::invalid_argument( + "intraprocess communication is not allowed with keep all history qos policy"); + } + if (qos_profile.depth() == 0) { + throw std::invalid_argument( + "intraprocess communication is not allowed with 0 depth qos policy"); + } + if (qos_profile.durability() != rclcpp::DurabilityPolicy::Volatile) { + throw std::invalid_argument( + "intraprocess communication allowed only with volatile durability"); + } + + // TODO: Adding intra process support [very low priority] + // // First create a SubscriptionIntraProcess which will be given to the intra-process + // manager. auto context = node_base->get_context(); subscription_intra_process_ = + // std::make_shared( + // callback, + // options.get_allocator(), + // context, + // this->get_topic_name(), // important to get like this, as it has the fully-qualified + // name qos_profile, resolve_intra_process_buffer_type( options.intra_process_buffer_type, + // callback )); + // TRACEPOINT( + // rclcpp_subscription_init, + // static_cast(get_subscription_handle().get()), + // static_cast(subscription_intra_process_.get())); + // + // // Add it to the intra process manager. + // using rclcpp::experimental::IntraProcessManager; + // auto ipm = context->get_sub_context(); + // uint64_t intra_process_subscription_id = ipm->add_subscription( + // subscription_intra_process_ ); this->setup_intra_process( intra_process_subscription_id, + // ipm ); + } + + TRACEPOINT(rclcpp_subscription_init, static_cast(get_subscription_handle().get()), + static_cast(this)); + TRACEPOINT(rclcpp_subscription_callback_added, static_cast(this), + static_cast(&callback_)); + // The callback object gets copied, so if registration is done too early/before this point + // (e.g. in `AnySubscriptionCallback::set()`), its address won't match any address used later + // in subsequent tracepoints. +#ifndef TRACETOOLS_DISABLED + callback_.register_callback_for_tracing(); +#endif + RBF2_DEBUG_STREAM("Subscribed to: " << topic_name); +} + +BabelFishSubscription::~BabelFishSubscription() { + RBF2_DEBUG_STREAM("Unsubscribed from: " << get_topic_name()); +} + +std::shared_ptr BabelFishSubscription::create_message() { + return createContainer(*type_support_); +} + +std::shared_ptr BabelFishSubscription::create_serialized_message() { + return std::make_shared(0); +} + +void BabelFishSubscription::handle_message(std::shared_ptr& message, + const rclcpp::MessageInfo& message_info) { + callback_.dispatch(CompoundMessage::make_shared(*type_support_, message), message_info); +} + +void BabelFishSubscription::handle_serialized_message( + const std::shared_ptr& serialized_message, + const rclcpp::MessageInfo& message_info) { + callback_.dispatch(serialized_message, message_info); +} + +void BabelFishSubscription::handle_loaned_message(void* loaned_message, + const rclcpp::MessageInfo& message_info) { + // Not handled + (void)loaned_message; + (void)message_info; +} + +void BabelFishSubscription::return_message(std::shared_ptr& message) { + auto typed_message = std::static_pointer_cast(message); + return_serialized_message(typed_message); +} + +void BabelFishSubscription::return_serialized_message( + std::shared_ptr& message) { + message.reset(); +} + +bool BabelFishSubscription::take(CompoundMessage& message_out, rclcpp::MessageInfo& info_out) { + std::shared_ptr type_erased = create_message(); + if (type_erased == nullptr || !take_type_erased(type_erased.get(), info_out)) return false; + message_out = CompoundMessage(*type_support_, std::move(type_erased)); + return true; +} + +MessageTypeSupport::ConstSharedPtr BabelFishSubscription::get_message_type_support() const { + return type_support_; +} + +std::string BabelFishSubscription::get_message_type() const { + return type_support_->name; +} +} // namespace ros2_babel_fish diff --git a/ros2_foxglove_bridge/src/ros2_babel_fish/detail/topic.cpp b/ros2_foxglove_bridge/src/ros2_babel_fish/detail/topic.cpp new file mode 100644 index 0000000..55c8105 --- /dev/null +++ b/ros2_foxglove_bridge/src/ros2_babel_fish/detail/topic.cpp @@ -0,0 +1,99 @@ +// +// Created by Stefan Fabian on 27.07.21. +// + +#include "ros2_babel_fish/detail/topic.hpp" + +#include +#include +#include + +namespace ros2_babel_fish { +namespace impl { +namespace { +bool has_topic(const rclcpp::Node& node, const std::string& topic, + std::vector& types) { + const std::map>& topics = node.get_topic_names_and_types(); + auto it = std::find_if(topics.begin(), topics.end(), [&topic](const auto& entry) { + return entry.first == topic; + }); + if (it == topics.end()) return false; + types = it->second; + return true; +} +} // namespace + +bool wait_for_topic_and_type_nanoseconds(rclcpp::Node& node, const std::string& topic, + std::vector& types, + std::chrono::nanoseconds timeout) { + auto start = std::chrono::steady_clock::now(); + auto event = node.get_graph_event(); + if (has_topic(node, topic, types)) return true; + if (timeout == std::chrono::nanoseconds(0)) { + // check was non-blocking, return immediately + return false; + } + std::chrono::nanoseconds time_to_wait = timeout > std::chrono::nanoseconds(0) + ? timeout - (std::chrono::steady_clock::now() - start) + : std::chrono::nanoseconds::max(); + if (time_to_wait < std::chrono::nanoseconds(0)) { + // check consumed entire timeout, return immediately + return false; + } + do { + if (!rclcpp::ok()) { + return false; + } + // Limit each wait to 100ms to workaround an issue specific to the Connext RMW implementation. + // A race condition means that graph changes for services becoming available may trigger the + // wait set to wake up, but then not be reported as ready immediately after the wake up + // (see https://github.com/ros2/rmw_connext/issues/201) + // If no other graph events occur, the wait set will not be triggered again until the timeout + // has been reached, despite the service being available, so we artificially limit the wait + // time to limit the delay. + node.wait_for_graph_change(event, + std::min(time_to_wait, std::chrono::nanoseconds(RCL_MS_TO_NS(100)))); + // Because of the aforementioned race condition, we check if the topic is available even if the + // graph event wasn't triggered. + event->check_and_clear(); + if (has_topic(node, topic, types)) return true; + + // topic not available, wait if a timeout was specified + if (timeout > std::chrono::nanoseconds(0)) { + time_to_wait = timeout - (std::chrono::steady_clock::now() - start); + } + } while (time_to_wait > std::chrono::nanoseconds(0)); + return false; // timeout exceeded while waiting for the topic +} + +bool wait_for_topic_nanoseconds(rclcpp::Node& node, const std::string& topic, + std::chrono::nanoseconds timeout) { + std::vector types; + return wait_for_topic_and_type_nanoseconds(node, topic, types, timeout); +} +} // namespace impl + +std::string resolve_topic(const rclcpp::Node& node, const std::string& topic) { + std::string resolved_topic = + rclcpp::extend_name_with_sub_namespace(topic, node.get_sub_namespace()); + // Until >=galactic we need to expand it manually // TODO Check how to solve this in galactic + if (!resolved_topic.empty() && resolved_topic.front() == '~') { + resolved_topic = std::string(node.get_fully_qualified_name()) + resolved_topic.substr(1); + } + // char * output_cstr = nullptr; + // auto allocator = rcl_get_default_allocator(); + // rcl_ret_t ret = rcl_node_resolve_name( + // node.get(), + // topic_.c_str(), + // allocator, + // false, + // true, + // &output_cstr); + // if (RCL_RET_OK != ret) { + // throw_from_rcl_error(ret, "failed to resolve name", rcl_get_error_state()); + // } + // topic_ = output_cstr + // allocator.deallocate(output_cstr, allocator.state); + return resolved_topic; +} +} // namespace ros2_babel_fish diff --git a/ros2_foxglove_bridge/src/ros2_babel_fish/idl/providers/local_type_support_provider.cpp b/ros2_foxglove_bridge/src/ros2_babel_fish/idl/providers/local_type_support_provider.cpp new file mode 100644 index 0000000..a2adc4f --- /dev/null +++ b/ros2_foxglove_bridge/src/ros2_babel_fish/idl/providers/local_type_support_provider.cpp @@ -0,0 +1,319 @@ +// Copyright (c) 2021 Stefan Fabian. All rights reserved. +// Licensed under the MIT license. See LICENSE file in the project root for full license +// information. + +#include "ros2_babel_fish/idl/providers/local_type_support_provider.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ros2_babel_fish/exceptions/babel_fish_exception.hpp" + +namespace ros2_babel_fish { +namespace { +// ==== This block of code was taken from rosbag2_cpp and is under the following license === +// Copyright 2018, Bosch Software Innovations GmbH. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +std::string get_typesupport_library_path(const std::string& package_name, + const std::string& typesupport_identifier) { + const char* filename_prefix; + const char* filename_extension; + const char* dynamic_library_folder; +#ifdef _WIN32 + filename_prefix = ""; + filename_extension = ".dll"; + dynamic_library_folder = "/bin/"; +#elif __APPLE__ + filename_prefix = "lib"; + filename_extension = ".dylib"; + dynamic_library_folder = "/lib/"; +#else + filename_prefix = "lib"; + filename_extension = ".so"; + dynamic_library_folder = "/lib/"; +#endif + + std::string package_prefix; + try { + package_prefix = ament_index_cpp::get_package_prefix(package_name); + } catch (ament_index_cpp::PackageNotFoundError& e) { + throw BabelFishException(e.what()); + } + + auto library_path = package_prefix + dynamic_library_folder + filename_prefix + package_name + + "__" + typesupport_identifier + filename_extension; + return library_path; +} + +std::tuple extract_type_identifier( + const std::string& full_type) { + char type_separator = '/'; + auto sep_position_back = full_type.find_last_of(type_separator); + auto sep_position_front = full_type.find_first_of(type_separator); + if (sep_position_back == std::string::npos || sep_position_back == 0 || + sep_position_back == full_type.length() - 1) { + throw BabelFishException("Message type '" + full_type + + "' is not of the form package/type and cannot be processed"); + } + + std::string package_name = full_type.substr(0, sep_position_front); + std::string middle_module = ""; + if (sep_position_back - sep_position_front > 0) { + middle_module = + full_type.substr(sep_position_front + 1, sep_position_back - sep_position_front - 1); + } + std::string type_name = full_type.substr(sep_position_back + 1); + + return std::make_tuple(package_name, middle_module, type_name); +} + +std::shared_ptr get_typesupport_library( + const std::string& type, const std::string& typesupport_identifier) { + auto package_name = std::get<0>(extract_type_identifier(type)); + auto library_path = get_typesupport_library_path(package_name, typesupport_identifier); + return std::make_shared(library_path); +} + +const rosidl_message_type_support_t* get_typesupport_handle( + const std::string& type, const std::string& typesupport_identifier, + const std::shared_ptr& library) { + if (nullptr == library) { + throw BabelFishException( + "rcpputils::SharedLibrary not initialized. Call get_typesupport_library first."); + } + + std::string package_name; + std::string middle_module; + std::string type_name; + std::tie(package_name, middle_module, type_name) = extract_type_identifier(type); + if (middle_module.empty()) middle_module = "msg"; + + std::stringstream rcutils_dynamic_loading_error; + rcutils_dynamic_loading_error + << "Something went wrong loading the typesupport library for message type " << package_name + << "/" << middle_module << "/" << type_name << "."; + + try { + auto symbol_name = typesupport_identifier + "__get_message_type_support_handle__" + + package_name + "__" + middle_module + "__" + type_name; + + if (!library->has_symbol(symbol_name)) { + throw std::runtime_error( + std::string( + " Could not find symbol for message type support handle getter. rcutils error: ") + + rcutils_get_error_string().str + + ". This is probably due to https://github.com/ros2/rosidl_typesupport/pull/114 not being " + "merged yet."); + } + + const rosidl_message_type_support_t* (*get_ts)() = nullptr; + get_ts = (decltype(get_ts))library->get_symbol(symbol_name); + + if (!get_ts) { + throw std::runtime_error{" Symbol of wrong type."}; + } + auto type_support = get_ts(); + return type_support; + } catch (std::runtime_error& e) { + throw BabelFishException(rcutils_dynamic_loading_error.str() + e.what()); + } +} +// ==== End of block === + +const rosidl_service_type_support_t* get_service_typesupport_handle( + const std::string& type, const std::string& typesupport_identifier, + const std::shared_ptr& library) { + if (nullptr == library) { + throw BabelFishException( + "rcpputils::SharedLibrary not initialized. Call get_typesupport_library first."); + } + + std::string package_name; + std::string middle_module; + std::string type_name; + std::tie(package_name, middle_module, type_name) = extract_type_identifier(type); + if (middle_module.empty()) middle_module = "srv"; + + std::stringstream rcutils_dynamic_loading_error; + rcutils_dynamic_loading_error + << "Something went wrong loading the typesupport library for service type " << package_name + << "/" << middle_module << "/" << type_name << " from library: " << library->get_library_path() + << "."; + + try { + auto symbol_name = typesupport_identifier + "__get_service_type_support_handle__" + + package_name + "__" + (middle_module.empty() ? "srv" : middle_module) + + "__" + type_name; + if (!library->has_symbol(symbol_name)) { + throw std::runtime_error( + std::string( + " Could not find symbol for message type support handle getter. rcutils error: ") + + rcutils_get_error_string().str + + ". This is probably due to https://github.com/ros2/rosidl_typesupport/pull/114 not being " + "merged yet."); + } + + const rosidl_service_type_support_t* (*get_ts)() = nullptr; + get_ts = (decltype(get_ts))library->get_symbol(symbol_name); + + if (!get_ts) { + throw std::runtime_error{" Symbol of wrong type."}; + } + auto type_support = get_ts(); + return type_support; + } catch (std::runtime_error& e) { + throw BabelFishException(rcutils_dynamic_loading_error.str() + e.what()); + } +} + +const rosidl_action_type_support_t* get_action_typesupport_handle( + const std::string& type, const std::string& typesupport_identifier, + const std::shared_ptr& library) { + if (nullptr == library) { + throw std::runtime_error( + "rcpputils::SharedLibrary not initialized. Call get_typesupport_library first."); + } + + std::string package_name; + std::string middle_module; + std::string type_name; + std::tie(package_name, middle_module, type_name) = extract_type_identifier(type); + if (middle_module.empty()) middle_module = "action"; + + std::stringstream rcutils_dynamic_loading_error; + rcutils_dynamic_loading_error + << "Something went wrong loading the typesupport library for action type " << package_name + << "/" << middle_module << "/" << type_name << " from library: " << library->get_library_path() + << "."; + + try { + auto symbol_name = typesupport_identifier + "__get_action_type_support_handle__" + + package_name + "__" + (middle_module.empty() ? "action" : middle_module) + + "__" + type_name; + + if (!library->has_symbol(symbol_name)) { + throw std::runtime_error( + std::string( + " Could not find symbol for message type support handle getter. rcutils error: ") + + rcutils_get_error_string().str + + ". This is probably due to https://github.com/ros2/rosidl_typesupport/pull/114 not being " + "merged yet."); + } + + const rosidl_action_type_support_t* (*get_ts)() = nullptr; + get_ts = (decltype(get_ts))library->get_symbol(symbol_name); + + if (!get_ts) { + throw std::runtime_error{" Symbol of wrong type."}; + } + auto type_support = get_ts(); + return type_support; + } catch (std::runtime_error& e) { + throw BabelFishException(rcutils_dynamic_loading_error.str() + e.what()); + } +} +} // namespace + +LocalTypeSupportProvider::LocalTypeSupportProvider() = default; + +MessageTypeSupport::ConstSharedPtr LocalTypeSupportProvider::getMessageTypeSupportImpl( + const std::string& type) const { + auto type_support_library = + get_typesupport_library(type, rosidl_typesupport_cpp::typesupport_identifier); + auto type_support_handle = get_typesupport_handle( + type, rosidl_typesupport_cpp::typesupport_identifier, type_support_library); + auto introspection_type_support_library = + get_typesupport_library(type, rosidl_typesupport_introspection_cpp::typesupport_identifier); + auto introspection_type_support_handle = + get_typesupport_handle(type, rosidl_typesupport_introspection_cpp::typesupport_identifier, + introspection_type_support_library); + return registerMessage(type, type_support_library, *type_support_handle, + introspection_type_support_library, *introspection_type_support_handle); +} + +ServiceTypeSupport::ConstSharedPtr LocalTypeSupportProvider::getServiceTypeSupportImpl( + const std::string& type) const { + auto type_support_library = + get_typesupport_library(type, rosidl_typesupport_cpp::typesupport_identifier); + auto type_support_handle = get_service_typesupport_handle( + type, rosidl_typesupport_cpp::typesupport_identifier, type_support_library); + // For introspection the cpp version is available + auto introspection_type_support_library = + get_typesupport_library(type, rosidl_typesupport_introspection_cpp::typesupport_identifier); + auto introspection_type_support_handle = get_service_typesupport_handle( + type, rosidl_typesupport_introspection_cpp::typesupport_identifier, + introspection_type_support_library); + return registerService(type, type_support_library, *type_support_handle, + introspection_type_support_library, *introspection_type_support_handle); +} + +ActionTypeSupport::ConstSharedPtr LocalTypeSupportProvider::getActionTypeSupportImpl( + const std::string& type) const { + auto type_support_library = + get_typesupport_library(type, rosidl_typesupport_cpp::typesupport_identifier); + auto type_support_handle = get_action_typesupport_handle( + type, rosidl_typesupport_cpp::typesupport_identifier, type_support_library); + auto introspection_type_support_library = + get_typesupport_library(type, rosidl_typesupport_introspection_cpp::typesupport_identifier); + + // There is no method to get the whole action introspection type support, hence, we need to get + // each separately + rosidl_action_type_support_t introspection; + introspection.goal_service_type_support = get_service_typesupport_handle( + type + "_SendGoal", rosidl_typesupport_introspection_cpp::typesupport_identifier, + introspection_type_support_library); + introspection.result_service_type_support = get_service_typesupport_handle( + type + "_GetResult", rosidl_typesupport_introspection_cpp::typesupport_identifier, + introspection_type_support_library); + introspection.feedback_message_type_support = get_typesupport_handle( + type + "_FeedbackMessage", rosidl_typesupport_introspection_cpp::typesupport_identifier, + introspection_type_support_library); + auto result = std::make_shared(); + result->name = type; + result->type_support_library = type_support_library; + result->type_support_handle = *type_support_handle; + result->introspection_type_support_library = introspection_type_support_library; + + result->goal_service_type_support = registerService( + type + "_SendGoal", type_support_library, *type_support_handle->goal_service_type_support, + introspection_type_support_library, *introspection.goal_service_type_support); + result->result_service_type_support = registerService( + type + "_GetResult", type_support_library, *type_support_handle->result_service_type_support, + introspection_type_support_library, *introspection.result_service_type_support); + result->feedback_message_type_support = registerMessage( + type + "_FeedbackMessage", type_support_library, + *type_support_handle->feedback_message_type_support, introspection_type_support_library, + *introspection.feedback_message_type_support); + + // The other type supports are from a different package and we have to look them up + result->cancel_service_type_support = getServiceTypeSupport("action_msgs/srv/CancelGoal"); + result->status_message_type_support = getMessageTypeSupport("action_msgs/msg/GoalStatusArray"); + introspection.cancel_service_type_support = + &result->cancel_service_type_support->introspection_type_support_handle; + introspection.status_message_type_support = + &result->status_message_type_support->introspection_type_support_handle; + result->introspection_type_support_handle = introspection; + + return registerAction(type, result); +} +} // namespace ros2_babel_fish diff --git a/ros2_foxglove_bridge/src/ros2_babel_fish/idl/serialization.cpp b/ros2_foxglove_bridge/src/ros2_babel_fish/idl/serialization.cpp new file mode 100644 index 0000000..b29baed --- /dev/null +++ b/ros2_foxglove_bridge/src/ros2_babel_fish/idl/serialization.cpp @@ -0,0 +1,27 @@ +// +// Created by stefan on 23.01.21. +// + +#include "ros2_babel_fish/idl/serialization.hpp" + +#include "ros2_babel_fish/macros.hpp" +#include "ros2_babel_fish/messages/compound_message.hpp" +#include "ros2_babel_fish/messages/value_message.hpp" + +namespace ros2_babel_fish { +std::shared_ptr createContainer( + const rosidl_typesupport_introspection_cpp::MessageMembers& members, + rosidl_runtime_cpp::MessageInitialization initialization) { + auto result = std::shared_ptr(new unsigned char[members.size_of_], [members](void* data) { + members.fini_function(data); + delete[] static_cast(data); + }); + members.init_function(result.get(), initialization); + return result; +} + +std::shared_ptr createContainer(const MessageMembersIntrospection& members, + rosidl_runtime_cpp::MessageInitialization initialization) { + return createContainer(*members.value, initialization); +} +} // namespace ros2_babel_fish diff --git a/ros2_foxglove_bridge/src/ros2_babel_fish/idl/type_support_provider.cpp b/ros2_foxglove_bridge/src/ros2_babel_fish/idl/type_support_provider.cpp new file mode 100644 index 0000000..1fe6d07 --- /dev/null +++ b/ros2_foxglove_bridge/src/ros2_babel_fish/idl/type_support_provider.cpp @@ -0,0 +1,80 @@ +// Copyright (c) 2021 Stefan Fabian. All rights reserved. +// Licensed under the MIT license. See LICENSE file in the project root for full license +// information. + +#include "ros2_babel_fish/idl/type_support_provider.hpp" + +#include + +#include +#include + +#include "ros2_babel_fish/messages/message_types.hpp" + +namespace ros2_babel_fish { + +TypeSupportProvider::TypeSupportProvider() = default; + +MessageTypeSupport::ConstSharedPtr TypeSupportProvider::getMessageTypeSupport( + const std::string& type) const { + // Check cache + auto it = message_type_supports_.find(type); + if (it != message_type_supports_.end()) return it->second; + + return getMessageTypeSupportImpl(type); +} + +ServiceTypeSupport::ConstSharedPtr TypeSupportProvider::getServiceTypeSupport( + const std::string& type) const { + // Check cache + auto it = service_type_supports_.find(type); + if (it != service_type_supports_.end()) return it->second; + + return getServiceTypeSupportImpl(type); +} + +ActionTypeSupport::ConstSharedPtr TypeSupportProvider::getActionTypeSupport( + const std::string& type) const { + // Check cache + auto it = action_type_supports_.find(type); + if (it != action_type_supports_.end()) return it->second; + + return getActionTypeSupportImpl(type); +} + +MessageTypeSupport::ConstSharedPtr TypeSupportProvider::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 { + auto result = std::make_shared(); + result->name = name; + result->type_support_library = type_support_library; + result->type_support_handle = type_support; + result->introspection_type_support_library = introspection_type_support_library; + result->introspection_type_support_handle = introspection_type_support; + message_type_supports_.insert({name, result}); + return result; +} + +ServiceTypeSupport::ConstSharedPtr TypeSupportProvider::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 { + auto result = std::make_shared(); + result->name = name; + result->type_support_library = type_support_library; + result->type_support_handle = type_support; + result->introspection_type_support_library = introspection_type_support_library; + result->introspection_type_support_handle = introspection_type_support; + service_type_supports_.insert({name, result}); + return result; +} + +ActionTypeSupport::ConstSharedPtr TypeSupportProvider::registerAction( + const std::string& name, ActionTypeSupport::ConstSharedPtr type_support) const { + action_type_supports_.insert({name, type_support}); + return type_support; +} +} // namespace ros2_babel_fish diff --git a/ros2_foxglove_bridge/src/ros2_babel_fish/logging.hpp b/ros2_foxglove_bridge/src/ros2_babel_fish/logging.hpp new file mode 100644 index 0000000..37ce24c --- /dev/null +++ b/ros2_foxglove_bridge/src/ros2_babel_fish/logging.hpp @@ -0,0 +1,44 @@ +// +// Created by stefan on 03.08.21. +// + +#ifndef ROS2_BABEL_FISH_LOGGING_MACROS_HPP +#define ROS2_BABEL_FISH_LOGGING_MACROS_HPP + +#define RBF2_LOGGER ::rclcpp::get_logger("ros2_babel_fish") +#define RBF2_DEBUG(...) RCLCPP_DEBUG(RBF2_LOGGER, __VA_ARGS__) +#define RBF2_DEBUG_ONCE(...) RCLCPP_DEBUG_ONCE(RBF2_LOGGER, __VA_ARGS__) +#define RBF2_DEBUG_THROTTLE(...) RCLCPP_DEBUG_THROTTLE(RBF2_LOGGER, __VA_ARGS__) +#define RBF2_DEBUG_STREAM(...) RCLCPP_DEBUG_STREAM(RBF2_LOGGER, __VA_ARGS__) +#define RBF2_DEBUG_STREAM_ONCE(...) RCLCPP_DEBUG_STREAM_ONCE(RBF2_LOGGER, __VA_ARGS__) +#define RBF2_DEBUG_STREAM_THROTTLE(...) RCLCPP_DEBUG_STREAM_THROTTLE(RBF2_LOGGER, __VA_ARGS__) + +#define RBF2_INFO(...) RCLCPP_INFO(RBF2_LOGGER, __VA_ARGS__) +#define RBF2_INFO_ONCE(...) RCLCPP_INFO_ONCE(RBF2_LOGGER, __VA_ARGS__) +#define RBF2_INFO_THROTTLE(...) RCLCPP_INFO_THROTTLE(RBF2_LOGGER, __VA_ARGS__) +#define RBF2_INFO_STREAM(...) RCLCPP_INFO_STREAM(RBF2_LOGGER, __VA_ARGS__) +#define RBF2_INFO_STREAM_ONCE(...) RCLCPP_INFO_STREAM_ONCE(RBF2_LOGGER, __VA_ARGS__) +#define RBF2_INFO_STREAM_THROTTLE(...) RCLCPP_INFO_STREAM_THROTTLE(RBF2_LOGGER, __VA_ARGS__) + +#define RBF2_WARN(...) RCLCPP_WARN(RBF2_LOGGER, __VA_ARGS__) +#define RBF2_WARN_ONCE(...) RCLCPP_WARN_ONCE(RBF2_LOGGER, __VA_ARGS__) +#define RBF2_WARN_THROTTLE(...) RCLCPP_WARN_THROTTLE(RBF2_LOGGER, __VA_ARGS__) +#define RBF2_WARN_STREAM(...) RCLCPP_WARN_STREAM(RBF2_LOGGER, __VA_ARGS__) +#define RBF2_WARN_STREAM_ONCE(...) RCLCPP_WARN_STREAM_ONCE(RBF2_LOGGER, __VA_ARGS__) +#define RBF2_WARN_STREAM_THROTTLE(...) RCLCPP_WARN_STREAM_THROTTLE(RBF2_LOGGER, __VA_ARGS__) + +#define RBF2_ERROR(...) RCLCPP_ERROR(RBF2_LOGGER, __VA_ARGS__) +#define RBF2_ERROR_ONCE(...) RCLCPP_ERROR_ONCE(RBF2_LOGGER, __VA_ARGS__) +#define RBF2_ERROR_THROTTLE(...) RCLCPP_ERROR_THROTTLE(RBF2_LOGGER, __VA_ARGS__) +#define RBF2_ERROR_STREAM(...) RCLCPP_ERROR_STREAM(RBF2_LOGGER, __VA_ARGS__) +#define RBF2_ERROR_STREAM_ONCE(...) RCLCPP_ERROR_STREAM_ONCE(RBF2_LOGGER, __VA_ARGS__) +#define RBF2_ERROR_STREAM_THROTTLE(...) RCLCPP_ERROR_STREAM_THROTTLE(RBF2_LOGGER, __VA_ARGS__) + +#define RBF2_FATAL(...) RCLCPP_FATAL(RBF2_LOGGER, __VA_ARGS__) +#define RBF2_FATAL_ONCE(...) RCLCPP_FATAL_ONCE(RBF2_LOGGER, __VA_ARGS__) +#define RBF2_FATAL_THROTTLE(...) RCLCPP_FATAL_THROTTLE(RBF2_LOGGER, __VA_ARGS__) +#define RBF2_FATAL_STREAM(...) RCLCPP_FATAL_STREAM(RBF2_LOGGER, __VA_ARGS__) +#define RBF2_FATAL_STREAM_ONCE(...) RCLCPP_FATAL_STREAM_ONCE(RBF2_LOGGER, __VA_ARGS__) +#define RBF2_FATAL_STREAM_THROTTLE(...) RCLCPP_FATAL_STREAM_THROTTLE(RBF2_LOGGER, __VA_ARGS__) + +#endif // ROS2_BABEL_FISH_LOGGING_MACROS_HPP diff --git a/ros2_foxglove_bridge/src/ros2_babel_fish/messages/array_message.cpp b/ros2_foxglove_bridge/src/ros2_babel_fish/messages/array_message.cpp new file mode 100644 index 0000000..61d0cb3 --- /dev/null +++ b/ros2_foxglove_bridge/src/ros2_babel_fish/messages/array_message.cpp @@ -0,0 +1,9 @@ +// Copyright (c) 2021 Stefan Fabian. All rights reserved. +// Licensed under the MIT license. See LICENSE file in the project root for full license +// information. + +#include "ros2_babel_fish/messages/array_message.hpp" + +#include "ros2_babel_fish/macros.hpp" + +namespace ros2_babel_fish {} diff --git a/ros2_foxglove_bridge/src/ros2_babel_fish/messages/compound_message.cpp b/ros2_foxglove_bridge/src/ros2_babel_fish/messages/compound_message.cpp new file mode 100644 index 0000000..0b3dda9 --- /dev/null +++ b/ros2_foxglove_bridge/src/ros2_babel_fish/messages/compound_message.cpp @@ -0,0 +1,335 @@ +// Copyright (c) 2021 Stefan Fabian. All rights reserved. +// Licensed under the MIT license. See LICENSE file in the project root for full license +// information. + +#include "ros2_babel_fish/messages/compound_message.hpp" + +#include + +#include "ros2_babel_fish/exceptions/babel_fish_exception.hpp" +#include "ros2_babel_fish/idl/serialization.hpp" +#include "ros2_babel_fish/macros.hpp" +#include "ros2_babel_fish/messages/array_message.hpp" +#include "ros2_babel_fish/messages/value_message.hpp" + +namespace ros2_babel_fish { + +CompoundMessage::CompoundMessage() + : Message(MessageTypes::Compound, nullptr) + , members_(nullptr, nullptr) {} + +CompoundMessage::CompoundMessage(MessageMembersIntrospection members, std::shared_ptr data) + : Message(MessageTypes::Compound, std::move(data)) + , members_(std::move(members)) + , values_(members_.value->member_count_) {} + +CompoundMessage::CompoundMessage(MessageMembersIntrospection members, + rosidl_runtime_cpp::MessageInitialization init) + : CompoundMessage(members, createContainer(*members.value, init)) {} + +CompoundMessage::CompoundMessage(const CompoundMessage& other) + : CompoundMessage(other.members_, other.data_) {} + +std::string CompoundMessage::datatype() const { + return std::string(members_.value->message_namespace_) + "::" + members_.value->message_name_; +} + +std::string CompoundMessage::name() const { + static const std::regex namespace_regex("::"); + return std::regex_replace(members_.value->message_namespace_, namespace_regex, "/") + "/" + + members_.value->message_name_; +} + +bool CompoundMessage::containsKey(const std::string& key) const { + for (uint32_t i = 0; i < members_.value->member_count_; ++i) { + if (members_.value->members_[i].name_ == key) return true; + } + return false; +} + +std::vector CompoundMessage::keys() const { + std::vector result; + result.reserve(members_.value->member_count_); + for (uint32_t i = 0; i < members_.value->member_count_; ++i) { + result.emplace_back(members_.value->members_[i].name_); + } + return result; +} + +std::string CompoundMessage::keyAt(size_t index) { + return index >= members_.value->member_count_ ? "" : members_.value->members_[index].name_; +} + +CompoundMessage& CompoundMessage::operator=(const Message& other) { + _assign(other); + return *this; +} + +CompoundMessage& CompoundMessage::operator=(const builtin_interfaces::msg::Time& value) { + if (datatype() != "builtin_interfaces::msg::Time") + throw BabelFishException("Tried to _assign rclcpp::Time to '" + name() + + "' message which is not a 'builtin_interfaces/msg/Time' message!"); + (*this)["sec"] = value.sec; + (*this)["nanosec"] = value.nanosec; + return *this; +} + +CompoundMessage& CompoundMessage::operator=(const builtin_interfaces::msg::Duration& value) { + if (datatype() != "builtin_interfaces::msg::Duration") + throw BabelFishException("Tried to _assign rclcpp::Duration to '" + name() + + "' message which is not a 'builtin_interfaces/msg/Duration' message!"); + (*this)["sec"] = value.sec; + (*this)["nanosec"] = value.nanosec; + return *this; +} + +CompoundMessage& CompoundMessage::operator=(const rclcpp::Time& value) { + *this = (builtin_interfaces::msg::Time)value; + return *this; +} + +CompoundMessage& CompoundMessage::operator=(const rclcpp::Duration& value) { + *this = (builtin_interfaces::msg::Duration)value; + return *this; +} + +CompoundMessage& CompoundMessage::operator=(const CompoundMessage& other) { + if (this == &other) return *this; + if (members_.value != other.members_.value) { + if (other.members_.value->message_namespace_ != members_.value->message_namespace_ || + other.members_.value->message_name_ != members_.value->message_name_) + throw BabelFishException("Tried to _assign compound of name '" + other.name() + + "' to compound of name '" + name() + "'!"); + } + for (uint32_t i = 0; i < members_.value->member_count_; ++i) *(*this)[i] = *other[i]; + return *this; +} + +CompoundMessage& CompoundMessage::operator=(CompoundMessage&& other) noexcept { + data_ = std::move(other.data_); + type_ = other.type_; + members_ = std::move(other.members_); + values_ = std::move(other.values_); + initialized_values_ = other.initialized_values_; + return *this; +} + +void CompoundMessage::_assign(const Message& other) { + if (other.type() != MessageTypes::Compound) + throw BabelFishException("Tried to assign non-compound to compound message!"); + *this = static_cast(other); +} + +bool CompoundMessage::_isMessageEqual(const Message& o) const { + const auto& other = o.as(); + if (other.members_.value != members_.value) return false; + initValues(); + other.initValues(); + for (size_t i = 0; i < members_->member_count_; ++i) { + if (values_[i] != other.values_[i]) return false; + } + return true; +} + +namespace { + +template +void initValueMessage(Message::SharedPtr& message, const MessageMemberIntrospection& member, + const std::shared_ptr& data) { + message = ValueMessage::template make_shared(member, data); +} + +template +struct ArrayInit { + template + static void initArrayMessage(Message::SharedPtr& message, + const MessageMemberIntrospection& member, + const std::shared_ptr& data) { + message = ArrayMessage_::template make_shared(member, data); + } +}; + +template <> +template <> +void ArrayInit::initArrayMessage(Message::SharedPtr&, + const MessageMemberIntrospection&, + const std::shared_ptr&) { + throw std::runtime_error( + "Arrays of arrays are not supported by ROS2 (yet)! Please open an issue if this changed!"); +} + +template <> +template <> +void ArrayInit::initArrayMessage(Message::SharedPtr&, + const MessageMemberIntrospection&, + const std::shared_ptr&) { + throw std::runtime_error( + "Arrays of arrays are not supported by ROS2 (yet)! Please open an issue if this changed!"); +} + +template <> +template <> +void ArrayInit::initArrayMessage(Message::SharedPtr&, + const MessageMemberIntrospection&, + const std::shared_ptr&) { + throw std::runtime_error( + "Arrays of arrays are not supported by ROS2 (yet)! Please open an issue if this changed!"); +} + +template <> +template <> +void ArrayInit::initArrayMessage( + Message::SharedPtr& message, const MessageMemberIntrospection& member, + const std::shared_ptr& data) { + message = CompoundArrayMessage::make_shared(member, data); +} + +template <> +template <> +void ArrayInit::initArrayMessage( + Message::SharedPtr& message, const MessageMemberIntrospection& member, + const std::shared_ptr& data) { + message = BoundedCompoundArrayMessage::make_shared(member, data); +} + +template <> +template <> +void ArrayInit::initArrayMessage( + Message::SharedPtr& message, const MessageMemberIntrospection& member, + const std::shared_ptr& data) { + message = FixedLengthCompoundArrayMessage::make_shared(member, data); +} + +void initValue(Message::SharedPtr& message, const MessageMemberIntrospection& member, + const std::shared_ptr& data) { + if (member->is_array_) { + // Empty deleter with copy to data to make sure the subarea of memory the shared_ptr is pointing + // to isn't destroyed while this shared_ptr exists. + std::shared_ptr sub_data(static_cast(data.get()) + member->offset_, + [data](void*) { + (void)data; + }); + if (member->is_upper_bound_) { + using BoundedArrayInit = ArrayInit; + RBF2_TEMPLATE_CALL(BoundedArrayInit::initArrayMessage, member->type_id_, message, member, + sub_data); + } else if (member->array_size_ == 0) { + using NormalArrayInit = ArrayInit; + RBF2_TEMPLATE_CALL(NormalArrayInit::initArrayMessage, member->type_id_, message, member, + sub_data); + } else { + using FixedLengthArrayInit = ArrayInit; + RBF2_TEMPLATE_CALL(FixedLengthArrayInit::initArrayMessage, member->type_id_, message, member, + sub_data); + } + } else if (member->type_id_ == MessageTypes::Compound) { + // Empty deleter with copy to data to make sure the subarea of memory the shared_ptr is pointing + // to isn't destroyed while this shared_ptr exists. + std::shared_ptr sub_data(static_cast(data.get()) + member->offset_, + [data](void*) { + (void)data; + }); + message = CompoundMessage::template make_shared(member, std::move(sub_data)); + } else { + RBF2_TEMPLATE_CALL_VALUE_TYPES(initValueMessage, member->type_id_, message, member, data); + } +} +} // namespace + +Message& CompoundMessage::operator[](const std::string& key) { + for (uint32_t i = 0; i < members_.value->member_count_; ++i) { + if (members_.value->members_[i].name_ == key) { + if (values_[i] == nullptr) { + initValue(values_[i], members_.getMember(i), data_); + } + return *values_[i]; + } + } + throw std::out_of_range("Invalid key! '" + key + "' is not in " + + members_.value->message_namespace_ + + "::" + members_.value->message_name_ + "."); +} + +const Message& CompoundMessage::operator[](const std::string& key) const { + for (uint32_t i = 0; i < members_.value->member_count_; ++i) { + if (members_.value->members_[i].name_ == key) { + if (values_[i] == nullptr) { + initValue(values_[i], members_.getMember(i), data_); + } + return *values_[i]; + } + } + throw std::out_of_range("Invalid key! '" + key + "' is not in " + + members_.value->message_namespace_ + + "::" + members_.value->message_name_ + "."); +} + +std::vector CompoundMessage::values() { + initValues(); + return values_; +} + +std::vector CompoundMessage::values() const { + initValues(); + return {values_.begin(), values_.end()}; +} + +void CompoundMessage::initValues() const { + if (initialized_values_) return; + for (uint32_t i = 0; i < members_.value->member_count_; ++i) { + if (values_[i] != nullptr) continue; + initValue(values_[i], members_.getMember(i), data_); + } + initialized_values_ = true; +} + +Message::SharedPtr CompoundMessage::operator[](size_t index) { + if (values_[index] == nullptr) { + initValue(values_[index], members_.getMember(index), data_); + } + return values_[index]; +} + +Message::ConstSharedPtr CompoundMessage::operator[](size_t index) const { + if (values_[index] == nullptr) { + initValue(values_[index], members_.getMember(index), data_); + } + return values_[index]; +} + +std::shared_ptr CompoundMessage::type_erased_message() const { + return data_; +} + +std::shared_ptr CompoundMessage::type_erased_message() { + return data_; +} + +CompoundMessage CompoundMessage::clone() const { + CompoundMessage result( + members_, createContainer(members_, rosidl_runtime_cpp::MessageInitialization::SKIP)); + result = *this; + return result; +} + +bool CompoundMessage::isValid() const { + return data_ != nullptr; +} + +void CompoundMessage::onMoved() { + for (uint32_t i = 0; i < members_.value->member_count_; ++i) { + if (values_[i] == nullptr) continue; + if (values_[i]->type() != MessageType::Compound && values_[i]->type() != MessageType::Array) { + values_[i]->move(data_); + continue; + } + // Compound and array messages get a pointer to their exact memory location + std::shared_ptr sub_data( + static_cast(data_.get()) + members_->members_[i].offset_, + [data = this->data_](void*) { + (void)data; + }); + values_[i]->move(sub_data); + } +} +} // namespace ros2_babel_fish diff --git a/ros2_foxglove_bridge/src/ros2_babel_fish/messages/message.cpp b/ros2_foxglove_bridge/src/ros2_babel_fish/messages/message.cpp new file mode 100644 index 0000000..4d5b2b1 --- /dev/null +++ b/ros2_foxglove_bridge/src/ros2_babel_fish/messages/message.cpp @@ -0,0 +1,524 @@ +// Copyright (c) 2021 Stefan Fabian. All rights reserved. +// Licensed under the MIT license. See LICENSE file in the project root for full license +// information. + +#include "ros2_babel_fish/messages/message.hpp" + +#include + +#include "../logging.hpp" +#include "ros2_babel_fish/babel_fish.hpp" +#include "ros2_babel_fish/macros.hpp" +#include "ros2_babel_fish/messages/array_message.hpp" +#include "ros2_babel_fish/messages/compound_message.hpp" +#include "ros2_babel_fish/messages/message_type_traits.hpp" +#include "ros2_babel_fish/messages/value_message.hpp" +#include "ros2_babel_fish/method_invoke_helpers.hpp" + +namespace ros2_babel_fish { + +Message::Message(MessageType type, std::shared_ptr data) + : data_(std::move(data)) + , type_(type) {} + +Message& Message::operator[](const std::string&) { + throw BabelFishException( + "Tried to access child message on message object that does not support " + "child access by key."); +} + +const Message& Message::operator[](const std::string&) const { + throw BabelFishException( + "Tried to access child message on message object that does not support " + "child access by key."); +} + +Message::~Message() = default; + +namespace { + +template +constexpr + typename std::enable_if::is_signed != std::numeric_limits::is_signed, + bool>::type + inBounds() { + return false; +} + +template +constexpr + typename std::enable_if::is_signed == std::numeric_limits::is_signed, + bool>::type + inBounds() { + return std::numeric_limits::min() <= std::numeric_limits::min() && + std::numeric_limits::max() <= std::numeric_limits::max(); +} + +template +constexpr typename std::enable_if::value && !std::is_floating_point::value, + bool>::type +isCompatible() { + return false; +} + +template +constexpr typename std::enable_if::value, bool>::type isCompatible() { + return std::is_same::value; +} + +template +constexpr + typename std::enable_if::value && !std::is_same::value, bool>::type + isCompatible() { + if (std::is_same::value) return true; + if (std::is_floating_point::value) return false; + if (std::numeric_limits::is_signed && !std::numeric_limits::is_signed) return false; + return inBounds(); +} + +template +constexpr typename std::enable_if::value && !std::is_same::value, + bool>::type +isCompatible() { + return std::is_same::value || !std::is_same::value; +} + +template <> +constexpr bool isCompatible() { + return true; +} + +template +constexpr typename std::enable_if::value, bool>::type inBounds(const T&) { + return false; +} + +// is_integral is necessary to avoid a CLang tidy warning +template +typename std::enable_if::value && std::numeric_limits::is_signed && + !std::numeric_limits::is_signed, + bool>::type +inBounds(const T& val) { + return val >= 0 && + static_cast::type>(val) <= std::numeric_limits::max(); +} + +template +typename std::enable_if::value && !std::numeric_limits::is_signed && + std::numeric_limits::is_signed, + bool>::type +inBounds(const T& val) { + return val <= static_cast::type>(std::numeric_limits::max()); +} + +template +typename std::enable_if::value && + std::numeric_limits::is_signed == std::numeric_limits::is_signed, + bool>::type +inBounds(const T& val) { + return std::numeric_limits::min() <= val && val <= std::numeric_limits::max(); +} + +template +typename std::enable_if::value && !std::is_floating_point::value, + bool>::type +isCompatible(const T&) { + return false; +} + +//! Booleans are only compatible with booleans as they are semantically distinct from numbers. +template +typename std::enable_if::value, bool>::type isCompatible(const T&) { + return std::is_same::value; +} + +template +typename std::enable_if::value && !std::is_same::value, bool>::type +isCompatible(const T& val) { + if (std::is_same::value) return true; + if (std::is_floating_point::value) return false; + return inBounds(val); +} + +template +typename std::enable_if::value && !std::is_same::value, + bool>::type +isCompatible(const T&) { + return std::is_integral::value || sizeof(T) <= sizeof(U); +} + +template +struct ValueAssigner { + template + static void assignValue(Message& m, const ValueType& value) { + using namespace std::chrono_literals; + using namespace message_type_traits; + if (m.type() != message_type::value && !isCompatible(value)) + throw BabelFishException( + "Value does not fit into value message! Make sure you're using the correct type or at " + "least stay within the range of values for the message type!"); +#if RBF_WARN_ON_INCOMPATIBLE_TYPE + if (m.type() != message_type::value && !isCompatible()) { + rclcpp::Clock clock; + RBF2_WARN_THROTTLE( + clock, 5000, + "Assigned value fits but the type of the assignment can not be converted without loss of " + "information in some cases! This message is throttled to once per 5 seconds!"); + } +#endif + m.as>().setValue(static_cast(value)); + } +}; + +template <> +struct ValueAssigner { + template + static void assignValue(Message&, const ValueType&) { + throw BabelFishException("Can not assign non-boolean value to a boolean ValueMessage!"); + } +}; + +template <> +struct ValueAssigner { + template + static void assignValue(Message&, const ValueType&) { + throw BabelFishException("Can not assign non-wstring value to a wstring ValueMessage!"); + } +}; + +template <> +struct ValueAssigner { + template + static void assignValue(Message&, const ValueType&) { + throw BabelFishException("Can not assign non-string value to a string ValueMessage!"); + } +}; + +template <> +struct ValueAssigner { + template + static void assignValue(Message&, const ValueType&) { + throw BabelFishException("Can not assign value to a CompoundMessage!"); + } +}; + +template <> +struct ValueAssigner { + template + static void assignValue(Message&, const ValueType&) { + throw BabelFishException("Can not assign value to an array message!"); + } +}; + +// This is a workaround around the issue that GCC doesn't allow template specializations in a +// non-namespace scope and no template specializations for an unspecialized template +template +struct AssignerValue { + template + void operator()(ValueMessage& m, const ValueType& value) { + ValueAssigner::template assignValue(m, value); + } + + void operator()(CompoundMessage& m, const ValueType& value) { + ValueAssigner::template assignValue(m, value); + } + + void operator()(ArrayMessageBase& m, const ValueType& value) { + ValueAssigner::template assignValue(m, value); + } +}; + +template +void assignToValueMessage(Message& m, const T& value) { + invoke_for_message(m, AssignerValue{}, value); +} +} // namespace + +Message& Message::operator=(bool value) { + if (type() != MessageTypes::Bool) + throw BabelFishException("Can not _assign a boolean to a non-boolean ValueMessage!"); + as>() = value; + return *this; +} + +Message& Message::operator=(uint8_t value) { + assignToValueMessage(*this, value); + return *this; +} + +Message& Message::operator=(uint16_t value) { + assignToValueMessage(*this, value); + return *this; +} + +Message& Message::operator=(uint32_t value) { + assignToValueMessage(*this, value); + return *this; +} + +Message& Message::operator=(uint64_t value) { + assignToValueMessage(*this, value); + return *this; +} + +Message& Message::operator=(int8_t value) { + assignToValueMessage(*this, value); + return *this; +} + +Message& Message::operator=(int16_t value) { + assignToValueMessage(*this, value); + return *this; +} + +Message& Message::operator=(int32_t value) { + assignToValueMessage(*this, value); + return *this; +} + +Message& Message::operator=(int64_t value) { + assignToValueMessage(*this, value); + return *this; +} + +Message& Message::operator=(float value) { + assignToValueMessage(*this, value); + return *this; +} + +Message& Message::operator=(double value) { + assignToValueMessage(*this, value); + return *this; +} + +Message& Message::operator=(long double value) { + assignToValueMessage(*this, value); + return *this; +} + +Message& Message::operator=(const char* value) { + *this = std::string(value); + return *this; +} + +Message& Message::operator=(const std::string& value) { + if (type() != MessageTypes::String) + throw BabelFishException("Can not _assign a string to a non-string ValueMessage!"); + as>() = value; + return *this; +} + +Message& Message::operator=(const std::wstring& value) { + if (type() != MessageTypes::WString) + throw BabelFishException("Can not _assign a wstring to a non-wstring ValueMessage!"); + as>() = value; + return *this; +} + +Message& Message::operator=(const builtin_interfaces::msg::Time& value) { + if (type() != MessageTypes::Compound) + throw BabelFishException( + "Tried to _assign rclcpp::Time to message that is not a compound message!"); + as() = value; + return *this; +} + +Message& Message::operator=(const builtin_interfaces::msg::Duration& value) { + if (type() != MessageTypes::Compound) + throw BabelFishException( + "Tried to _assign rclcpp::Duration to message that is not a compound message!"); + as() = value; + return *this; +} + +Message& Message::operator=(const rclcpp::Time& value) { + *this = (builtin_interfaces::msg::Time)value; + return *this; +} + +Message& Message::operator=(const rclcpp::Duration& value) { + *this = (builtin_interfaces::msg::Duration)value; + return *this; +} + +Message& Message::operator=(const Message& other) { + if (this == &other) return *this; + _assign(other); + return *this; +} + +bool Message::isTime() const { + return type_ == MessageTypes::Compound && + as().datatype() == "builtin_interfaces::msg::Time"; +} + +bool Message::isDuration() const { + return type_ == MessageTypes::Compound && + as().datatype() == "builtin_interfaces::msg::Duration"; +} + +namespace { +template +struct ValueExtractor { + template + U operator()(const ValueMessage& m) { + using namespace message_type_traits; + T val = m.getValue(); + if (m.type() != message_type::value && !isCompatible(val)) + throw BabelFishException("Value does not fit into casted type!"); +#if RBF_WARN_ON_INCOMPATIBLE_TYPE + if (m.type() != message_type::value && !isCompatible()) { + rclcpp::Clock clock; + RBF2_WARN_THROTTLE( + clock, 5000, + "Value fits into casted type but it is smaller than the message type which may lead to " + "catastrophic failure in the future! This message is printed only once!"); + } +#endif + return static_cast(val); + } + + U operator()(const ValueMessage&) { + throw BabelFishException("Tried to retrieve non-string ValueMessage as string!"); + } + + U operator()(const ValueMessage&) { + throw BabelFishException("Tried to retrieve non-wstring ValueMessage as wstring!"); + } +}; + +template +T obtainValueAsType(const Message* m) { + using namespace message_type_traits; + if (m->type() == MessageTypes::Bool) + throw BabelFishException("Can not return value of boolean ValueMessage as non-boolean!"); + return invoke_for_value_message(*m, ValueExtractor{}); +} +} // namespace + +template <> +bool Message::value() const { + if (type() != MessageTypes::Bool) + throw BabelFishException("Can not return value of non-boolean ValueMessage as boolean!"); + return as>().getValue(); +} + +template <> +uint8_t Message::value() const { + return obtainValueAsType(this); +} + +template <> +char16_t Message::value() const { + return obtainValueAsType(this); +} + +template <> +uint16_t Message::value() const { + return obtainValueAsType(this); +} + +template <> +uint32_t Message::value() const { + return obtainValueAsType(this); +} + +template <> +uint64_t Message::value() const { + return obtainValueAsType(this); +} + +template <> +int8_t Message::value() const { + return obtainValueAsType(this); +} + +template <> +int16_t Message::value() const { + return obtainValueAsType(this); +} + +template <> +int32_t Message::value() const { + return obtainValueAsType(this); +} + +template <> +int64_t Message::value() const { + return obtainValueAsType(this); +} + +template <> +float Message::value() const { + return obtainValueAsType(this); +} + +template <> +double Message::value() const { + return obtainValueAsType(this); +} + +template <> +long double Message::value() const { + return obtainValueAsType(this); +} + +template <> +std::string Message::value() const { + if (type() == MessageTypes::WString) + throw BabelFishException("Can not return value of wstring ValueMessage as string!"); + if (type() != MessageTypes::String) + throw BabelFishException("Can not return value of non-string ValueMessage as string!"); + return as>().getValue(); +} + +template <> +std::wstring Message::value() const { + if (type() == MessageTypes::String) + throw BabelFishException("Can not return value of string ValueMessage as wstring!"); + if (type() != MessageTypes::WString) + throw BabelFishException("Can not return value of non-string ValueMessage as string!"); + return as>().getValue(); +} + +template <> +rclcpp::Time Message::value() const { + if (type() != MessageTypes::Compound) + throw BabelFishException( + "Tried to obtain rclcpp::Time from message that is not a compound message!"); + const auto& compound = as(); + if (compound.datatype() != "builtin_interfaces::msg::Time") + throw BabelFishException("Tried to obtain rclcpp::Time from '" + compound.name() + + "' message which is not a 'builtin_interfaces/msg/Time' message!"); + return { + *std::static_pointer_cast(compound.type_erased_message())}; +} + +template <> +rclcpp::Duration Message::value() const { + if (type() != MessageTypes::Compound) + throw BabelFishException( + "Tried to obtain rclcpp::Duration from message that is not a compound message!"); + const auto& compound = as(); + if (compound.datatype() != "builtin_interfaces::msg::Duration") + throw BabelFishException("Tried to obtain rclcpp::Duration from '" + compound.name() + + "' message which is not a 'builtin_interfaces/msg/Duration' message!"); + return {*std::static_pointer_cast( + compound.type_erased_message())}; +} + +bool Message::operator==(const Message& other) const { + if (this == &other) return true; + if (type_ != other.type_) return false; + return _isMessageEqual(other); +} + +// Need to come after value specializations +bool Message::operator==(const char* c) const { + return value() == c; +} + +bool Message::operator==(const wchar_t* c) const { + return value() == c; +} +} // namespace ros2_babel_fish diff --git a/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp b/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp index 9ef33e2..875e601 100644 --- a/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp +++ b/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp @@ -2,6 +2,9 @@ #include +#ifdef ENABLE_JSON_MESSAGES +#include +#endif #include namespace foxglove_bridge { @@ -25,6 +28,10 @@ FoxgloveBridge::FoxgloveBridge(const rclcpp::NodeOptions& options) foxglove::FOXGLOVE_BRIDGE_VERSION, foxglove::FOXGLOVE_BRIDGE_GIT_HASH, foxglove::WebSocketUserAgent()); +#ifdef ENABLE_JSON_MESSAGES + _babelFish = ros2_babel_fish::BabelFish::make_shared(); +#endif + declareParameters(this); const auto port = static_cast(this->get_parameter(PARAM_PORT).as_int()); @@ -61,7 +68,7 @@ FoxgloveBridge::FoxgloveBridge(const rclcpp::NodeOptions& options) if (_useSimTime) { serverOptions.capabilities.push_back(foxglove::CAPABILITY_TIME); } - serverOptions.supportedEncodings = {"cdr"}; + serverOptions.supportedEncodings = {"cdr", "json"}; serverOptions.metadata = {{"ROS_DISTRO", rosDistro}}; serverOptions.sendBufferLimitBytes = send_buffer_limit; serverOptions.sessionId = std::to_string(std::time(nullptr)); @@ -625,9 +632,10 @@ void FoxgloveBridge::clientAdvertise(const foxglove::ClientAdvertisement& advert publisherOptions.callback_group = _clientPublishCallbackGroup; auto publisher = this->create_generic_publisher(topicName, topicType, qos, publisherOptions); - RCLCPP_INFO(this->get_logger(), "Client %s is advertising \"%s\" (%s) on channel %d", + RCLCPP_INFO(this->get_logger(), + "Client %s is advertising \"%s\" (%s) on channel %d with encoding \"%s\"", _server->remoteEndpointString(hdl).c_str(), topicName.c_str(), topicType.c_str(), - advertisement.channelId); + advertisement.channelId, advertisement.encoding.c_str()); // Store the new topic advertisement clientPublications.emplace(advertisement.channelId, std::move(publisher)); @@ -700,14 +708,43 @@ void FoxgloveBridge::clientMessage(const foxglove::ClientMessage& message, Conne publisher = it2->second; } - // Copy the message payload into a SerializedMessage object - rclcpp::SerializedMessage serializedMessage{message.getLength()}; - auto& rclSerializedMsg = serializedMessage.get_rcl_serialized_message(); - std::memcpy(rclSerializedMsg.buffer, message.getData(), message.getLength()); - rclSerializedMsg.buffer_length = message.getLength(); + if (message.advertisement.encoding == "cdr") { + // Copy the message payload into a SerializedMessage object + rclcpp::SerializedMessage serializedMessage{message.getLength()}; + auto& rclSerializedMsg = serializedMessage.get_rcl_serialized_message(); + std::memcpy(rclSerializedMsg.buffer, message.getData(), message.getLength()); + rclSerializedMsg.buffer_length = message.getLength(); + + // Publish the message + publisher->publish(serializedMessage); +#ifdef ENABLE_JSON_MESSAGES + } else if (message.advertisement.encoding == "json") { + const auto jsonMessage = + std::string_view{reinterpret_cast(message.getData()), message.getLength()}; + ros2_babel_fish::CompoundMessage::SharedPtr rosMsg; + const auto maybeErr = + jsonMessageToRos(jsonMessage, message.advertisement.schemaName, _babelFish, rosMsg); + if (maybeErr) { + throw foxglove::ClientChannelError(message.advertisement.channelId, + std::string{"Dropping client message from "} + + _server->remoteEndpointString(hdl) + + " with encoding \"json\": " + maybeErr.value().what()); + } - // Publish the message - publisher->publish(serializedMessage); + // Publish the assembled ROS message + auto publisherHandle = publisher->get_publisher_handle(); + const auto status = + rcl_publish(publisherHandle.get(), rosMsg->type_erased_message().get(), nullptr); + if (RCL_RET_OK != status) { + rclcpp::exceptions::throw_from_rcl_error(status, "Failed to publish message"); + } +#endif + } else { + throw foxglove::ClientChannelError( + message.advertisement.channelId, + "Dropping client message from " + _server->remoteEndpointString(hdl) + + " with unknown encoding \"" + message.advertisement.encoding + "\""); + } } void FoxgloveBridge::setParameters(const std::vector& parameters, diff --git a/ros2_foxglove_bridge/tests/json_to_ros_test.cpp b/ros2_foxglove_bridge/tests/json_to_ros_test.cpp new file mode 100644 index 0000000..692c51f --- /dev/null +++ b/ros2_foxglove_bridge/tests/json_to_ros_test.cpp @@ -0,0 +1,42 @@ +#include + +#include + +TEST(JsonToRosTest, EmptyStringMsg) { + const std::string payload = "{}"; + + auto babelFish = ros2_babel_fish::BabelFish::make_shared(); + + ros2_babel_fish::CompoundMessage::SharedPtr output; + auto res = foxglove_bridge::jsonMessageToRos(payload, "std_msgs/msg/String", babelFish, output); + ASSERT_FALSE(res.has_value()) << "Error converting JSON to ROS: " << res.value().what(); + ASSERT_TRUE(output) << "Output message is null"; + EXPECT_EQ(output->datatype(), "std_msgs::msg::String"); + EXPECT_EQ(output->memberCount(), 1); + EXPECT_EQ((*output)["data"].type(), ros2_babel_fish::MessageType::String); + EXPECT_EQ((*output)["data"].value(), ""); + EXPECT_TRUE(output->type_erased_message()); +} + +TEST(JsonToRosTest, StringMsg) { + const std::string payload = R"( + { "data": "Hello, World!" } + )"; + + auto babelFish = ros2_babel_fish::BabelFish::make_shared(); + + ros2_babel_fish::CompoundMessage::SharedPtr output; + auto res = foxglove_bridge::jsonMessageToRos(payload, "std_msgs/msg/String", babelFish, output); + ASSERT_FALSE(res.has_value()) << "Error converting JSON to ROS: " << res.value().what(); + ASSERT_TRUE(output) << "Output message is null"; + EXPECT_EQ(output->datatype(), "std_msgs::msg::String"); + EXPECT_EQ(output->memberCount(), 1); + EXPECT_EQ((*output)["data"].type(), ros2_babel_fish::MessageType::String); + EXPECT_EQ((*output)["data"].value(), "Hello, World!"); + EXPECT_TRUE(output->type_erased_message()); +} + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}