Repository: foxglove/ros-foxglove-bridge Branch: main Commit: 42c0ba4fffee Files: 53 Total size: 229.4 KB Directory structure: gitextract_fu2mg7t_/ ├── .clang-format ├── .gitattributes ├── .github/ │ └── workflows/ │ └── ci.yml ├── .gitignore ├── .vscode/ │ ├── c_cpp_properties.json │ ├── launch.json │ └── settings.json ├── CHANGELOG.rst ├── CMakeLists.txt ├── Dockerfile.ros1 ├── LICENSE ├── Makefile ├── README.md ├── download_test_data.sh ├── foxglove_bridge_base/ │ ├── include/ │ │ └── foxglove_bridge/ │ │ ├── base64.hpp │ │ ├── callback_queue.hpp │ │ ├── common.hpp │ │ ├── foxglove_bridge.hpp │ │ ├── message_definition_cache.hpp │ │ ├── parameter.hpp │ │ ├── regex_utils.hpp │ │ ├── serialization.hpp │ │ ├── server_factory.hpp │ │ ├── server_interface.hpp │ │ ├── test/ │ │ │ └── test_client.hpp │ │ ├── websocket_client.hpp │ │ ├── websocket_logging.hpp │ │ ├── websocket_notls.hpp │ │ ├── websocket_server.hpp │ │ └── websocket_tls.hpp │ ├── src/ │ │ ├── base64.cpp │ │ ├── foxglove_bridge.cpp │ │ ├── parameter.cpp │ │ ├── serialization.cpp │ │ ├── server_factory.cpp │ │ ├── test/ │ │ │ └── test_client.cpp │ │ └── version.cpp.in │ └── tests/ │ ├── base64_test.cpp │ ├── serialization_test.cpp │ └── version_test.cpp ├── nodelets.xml ├── package.xml ├── ros1_foxglove_bridge/ │ ├── include/ │ │ └── foxglove_bridge/ │ │ ├── generic_service.hpp │ │ ├── param_utils.hpp │ │ └── service_utils.hpp │ ├── launch/ │ │ └── foxglove_bridge.launch │ ├── src/ │ │ ├── param_utils.cpp │ │ ├── ros1_foxglove_bridge_node.cpp │ │ ├── ros1_foxglove_bridge_nodelet.cpp │ │ └── service_utils.cpp │ └── tests/ │ ├── smoke.test │ └── smoke_test.cpp └── scripts/ └── format.py ================================================ FILE CONTENTS ================================================ ================================================ FILE: .clang-format ================================================ --- Language: Cpp Standard: c++17 BasedOnStyle: Google AllowShortFunctionsOnASingleLine: Empty AllowShortLambdasOnASingleLine: Empty AccessModifierOffset: -2 TabWidth: 2 ContinuationIndentWidth: 2 UseTab: Never BreakConstructorInitializers: BeforeComma ColumnLimit: 100 ConstructorInitializerAllOnOneLineOrOnePerLine: false DerivePointerAlignment: false FixNamespaceComments: true PointerAlignment: Left ReflowComments: true SortIncludes: true IncludeCategories: - Regex: "^\".*" Priority: 4 SortPriority: 0 - Regex: "^`_) * avoid requesting parameters from unresponsive nodes (`#345 `_) * Update default `asset_uri_allowlist` parameter to allow dashes (`#347 `_) * reorganize devcontainer dockerfile (`#350 `_) * Use RCLCPP_VERSION_GTE from rclcpp/version.h in generic_client.cpp (`#344 `_) * Fixed logging typo (`#343 `_) * Contributors: Hans-Joachim Krauch, Meet Gandhi, johannesschrimpf 0.8.4 (2025-02-21) ------------------ * also advertise channels for ROS1 topics without publishers (`#341 `_) * Contributors: Hans-Joachim Krauch 0.8.3 (2025-02-03) ------------------ * add best_effort_qos_topic_whitelist param (`#329 `_) * Add missing functional include in message_definition_cache.cpp (`#334 `_) * Contributors: David Revay, Silvio Traversaro 0.8.2 (2024-12-1) ------------------ * Fix "no matching function" error on yocto kirkstone (`#331 `_) * Contributors: Graham Harison 0.8.1 (2024-11-26) ------------------ * Improve Error Reporting and Reduce Log Redundancy (`#327 `_) * Contributors: Robin Dumas 0.8.0 (2024-07-31) ------------------ * Fix usage of deprecated AsyncParametersClient constructor (`#319 `_) * Add ROS2 JSON publishing support (`#307 `_) * Contributors: Davide Faconti, Hans-Joachim Krauch 0.7.10 (2024-07-12) ------------------- * Make ROS1 service type retrieval more robust (`#316 `_) * Contributors: Hans-Joachim Krauch 0.7.9 (2024-07-05) ------------------ * Fix parsing of IDL message definitions (`#313 `_) * Support publishing client message as loaned message (`#314 `_) * fix: remove extra ";" in websocket_server.hpp (`#311 `_) * Fix rolling smoke tests crashing (`#309 `_) * Contributors: Andrey Milko, Hans-Joachim Krauch 0.7.8 (2024-06-11) ------------------ * Fix srv definition parsing failing due to carriage return (`#303 `_) * Contributors: Hans-Joachim Krauch 0.7.7 (2024-05-21) ------------------ * send service call failure operation (`#298 `_) * Fix service definition parsing on ROS rolling (`#293 `_) * Update docs to discourage users from using websocket compression (`#297 `_) * Update README.md to remove '$ ' so that you can copy and run command (`#294 `_) * Fix typo in ROS2 launch file example (`#296 `_) * Contributors: Felipe Galindo, Hans-Joachim Krauch, Jacob Bandes-Storch, Roman Shtylman 0.7.6 (2024-02-26) ------------------ * Fix rolling builds (`#289 `_) * Remove dual ROS 1+2 devcontainer, remove ROS Galactic from the support matrix (`#285 `_) * Contributors: Hans-Joachim Krauch, John Hurliman 0.7.5 (2023-12-29) ------------------ * Add ROS 2 dependency for ament_index_cpp (`#281 `_) * Contributors: Chris Lalancette 0.7.4 (2023-12-14) ------------------ * Solved bug with incompatible QoS policies * added explicit call to ParameterValue() to avoid clang error (`#277 `_) * Add iron release badge to readme (`#271 `_) * Contributors: Hans-Joachim Krauch, Ted 0.7.3 (2023-10-25) ------------------ * Fix `asset_uri_whitelist` regex backtracking issue, add more extensions (`#270 `_) * [ROS1] Fix callback accessing invalid reference to promise (`#268 `_) * Contributors: Hans-Joachim Krauch 0.7.2 (2023-09-12) ------------------ * Fix invalid pointers not being caught (`#265 `_) * Make ROS1 service type retrieval more robust (`#263 `_) * Contributors: Hans-Joachim Krauch 0.7.1 (2023-08-21) ------------------ * Communicate double / double array parameters with type info, explicitly cast when set from integer (`#256 `_) * Make ROS 2 smoke tests less flaky (`#260 `_) * Add debug config for ros2 smoke test (`#257 `_) * Handle client disconnection in message handler thread (`#259 `_) * Reduce smoke test flakiness (`#258 `_) * Server code improvements (`#250 `_) * Contributors: Hans-Joachim Krauch 0.7.0 (2023-07-12) ------------------ * Fix ROS2 launch file install rule not installing launch subfolder (`#243 `_) * Support building with boost asio (`#247 `_) * Avoid usage of tmpnam() for creating random filename (`#246 `_) * Implement ws-protocol's `fetchAsset` specification (`#232 `_) * Use `--include-eol-distros` for `rosdep` to fix melodic builds (`#244 `_) * Reduce logging severity for parameter retrieval logs (`#240 `_) * Contributors: Hans-Joachim Krauch, Micah Guttman 0.6.4 (2023-07-05) ------------------ * Assume publisher qos depth of 1 if the middleware reports the qos history as unknown (`#239 `_) * devcontainer: Use `--include-eol-distros` for `rosdep update` (`#237 `_) * Contributors: Hans-Joachim Krauch 0.6.3 (2023-06-16) ------------------ * Add iron build to CI (`#234 `_) * Fix QoS history being unknown when copied from existing publisher (`#233 `_) * Extract ROS 2 bridge header (`#228 `_) * Contributors: Hans-Joachim Krauch, Milan Vukov 0.6.2 (2023-05-11) ------------------ * Fix connection graph updates to due incorrect use of std::set_difference (`#226 `_) * Contributors: Ivan Nenakhov 0.6.1 (2023-05-05) ------------------ * Fix warning messages not being logged (`#224 `_) * Contributors: Hans-Joachim Krauch 0.6.0 (2023-05-04) ------------------ * Add support for nested parameters (ROS1) (`#221 `_) * Catch exceptions thrown in handler functions, send status to client (`#210 `_) * Fix unhandled xmlrpc exception (`#218 `_) * Add support for action topic and services (ROS2) (`#214 `_) * Add parameter to include hidden topics and services (ROS 2) (`#216 `_) * Add workaround for publishers not being cleaned up after they got destroyed (`#215 `_) * Fix error when compiling with C++20 (`#212 `_) * Devcontainer improvements (`#213 `_) * Add parameter for minimum subscription QoS depth (`#211 `_) * Log version and commit hash when node is started (`#209 `_) * Contributors: Hans-Joachim Krauch 0.5.3 (2023-03-31) ------------------ * Fix publishers being created with invalid QoS profile (`#205 `_) * Contributors: Hans-Joachim Krauch 0.5.2 (2023-03-29) ------------------ * Notify client when Server's send buffer limit has been reached (`#201 `_) * Add support for byte array params (`#199 `_) * Do not allow connection output buffer to exceed configured limit (`#196 `_) * Fix exception parameter not being used (`#194 `_) * Contributors: Hans-Joachim Krauch 0.5.1 (2023-03-09) ------------------ * Add more exception handling (`#191 `_) * [ROS1] Fix exception not being caught when retrieving service type (`#190 `_) * Devcontainer: Use catkin tools, add build commands for ros1 (`#188 `_) * Contributors: Hans-Joachim Krauch 0.5.0 (2023-03-08) ------------------ * Add support for `schemaEncoding` field (`#186 `_) * Use QoS profile of existing publishers (if available) when creating new publishers (`#184 `_) * Make server more independent of given server configurations (`#185 `_) * Add parameter `client_topic_whitelist` for whitelisting client-published topics (`#181 `_) * Make server capabilities configurable (`#182 `_) * Fix action topic log spam (`#179 `_) * Remove (clang specific) compiler flag -Wmost (`#177 `_) * Improve the way compiler flags are set, use clang as default compiler (`#175 `_) * Avoid re-advertising existing channels when advertising new channels (`#172 `_) * Allow subscribing to connection graph updates (`#167 `_) * Contributors: Hans-Joachim Krauch 0.4.1 (2023-02-17) ------------------ * Run client handler functions in separate thread (`#165 `_) * Fix compilation error due to mismatched new-delete (`#163 `_) * Decouple server implementation (`#156 `_) * ROS2 parameter fixes (`#169 `_) * Fix program crash due to unhandled exception when creating publisher with invalid topic name (`#168 `_) * Contributors: Hans-Joachim Krauch 0.4.0 (2023-02-15) ------------------ * Update README with suggestion to build from source, minor fixes * Do not build docker images, remove corresponding documentation (`#159 `_) * Add option to use permessage-deflate compression (`#152 `_) * Improve launch file documentation, add missing launch file arguments (`#158 `_) * Allow unsetting (deleting) parameters (`#145 `_) * Improve mutex usage (`#154 `_) * Add sessionId to serverInfo (`#153 `_) * Performance improvements (`#151 `_) * Add ROS2 support for calling server-advertised services (`#142 `_) * Add ROS1 support for calling server-advertised services (`#136 `_) * ROS2 smoke test: Increase default timeout 8->10 seconds (`#143 `_) * Fix flaky parameter test (noetic) (`#141 `_) * Always --pull when building docker images in the makefile (`#140 `_) * Fix failed tests not causing CI to fail (`#138 `_) * Fix setting `int` / `int[]` parameters not working (ROS 1) (`#135 `_) * Send ROS_DISTRO to clients via metadata field (`#134 `_) * Communicate supported encodings for client-side publishing (`#131 `_) * Fix client advertised channels not being updated on unadvertise (`#132 `_) * Add support for optional request id for `setParameter` operation (`#133 `_) * Fix exception when setting parameter to empty array (`#130 `_) * Fix wrong parameter field names being used (`#129 `_) * Add parameter support (`#112 `_) * Add throttled logging when send buffer is full (`#128 `_) * Contributors: Hans-Joachim Krauch, John Hurliman 0.3.0 (2023-01-04) ------------------ * Add launch files, add install instructions to README (`#125 `_) * Drop messages when connection send buffer limit has been reached (`#126 `_) * Remove references to galactic support from README (`#117 `_) * Add missing build instructions (`#123 `_) * Use a single reentrant callback group for all subscriptions (`#122 `_) * Fix clang compilation errors (`#119 `_) * Publish binary time data when `use_sim_time` parameter is `true` (`#114 `_) * Optimize Dockerfiles (`#110 `_) * Contributors: Hans-Joachim Krauch, Ruffin 0.2.2 (2022-12-12) ------------------ * Fix messages not being received anymore after unadvertising a client publication (`#109 `_) * Allow to whitelist topics via a ROS paramater (`#108 `_) * Contributors: Hans-Joachim Krauch 0.2.1 (2022-12-05) ------------------ * Fix compilation on platforms where size_t is defined as `unsigned int` * Contributors: Hans-Joachim Krauch 0.2.0 (2022-12-01) ------------------ * Add support for client channels (`#66 `_) * Add smoke tests (`#72 `_) * Update package maintainers (`#70 `_) * [ROS2]: Fix messages not being received anymore after unsubscribing a topic (`#92 `_) * [ROS2]: Refactor node as a component (`#63 `_) * [ROS2]: Fix message definition loading for `.msg` or `.idl` files not located in `msg/` (`#95 `_) * [ROS1]: Mirror ROS 2 node behavior when `/clock`` topic is present (`#99 `_) * [ROS1]: Fix topic discovery function not being called frequently at startup (`#68 `_) * Contributors: Hans-Joachim Krauch, Jacob Bandes-Storch, John Hurliman 0.1.0 (2022-11-21) ------------------ * Initial release, topic subscription only ================================================ FILE: CMakeLists.txt ================================================ cmake_minimum_required(VERSION 3.10.2) if(POLICY CMP0048) cmake_policy(SET CMP0048 NEW) set(CMAKE_POLICY_DEFAULT_CMP0048 NEW) endif() if(POLICY CMP0024) cmake_policy(SET CMP0024 NEW) set(CMAKE_POLICY_DEFAULT_CMP0024 NEW) endif() project(foxglove_bridge LANGUAGES CXX VERSION 0.8.5) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) macro(enable_strict_compiler_warnings target) if (MSVC) target_compile_options(${target} PRIVATE /WX /W4) elseif(CMAKE_CXX_COMPILER_ID MATCHES "Clang") target_compile_options(${target} PRIVATE -Wall -Wextra -Wpedantic -Werror -Wold-style-cast -Wfloat-equal -Wmost -Wunused-exception-parameter) else() target_compile_options(${target} PRIVATE -Wall -Wextra -Wpedantic -Werror -Wold-style-cast -Wfloat-equal) endif() endmacro() find_package(nlohmann_json QUIET) find_package(OpenSSL REQUIRED) find_package(Threads REQUIRED) find_package(websocketpp REQUIRED) find_package(ZLIB REQUIRED) if(NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE RelWithDebInfo) endif() option(USE_FOXGLOVE_SDK "Build with Foxglove SDK" OFF) add_definitions(-DUSE_FOXGLOVE_SDK=${USE_FOXGLOVE_SDK}) # Determine wheter to use standalone or boost asio option(USE_ASIO_STANDALONE "Build with standalone ASIO" ON) if(USE_ASIO_STANDALONE) message(STATUS "Using standalone ASIO") add_definitions(-DASIO_STANDALONE) else() message(STATUS "Using Boost ASIO") find_package(Boost REQUIRED) endif(USE_ASIO_STANDALONE) # Detect big-endian architectures include(TestBigEndian) TEST_BIG_ENDIAN(ENDIAN) if (ENDIAN) add_compile_definitions(ARCH_IS_BIG_ENDIAN=1) endif() if(USE_FOXGLOVE_SDK) if (CMAKE_SYSTEM_NAME STREQUAL "Linux" AND CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64") set(FOXGLOVE_SDK_RELEASES "https://github.com/foxglove/foxglove-sdk/releases/download/sdk%2Fv0.10.0/foxglove-v0.10.0-cpp-aarch64-unknown-linux-gnu.zip" "81e2379e3a08160c023779eab0fe5c5764ace3b30c8727b0030ac7dc8b415016") elseif(CMAKE_SYSTEM_NAME STREQUAL "Linux" AND CMAKE_SYSTEM_PROCESSOR STREQUAL "x86_64") set(FOXGLOVE_SDK_RELEASES "https://github.com/foxglove/foxglove-sdk/releases/download/sdk%2Fv0.10.0/foxglove-v0.10.0-cpp-x86_64-unknown-linux-gnu.zip" "209d34a8703d44a129c559493e1a3fb215888b4d5973622750ac28b1c345946c") else() message(FATAL_ERROR "Unsupported platform: ${CMAKE_SYSTEM_PROCESSOR}-${CMAKE_SYSTEM_NAME}") endif() list(GET FOXGLOVE_SDK_RELEASES 0 url) list(GET FOXGLOVE_SDK_RELEASES 1 hash) # Download Foxglove SDK shared lib include(FetchContent) FetchContent_Declare( foxglove_sdk URL ${url} URL_HASH SHA256=${hash} ) FetchContent_MakeAvailable(foxglove_sdk) add_library(foxglove_sdk STATIC) target_include_directories(foxglove_sdk SYSTEM PUBLIC $ $ ) file(GLOB_RECURSE FOXGLOVE_SDK_SOURCES CONFIGURE_DEPENDS "${foxglove_sdk_SOURCE_DIR}/src/*.cpp") target_sources(foxglove_sdk PRIVATE ${FOXGLOVE_SDK_SOURCES}) set_target_properties(foxglove_sdk PROPERTIES POSITION_INDEPENDENT_CODE ON) target_link_libraries(foxglove_sdk PRIVATE ${foxglove_sdk_SOURCE_DIR}/lib/libfoxglove.a) endif() # Get git commit hash and replace variables in version.cpp.in find_program(GIT_SCM git DOC "Git version control") if (GIT_SCM) execute_process( COMMAND ${GIT_SCM} describe --always --dirty --exclude="*" WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} OUTPUT_VARIABLE FOXGLOVE_BRIDGE_GIT_HASH OUTPUT_STRIP_TRAILING_WHITESPACE ) endif() set(FOXGLOVE_BRIDGE_VERSION "${CMAKE_PROJECT_VERSION}") configure_file(foxglove_bridge_base/src/version.cpp.in foxglove_bridge_base/src/version.cpp @ONLY) # Build the foxglove_bridge_base library add_library(foxglove_bridge_base SHARED foxglove_bridge_base/src/base64.cpp foxglove_bridge_base/src/foxglove_bridge.cpp foxglove_bridge_base/src/parameter.cpp foxglove_bridge_base/src/serialization.cpp foxglove_bridge_base/src/server_factory.cpp foxglove_bridge_base/src/test/test_client.cpp # Generated: ${CMAKE_CURRENT_BINARY_DIR}/foxglove_bridge_base/src/version.cpp ) target_include_directories(foxglove_bridge_base PUBLIC $ $ ) target_link_libraries(foxglove_bridge_base OpenSSL::Crypto OpenSSL::SSL ZLIB::ZLIB ${CMAKE_THREAD_LIBS_INIT} ) if(nlohmann_json_FOUND) target_link_libraries(foxglove_bridge_base nlohmann_json::nlohmann_json) else() message(STATUS "nlohmann_json not found, will search at compile time") endif() enable_strict_compiler_warnings(foxglove_bridge_base) message(STATUS "ROS_VERSION: " $ENV{ROS_VERSION}) message(STATUS "ROS_DISTRO: " $ENV{ROS_DISTRO}) message(STATUS "ROS_ROOT: " $ENV{ROS_ROOT}) # ROS 1 if(CATKIN_DEVEL_PREFIX OR catkin_FOUND OR CATKIN_BUILD_BINARY_PACKAGE) message(STATUS "Building with catkin") set(ROS_BUILD_TYPE "catkin") find_package(catkin REQUIRED COMPONENTS nodelet resource_retriever ros_babel_fish rosgraph_msgs roslib roscpp) find_package(Boost REQUIRED) catkin_package( INCLUDE_DIRS foxglove_bridge_base/include LIBRARIES foxglove_bridge_base foxglove_bridge_nodelet CATKIN_DEPENDS nodelet resource_retriever ros_babel_fish rosgraph_msgs roslib roscpp DEPENDS Boost ) add_library(foxglove_bridge_nodelet ros1_foxglove_bridge/src/ros1_foxglove_bridge_nodelet.cpp ros1_foxglove_bridge/src/param_utils.cpp ros1_foxglove_bridge/src/service_utils.cpp ) target_include_directories(foxglove_bridge_nodelet SYSTEM PRIVATE $ $ $ ${catkin_INCLUDE_DIRS} ) target_link_libraries(foxglove_bridge_nodelet foxglove_bridge_base ${catkin_LIBRARIES}) enable_strict_compiler_warnings(foxglove_bridge_nodelet) add_executable(foxglove_bridge ros1_foxglove_bridge/src/ros1_foxglove_bridge_node.cpp) target_include_directories(foxglove_bridge SYSTEM PRIVATE ${catkin_INCLUDE_DIRS}) target_link_libraries(foxglove_bridge ${catkin_LIBRARIES}) enable_strict_compiler_warnings(foxglove_bridge) else() message(FATAL_ERROR "Could not find catkin") endif() #### TESTS ##################################################################### if (CATKIN_ENABLE_TESTING) message(STATUS "Building tests with catkin") find_package(catkin REQUIRED COMPONENTS roscpp std_msgs std_srvs) if(NOT "$ENV{ROS_DISTRO}" STREQUAL "melodic") find_package(GTest REQUIRED) endif() find_package(rostest REQUIRED) find_package(Boost REQUIRED COMPONENTS system) catkin_add_gtest(version_test foxglove_bridge_base/tests/version_test.cpp) target_link_libraries(version_test foxglove_bridge_base ${Boost_LIBRARIES}) enable_strict_compiler_warnings(version_test) catkin_add_gtest(serialization_test foxglove_bridge_base/tests/serialization_test.cpp) target_link_libraries(serialization_test foxglove_bridge_base ${Boost_LIBRARIES}) enable_strict_compiler_warnings(foxglove_bridge) catkin_add_gtest(base64_test foxglove_bridge_base/tests/base64_test.cpp) target_link_libraries(base64_test foxglove_bridge_base ${Boost_LIBRARIES}) enable_strict_compiler_warnings(foxglove_bridge) add_rostest_gtest(smoke_test ros1_foxglove_bridge/tests/smoke.test ros1_foxglove_bridge/tests/smoke_test.cpp) target_include_directories(smoke_test SYSTEM PRIVATE $ ${catkin_INCLUDE_DIRS} $ ) target_link_libraries(smoke_test foxglove_bridge_base ${catkin_LIBRARIES}) enable_strict_compiler_warnings(smoke_test) endif() #### INSTALL ################################################################### install(TARGETS foxglove_bridge RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) install(TARGETS foxglove_bridge_base foxglove_bridge_nodelet ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} ) install(FILES nodelets.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) install(DIRECTORY ros1_foxglove_bridge/launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch ) ================================================ FILE: Dockerfile.ros1 ================================================ ARG ROS_DISTRIBUTION=noetic FROM ros:$ROS_DISTRIBUTION-ros-base # Update apt keys for EOL distros. For current distros this is already in the base docker image. # https://github.com/osrf/docker_images/commit/eb5634cf92ba079897e44fb7541d3b78aa6cf717 # https://discourse.ros.org/t/ros-signing-key-migration-guide/43937 ADD --checksum=sha256:c0cc26f70da91a4fa5a613278a53885184e91df45214ab34e1bae0f3a44f83ea https://github.com/ros-infrastructure/ros-apt-source/releases/download/1.1.0/ros-apt-source_1.1.0.focal_all.deb /tmp/ros-apt-source.deb RUN rm -f /etc/apt/sources.list.d/ros1-latest.list \ && apt-get install /tmp/ros-apt-source.deb \ && rm -f /tmp/ros-apt-source.deb \ && rm -rf /var/lib/apt/lists/* # Install clang and set as default compiler. RUN apt-get update && apt-get install -y --no-install-recommends \ clang \ && rm -rf /var/lib/apt/lists/* ENV CC=clang ENV CXX=clang++ # Set environment and working directory ENV ROS_WS /ros1_ws WORKDIR $ROS_WS # Add package.xml so we can install package dependencies. COPY package.xml src/ros-foxglove-bridge/ # Install rosdep dependencies RUN . /opt/ros/$ROS_DISTRO/setup.sh && \ apt-get update && rosdep update --include-eol-distros && rosdep install -y \ --from-paths \ src \ --ignore-src \ && rm -rf /var/lib/apt/lists/* # Add common files and ROS 1 source code COPY CMakeLists.txt src/ros-foxglove-bridge/CMakeLists.txt COPY foxglove_bridge_base src/ros-foxglove-bridge/foxglove_bridge_base COPY nodelets.xml src/ros-foxglove-bridge/nodelets.xml COPY ros1_foxglove_bridge src/ros-foxglove-bridge/ros1_foxglove_bridge ARG USE_ASIO_STANDALONE=ON # Build the Catkin workspace RUN . /opt/ros/$ROS_DISTRO/setup.sh \ && catkin_make -DUSE_ASIO_STANDALONE=$USE_ASIO_STANDALONE # source workspace from entrypoint RUN sed --in-place \ 's|^source .*|source "$ROS_WS/devel/setup.bash"|' \ /ros_entrypoint.sh # Run foxglove_bridge CMD ["rosrun", "foxglove_bridge", "foxglove_bridge"] ================================================ FILE: LICENSE ================================================ MIT License Copyright (c) 2022 Foxglove Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. ================================================ FILE: Makefile ================================================ ROS1_DISTRIBUTIONS := melodic noetic define generate_ros1_targets .PHONY: $(1) $(1): docker build -t foxglove_bridge_$(1) --pull -f Dockerfile.ros1 --build-arg ROS_DISTRIBUTION=$(1) . .PHONY: $(1)-test $(1)-test: $(1) docker run -t --rm foxglove_bridge_$(1) bash -c "catkin_make run_tests && catkin_test_results" .PHONY: $(1)-boost-asio $(1)-boost-asio: docker build -t foxglove_bridge_$(1)_boost_asio --pull -f Dockerfile.ros1 --build-arg ROS_DISTRIBUTION=$(1) --build-arg USE_ASIO_STANDALONE=OFF . .PHONY: $(1)-test-boost-asio $(1)-test-boost-asio: $(1)-boost-asio docker run -t --rm foxglove_bridge_$(1)_boost_asio bash -c "catkin_make run_tests && catkin_test_results" endef $(foreach distribution,$(ROS1_DISTRIBUTIONS),$(eval $(call generate_ros1_targets,$(strip $(distribution))))) default: ros1 .PHONY: ros1 ros1: docker build -t foxglove_bridge_ros1 --pull -f Dockerfile.ros1 . clean: docker rmi $(docker images --filter=reference="foxglove_bridge_*" -q) ================================================ FILE: README.md ================================================ foxglove_bridge =============== High performance ROS 1 WebSocket bridge using the Foxglove WebSocket protocol, written in C++. If you are looking for the ROS 2 WebSocket bridge, please go to https://github.com/foxglove/foxglove-sdk. ## Motivation Live debugging of ROS systems has traditionally relied on running ROS tooling such as rviz. This requires either a GUI and connected peripherals on the robot, or replicating the same ROS environment on a network-connected development machine including the same version of ROS, all custom message definitions, etc. To overcome this limitation and allow remote debugging from web tooling or non-ROS systems, rosbridge was developed. However, rosbridge suffers from performance problems with high frequency topics and/or large messages, and the protocol does not support full visibility into ROS systems such as interacting with parameters or seeing the full graph of publishers and subscribers. The `foxglove_bridge` uses the [Foxglove WebSocket protocol](https://github.com/foxglove/ws-protocol), a similar protocol to rosbridge but with the ability to support additional schema formats, parameters, graph introspection, and non-ROS systems. The bridge is written in C++ and designed for high performance with low overhead to minimize the impact to your robot stack. ## Installation **Note**: While binary packages are available for ROS 1, all ROS 1 distributions are End-of-Life. That means that new binary packages of `foxglove_bridge` cannot be released into those distributions, and the versions are quite outdated. It is highly recommended to [build foxglove_bridge from source](#building-from-source) for the latest bug fixes. For similar reasons, this `foxglove_bridge` package for ROS 1 is in maintenance mode and only bug fixes will be applied. ## Running the bridge To run the bridge node, it is recommended to use the provided launch file: **ROS 1** ```bash roslaunch --screen foxglove_bridge foxglove_bridge.launch port:=8765 ``` ```xml ``` ### Configuration Parameters are provided to configure the behavior of the bridge. These parameters must be set at initialization through a launch file or the command line, they cannot be modified at runtime. * __port__: The TCP port to bind the WebSocket server to. Must be a valid TCP port number, or 0 to use a random port. Defaults to `8765`. * __address__: The host address to bind the WebSocket server to. Defaults to `0.0.0.0`, listening on all interfaces by default. Change this to `127.0.0.1` (or `::1` for IPv6) to only accept connections from the local machine. * __tls__: If `true`, use Transport Layer Security (TLS) for encrypted communication. Defaults to `false`. * __certfile__: Path to the certificate to use for TLS. Required when __tls__ is set to `true`. Defaults to `""`. * __keyfile__: Path to the private key to use for TLS. Required when __tls__ is set to `true`. Defaults to `""`. * __topic_whitelist__: List of regular expressions ([ECMAScript grammar](https://en.cppreference.com/w/cpp/regex/ecmascript)) of whitelisted topic names. Defaults to `[".*"]`. * __service_whitelist__: List of regular expressions ([ECMAScript grammar](https://en.cppreference.com/w/cpp/regex/ecmascript)) of whitelisted service names. Defaults to `[".*"]`. * __param_whitelist__: List of regular expressions ([ECMAScript grammar](https://en.cppreference.com/w/cpp/regex/ecmascript)) of whitelisted parameter names. Defaults to `[".*"]`. * __client_topic_whitelist__: List of regular expressions ([ECMAScript grammar](https://en.cppreference.com/w/cpp/regex/ecmascript)) of whitelisted client-published topic names. Defaults to `[".*"]`. * __send_buffer_limit__: Connection send buffer limit in bytes. Messages will be dropped when a connection's send buffer reaches this limit to avoid a queue of outdated messages building up. Defaults to `10000000` (10 MB). * __use_compression__: Use websocket compression (permessage-deflate). It is recommended to leave this turned off as it increases CPU usage and per-message compression often yields low compression ratios for robotics data. Defaults to `false`. * __capabilities__: List of supported [server capabilities](https://github.com/foxglove/ws-protocol/blob/main/docs/spec.md). Defaults to `[clientPublish,parameters,parametersSubscribe,services,connectionGraph,assets]`. * __asset_uri_allowlist__: List of regular expressions ([ECMAScript grammar](https://en.cppreference.com/w/cpp/regex/ecmascript)) of allowed asset URIs. Uses the [resource_retriever](https://index.ros.org/p/resource_retriever/github-ros-resource_retriever) to resolve `package://`, `file://` or `http(s)://` URIs. Note that this list should be carefully configured such that no confidential files are accidentally exposed over the websocket connection. As an extra security measure, URIs containing two consecutive dots (`..`) are disallowed as they could be used to construct URIs that would allow retrieval of confidential files if the allowlist is not configured strict enough (e.g. `package:///../../../secret.txt`). Defaults to `["^package://(?:[-\w%]+/)*[-\w%]+\.(?:dae|fbx|glb|gltf|jpeg|jpg|mtl|obj|png|stl|tif|tiff|urdf|webp|xacro)$"]`. * __max_update_ms__: The maximum number of milliseconds to wait in between polling `roscore` for new topics, services, or parameters. Defaults to `5000`. * __service_type_retrieval_timeout_ms__: Max number of milliseconds for retrieving a services type information. Defaults to `250`. ## Building from source ### Fetch source, install dependencies, and build for ROS 1 ```bash source /opt/ros/noetic/setup.bash mkdir -p foxglove_bridge_ws/src cd foxglove_bridge_ws git clone https://github.com/foxglove/ros-foxglove-bridge.git src/ros-foxglove-bridge rosdep update rosdep install --ignore-src --default-yes --from-path src catkin_make install source install/setup.bash ``` ## Clients [Foxglove](https://foxglove.dev/) connects to foxglove_bridge for live robotics visualization. ## License `foxglove_bridge` is released with a MIT license. For full terms and conditions, see the [LICENSE](LICENSE) file. ================================================ FILE: download_test_data.sh ================================================ #!/usr/bin/env bash mkdir -p data cd data git clone https://github.com/foxglove/ros-foxglove-bridge-benchmark-assets.git . ================================================ FILE: foxglove_bridge_base/include/foxglove_bridge/base64.hpp ================================================ #pragma once #include #include #include #include namespace foxglove_ws { std::string base64Encode(const std::string_view& input); std::vector base64Decode(const std::string& input); } // namespace foxglove_ws ================================================ FILE: foxglove_bridge_base/include/foxglove_bridge/callback_queue.hpp ================================================ #pragma once #include #include #include #include #include #include #include #include "websocket_logging.hpp" namespace foxglove_ws { class CallbackQueue { public: CallbackQueue(LogCallback logCallback, size_t numThreads = 1) : _logCallback(logCallback) , _quit(false) { for (size_t i = 0; i < numThreads; ++i) { _workerThreads.push_back(std::thread(&CallbackQueue::doWork, this)); } } ~CallbackQueue() { stop(); } void stop() { _quit = true; _cv.notify_all(); for (auto& thread : _workerThreads) { thread.join(); } } void addCallback(std::function cb) { if (_quit) { return; } std::unique_lock lock(_mutex); _callbackQueue.push_back(cb); _cv.notify_one(); } private: void doWork() { while (!_quit) { std::unique_lock lock(_mutex); _cv.wait(lock, [this] { return (_quit || !_callbackQueue.empty()); }); if (_quit) { break; } else if (!_callbackQueue.empty()) { std::function cb = _callbackQueue.front(); _callbackQueue.pop_front(); lock.unlock(); try { cb(); } catch (const std::exception& ex) { // Should never get here if we catch all exceptions in the callbacks. const std::string msg = std::string("Caught unhandled exception in calback_queue") + ex.what(); _logCallback(WebSocketLogLevel::Error, msg.c_str()); } catch (...) { _logCallback(WebSocketLogLevel::Error, "Caught unhandled exception in calback_queue"); } } } } LogCallback _logCallback; std::atomic _quit; std::mutex _mutex; std::condition_variable _cv; std::deque> _callbackQueue; std::vector _workerThreads; }; } // namespace foxglove_ws ================================================ FILE: foxglove_bridge_base/include/foxglove_bridge/common.hpp ================================================ #pragma once #include #include #include #include #include #include namespace foxglove_ws { #ifdef USE_FOXGLOVE_SDK constexpr char SUPPORTED_SUBPROTOCOL[] = "foxglove.sdk.v1"; #else constexpr char SUPPORTED_SUBPROTOCOL[] = "foxglove.websocket.v1"; #endif constexpr char CAPABILITY_CLIENT_PUBLISH[] = "clientPublish"; constexpr char CAPABILITY_TIME[] = "time"; constexpr char CAPABILITY_PARAMETERS[] = "parameters"; constexpr char CAPABILITY_PARAMETERS_SUBSCRIBE[] = "parametersSubscribe"; constexpr char CAPABILITY_SERVICES[] = "services"; constexpr char CAPABILITY_CONNECTION_GRAPH[] = "connectionGraph"; constexpr char CAPABILITY_ASSETS[] = "assets"; constexpr std::array DEFAULT_CAPABILITIES = { CAPABILITY_CLIENT_PUBLISH, CAPABILITY_CONNECTION_GRAPH, CAPABILITY_PARAMETERS_SUBSCRIBE, CAPABILITY_PARAMETERS, CAPABILITY_SERVICES, CAPABILITY_ASSETS, }; using ChannelId = uint32_t; using ClientChannelId = uint32_t; using SubscriptionId = uint32_t; using ServiceId = uint32_t; enum class BinaryOpcode : uint8_t { MESSAGE_DATA = 1, TIME_DATA = 2, SERVICE_CALL_RESPONSE = 3, FETCH_ASSET_RESPONSE = 4, }; enum class ClientBinaryOpcode : uint8_t { MESSAGE_DATA = 1, SERVICE_CALL_REQUEST = 2, }; enum class WebSocketLogLevel { Debug, Info, Warn, Error, Critical, }; struct ChannelWithoutId { std::string topic; std::string encoding; std::string schemaName; std::string schema; std::optional schemaEncoding; bool operator==(const ChannelWithoutId& other) const { return topic == other.topic && encoding == other.encoding && schemaName == other.schemaName && schema == other.schema && schemaEncoding == other.schemaEncoding; } }; struct Channel : ChannelWithoutId { ChannelId id; Channel() = default; // requirement for json conversions. Channel(ChannelId id, ChannelWithoutId ch) : ChannelWithoutId(std::move(ch)) , id(id) {} bool operator==(const Channel& other) const { return id == other.id && ChannelWithoutId::operator==(other); } }; struct ClientAdvertisement { ClientChannelId channelId; std::string topic; std::string encoding; std::string schemaName; std::vector schema; }; struct ClientMessage { uint64_t logTime; uint64_t publishTime; uint32_t sequence; ClientAdvertisement advertisement; size_t dataLength; std::vector data; ClientMessage(uint64_t logTime, uint64_t publishTime, uint32_t sequence, const ClientAdvertisement& advertisement, size_t dataLength, const uint8_t* rawData) : logTime(logTime) , publishTime(publishTime) , sequence(sequence) , advertisement(advertisement) , dataLength(dataLength) , data(dataLength) { std::memcpy(data.data(), rawData, dataLength); } static const size_t MSG_PAYLOAD_OFFSET = 5; const uint8_t* getData() const { return data.data() + MSG_PAYLOAD_OFFSET; } std::size_t getLength() const { return data.size() - MSG_PAYLOAD_OFFSET; } }; struct ServiceWithoutId { std::string name; std::string type; std::string requestSchema; std::string responseSchema; }; struct Service : ServiceWithoutId { ServiceId id; Service() = default; Service(const ServiceWithoutId& s, const ServiceId& id) : ServiceWithoutId(s) , id(id) {} }; struct ServiceResponse { ServiceId serviceId; uint32_t callId; std::string encoding; std::vector data; size_t size() const { return 4 + 4 + 4 + encoding.size() + data.size(); } void read(const uint8_t* data, size_t size); void write(uint8_t* data) const; bool operator==(const ServiceResponse& other) const { return serviceId == other.serviceId && callId == other.callId && encoding == other.encoding && data == other.data; } }; using ServiceRequest = ServiceResponse; enum class FetchAssetStatus : uint8_t { Success = 0, Error = 1, }; struct FetchAssetResponse { uint32_t requestId; FetchAssetStatus status; std::string errorMessage; std::vector data; }; } // namespace foxglove_ws ================================================ FILE: foxglove_bridge_base/include/foxglove_bridge/foxglove_bridge.hpp ================================================ #pragma once namespace foxglove { const char* WebSocketUserAgent(); extern const char FOXGLOVE_BRIDGE_VERSION[]; extern const char FOXGLOVE_BRIDGE_GIT_HASH[]; } // namespace foxglove ================================================ FILE: foxglove_bridge_base/include/foxglove_bridge/message_definition_cache.hpp ================================================ #pragma once #include #include #include #include #include namespace foxglove { // Taken from // https://github.com/ros2/rosidl/blob/a57baea5/rosidl_parser/rosidl_parser/definition.py constexpr char SERVICE_REQUEST_MESSAGE_SUFFIX[] = "_Request"; constexpr char SERVICE_RESPONSE_MESSAGE_SUFFIX[] = "_Response"; constexpr char ACTION_GOAL_SERVICE_SUFFIX[] = "_SendGoal"; constexpr char ACTION_RESULT_SERVICE_SUFFIX[] = "_GetResult"; constexpr char ACTION_FEEDBACK_MESSAGE_SUFFIX[] = "_FeedbackMessage"; enum struct MessageDefinitionFormat { IDL, MSG, }; struct MessageSpec { MessageSpec(MessageDefinitionFormat format, std::string text, const std::string& package_context); const std::set dependencies; const std::string text; MessageDefinitionFormat format; }; struct DefinitionIdentifier { MessageDefinitionFormat format; std::string package_resource_name; bool operator==(const DefinitionIdentifier& di) const { return (format == di.format) && (package_resource_name == di.package_resource_name); } }; class DefinitionNotFoundError : public std::exception { private: std::string name_; public: explicit DefinitionNotFoundError(std::string name) : name_(std::move(name)) {} const char* what() const noexcept override { return name_.c_str(); } }; class MessageDefinitionCache final { public: /** * Concatenate the message definition with its dependencies into a self-contained schema. * The format is different for MSG and IDL definitions, and is described fully here: * [MSG](https://mcap.dev/specification/appendix.html#ros2msg-data-format) * [IDL](https://mcap.dev/specification/appendix.html#ros2idl-data-format) * Throws DefinitionNotFoundError if one or more definition files are missing for the given * package resource name. */ std::pair get_full_text( const std::string& package_resource_name); private: struct DefinitionIdentifierHash { std::size_t operator()(const DefinitionIdentifier& di) const { std::size_t h1 = std::hash()(di.format); std::size_t h2 = std::hash()(di.package_resource_name); return h1 ^ h2; } }; /** * Load and parse the message file referenced by the given datatype, or return it from * msg_specs_by_datatype */ const MessageSpec& load_message_spec(const DefinitionIdentifier& definition_identifier); std::unordered_map msg_specs_by_definition_identifier_; std::unordered_map full_text_cache_; }; std::set parse_dependencies(MessageDefinitionFormat format, const std::string& text, const std::string& package_context); } // namespace foxglove ================================================ FILE: foxglove_bridge_base/include/foxglove_bridge/parameter.hpp ================================================ #pragma once #include #include #include #include #include namespace foxglove_ws { enum class ParameterSubscriptionOperation { SUBSCRIBE, UNSUBSCRIBE, }; enum class ParameterType { PARAMETER_NOT_SET, PARAMETER_BOOL, PARAMETER_INTEGER, PARAMETER_DOUBLE, PARAMETER_STRING, PARAMETER_ARRAY, PARAMETER_STRUCT, // ROS 1 only PARAMETER_BYTE_ARRAY, // ROS 2 only }; class ParameterValue { public: ParameterValue(); ParameterValue(bool value); ParameterValue(int value); ParameterValue(int64_t value); ParameterValue(double value); ParameterValue(const std::string& value); ParameterValue(const char* value); ParameterValue(const std::vector& value); ParameterValue(const std::vector& value); ParameterValue(const std::unordered_map& value); inline ParameterType getType() const { return _type; } template inline const T& getValue() const { return std::any_cast(_value); } private: ParameterType _type; std::any _value; }; class Parameter { public: Parameter(); Parameter(const std::string& name); Parameter(const std::string& name, const ParameterValue& value); inline const std::string& getName() const { return _name; } inline ParameterType getType() const { return _value.getType(); } inline const ParameterValue& getValue() const { return _value; } private: std::string _name; ParameterValue _value; }; } // namespace foxglove_ws ================================================ FILE: foxglove_bridge_base/include/foxglove_bridge/regex_utils.hpp ================================================ #pragma once #include #include #include #include namespace foxglove_ws { inline bool isWhitelisted(const std::string& name, const std::vector& regexPatterns) { return std::find_if(regexPatterns.begin(), regexPatterns.end(), [name](const auto& regex) { return std::regex_match(name, regex); }) != regexPatterns.end(); } } // namespace foxglove_ws ================================================ FILE: foxglove_bridge_base/include/foxglove_bridge/serialization.hpp ================================================ #pragma once #include #include #include "common.hpp" #include "parameter.hpp" namespace foxglove_ws { inline void WriteUint64LE(uint8_t* buf, uint64_t val) { #ifdef ARCH_IS_BIG_ENDIAN buf[0] = val & 0xff; buf[1] = (val >> 8) & 0xff; buf[2] = (val >> 16) & 0xff; buf[3] = (val >> 24) & 0xff; buf[4] = (val >> 32) & 0xff; buf[5] = (val >> 40) & 0xff; buf[6] = (val >> 48) & 0xff; buf[7] = (val >> 56) & 0xff; #else reinterpret_cast(buf)[0] = val; #endif } inline void WriteUint32LE(uint8_t* buf, uint32_t val) { #ifdef ARCH_IS_BIG_ENDIAN buf[0] = val & 0xff; buf[1] = (val >> 8) & 0xff; buf[2] = (val >> 16) & 0xff; buf[3] = (val >> 24) & 0xff; #else reinterpret_cast(buf)[0] = val; #endif } inline uint32_t ReadUint32LE(const uint8_t* buf) { #ifdef ARCH_IS_BIG_ENDIAN uint32_t val = (bytes[0] << 24) + (bytes[1] << 16) + (bytes[2] << 8) + bytes[3]; return val; #else return reinterpret_cast(buf)[0]; #endif } void to_json(nlohmann::json& j, const Channel& c); void from_json(const nlohmann::json& j, Channel& c); void to_json(nlohmann::json& j, const ParameterValue& p); void from_json(const nlohmann::json& j, ParameterValue& p); void to_json(nlohmann::json& j, const Parameter& p); void from_json(const nlohmann::json& j, Parameter& p); void to_json(nlohmann::json& j, const Service& p); void from_json(const nlohmann::json& j, Service& p); } // namespace foxglove_ws ================================================ FILE: foxglove_bridge_base/include/foxglove_bridge/server_factory.hpp ================================================ #pragma once #include #include #include #include "common.hpp" #include "server_interface.hpp" namespace foxglove_ws { class ServerFactory { public: template static std::unique_ptr> createServer( const std::string& name, const std::function& logHandler, const ServerOptions& options); }; } // namespace foxglove_ws ================================================ FILE: foxglove_bridge_base/include/foxglove_bridge/server_interface.hpp ================================================ #pragma once #include #include #include #include #include #include #include #include "common.hpp" #include "parameter.hpp" namespace foxglove_ws { constexpr size_t DEFAULT_SEND_BUFFER_LIMIT_BYTES = 10000000UL; // 10 MB using MapOfSets = std::unordered_map>; template class ExeptionWithId : public std::runtime_error { public: explicit ExeptionWithId(IdType id, const std::string& what_arg) : std::runtime_error(what_arg) , _id(id) {} IdType id() const { return _id; } private: IdType _id; }; class ChannelError : public ExeptionWithId { using ExeptionWithId::ExeptionWithId; }; class ClientChannelError : public ExeptionWithId { using ExeptionWithId::ExeptionWithId; }; class ServiceError : public ExeptionWithId { using ExeptionWithId::ExeptionWithId; }; struct ServerOptions { std::vector capabilities; std::vector supportedEncodings; std::unordered_map metadata; size_t sendBufferLimitBytes = DEFAULT_SEND_BUFFER_LIMIT_BYTES; bool useTls = false; std::string certfile = ""; std::string keyfile = ""; std::string sessionId; bool useCompression = false; std::vector clientTopicWhitelistPatterns; }; template struct ServerHandlers { std::function subscribeHandler; std::function unsubscribeHandler; std::function clientAdvertiseHandler; std::function clientUnadvertiseHandler; std::function clientMessageHandler; std::function&, const std::optional&, ConnectionHandle)> parameterRequestHandler; std::function&, const std::optional&, ConnectionHandle)> parameterChangeHandler; std::function&, ParameterSubscriptionOperation, ConnectionHandle)> parameterSubscriptionHandler; std::function serviceRequestHandler; std::function subscribeConnectionGraphHandler; std::function fetchAssetHandler; }; template class ServerInterface { public: virtual ~ServerInterface() {} virtual void start(const std::string& host, uint16_t port) = 0; virtual void stop() = 0; virtual std::vector addChannels(const std::vector& channels) = 0; virtual void removeChannels(const std::vector& channelIds) = 0; virtual void publishParameterValues(ConnectionHandle clientHandle, const std::vector& parameters, const std::optional& requestId) = 0; virtual void updateParameterValues(const std::vector& parameters) = 0; virtual std::vector addServices(const std::vector& services) = 0; virtual void removeServices(const std::vector& serviceIds) = 0; virtual void setHandlers(ServerHandlers&& handlers) = 0; virtual void sendMessage(ConnectionHandle clientHandle, ChannelId chanId, uint64_t timestamp, const uint8_t* payload, size_t payloadSize) = 0; virtual void broadcastTime(uint64_t timestamp) = 0; virtual void sendServiceResponse(ConnectionHandle clientHandle, const ServiceResponse& response) = 0; virtual void sendServiceFailure(ConnectionHandle clientHandle, ServiceId serviceId, uint32_t callId, const std::string& message) = 0; virtual void updateConnectionGraph(const MapOfSets& publishedTopics, const MapOfSets& subscribedTopics, const MapOfSets& advertisedServices) = 0; virtual void sendFetchAssetResponse(ConnectionHandle clientHandle, const FetchAssetResponse& response) = 0; virtual uint16_t getPort() = 0; virtual std::string remoteEndpointString(ConnectionHandle clientHandle) = 0; }; } // namespace foxglove_ws ================================================ FILE: foxglove_bridge_base/include/foxglove_bridge/test/test_client.hpp ================================================ #pragma once #include #include #include #include #include "../parameter.hpp" #include "../websocket_client.hpp" namespace foxglove_ws { std::future> waitForChannelMsg(ClientInterface* client, SubscriptionId subscriptionId); std::future> waitForParameters(std::shared_ptr client, const std::string& requestId = std::string()); std::future waitForServiceResponse(std::shared_ptr client); std::future waitForService(std::shared_ptr client, const std::string& serviceName); std::future waitForChannel(std::shared_ptr client, const std::string& topicName); std::future waitForFetchAssetResponse(std::shared_ptr client); extern template class Client; } // namespace foxglove_ws ================================================ FILE: foxglove_bridge_base/include/foxglove_bridge/websocket_client.hpp ================================================ #pragma once #include #include #include #include #include #include #include #include #include #include #include "common.hpp" #include "parameter.hpp" #include "serialization.hpp" namespace foxglove_ws { inline void to_json(nlohmann::json& j, const ClientAdvertisement& p) { j = nlohmann::json{{"id", p.channelId}, {"topic", p.topic}, {"encoding", p.encoding}, {"schemaName", p.schemaName}}; } using TextMessageHandler = std::function; using BinaryMessageHandler = std::function; using OpCode = websocketpp::frame::opcode::value; class ClientInterface { public: virtual void connect( const std::string& uri, std::function onOpenHandler, std::function onCloseHandler = nullptr) = 0; virtual std::future connect(const std::string& uri) = 0; virtual void close() = 0; virtual void subscribe( const std::vector>& subscriptions) = 0; virtual void unsubscribe(const std::vector& subscriptionIds) = 0; virtual void advertise(const std::vector& channels) = 0; virtual void unadvertise(const std::vector& channelIds) = 0; virtual void publish(ClientChannelId channelId, const uint8_t* buffer, size_t size) = 0; virtual void sendServiceRequest(const ServiceRequest& request) = 0; virtual void getParameters(const std::vector& parameterNames, const std::optional& requestId) = 0; virtual void setParameters(const std::vector& parameters, const std::optional& requestId) = 0; virtual void subscribeParameterUpdates(const std::vector& parameterNames) = 0; virtual void unsubscribeParameterUpdates(const std::vector& parameterNames) = 0; virtual void fetchAsset(const std::string& name, uint32_t requestId) = 0; virtual void setTextMessageHandler(TextMessageHandler handler) = 0; virtual void setBinaryMessageHandler(BinaryMessageHandler handler) = 0; }; template class Client : public ClientInterface { public: using ClientType = websocketpp::client; using MessagePtr = typename ClientType::message_ptr; using ConnectionPtr = typename ClientType::connection_ptr; Client() { _endpoint.clear_access_channels(websocketpp::log::alevel::all); _endpoint.clear_error_channels(websocketpp::log::elevel::all); _endpoint.init_asio(); _endpoint.start_perpetual(); _endpoint.set_message_handler( bind(&Client::messageHandler, this, std::placeholders::_1, std::placeholders::_2)); _thread.reset(new websocketpp::lib::thread(&ClientType::run, &_endpoint)); } virtual ~Client() { close(); _endpoint.stop_perpetual(); _thread->join(); } void connect(const std::string& uri, std::function onOpenHandler, std::function onCloseHandler = nullptr) override { std::unique_lock lock(_mutex); websocketpp::lib::error_code ec; _con = _endpoint.get_connection(uri, ec); if (ec) { throw std::runtime_error("Failed to get connection from URI " + uri); } if (onOpenHandler) { _con->set_open_handler(onOpenHandler); } if (onCloseHandler) { _con->set_close_handler(onCloseHandler); } _con->add_subprotocol(SUPPORTED_SUBPROTOCOL); _endpoint.connect(_con); } std::future connect(const std::string& uri) override { auto promise = std::make_shared>(); auto future = promise->get_future(); connect(uri, [p = std::move(promise)](websocketpp::connection_hdl) mutable { p->set_value(); }); return future; } void close() override { std::unique_lock lock(_mutex); if (!_con) { return; // Already disconnected } _endpoint.close(_con, websocketpp::close::status::going_away, ""); _con.reset(); } void messageHandler(websocketpp::connection_hdl hdl, MessagePtr msg) { (void)hdl; const OpCode op = msg->get_opcode(); switch (op) { case OpCode::TEXT: { std::shared_lock lock(_mutex); if (_textMessageHandler) { _textMessageHandler(msg->get_payload()); } } break; case OpCode::BINARY: { std::shared_lock lock(_mutex); const auto& payload = msg->get_payload(); if (_binaryMessageHandler) { _binaryMessageHandler(reinterpret_cast(payload.data()), payload.size()); } } break; default: break; } } void subscribe(const std::vector>& subscriptions) override { nlohmann::json subscriptionsJson; for (const auto& [subId, channelId] : subscriptions) { subscriptionsJson.push_back({{"id", subId}, {"channelId", channelId}}); } const std::string payload = nlohmann::json{{"op", "subscribe"}, {"subscriptions", std::move(subscriptionsJson)}}.dump(); sendText(payload); } void unsubscribe(const std::vector& subscriptionIds) override { const std::string payload = nlohmann::json{{"op", "unsubscribe"}, {"subscriptionIds", subscriptionIds}}.dump(); sendText(payload); } void advertise(const std::vector& channels) override { const std::string payload = nlohmann::json{{"op", "advertise"}, {"channels", channels}}.dump(); sendText(payload); } void unadvertise(const std::vector& channelIds) override { const std::string payload = nlohmann::json{{"op", "unadvertise"}, {"channelIds", channelIds}}.dump(); sendText(payload); } void publish(ClientChannelId channelId, const uint8_t* buffer, size_t size) override { std::vector payload(1 + 4 + size); payload[0] = uint8_t(ClientBinaryOpcode::MESSAGE_DATA); foxglove_ws::WriteUint32LE(payload.data() + 1, channelId); std::memcpy(payload.data() + 1 + 4, buffer, size); sendBinary(payload.data(), payload.size()); } void sendServiceRequest(const ServiceRequest& request) override { std::vector payload(1 + request.size()); payload[0] = uint8_t(ClientBinaryOpcode::SERVICE_CALL_REQUEST); request.write(payload.data() + 1); sendBinary(payload.data(), payload.size()); } void getParameters(const std::vector& parameterNames, const std::optional& requestId = std::nullopt) override { nlohmann::json jsonPayload{{"op", "getParameters"}, {"parameterNames", parameterNames}}; if (requestId) { jsonPayload["id"] = requestId.value(); } sendText(jsonPayload.dump()); } void setParameters(const std::vector& parameters, const std::optional& requestId = std::nullopt) override { nlohmann::json jsonPayload{{"op", "setParameters"}, {"parameters", parameters}}; if (requestId) { jsonPayload["id"] = requestId.value(); } sendText(jsonPayload.dump()); } void subscribeParameterUpdates(const std::vector& parameterNames) override { nlohmann::json jsonPayload{{"op", "subscribeParameterUpdates"}, {"parameterNames", parameterNames}}; sendText(jsonPayload.dump()); } void unsubscribeParameterUpdates(const std::vector& parameterNames) override { nlohmann::json jsonPayload{{"op", "unsubscribeParameterUpdates"}, {"parameterNames", parameterNames}}; sendText(jsonPayload.dump()); } void fetchAsset(const std::string& uri, uint32_t requestId) override { nlohmann::json jsonPayload{{"op", "fetchAsset"}, {"uri", uri}, {"requestId", requestId}}; sendText(jsonPayload.dump()); } void setTextMessageHandler(TextMessageHandler handler) override { std::unique_lock lock(_mutex); _textMessageHandler = std::move(handler); } void setBinaryMessageHandler(BinaryMessageHandler handler) override { std::unique_lock lock(_mutex); _binaryMessageHandler = std::move(handler); } void sendText(const std::string& payload) { std::shared_lock lock(_mutex); _endpoint.send(_con, payload, OpCode::TEXT); } void sendBinary(const uint8_t* data, size_t dataLength) { std::shared_lock lock(_mutex); _endpoint.send(_con, data, dataLength, OpCode::BINARY); } protected: ClientType _endpoint; websocketpp::lib::shared_ptr _thread; ConnectionPtr _con; std::shared_mutex _mutex; TextMessageHandler _textMessageHandler; BinaryMessageHandler _binaryMessageHandler; }; } // namespace foxglove_ws ================================================ FILE: foxglove_bridge_base/include/foxglove_bridge/websocket_logging.hpp ================================================ #pragma once #include #include #include #include "common.hpp" namespace foxglove_ws { using LogCallback = std::function; inline std::string IPAddressToString(const websocketpp::lib::asio::ip::address& addr) { if (addr.is_v6()) { return "[" + addr.to_string() + "]"; } return addr.to_string(); } inline void NoOpLogCallback(WebSocketLogLevel, char const*) {} class CallbackLogger { public: using channel_type_hint = websocketpp::log::channel_type_hint; CallbackLogger(channel_type_hint::value hint = channel_type_hint::access) : _staticChannels(0xffffffff) , _dynamicChannels(0) , _channelTypeHint(hint) , _callback(NoOpLogCallback) {} CallbackLogger(websocketpp::log::level channels, channel_type_hint::value hint = channel_type_hint::access) : _staticChannels(channels) , _dynamicChannels(0) , _channelTypeHint(hint) , _callback(NoOpLogCallback) {} void set_callback(LogCallback callback) { _callback = callback; } void set_channels(websocketpp::log::level channels) { if (channels == 0) { clear_channels(0xffffffff); return; } _dynamicChannels |= (channels & _staticChannels); } void clear_channels(websocketpp::log::level channels) { _dynamicChannels &= ~channels; } void write(websocketpp::log::level channel, std::string const& msg) { write(channel, msg.c_str()); } void write(websocketpp::log::level channel, char const* msg) { if (!this->dynamic_test(channel)) { return; } if (_channelTypeHint == channel_type_hint::access) { _callback(WebSocketLogLevel::Info, msg); } else { if (channel == websocketpp::log::elevel::devel) { _callback(WebSocketLogLevel::Debug, msg); } else if (channel == websocketpp::log::elevel::library) { _callback(WebSocketLogLevel::Debug, msg); } else if (channel == websocketpp::log::elevel::info) { _callback(WebSocketLogLevel::Info, msg); } else if (channel == websocketpp::log::elevel::warn) { _callback(WebSocketLogLevel::Warn, msg); } else if (channel == websocketpp::log::elevel::rerror) { _callback(WebSocketLogLevel::Error, msg); } else if (channel == websocketpp::log::elevel::fatal) { _callback(WebSocketLogLevel::Critical, msg); } } } constexpr bool static_test(websocketpp::log::level channel) const { return ((channel & _staticChannels) != 0); } bool dynamic_test(websocketpp::log::level channel) { return ((channel & _dynamicChannels) != 0); } private: websocketpp::log::level const _staticChannels; websocketpp::log::level _dynamicChannels; channel_type_hint::value _channelTypeHint; LogCallback _callback; }; } // namespace foxglove_ws ================================================ FILE: foxglove_bridge_base/include/foxglove_bridge/websocket_notls.hpp ================================================ #pragma once #include #include #include #include "./websocket_logging.hpp" namespace foxglove_ws { struct WebSocketNoTls : public websocketpp::config::core { typedef WebSocketNoTls type; typedef core base; typedef base::concurrency_type concurrency_type; typedef base::request_type request_type; typedef base::response_type response_type; typedef base::message_type message_type; typedef base::con_msg_manager_type con_msg_manager_type; typedef base::endpoint_msg_manager_type endpoint_msg_manager_type; typedef CallbackLogger alog_type; typedef CallbackLogger elog_type; typedef base::rng_type rng_type; struct transport_config : public base::transport_config { typedef type::concurrency_type concurrency_type; typedef CallbackLogger alog_type; typedef CallbackLogger elog_type; typedef type::request_type request_type; typedef type::response_type response_type; typedef websocketpp::transport::asio::basic_socket::endpoint socket_type; }; typedef websocketpp::transport::asio::endpoint transport_type; struct permessage_deflate_config {}; typedef websocketpp::extensions::permessage_deflate::enabled permessage_deflate_type; }; } // namespace foxglove_ws ================================================ FILE: foxglove_bridge_base/include/foxglove_bridge/websocket_server.hpp ================================================ #pragma once #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "callback_queue.hpp" #include "common.hpp" #include "parameter.hpp" #include "regex_utils.hpp" #include "serialization.hpp" #include "server_interface.hpp" #include "websocket_logging.hpp" // Debounce a function call (tied to the line number) // This macro takes in a function and the debounce time in milliseconds #define FOXGLOVE_DEBOUNCE(f, ms) \ { \ static auto last_call = std::chrono::system_clock::now(); \ const auto now = std::chrono::system_clock::now(); \ if (std::chrono::duration_cast(now - last_call).count() > ms) { \ last_call = now; \ f(); \ } \ } namespace { constexpr uint32_t StringHash(const std::string_view str) { uint32_t result = 0x811C9DC5; // FNV-1a 32-bit algorithm for (char c : str) { result = (static_cast(c) ^ result) * 0x01000193; } return result; } constexpr auto SUBSCRIBE = StringHash("subscribe"); constexpr auto UNSUBSCRIBE = StringHash("unsubscribe"); constexpr auto ADVERTISE = StringHash("advertise"); constexpr auto UNADVERTISE = StringHash("unadvertise"); constexpr auto GET_PARAMETERS = StringHash("getParameters"); constexpr auto SET_PARAMETERS = StringHash("setParameters"); constexpr auto SUBSCRIBE_PARAMETER_UPDATES = StringHash("subscribeParameterUpdates"); constexpr auto UNSUBSCRIBE_PARAMETER_UPDATES = StringHash("unsubscribeParameterUpdates"); constexpr auto SUBSCRIBE_CONNECTION_GRAPH = StringHash("subscribeConnectionGraph"); constexpr auto UNSUBSCRIBE_CONNECTION_GRAPH = StringHash("unsubscribeConnectionGraph"); constexpr auto FETCH_ASSET = StringHash("fetchAsset"); } // namespace namespace foxglove_ws { using json = nlohmann::json; using ConnHandle = websocketpp::connection_hdl; using OpCode = websocketpp::frame::opcode::value; static const websocketpp::log::level APP = websocketpp::log::alevel::app; static const websocketpp::log::level WARNING = websocketpp::log::elevel::warn; static const websocketpp::log::level RECOVERABLE = websocketpp::log::elevel::rerror; /// Map of required capability by client operation (text). const std::unordered_map CAPABILITY_BY_CLIENT_OPERATION = { // {"subscribe", }, // No required capability. // {"unsubscribe", }, // No required capability. {"advertise", CAPABILITY_CLIENT_PUBLISH}, {"unadvertise", CAPABILITY_CLIENT_PUBLISH}, {"getParameters", CAPABILITY_PARAMETERS}, {"setParameters", CAPABILITY_PARAMETERS}, {"subscribeParameterUpdates", CAPABILITY_PARAMETERS_SUBSCRIBE}, {"unsubscribeParameterUpdates", CAPABILITY_PARAMETERS_SUBSCRIBE}, {"subscribeConnectionGraph", CAPABILITY_CONNECTION_GRAPH}, {"unsubscribeConnectionGraph", CAPABILITY_CONNECTION_GRAPH}, {"fetchAsset", CAPABILITY_ASSETS}, }; /// Map of required capability by client operation (binary). const std::unordered_map CAPABILITY_BY_CLIENT_BINARY_OPERATION = { {ClientBinaryOpcode::MESSAGE_DATA, CAPABILITY_CLIENT_PUBLISH}, {ClientBinaryOpcode::SERVICE_CALL_REQUEST, CAPABILITY_SERVICES}, }; enum class StatusLevel : uint8_t { Info = 0, Warning = 1, Error = 2, }; constexpr websocketpp::log::level StatusLevelToLogLevel(StatusLevel level) { switch (level) { case StatusLevel::Info: return APP; case StatusLevel::Warning: return WARNING; case StatusLevel::Error: return RECOVERABLE; default: return RECOVERABLE; } } template class Server final : public ServerInterface { public: using ServerType = websocketpp::server; using ConnectionType = websocketpp::connection; using MessagePtr = typename ServerType::message_ptr; using Tcp = websocketpp::lib::asio::ip::tcp; explicit Server(std::string name, LogCallback logger, const ServerOptions& options); virtual ~Server(); Server(const Server&) = delete; Server(Server&&) = delete; Server& operator=(const Server&) = delete; Server& operator=(Server&&) = delete; void start(const std::string& host, uint16_t port) override; void stop() override; std::vector addChannels(const std::vector& channels) override; void removeChannels(const std::vector& channelIds) override; void publishParameterValues(ConnHandle clientHandle, const std::vector& parameters, const std::optional& requestId = std::nullopt) override; void updateParameterValues(const std::vector& parameters) override; std::vector addServices(const std::vector& services) override; void removeServices(const std::vector& serviceIds) override; void setHandlers(ServerHandlers&& handlers) override; void sendMessage(ConnHandle clientHandle, ChannelId chanId, uint64_t timestamp, const uint8_t* payload, size_t payloadSize) override; void broadcastTime(uint64_t timestamp) override; void sendServiceResponse(ConnHandle clientHandle, const ServiceResponse& response) override; void sendServiceFailure(ConnHandle clientHandle, ServiceId serviceId, uint32_t callId, const std::string& message) override; void updateConnectionGraph(const MapOfSets& publishedTopics, const MapOfSets& subscribedTopics, const MapOfSets& advertisedServices) override; void sendFetchAssetResponse(ConnHandle clientHandle, const FetchAssetResponse& response) override; uint16_t getPort() override; std::string remoteEndpointString(ConnHandle clientHandle) override; private: struct ClientInfo { std::string name; ConnHandle handle; std::unordered_map subscriptionsByChannel; std::unordered_set advertisedChannels; bool subscribedToConnectionGraph = false; explicit ClientInfo(const std::string& name, ConnHandle handle) : name(name) , handle(handle) {} ClientInfo(const ClientInfo&) = delete; ClientInfo& operator=(const ClientInfo&) = delete; ClientInfo(ClientInfo&&) = default; ClientInfo& operator=(ClientInfo&&) = default; }; std::string _name; LogCallback _logger; ServerOptions _options; ServerType _server; std::unique_ptr _serverThread; std::unique_ptr _handlerCallbackQueue; uint32_t _nextChannelId = 0; std::map> _clients; std::unordered_map _channels; std::map, std::owner_less<>> _clientChannels; std::map, std::owner_less<>> _clientParamSubscriptions; ServiceId _nextServiceId = 0; std::unordered_map _services; ServerHandlers _handlers; std::shared_mutex _clientsMutex; std::shared_mutex _channelsMutex; std::shared_mutex _clientChannelsMutex; std::shared_mutex _servicesMutex; std::mutex _clientParamSubscriptionsMutex; struct { int subscriptionCount = 0; MapOfSets publishedTopics; MapOfSets subscribedTopics; MapOfSets advertisedServices; } _connectionGraph; std::shared_mutex _connectionGraphMutex; void setupTlsHandler(); void socketInit(ConnHandle hdl); bool validateConnection(ConnHandle hdl); void handleConnectionOpened(ConnHandle hdl); void handleConnectionClosed(ConnHandle hdl); void handleMessage(ConnHandle hdl, MessagePtr msg); void handleTextMessage(ConnHandle hdl, MessagePtr msg); void handleBinaryMessage(ConnHandle hdl, MessagePtr msg); void sendJson(ConnHandle hdl, json&& payload); void sendJsonRaw(ConnHandle hdl, const std::string& payload); void sendBinary(ConnHandle hdl, const uint8_t* payload, size_t payloadSize); void sendStatusAndLogMsg(ConnHandle clientHandle, const StatusLevel level, const std::string& message); void unsubscribeParamsWithoutSubscriptions(ConnHandle hdl, const std::unordered_set& paramNames); bool isParameterSubscribed(const std::string& paramName) const; bool hasCapability(const std::string& capability) const; bool hasHandler(uint32_t op) const; void handleSubscribe(const nlohmann::json& payload, ConnHandle hdl); void handleUnsubscribe(const nlohmann::json& payload, ConnHandle hdl); void handleAdvertise(const nlohmann::json& payload, ConnHandle hdl); void handleUnadvertise(const nlohmann::json& payload, ConnHandle hdl); void handleGetParameters(const nlohmann::json& payload, ConnHandle hdl); void handleSetParameters(const nlohmann::json& payload, ConnHandle hdl); void handleSubscribeParameterUpdates(const nlohmann::json& payload, ConnHandle hdl); void handleUnsubscribeParameterUpdates(const nlohmann::json& payload, ConnHandle hdl); void handleSubscribeConnectionGraph(ConnHandle hdl); void handleUnsubscribeConnectionGraph(ConnHandle hdl); void handleFetchAsset(const nlohmann::json& payload, ConnHandle hdl); }; template inline Server::Server(std::string name, LogCallback logger, const ServerOptions& options) : _name(std::move(name)) , _logger(logger) , _options(options) { // Redirect logging _server.get_alog().set_callback(_logger); _server.get_elog().set_callback(_logger); websocketpp::lib::error_code ec; _server.init_asio(ec); if (ec) { throw std::runtime_error("Failed to initialize websocket server: " + ec.message()); } _server.clear_access_channels(websocketpp::log::alevel::all); _server.set_access_channels(APP); _server.set_tcp_pre_init_handler(std::bind(&Server::socketInit, this, std::placeholders::_1)); this->setupTlsHandler(); _server.set_validate_handler(std::bind(&Server::validateConnection, this, std::placeholders::_1)); _server.set_open_handler(std::bind(&Server::handleConnectionOpened, this, std::placeholders::_1)); _server.set_close_handler([this](ConnHandle hdl) { _handlerCallbackQueue->addCallback([this, hdl]() { this->handleConnectionClosed(hdl); }); }); _server.set_message_handler([this](ConnHandle hdl, MessagePtr msg) { _handlerCallbackQueue->addCallback([this, hdl, msg]() { this->handleMessage(hdl, msg); }); }); _server.set_reuse_addr(true); _server.set_listen_backlog(128); // Callback queue for handling client requests and disconnections. _handlerCallbackQueue = std::make_unique(_logger, /*numThreads=*/1ul); } template inline Server::~Server() {} template inline void Server::socketInit(ConnHandle hdl) { websocketpp::lib::asio::error_code ec; _server.get_con_from_hdl(hdl)->get_raw_socket().set_option(Tcp::no_delay(true), ec); if (ec) { _server.get_elog().write(RECOVERABLE, "Failed to set TCP_NODELAY: " + ec.message()); } } template inline bool Server::validateConnection(ConnHandle hdl) { auto con = _server.get_con_from_hdl(hdl); const auto& subprotocols = con->get_requested_subprotocols(); if (std::find(subprotocols.begin(), subprotocols.end(), SUPPORTED_SUBPROTOCOL) != subprotocols.end()) { con->select_subprotocol(SUPPORTED_SUBPROTOCOL); return true; } _server.get_alog().write(APP, "Rejecting client " + remoteEndpointString(hdl) + " which did not declare support for subprotocol " + SUPPORTED_SUBPROTOCOL); return false; } template inline void Server::handleConnectionOpened(ConnHandle hdl) { auto con = _server.get_con_from_hdl(hdl); const auto endpoint = remoteEndpointString(hdl); _server.get_alog().write(APP, "Client " + endpoint + " connected via " + con->get_resource()); { std::unique_lock lock(_clientsMutex); _clients.emplace(hdl, ClientInfo(endpoint, hdl)); } con->send(json({ {"op", "serverInfo"}, {"name", _name}, {"capabilities", _options.capabilities}, {"supportedEncodings", _options.supportedEncodings}, {"metadata", _options.metadata}, {"sessionId", _options.sessionId}, }) .dump()); std::vector channels; { std::shared_lock lock(_channelsMutex); for (const auto& [id, channel] : _channels) { (void)id; channels.push_back(channel); } } sendJson(hdl, { {"op", "advertise"}, {"channels", std::move(channels)}, }); std::vector services; { std::shared_lock lock(_servicesMutex); for (const auto& [id, service] : _services) { services.push_back(Service(service, id)); } } sendJson(hdl, { {"op", "advertiseServices"}, {"services", std::move(services)}, }); } template inline void Server::handleConnectionClosed(ConnHandle hdl) { std::unordered_map oldSubscriptionsByChannel; std::unordered_set oldAdvertisedChannels; std::string clientName; bool wasSubscribedToConnectionGraph; { std::unique_lock lock(_clientsMutex); const auto clientIt = _clients.find(hdl); if (clientIt == _clients.end()) { _server.get_elog().write(RECOVERABLE, "Client " + remoteEndpointString(hdl) + " disconnected but not found in _clients"); return; } const auto& client = clientIt->second; clientName = client.name; _server.get_alog().write(APP, "Client " + clientName + " disconnected"); oldSubscriptionsByChannel = std::move(client.subscriptionsByChannel); oldAdvertisedChannels = std::move(client.advertisedChannels); wasSubscribedToConnectionGraph = client.subscribedToConnectionGraph; _clients.erase(clientIt); } // Unadvertise all channels this client advertised for (const auto clientChannelId : oldAdvertisedChannels) { _server.get_alog().write(APP, "Client " + clientName + " unadvertising channel " + std::to_string(clientChannelId) + " due to disconnect"); if (_handlers.clientUnadvertiseHandler) { try { _handlers.clientUnadvertiseHandler(clientChannelId, hdl); } catch (const std::exception& ex) { _server.get_elog().write( RECOVERABLE, "Exception caught when closing connection: " + std::string(ex.what())); } catch (...) { _server.get_elog().write(RECOVERABLE, "Exception caught when closing connection"); } } } { std::unique_lock lock(_clientChannelsMutex); _clientChannels.erase(hdl); } // Unsubscribe all channels this client subscribed to if (_handlers.unsubscribeHandler) { for (const auto& [chanId, subs] : oldSubscriptionsByChannel) { (void)subs; try { _handlers.unsubscribeHandler(chanId, hdl); } catch (const std::exception& ex) { _server.get_elog().write( RECOVERABLE, "Exception caught when closing connection: " + std::string(ex.what())); } catch (...) { _server.get_elog().write(RECOVERABLE, "Exception caught when closing connection"); } } } // Unsubscribe from parameters this client subscribed to std::unordered_set clientSubscribedParameters; { std::lock_guard lock(_clientParamSubscriptionsMutex); clientSubscribedParameters = _clientParamSubscriptions[hdl]; _clientParamSubscriptions.erase(hdl); } unsubscribeParamsWithoutSubscriptions(hdl, clientSubscribedParameters); if (wasSubscribedToConnectionGraph) { std::unique_lock lock(_connectionGraphMutex); _connectionGraph.subscriptionCount--; if (_connectionGraph.subscriptionCount == 0 && _handlers.subscribeConnectionGraphHandler) { _server.get_alog().write(APP, "Unsubscribing from connection graph updates."); try { _handlers.subscribeConnectionGraphHandler(false); } catch (const std::exception& ex) { _server.get_elog().write( RECOVERABLE, "Exception caught when closing connection: " + std::string(ex.what())); } catch (...) { _server.get_elog().write(RECOVERABLE, "Exception caught when closing connection"); } } } } // namespace foxglove_ws template inline void Server::setHandlers(ServerHandlers&& handlers) { _handlers = handlers; } template inline void Server::stop() { if (_server.stopped()) { return; } _server.get_alog().write(APP, "Stopping WebSocket server"); websocketpp::lib::error_code ec; _server.stop_perpetual(); if (_server.is_listening()) { _server.stop_listening(ec); if (ec) { _server.get_elog().write(RECOVERABLE, "Failed to stop listening: " + ec.message()); } } std::vector> connections; { std::shared_lock lock(_clientsMutex); connections.reserve(_clients.size()); for (const auto& [hdl, client] : _clients) { (void)client; if (auto connection = _server.get_con_from_hdl(hdl, ec)) { connections.push_back(connection); } } } if (!connections.empty()) { _server.get_alog().write( APP, "Closing " + std::to_string(connections.size()) + " client connection(s)"); // Iterate over all client connections and start the close connection handshake for (const auto& connection : connections) { connection->close(websocketpp::close::status::going_away, "server shutdown", ec); if (ec) { _server.get_elog().write(RECOVERABLE, "Failed to close connection: " + ec.message()); } } // Wait for all connections to close constexpr size_t MAX_SHUTDOWN_MS = 1000; constexpr size_t SLEEP_MS = 10; size_t durationMs = 0; while (!_server.stopped() && durationMs < MAX_SHUTDOWN_MS) { std::this_thread::sleep_for(std::chrono::milliseconds(SLEEP_MS)); _server.poll_one(); durationMs += SLEEP_MS; } if (!_server.stopped()) { _server.get_elog().write(RECOVERABLE, "Failed to close all connections, forcefully stopping"); for (const auto& hdl : connections) { if (auto con = _server.get_con_from_hdl(hdl, ec)) { _server.get_elog().write(RECOVERABLE, "Terminating connection to " + remoteEndpointString(hdl)); con->terminate(ec); } } _server.stop(); } } _server.get_alog().write(APP, "All WebSocket connections closed"); if (_serverThread) { _server.get_alog().write(APP, "Waiting for WebSocket server run loop to terminate"); _serverThread->join(); _serverThread.reset(); _server.get_alog().write(APP, "WebSocket server run loop terminated"); } std::unique_lock lock(_clientsMutex); _clients.clear(); } template inline void Server::start(const std::string& host, uint16_t port) { if (_serverThread) { throw std::runtime_error("Server already started"); } websocketpp::lib::error_code ec; _server.listen(host, std::to_string(port), ec); if (ec) { throw std::runtime_error("Failed to listen on port " + std::to_string(port) + ": " + ec.message()); } _server.start_accept(ec); if (ec) { throw std::runtime_error("Failed to start accepting connections: " + ec.message()); } _serverThread = std::make_unique([this]() { _server.get_alog().write(APP, "WebSocket server run loop started"); _server.run(); _server.get_alog().write(APP, "WebSocket server run loop stopped"); }); if (!_server.is_listening()) { throw std::runtime_error("WebSocket server failed to listen on port " + std::to_string(port)); } websocketpp::lib::asio::error_code asioEc; auto endpoint = _server.get_local_endpoint(asioEc); if (asioEc) { throw std::runtime_error("Failed to resolve the local endpoint: " + asioEc.message()); } const std::string protocol = _options.useTls ? "wss" : "ws"; auto address = endpoint.address(); _server.get_alog().write(APP, "WebSocket server listening at " + protocol + "://" + IPAddressToString(address) + ":" + std::to_string(endpoint.port())); } template inline void Server::sendJson(ConnHandle hdl, json&& payload) { try { _server.send(hdl, std::move(payload).dump(), OpCode::TEXT); } catch (std::exception const& e) { _server.get_elog().write(RECOVERABLE, e.what()); } } template inline void Server::sendJsonRaw(ConnHandle hdl, const std::string& payload) { try { _server.send(hdl, payload, OpCode::TEXT); } catch (std::exception const& e) { _server.get_elog().write(RECOVERABLE, e.what()); } } template inline void Server::sendBinary(ConnHandle hdl, const uint8_t* payload, size_t payloadSize) { try { _server.send(hdl, payload, payloadSize, OpCode::BINARY); } catch (std::exception const& e) { _server.get_elog().write(RECOVERABLE, e.what()); } } template inline void Server::sendStatusAndLogMsg(ConnHandle clientHandle, const StatusLevel level, const std::string& message) { const std::string endpoint = remoteEndpointString(clientHandle); const std::string logMessage = endpoint + ": " + message; const auto logLevel = StatusLevelToLogLevel(level); auto logger = level == StatusLevel::Info ? _server.get_alog() : _server.get_elog(); logger.write(logLevel, logMessage); sendJson(clientHandle, json{ {"op", "status"}, {"level", static_cast(level)}, {"message", message}, }); } template inline void Server::handleMessage(ConnHandle hdl, MessagePtr msg) { const OpCode op = msg->get_opcode(); try { if (op == OpCode::TEXT) { handleTextMessage(hdl, msg); } else if (op == OpCode::BINARY) { handleBinaryMessage(hdl, msg); } } catch (const std::exception& e) { sendStatusAndLogMsg(hdl, StatusLevel::Error, e.what()); } catch (...) { sendStatusAndLogMsg(hdl, StatusLevel::Error, "Exception occurred when executing message handler"); } } template inline void Server::handleTextMessage(ConnHandle hdl, MessagePtr msg) { const json payload = json::parse(msg->get_payload()); const std::string& op = payload.at("op").get(); const auto requiredCapabilityIt = CAPABILITY_BY_CLIENT_OPERATION.find(op); if (requiredCapabilityIt != CAPABILITY_BY_CLIENT_OPERATION.end() && !hasCapability(requiredCapabilityIt->second)) { sendStatusAndLogMsg(hdl, StatusLevel::Error, "Operation '" + op + "' not supported as server capability '" + requiredCapabilityIt->second + "' is missing"); return; } if (!hasHandler(StringHash(op))) { sendStatusAndLogMsg( hdl, StatusLevel::Error, "Operation '" + op + "' not supported as server handler function is missing"); return; } try { switch (StringHash(op)) { case SUBSCRIBE: handleSubscribe(payload, hdl); break; case UNSUBSCRIBE: handleUnsubscribe(payload, hdl); break; case ADVERTISE: handleAdvertise(payload, hdl); break; case UNADVERTISE: handleUnadvertise(payload, hdl); break; case GET_PARAMETERS: handleGetParameters(payload, hdl); break; case SET_PARAMETERS: handleSetParameters(payload, hdl); break; case SUBSCRIBE_PARAMETER_UPDATES: handleSubscribeParameterUpdates(payload, hdl); break; case UNSUBSCRIBE_PARAMETER_UPDATES: handleUnsubscribeParameterUpdates(payload, hdl); break; case SUBSCRIBE_CONNECTION_GRAPH: handleSubscribeConnectionGraph(hdl); break; case UNSUBSCRIBE_CONNECTION_GRAPH: handleUnsubscribeConnectionGraph(hdl); break; case FETCH_ASSET: handleFetchAsset(payload, hdl); break; default: sendStatusAndLogMsg(hdl, StatusLevel::Error, "Unrecognized client opcode \"" + op + "\""); break; } } catch (const ExeptionWithId& e) { const std::string postfix = " (op: " + op + ", id: " + std::to_string(e.id()) + ")"; sendStatusAndLogMsg(hdl, StatusLevel::Error, e.what() + postfix); } catch (const std::exception& e) { const std::string postfix = " (op: " + op + ")"; sendStatusAndLogMsg(hdl, StatusLevel::Error, e.what() + postfix); } catch (...) { const std::string postfix = " (op: " + op + ")"; sendStatusAndLogMsg(hdl, StatusLevel::Error, "Failed to execute handler" + postfix); } } // namespace foxglove_ws template inline void Server::handleBinaryMessage(ConnHandle hdl, MessagePtr msg) { const auto& payload = msg->get_payload(); const uint8_t* data = reinterpret_cast(payload.data()); const size_t length = payload.size(); if (length < 1) { sendStatusAndLogMsg(hdl, StatusLevel::Error, "Received an empty binary message"); return; } const auto op = static_cast(data[0]); const auto requiredCapabilityIt = CAPABILITY_BY_CLIENT_BINARY_OPERATION.find(op); if (requiredCapabilityIt != CAPABILITY_BY_CLIENT_BINARY_OPERATION.end() && !hasCapability(requiredCapabilityIt->second)) { sendStatusAndLogMsg(hdl, StatusLevel::Error, "Binary operation '" + std::to_string(static_cast(op)) + "' not supported as server capability '" + requiredCapabilityIt->second + "' is missing"); return; } switch (op) { case ClientBinaryOpcode::MESSAGE_DATA: { if (!_handlers.clientMessageHandler) { return; } if (length < 5) { sendStatusAndLogMsg(hdl, StatusLevel::Error, "Invalid message length " + std::to_string(length)); return; } const auto timestamp = std::chrono::duration_cast( std::chrono::high_resolution_clock::now().time_since_epoch()) .count(); const ClientChannelId channelId = *reinterpret_cast(data + 1); std::shared_lock lock(_clientChannelsMutex); auto clientPublicationsIt = _clientChannels.find(hdl); if (clientPublicationsIt == _clientChannels.end()) { sendStatusAndLogMsg(hdl, StatusLevel::Error, "Client has no advertised channels"); return; } auto& clientPublications = clientPublicationsIt->second; const auto& channelIt = clientPublications.find(channelId); if (channelIt == clientPublications.end()) { sendStatusAndLogMsg(hdl, StatusLevel::Error, "Channel " + std::to_string(channelId) + " is not advertised"); return; } try { const auto& advertisement = channelIt->second; const uint32_t sequence = 0; const ClientMessage clientMessage{static_cast(timestamp), static_cast(timestamp), sequence, advertisement, length, data}; _handlers.clientMessageHandler(clientMessage, hdl); } catch (const ClientChannelError& e) { sendStatusAndLogMsg(hdl, StatusLevel::Error, e.what()); } catch (...) { sendStatusAndLogMsg(hdl, StatusLevel::Error, "clientMessage: Failed to execute handler"); } } break; case ClientBinaryOpcode::SERVICE_CALL_REQUEST: { ServiceRequest request; if (length < request.size()) { const std::string errMessage = "Invalid service call request length " + std::to_string(length); sendServiceFailure(hdl, request.serviceId, request.callId, errMessage); _server.get_elog().write(RECOVERABLE, errMessage); return; } request.read(data + 1, length - 1); { std::shared_lock lock(_servicesMutex); if (_services.find(request.serviceId) == _services.end()) { const std::string errMessage = "Service " + std::to_string(request.serviceId) + " is not advertised"; sendServiceFailure(hdl, request.serviceId, request.callId, errMessage); _server.get_elog().write(RECOVERABLE, errMessage); return; } } try { if (!_handlers.serviceRequestHandler) { throw foxglove_ws::ServiceError(request.serviceId, "No service handler"); } _handlers.serviceRequestHandler(request, hdl); } catch (const std::exception& e) { sendServiceFailure(hdl, request.serviceId, request.callId, e.what()); _server.get_elog().write(RECOVERABLE, e.what()); } } break; default: { sendStatusAndLogMsg(hdl, StatusLevel::Error, "Unrecognized client opcode " + std::to_string(uint8_t(op))); } break; } } template inline std::vector Server::addChannels( const std::vector& channels) { if (channels.empty()) { return {}; } std::vector channelIds; channelIds.reserve(channels.size()); json::array_t channelsJson; { std::unique_lock lock(_channelsMutex); for (const auto& channelWithoutId : channels) { const auto newId = ++_nextChannelId; channelIds.push_back(newId); Channel newChannel{newId, channelWithoutId}; channelsJson.push_back(newChannel); _channels.emplace(newId, std::move(newChannel)); } } const auto msg = json{{"op", "advertise"}, {"channels", channelsJson}}.dump(); std::shared_lock clientsLock(_clientsMutex); for (const auto& [hdl, clientInfo] : _clients) { (void)clientInfo; sendJsonRaw(hdl, msg); } return channelIds; } template inline void Server::removeChannels(const std::vector& channelIds) { if (channelIds.empty()) { return; } { std::unique_lock channelsLock(_channelsMutex); for (auto channelId : channelIds) { _channels.erase(channelId); } } const auto msg = json{{"op", "unadvertise"}, {"channelIds", channelIds}}.dump(); std::unique_lock clientsLock(_clientsMutex); for (auto& [hdl, clientInfo] : _clients) { for (auto channelId : channelIds) { if (const auto it = clientInfo.subscriptionsByChannel.find(channelId); it != clientInfo.subscriptionsByChannel.end()) { clientInfo.subscriptionsByChannel.erase(it); } } sendJsonRaw(hdl, msg); } } template inline void Server::publishParameterValues( ConnHandle hdl, const std::vector& parameters, const std::optional& requestId) { // Filter out parameters which are not set. std::vector nonEmptyParameters; std::copy_if(parameters.begin(), parameters.end(), std::back_inserter(nonEmptyParameters), [](const auto& p) { return p.getType() != ParameterType::PARAMETER_NOT_SET; }); nlohmann::json jsonPayload{{"op", "parameterValues"}, {"parameters", nonEmptyParameters}}; if (requestId) { jsonPayload["id"] = requestId.value(); } sendJsonRaw(hdl, jsonPayload.dump()); } template inline void Server::updateParameterValues( const std::vector& parameters) { std::lock_guard lock(_clientParamSubscriptionsMutex); for (const auto& clientParamSubscriptions : _clientParamSubscriptions) { std::vector paramsToSendToClient; // Only consider parameters that are subscribed by the client std::copy_if(parameters.begin(), parameters.end(), std::back_inserter(paramsToSendToClient), [clientParamSubscriptions](const Parameter& param) { return clientParamSubscriptions.second.find(param.getName()) != clientParamSubscriptions.second.end(); }); if (!paramsToSendToClient.empty()) { publishParameterValues(clientParamSubscriptions.first, paramsToSendToClient); } } } template inline std::vector Server::addServices( const std::vector& services) { if (services.empty()) { return {}; } std::unique_lock lock(_servicesMutex); std::vector serviceIds; json newServices; for (const auto& service : services) { const ServiceId serviceId = ++_nextServiceId; _services.emplace(serviceId, service); serviceIds.push_back(serviceId); newServices.push_back(Service(service, serviceId)); } const auto msg = json{{"op", "advertiseServices"}, {"services", std::move(newServices)}}.dump(); std::shared_lock clientsLock(_clientsMutex); for (const auto& [hdl, clientInfo] : _clients) { (void)clientInfo; sendJsonRaw(hdl, msg); } return serviceIds; } template inline void Server::removeServices(const std::vector& serviceIds) { std::unique_lock lock(_servicesMutex); std::vector removedServices; for (const auto& serviceId : serviceIds) { if (const auto it = _services.find(serviceId); it != _services.end()) { _services.erase(it); removedServices.push_back(serviceId); } } if (!removedServices.empty()) { const auto msg = json{{"op", "unadvertiseServices"}, {"serviceIds", std::move(removedServices)}}.dump(); std::shared_lock clientsLock(_clientsMutex); for (const auto& [hdl, clientInfo] : _clients) { (void)clientInfo; sendJsonRaw(hdl, msg); } } } template inline void Server::sendMessage(ConnHandle clientHandle, ChannelId chanId, uint64_t timestamp, const uint8_t* payload, size_t payloadSize) { websocketpp::lib::error_code ec; const auto con = _server.get_con_from_hdl(clientHandle, ec); if (ec || !con) { return; } const auto bufferSizeinBytes = con->get_buffered_amount(); if (bufferSizeinBytes + payloadSize >= _options.sendBufferLimitBytes) { const auto logFn = [this, clientHandle]() { sendStatusAndLogMsg(clientHandle, StatusLevel::Warning, "Send buffer limit reached"); }; FOXGLOVE_DEBOUNCE(logFn, 2500); return; } SubscriptionId subId = std::numeric_limits::max(); { std::shared_lock lock(_clientsMutex); const auto clientHandleAndInfoIt = _clients.find(clientHandle); if (clientHandleAndInfoIt == _clients.end()) { return; // Client got removed in the meantime. } const auto& client = clientHandleAndInfoIt->second; const auto& subs = client.subscriptionsByChannel.find(chanId); if (subs == client.subscriptionsByChannel.end()) { return; // Client not subscribed to this channel. } subId = subs->second; } std::array msgHeader; msgHeader[0] = uint8_t(BinaryOpcode::MESSAGE_DATA); foxglove_ws::WriteUint32LE(msgHeader.data() + 1, subId); foxglove_ws::WriteUint64LE(msgHeader.data() + 5, timestamp); const size_t messageSize = msgHeader.size() + payloadSize; auto message = con->get_message(OpCode::BINARY, messageSize); message->set_compressed(_options.useCompression); message->set_payload(msgHeader.data(), msgHeader.size()); message->append_payload(payload, payloadSize); con->send(message); } template inline void Server::broadcastTime(uint64_t timestamp) { std::array message; message[0] = uint8_t(BinaryOpcode::TIME_DATA); foxglove_ws::WriteUint64LE(message.data() + 1, timestamp); std::shared_lock lock(_clientsMutex); for (const auto& [hdl, clientInfo] : _clients) { (void)clientInfo; sendBinary(hdl, message.data(), message.size()); } } template inline void Server::sendServiceResponse(ConnHandle clientHandle, const ServiceResponse& response) { std::vector payload(1 + response.size()); payload[0] = uint8_t(BinaryOpcode::SERVICE_CALL_RESPONSE); response.write(payload.data() + 1); sendBinary(clientHandle, payload.data(), payload.size()); } template inline uint16_t Server::getPort() { websocketpp::lib::asio::error_code ec; auto endpoint = _server.get_local_endpoint(ec); if (ec) { throw std::runtime_error("Server not listening on any port. Has it been started before?"); } return endpoint.port(); } template inline void Server::sendServiceFailure(ConnHandle clientHandle, ServiceId serviceId, uint32_t callId, const std::string& message) { sendJson(clientHandle, json{{"op", "serviceCallFailure"}, {"serviceId", serviceId}, {"callId", callId}, {"message", message}}); } template inline void Server::updateConnectionGraph( const MapOfSets& publishedTopics, const MapOfSets& subscribedTopics, const MapOfSets& advertisedServices) { json::array_t publisherDiff, subscriberDiff, servicesDiff; std::unordered_set topicNames, serviceNames; std::unordered_set knownTopicNames, knownServiceNames; { std::unique_lock lock(_connectionGraphMutex); for (const auto& [name, publisherIds] : publishedTopics) { const auto it = _connectionGraph.publishedTopics.find(name); if (it == _connectionGraph.publishedTopics.end() || _connectionGraph.publishedTopics[name] != publisherIds) { publisherDiff.push_back(nlohmann::json{{"name", name}, {"publisherIds", publisherIds}}); } topicNames.insert(name); } for (const auto& [name, subscriberIds] : subscribedTopics) { const auto it = _connectionGraph.subscribedTopics.find(name); if (it == _connectionGraph.subscribedTopics.end() || _connectionGraph.subscribedTopics[name] != subscriberIds) { subscriberDiff.push_back(nlohmann::json{{"name", name}, {"subscriberIds", subscriberIds}}); } topicNames.insert(name); } for (const auto& [name, providerIds] : advertisedServices) { const auto it = _connectionGraph.advertisedServices.find(name); if (it == _connectionGraph.advertisedServices.end() || _connectionGraph.advertisedServices[name] != providerIds) { servicesDiff.push_back(nlohmann::json{{"name", name}, {"providerIds", providerIds}}); } serviceNames.insert(name); } for (const auto& nameWithIds : _connectionGraph.publishedTopics) { knownTopicNames.insert(nameWithIds.first); } for (const auto& nameWithIds : _connectionGraph.subscribedTopics) { knownTopicNames.insert(nameWithIds.first); } for (const auto& nameWithIds : _connectionGraph.advertisedServices) { knownServiceNames.insert(nameWithIds.first); } _connectionGraph.publishedTopics = publishedTopics; _connectionGraph.subscribedTopics = subscribedTopics; _connectionGraph.advertisedServices = advertisedServices; } std::vector removedTopics, removedServices; std::copy_if(knownTopicNames.begin(), knownTopicNames.end(), std::back_inserter(removedTopics), [&topicNames](const std::string& topic) { return topicNames.find(topic) == topicNames.end(); }); std::copy_if(knownServiceNames.begin(), knownServiceNames.end(), std::back_inserter(removedServices), [&serviceNames](const std::string& service) { return serviceNames.find(service) == serviceNames.end(); }); if (publisherDiff.empty() && subscriberDiff.empty() && servicesDiff.empty() && removedTopics.empty() && removedServices.empty()) { return; } const json msg = { {"op", "connectionGraphUpdate"}, {"publishedTopics", publisherDiff}, {"subscribedTopics", subscriberDiff}, {"advertisedServices", servicesDiff}, {"removedTopics", removedTopics}, {"removedServices", removedServices}, }; const auto payload = msg.dump(); std::shared_lock clientsLock(_clientsMutex); for (const auto& [hdl, clientInfo] : _clients) { if (clientInfo.subscribedToConnectionGraph) { _server.send(hdl, payload, OpCode::TEXT); } } } template inline std::string Server::remoteEndpointString(ConnHandle clientHandle) { websocketpp::lib::error_code ec; const auto con = _server.get_con_from_hdl(clientHandle, ec); return con ? con->get_remote_endpoint() : "(unknown)"; } template inline bool Server::isParameterSubscribed(const std::string& paramName) const { return std::find_if(_clientParamSubscriptions.begin(), _clientParamSubscriptions.end(), [paramName](const auto& paramSubscriptions) { return paramSubscriptions.second.find(paramName) != paramSubscriptions.second.end(); }) != _clientParamSubscriptions.end(); } template inline void Server::unsubscribeParamsWithoutSubscriptions( ConnHandle hdl, const std::unordered_set& paramNames) { std::vector paramsToUnsubscribe; { std::lock_guard lock(_clientParamSubscriptionsMutex); std::copy_if(paramNames.begin(), paramNames.end(), std::back_inserter(paramsToUnsubscribe), [this](const std::string& paramName) { return !isParameterSubscribed(paramName); }); } if (_handlers.parameterSubscriptionHandler && !paramsToUnsubscribe.empty()) { for (const auto& param : paramsToUnsubscribe) { _server.get_alog().write(APP, "Unsubscribing from parameter '" + param + "'."); } try { _handlers.parameterSubscriptionHandler(paramsToUnsubscribe, ParameterSubscriptionOperation::UNSUBSCRIBE, hdl); } catch (const std::exception& e) { sendStatusAndLogMsg(hdl, StatusLevel::Error, e.what()); } catch (...) { sendStatusAndLogMsg(hdl, StatusLevel::Error, "Failed to unsubscribe from one more more parameters"); } } } template inline bool Server::hasCapability(const std::string& capability) const { return std::find(_options.capabilities.begin(), _options.capabilities.end(), capability) != _options.capabilities.end(); } template inline bool Server::hasHandler(uint32_t op) const { switch (op) { case SUBSCRIBE: return bool(_handlers.subscribeHandler); case UNSUBSCRIBE: return bool(_handlers.unsubscribeHandler); case ADVERTISE: return bool(_handlers.clientAdvertiseHandler); case UNADVERTISE: return bool(_handlers.clientUnadvertiseHandler); case GET_PARAMETERS: return bool(_handlers.parameterRequestHandler); case SET_PARAMETERS: return bool(_handlers.parameterChangeHandler); case SUBSCRIBE_PARAMETER_UPDATES: case UNSUBSCRIBE_PARAMETER_UPDATES: return bool(_handlers.parameterSubscriptionHandler); case SUBSCRIBE_CONNECTION_GRAPH: case UNSUBSCRIBE_CONNECTION_GRAPH: return bool(_handlers.subscribeConnectionGraphHandler); case FETCH_ASSET: return bool(_handlers.fetchAssetHandler); default: throw std::runtime_error("Unknown operation: " + std::to_string(op)); } } template void Server::handleSubscribe(const nlohmann::json& payload, ConnHandle hdl) { std::unordered_map clientSubscriptionsByChannel; { std::shared_lock clientsLock(_clientsMutex); clientSubscriptionsByChannel = _clients.at(hdl).subscriptionsByChannel; } const auto findSubscriptionBySubId = [](const std::unordered_map& subscriptionsByChannel, SubscriptionId subId) { return std::find_if(subscriptionsByChannel.begin(), subscriptionsByChannel.end(), [&subId](const auto& mo) { return mo.second == subId; }); }; for (const auto& sub : payload.at("subscriptions")) { SubscriptionId subId = sub.at("id"); ChannelId channelId = sub.at("channelId"); if (findSubscriptionBySubId(clientSubscriptionsByChannel, subId) != clientSubscriptionsByChannel.end()) { sendStatusAndLogMsg(hdl, StatusLevel::Error, "Client subscription id " + std::to_string(subId) + " was already used; ignoring subscription"); continue; } const auto& channelIt = _channels.find(channelId); if (channelIt == _channels.end()) { sendStatusAndLogMsg( hdl, StatusLevel::Warning, "Channel " + std::to_string(channelId) + " is not available; ignoring subscription"); continue; } { std::unique_lock clientsLock(_clientsMutex); _clients.at(hdl).subscriptionsByChannel.emplace(channelId, subId); } // In case the subscribeHandler triggers an immediate sendMessage, this must be done *after* // adding to subscriptionsByChannel, to prevent the message from being dropped _handlers.subscribeHandler(channelId, hdl); } } template void Server::handleUnsubscribe(const nlohmann::json& payload, ConnHandle hdl) { std::unordered_map clientSubscriptionsByChannel; { std::shared_lock clientsLock(_clientsMutex); clientSubscriptionsByChannel = _clients.at(hdl).subscriptionsByChannel; } const auto findSubscriptionBySubId = [](const std::unordered_map& subscriptionsByChannel, SubscriptionId subId) { return std::find_if(subscriptionsByChannel.begin(), subscriptionsByChannel.end(), [&subId](const auto& mo) { return mo.second == subId; }); }; for (const auto& subIdJson : payload.at("subscriptionIds")) { SubscriptionId subId = subIdJson; const auto& sub = findSubscriptionBySubId(clientSubscriptionsByChannel, subId); if (sub == clientSubscriptionsByChannel.end()) { sendStatusAndLogMsg(hdl, StatusLevel::Warning, "Client subscription id " + std::to_string(subId) + " did not exist; ignoring unsubscription"); continue; } ChannelId chanId = sub->first; _handlers.unsubscribeHandler(chanId, hdl); std::unique_lock clientsLock(_clientsMutex); _clients.at(hdl).subscriptionsByChannel.erase(chanId); } } template void Server::handleAdvertise(const nlohmann::json& payload, ConnHandle hdl) { std::unique_lock clientChannelsLock(_clientChannelsMutex); auto [clientPublicationsIt, isFirstPublication] = _clientChannels.emplace(hdl, std::unordered_map()); auto& clientPublications = clientPublicationsIt->second; for (const auto& chan : payload.at("channels")) { ClientChannelId channelId = chan.at("id"); if (!isFirstPublication && clientPublications.find(channelId) != clientPublications.end()) { sendStatusAndLogMsg(hdl, StatusLevel::Error, "Channel " + std::to_string(channelId) + " was already advertised"); continue; } const auto topic = chan.at("topic").get(); if (!isWhitelisted(topic, _options.clientTopicWhitelistPatterns)) { sendStatusAndLogMsg(hdl, StatusLevel::Error, "Can't advertise channel " + std::to_string(channelId) + ", topic '" + topic + "' not whitelisted"); continue; } ClientAdvertisement advertisement{}; advertisement.channelId = channelId; advertisement.topic = topic; advertisement.encoding = chan.at("encoding").get(); advertisement.schemaName = chan.at("schemaName").get(); _handlers.clientAdvertiseHandler(advertisement, hdl); std::unique_lock clientsLock(_clientsMutex); _clients.at(hdl).advertisedChannels.emplace(channelId); clientPublications.emplace(channelId, advertisement); } } template void Server::handleUnadvertise(const nlohmann::json& payload, ConnHandle hdl) { std::unique_lock clientChannelsLock(_clientChannelsMutex); auto clientPublicationsIt = _clientChannels.find(hdl); if (clientPublicationsIt == _clientChannels.end()) { sendStatusAndLogMsg(hdl, StatusLevel::Error, "Client has no advertised channels"); return; } auto& clientPublications = clientPublicationsIt->second; for (const auto& chanIdJson : payload.at("channelIds")) { ClientChannelId channelId = chanIdJson.get(); const auto& channelIt = clientPublications.find(channelId); if (channelIt == clientPublications.end()) { continue; } _handlers.clientUnadvertiseHandler(channelId, hdl); std::unique_lock clientsLock(_clientsMutex); auto& clientInfo = _clients.at(hdl); clientPublications.erase(channelIt); const auto advertisedChannelIt = clientInfo.advertisedChannels.find(channelId); if (advertisedChannelIt != clientInfo.advertisedChannels.end()) { clientInfo.advertisedChannels.erase(advertisedChannelIt); } } } template void Server::handleGetParameters(const nlohmann::json& payload, ConnHandle hdl) { const auto paramNames = payload.at("parameterNames").get>(); const auto requestId = payload.find("id") == payload.end() ? std::nullopt : std::optional(payload["id"].get()); _handlers.parameterRequestHandler(paramNames, requestId, hdl); } template void Server::handleSetParameters(const nlohmann::json& payload, ConnHandle hdl) { const auto parameters = payload.at("parameters").get>(); const auto requestId = payload.find("id") == payload.end() ? std::nullopt : std::optional(payload["id"].get()); _handlers.parameterChangeHandler(parameters, requestId, hdl); } template void Server::handleSubscribeParameterUpdates(const nlohmann::json& payload, ConnHandle hdl) { const auto paramNames = payload.at("parameterNames").get>(); std::vector paramsToSubscribe; { // Only consider parameters that are not subscribed yet (by this or by other clients) std::lock_guard lock(_clientParamSubscriptionsMutex); std::copy_if(paramNames.begin(), paramNames.end(), std::back_inserter(paramsToSubscribe), [this](const std::string& paramName) { return !isParameterSubscribed(paramName); }); // Update the client's parameter subscriptions. auto& clientSubscribedParams = _clientParamSubscriptions[hdl]; clientSubscribedParams.insert(paramNames.begin(), paramNames.end()); } if (!paramsToSubscribe.empty()) { _handlers.parameterSubscriptionHandler(paramsToSubscribe, ParameterSubscriptionOperation::SUBSCRIBE, hdl); } } template void Server::handleUnsubscribeParameterUpdates(const nlohmann::json& payload, ConnHandle hdl) { const auto paramNames = payload.at("parameterNames").get>(); { std::lock_guard lock(_clientParamSubscriptionsMutex); auto& clientSubscribedParams = _clientParamSubscriptions[hdl]; for (const auto& paramName : paramNames) { clientSubscribedParams.erase(paramName); } } unsubscribeParamsWithoutSubscriptions(hdl, paramNames); } template void Server::handleSubscribeConnectionGraph(ConnHandle hdl) { bool subscribeToConnnectionGraph = false; { std::unique_lock lock(_connectionGraphMutex); _connectionGraph.subscriptionCount++; subscribeToConnnectionGraph = _connectionGraph.subscriptionCount == 1; } if (subscribeToConnnectionGraph) { // First subscriber, let the handler know that we are interested in updates. _server.get_alog().write(APP, "Subscribing to connection graph updates."); _handlers.subscribeConnectionGraphHandler(true); std::unique_lock clientsLock(_clientsMutex); _clients.at(hdl).subscribedToConnectionGraph = true; } json::array_t publishedTopicsJson, subscribedTopicsJson, advertisedServicesJson; { std::shared_lock lock(_connectionGraphMutex); for (const auto& [name, ids] : _connectionGraph.publishedTopics) { publishedTopicsJson.push_back(nlohmann::json{{"name", name}, {"publisherIds", ids}}); } for (const auto& [name, ids] : _connectionGraph.subscribedTopics) { subscribedTopicsJson.push_back(nlohmann::json{{"name", name}, {"subscriberIds", ids}}); } for (const auto& [name, ids] : _connectionGraph.advertisedServices) { advertisedServicesJson.push_back(nlohmann::json{{"name", name}, {"providerIds", ids}}); } } const json jsonMsg = { {"op", "connectionGraphUpdate"}, {"publishedTopics", publishedTopicsJson}, {"subscribedTopics", subscribedTopicsJson}, {"advertisedServices", advertisedServicesJson}, {"removedTopics", json::array()}, {"removedServices", json::array()}, }; sendJsonRaw(hdl, jsonMsg.dump()); } template void Server::handleUnsubscribeConnectionGraph(ConnHandle hdl) { bool clientWasSubscribed = false; { std::unique_lock clientsLock(_clientsMutex); auto& clientInfo = _clients.at(hdl); if (clientInfo.subscribedToConnectionGraph) { clientWasSubscribed = true; clientInfo.subscribedToConnectionGraph = false; } } if (clientWasSubscribed) { bool unsubscribeFromConnnectionGraph = false; { std::unique_lock lock(_connectionGraphMutex); _connectionGraph.subscriptionCount--; unsubscribeFromConnnectionGraph = _connectionGraph.subscriptionCount == 0; } if (unsubscribeFromConnnectionGraph) { _server.get_alog().write(APP, "Unsubscribing from connection graph updates."); _handlers.subscribeConnectionGraphHandler(false); } } else { sendStatusAndLogMsg(hdl, StatusLevel::Error, "Client was not subscribed to connection graph updates"); } } template void Server::handleFetchAsset(const nlohmann::json& payload, ConnHandle hdl) { const auto uri = payload.at("uri").get(); const auto requestId = payload.at("requestId").get(); _handlers.fetchAssetHandler(uri, requestId, hdl); } template inline void Server::sendFetchAssetResponse( ConnHandle clientHandle, const FetchAssetResponse& response) { websocketpp::lib::error_code ec; const auto con = _server.get_con_from_hdl(clientHandle, ec); if (ec || !con) { return; } const size_t errMsgSize = response.status == FetchAssetStatus::Error ? response.errorMessage.size() : 0ul; const size_t dataSize = response.status == FetchAssetStatus::Success ? response.data.size() : 0ul; const size_t messageSize = 1 + 4 + 1 + 4 + errMsgSize + dataSize; auto message = con->get_message(OpCode::BINARY, messageSize); const auto op = BinaryOpcode::FETCH_ASSET_RESPONSE; message->append_payload(&op, 1); std::array uint32Data; foxglove_ws::WriteUint32LE(uint32Data.data(), response.requestId); message->append_payload(uint32Data.data(), uint32Data.size()); const uint8_t status = static_cast(response.status); message->append_payload(&status, 1); foxglove_ws::WriteUint32LE(uint32Data.data(), response.errorMessage.size()); message->append_payload(uint32Data.data(), uint32Data.size()); message->append_payload(response.errorMessage.data(), errMsgSize); message->append_payload(response.data.data(), dataSize); con->send(message); } } // namespace foxglove_ws ================================================ FILE: foxglove_bridge_base/include/foxglove_bridge/websocket_tls.hpp ================================================ #pragma once #include #include #include "./websocket_logging.hpp" namespace foxglove_ws { struct WebSocketTls : public websocketpp::config::core { typedef WebSocketTls type; typedef core base; typedef base::concurrency_type concurrency_type; typedef base::request_type request_type; typedef base::response_type response_type; typedef base::message_type message_type; typedef base::con_msg_manager_type con_msg_manager_type; typedef base::endpoint_msg_manager_type endpoint_msg_manager_type; typedef CallbackLogger alog_type; typedef CallbackLogger elog_type; typedef base::rng_type rng_type; struct transport_config : public base::transport_config { typedef type::concurrency_type concurrency_type; typedef CallbackLogger alog_type; typedef CallbackLogger elog_type; typedef type::request_type request_type; typedef type::response_type response_type; typedef websocketpp::transport::asio::tls_socket::endpoint socket_type; }; typedef websocketpp::transport::asio::endpoint transport_type; struct permessage_deflate_config {}; typedef websocketpp::extensions::permessage_deflate::enabled permessage_deflate_type; }; } // namespace foxglove_ws ================================================ FILE: foxglove_bridge_base/src/base64.cpp ================================================ #include #include namespace foxglove_ws { // Adapted from: // https://gist.github.com/tomykaira/f0fd86b6c73063283afe550bc5d77594 // https://github.com/protocolbuffers/protobuf/blob/01fe22219a0/src/google/protobuf/compiler/csharp/csharp_helpers.cc#L346 std::string base64Encode(const std::string_view& input) { constexpr const char ALPHABET[] = "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/"; std::string result; // Every 3 bytes of data yields 4 bytes of output result.reserve((input.size() + (3 - 1 /* round up */)) / 3 * 4); // Unsigned values are required for bit-shifts below to work properly const unsigned char* data = reinterpret_cast(input.data()); size_t i = 0; for (; i + 2 < input.size(); i += 3) { result.push_back(ALPHABET[data[i] >> 2]); result.push_back(ALPHABET[((data[i] & 0b11) << 4) | (data[i + 1] >> 4)]); result.push_back(ALPHABET[((data[i + 1] & 0b1111) << 2) | (data[i + 2] >> 6)]); result.push_back(ALPHABET[data[i + 2] & 0b111111]); } switch (input.size() - i) { case 2: result.push_back(ALPHABET[data[i] >> 2]); result.push_back(ALPHABET[((data[i] & 0b11) << 4) | (data[i + 1] >> 4)]); result.push_back(ALPHABET[(data[i + 1] & 0b1111) << 2]); result.push_back('='); break; case 1: result.push_back(ALPHABET[data[i] >> 2]); result.push_back(ALPHABET[(data[i] & 0b11) << 4]); result.push_back('='); result.push_back('='); break; } return result; } // Adapted from: // https://github.com/mvorbrodt/blog/blob/cd46051e180/src/base64.hpp#L55-L110 std::vector base64Decode(const std::string& input) { if (input.length() % 4) { throw std::runtime_error("Invalid base64 length!"); } constexpr char kPadCharacter = '='; std::size_t padding{}; if (input.length()) { if (input[input.length() - 1] == kPadCharacter) padding++; if (input[input.length() - 2] == kPadCharacter) padding++; } std::vector decoded; decoded.reserve(((input.length() / 4) * 3) - padding); std::uint32_t temp{}; auto it = input.begin(); while (it < input.end()) { for (std::size_t i = 0; i < 4; ++i) { temp <<= 6; if (*it >= 0x41 && *it <= 0x5A) temp |= *it - 0x41; else if (*it >= 0x61 && *it <= 0x7A) temp |= *it - 0x47; else if (*it >= 0x30 && *it <= 0x39) temp |= *it + 0x04; else if (*it == 0x2B) temp |= 0x3E; else if (*it == 0x2F) temp |= 0x3F; else if (*it == kPadCharacter) { switch (input.end() - it) { case 1: decoded.push_back((temp >> 16) & 0x000000FF); decoded.push_back((temp >> 8) & 0x000000FF); return decoded; case 2: decoded.push_back((temp >> 10) & 0x000000FF); return decoded; default: throw std::runtime_error("Invalid padding in base64!"); } } else throw std::runtime_error("Invalid character in base64!"); ++it; } decoded.push_back((temp >> 16) & 0x000000FF); decoded.push_back((temp >> 8) & 0x000000FF); decoded.push_back((temp)&0x000000FF); } return decoded; } } // namespace foxglove_ws ================================================ FILE: foxglove_bridge_base/src/foxglove_bridge.cpp ================================================ #include "foxglove_bridge/foxglove_bridge.hpp" #include "websocketpp/version.hpp" namespace foxglove { const char* WebSocketUserAgent() { return websocketpp::user_agent; } } // namespace foxglove ================================================ FILE: foxglove_bridge_base/src/parameter.cpp ================================================ #include namespace foxglove_ws { ParameterValue::ParameterValue() : _type(ParameterType::PARAMETER_NOT_SET) {} ParameterValue::ParameterValue(bool value) : _type(ParameterType::PARAMETER_BOOL) , _value(value) {} ParameterValue::ParameterValue(int value) : _type(ParameterType::PARAMETER_INTEGER) , _value(static_cast(value)) {} ParameterValue::ParameterValue(int64_t value) : _type(ParameterType::PARAMETER_INTEGER) , _value(value) {} ParameterValue::ParameterValue(double value) : _type(ParameterType::PARAMETER_DOUBLE) , _value(value) {} ParameterValue::ParameterValue(const std::string& value) : _type(ParameterType::PARAMETER_STRING) , _value(value) {} ParameterValue::ParameterValue(const char* value) : _type(ParameterType::PARAMETER_STRING) , _value(std::string(value)) {} ParameterValue::ParameterValue(const std::vector& value) : _type(ParameterType::PARAMETER_BYTE_ARRAY) , _value(value) {} ParameterValue::ParameterValue(const std::vector& value) : _type(ParameterType::PARAMETER_ARRAY) , _value(value) {} ParameterValue::ParameterValue(const std::unordered_map& value) : _type(ParameterType::PARAMETER_STRUCT) , _value(value) {} Parameter::Parameter() {} Parameter::Parameter(const std::string& name) : _name(name) , _value(ParameterValue()) {} Parameter::Parameter(const std::string& name, const ParameterValue& value) : _name(name) , _value(value) {} } // namespace foxglove_ws ================================================ FILE: foxglove_bridge_base/src/serialization.cpp ================================================ #include #include namespace foxglove_ws { void to_json(nlohmann::json& j, const Channel& c) { j = { {"id", c.id}, {"topic", c.topic}, {"encoding", c.encoding}, {"schemaName", c.schemaName}, {"schema", c.schema}, }; if (c.schemaEncoding.has_value()) { j["schemaEncoding"] = c.schemaEncoding.value(); } } void from_json(const nlohmann::json& j, Channel& c) { const auto schemaEncoding = j.find("schemaEncoding") == j.end() ? std::optional(std::nullopt) : std::optional(j["schemaEncoding"].get()); ChannelWithoutId channelWithoutId{j["topic"].get(), j["encoding"].get(), j["schemaName"].get(), j["schema"].get(), schemaEncoding}; c = Channel(j["id"].get(), channelWithoutId); } void to_json(nlohmann::json& j, const ParameterValue& p) { const auto paramType = p.getType(); if (paramType == ParameterType::PARAMETER_BOOL) { j = p.getValue(); } else if (paramType == ParameterType::PARAMETER_INTEGER) { j = p.getValue(); } else if (paramType == ParameterType::PARAMETER_DOUBLE) { j = p.getValue(); } else if (paramType == ParameterType::PARAMETER_STRING) { j = p.getValue(); } else if (paramType == ParameterType::PARAMETER_BYTE_ARRAY) { const auto& paramValue = p.getValue>(); const std::string_view strValue(reinterpret_cast(paramValue.data()), paramValue.size()); j = base64Encode(strValue); } else if (paramType == ParameterType::PARAMETER_STRUCT) { j = p.getValue>(); } else if (paramType == ParameterType::PARAMETER_ARRAY) { j = p.getValue>(); } else if (paramType == ParameterType::PARAMETER_NOT_SET) { // empty value. } } void from_json(const nlohmann::json& j, ParameterValue& p) { const auto jsonType = j.type(); if (jsonType == nlohmann::detail::value_t::string) { p = ParameterValue(j.get()); } else if (jsonType == nlohmann::detail::value_t::boolean) { p = ParameterValue(j.get()); } else if (jsonType == nlohmann::detail::value_t::number_integer) { p = ParameterValue(j.get()); } else if (jsonType == nlohmann::detail::value_t::number_unsigned) { p = ParameterValue(j.get()); } else if (jsonType == nlohmann::detail::value_t::number_float) { p = ParameterValue(j.get()); } else if (jsonType == nlohmann::detail::value_t::object) { p = ParameterValue(j.get>()); } else if (jsonType == nlohmann::detail::value_t::array) { p = ParameterValue(j.get>()); } } void to_json(nlohmann::json& j, const Parameter& p) { j["name"] = p.getName(); if (p.getType() == ParameterType::PARAMETER_NOT_SET) { return; } to_json(j["value"], p.getValue()); if (p.getType() == ParameterType::PARAMETER_BYTE_ARRAY) { j["type"] = "byte_array"; } else if (p.getType() == ParameterType::PARAMETER_DOUBLE) { j["type"] = "float64"; } else if (p.getType() == ParameterType::PARAMETER_ARRAY) { const auto& vec = p.getValue().getValue>(); if (!vec.empty() && vec.front().getType() == ParameterType::PARAMETER_DOUBLE) { j["type"] = "float64_array"; } } } void from_json(const nlohmann::json& j, Parameter& p) { const auto name = j["name"].get(); if (j.find("value") == j.end()) { p = Parameter(name); // Value is not set (undefined). return; } ParameterValue pValue; from_json(j["value"], pValue); const auto typeIt = j.find("type"); const std::string type = typeIt != j.end() ? typeIt->get() : ""; if (pValue.getType() == ParameterType::PARAMETER_STRING && type == "byte_array") { p = Parameter(name, base64Decode(pValue.getValue())); } else if (pValue.getType() == ParameterType::PARAMETER_INTEGER && type == "float64") { // Explicitly cast integer value to double. p = Parameter(name, static_cast(pValue.getValue())); } else if (pValue.getType() == ParameterType::PARAMETER_ARRAY && type == "float64_array") { // Explicitly cast elements to double, if possible. auto values = pValue.getValue>(); for (ParameterValue& value : values) { if (value.getType() == ParameterType::PARAMETER_INTEGER) { value = ParameterValue(static_cast(value.getValue())); } else if (value.getType() != ParameterType::PARAMETER_DOUBLE) { throw std::runtime_error("Parameter '" + name + "' (float64_array) contains non-numeric elements."); } } p = Parameter(name, values); } else { p = Parameter(name, pValue); } } void to_json(nlohmann::json& j, const Service& service) { j = { {"id", service.id}, {"name", service.name}, {"type", service.type}, {"request", {{"schema", service.requestSchema}}}, {"response", {{"schema", service.responseSchema}}}, }; } void from_json(const nlohmann::json& j, Service& p) { p.id = j["id"].get(); p.name = j["name"].get(); p.type = j["type"].get(); if (j.find("request") != j.end() && j["request"].find("schema") != j["request"].end()) { p.requestSchema = j["request"]["schema"].get(); } else if (j.find("requestSchema") != j.end()) { throw std::runtime_error("Field 'requestSchema' (found in service " + p.name + ") is deprecated. Use 'request' instead."); } else { throw std::runtime_error("Service '" + p.name + "' has no request schema"); } if (j.find("response") != j.end() && j["response"].find("schema") != j["response"].end()) { p.responseSchema = j["response"]["schema"].get(); } else if (j.find("responseSchema") != j.end()) { throw std::runtime_error("Field 'responseSchema' (found in service " + p.name + ") is deprecated. Use 'response' instead."); } else { throw std::runtime_error("Service '" + p.name + "' has no response schema"); } } void ServiceResponse::read(const uint8_t* data, size_t dataLength) { size_t offset = 0; this->serviceId = ReadUint32LE(data + offset); offset += 4; this->callId = ReadUint32LE(data + offset); offset += 4; const size_t encondingLength = static_cast(ReadUint32LE(data + offset)); offset += 4; this->encoding = std::string(reinterpret_cast(data + offset), encondingLength); offset += encondingLength; const auto payloadLength = dataLength - offset; this->data.resize(payloadLength); std::memcpy(this->data.data(), data + offset, payloadLength); } void ServiceResponse::write(uint8_t* data) const { size_t offset = 0; foxglove_ws::WriteUint32LE(data + offset, this->serviceId); offset += 4; foxglove_ws::WriteUint32LE(data + offset, this->callId); offset += 4; foxglove_ws::WriteUint32LE(data + offset, static_cast(this->encoding.size())); offset += 4; std::memcpy(data + offset, this->encoding.data(), this->encoding.size()); offset += this->encoding.size(); std::memcpy(data + offset, this->data.data(), this->data.size()); } } // namespace foxglove_ws ================================================ FILE: foxglove_bridge_base/src/server_factory.cpp ================================================ #include #include #include #include #include namespace foxglove_ws { template <> std::unique_ptr> ServerFactory::createServer( const std::string& name, const std::function& logHandler, const ServerOptions& options) { if (options.useTls) { return std::make_unique>(name, logHandler, options); } else { return std::make_unique>(name, logHandler, options); } } template <> inline void Server::setupTlsHandler() { _server.get_alog().write(APP, "Server running without TLS"); } template <> inline void Server::setupTlsHandler() { _server.set_tls_init_handler([this](ConnHandle hdl) { (void)hdl; namespace asio = websocketpp::lib::asio; auto ctx = websocketpp::lib::make_shared(asio::ssl::context::sslv23); try { ctx->set_options(asio::ssl::context::default_workarounds | asio::ssl::context::no_tlsv1 | asio::ssl::context::no_sslv2 | asio::ssl::context::no_sslv3); ctx->use_certificate_chain_file(_options.certfile); ctx->use_private_key_file(_options.keyfile, asio::ssl::context::pem); // Ciphers are taken from the websocketpp example echo tls server: // https://github.com/zaphoyd/websocketpp/blob/1b11fd301/examples/echo_server_tls/echo_server_tls.cpp#L119 constexpr char ciphers[] = "ECDHE-RSA-AES128-GCM-SHA256:ECDHE-ECDSA-AES128-GCM-SHA256:ECDHE-RSA-AES256-GCM-SHA384:" "ECDHE-ECDSA-AES256-GCM-SHA384:DHE-RSA-AES128-GCM-SHA256:DHE-DSS-AES128-GCM-SHA256:kEDH+" "AESGCM:ECDHE-RSA-AES128-SHA256:ECDHE-ECDSA-AES128-SHA256:ECDHE-RSA-AES128-SHA:ECDHE-ECDSA-" "AES128-SHA:ECDHE-RSA-AES256-SHA384:ECDHE-ECDSA-AES256-SHA384:ECDHE-RSA-AES256-SHA:ECDHE-" "ECDSA-AES256-SHA:DHE-RSA-AES128-SHA256:DHE-RSA-AES128-SHA:DHE-DSS-AES128-SHA256:DHE-RSA-" "AES256-SHA256:DHE-DSS-AES256-SHA:DHE-RSA-AES256-SHA:!aNULL:!eNULL:!EXPORT:!DES:!RC4:!3DES:" "!MD5:!PSK"; if (SSL_CTX_set_cipher_list(ctx->native_handle(), ciphers) != 1) { _server.get_elog().write(RECOVERABLE, "Error setting cipher list"); } } catch (const std::exception& ex) { _server.get_elog().write(RECOVERABLE, std::string("Exception in TLS handshake: ") + ex.what()); } return ctx; }); } } // namespace foxglove_ws ================================================ FILE: foxglove_bridge_base/src/test/test_client.cpp ================================================ #include #include #include #include #include namespace foxglove_ws { std::future> waitForChannelMsg(ClientInterface* client, SubscriptionId subscriptionId) { // Set up binary message handler to resolve when a binary message has been received auto promise = std::make_shared>>(); auto future = promise->get_future(); client->setBinaryMessageHandler( [promise = std::move(promise), subscriptionId](const uint8_t* data, size_t dataLength) { if (ReadUint32LE(data + 1) != subscriptionId) { return; } const size_t offset = 1 + 4 + 8; std::vector dataCopy(dataLength - offset); std::memcpy(dataCopy.data(), data + offset, dataLength - offset); promise->set_value(std::move(dataCopy)); }); return future; } std::future> waitForParameters(std::shared_ptr client, const std::string& requestId) { auto promise = std::make_shared>>(); auto future = promise->get_future(); client->setTextMessageHandler( [promise = std::move(promise), requestId](const std::string& payload) { const auto msg = nlohmann::json::parse(payload); const auto& op = msg["op"].get(); const auto id = msg.value("id", ""); if (op == "parameterValues" && (requestId.empty() || requestId == id)) { const auto parameters = msg["parameters"].get>(); promise->set_value(std::move(parameters)); } }); return future; } std::future waitForServiceResponse(std::shared_ptr client) { auto promise = std::make_shared>(); auto future = promise->get_future(); client->setBinaryMessageHandler( [promise = std::move(promise)](const uint8_t* data, size_t dataLength) mutable { if (static_cast(data[0]) != BinaryOpcode::SERVICE_CALL_RESPONSE) { return; } foxglove_ws::ServiceResponse response; response.read(data + 1, dataLength - 1); promise->set_value(response); }); return future; } std::future waitForService(std::shared_ptr client, const std::string& serviceName) { auto promise = std::make_shared>(); auto future = promise->get_future(); client->setTextMessageHandler( [promise = std::move(promise), serviceName](const std::string& payload) mutable { const auto msg = nlohmann::json::parse(payload); const auto& op = msg["op"].get(); if (op == "advertiseServices") { const auto services = msg["services"].get>(); for (const auto& service : services) { if (service.name == serviceName) { promise->set_value(service); break; } } } }); return future; } std::future waitForChannel(std::shared_ptr client, const std::string& topicName) { auto promise = std::make_shared>(); auto future = promise->get_future(); client->setTextMessageHandler( [promise = std::move(promise), topicName](const std::string& payload) mutable { const auto msg = nlohmann::json::parse(payload); const auto& op = msg["op"].get(); if (op == "advertise") { const auto channels = msg["channels"].get>(); for (const auto& channel : channels) { if (channel.topic == topicName) { promise->set_value(channel); break; } } } }); return future; } std::future waitForFetchAssetResponse(std::shared_ptr client) { auto promise = std::make_shared>(); auto future = promise->get_future(); client->setBinaryMessageHandler( [promise = std::move(promise)](const uint8_t* data, size_t dataLength) mutable { if (static_cast(data[0]) != BinaryOpcode::FETCH_ASSET_RESPONSE) { return; } foxglove_ws::FetchAssetResponse response; size_t offset = 1; response.requestId = ReadUint32LE(data + offset); offset += 4; response.status = static_cast(data[offset]); offset += 1; const size_t errorMsgLength = static_cast(ReadUint32LE(data + offset)); offset += 4; response.errorMessage = std::string(reinterpret_cast(data + offset), errorMsgLength); offset += errorMsgLength; const auto payloadLength = dataLength - offset; response.data.resize(payloadLength); std::memcpy(response.data.data(), data + offset, payloadLength); promise->set_value(response); }); return future; } // Explicit template instantiation template class Client; } // namespace foxglove_ws ================================================ FILE: foxglove_bridge_base/src/version.cpp.in ================================================ #include namespace foxglove { const char FOXGLOVE_BRIDGE_VERSION[] = "@FOXGLOVE_BRIDGE_VERSION@"; const char FOXGLOVE_BRIDGE_GIT_HASH[] = "@FOXGLOVE_BRIDGE_GIT_HASH@"; } // namespace foxglove ================================================ FILE: foxglove_bridge_base/tests/base64_test.cpp ================================================ #include #include TEST(Base64Test, EncodingTest) { constexpr char arr[] = {'A', 'B', 'C', 'D'}; const std::string_view sv(arr, sizeof(arr)); const std::string b64encoded = foxglove_ws::base64Encode(sv); EXPECT_EQ(b64encoded, "QUJDRA=="); } TEST(Base64Test, DecodeTest) { const std::vector expectedVal = {0x00, 0xFF, 0x01, 0xFE}; EXPECT_EQ(foxglove_ws::base64Decode("AP8B/g=="), expectedVal); } TEST(Base64Test, DecodeInvalidStringTest) { // String length not multiple of 4 EXPECT_THROW(foxglove_ws::base64Decode("faefa"), std::runtime_error); // Invalid characters EXPECT_THROW(foxglove_ws::base64Decode("fa^ef a"), std::runtime_error); } int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } ================================================ FILE: foxglove_bridge_base/tests/serialization_test.cpp ================================================ #include #include TEST(SerializationTest, ServiceRequestSerialization) { foxglove_ws::ServiceRequest req; req.serviceId = 2; req.callId = 1; req.encoding = "json"; req.data = {1, 2, 3}; std::vector data(req.size()); req.write(data.data()); foxglove_ws::ServiceRequest req2; req2.read(data.data(), data.size()); EXPECT_EQ(req.serviceId, req2.serviceId); EXPECT_EQ(req.callId, req2.callId); EXPECT_EQ(req.encoding, req2.encoding); EXPECT_EQ(req.data.size(), req2.data.size()); EXPECT_EQ(req.data, req2.data); } int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } ================================================ FILE: foxglove_bridge_base/tests/version_test.cpp ================================================ #include #include #include TEST(VersionTest, TestWebSocketVersion) { // ex: "WebSocket++/0.8.1" const std::string version = foxglove::WebSocketUserAgent(); EXPECT_EQ(version.substr(0, 14), "WebSocket++/0."); } int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } ================================================ FILE: nodelets.xml ================================================ Foxglove bridge nodelet. ================================================ FILE: package.xml ================================================ foxglove_bridge 0.8.5 ROS Foxglove Bridge MIT https://github.com/foxglove/ros-foxglove-bridge Foxglove Foxglove catkin nodelet ros_babel_fish roscpp roslib gtest rostest rosunit std_msgs std_srvs asio libssl-dev libwebsocketpp-dev nlohmann-json-dev ros_environment zlib openssl zlib resource_retriever rosgraph_msgs catkin ================================================ FILE: ros1_foxglove_bridge/include/foxglove_bridge/generic_service.hpp ================================================ #pragma once #include #include #include #include namespace foxglove_bridge { struct GenericService { std::string type; std::string md5sum; std::vector data; template inline void write(Stream& stream) const { std::memcpy(stream.getData(), data.data(), data.size()); } template inline void read(Stream& stream) { data.resize(stream.getLength()); std::memcpy(data.data(), stream.getData(), stream.getLength()); } }; } // namespace foxglove_bridge namespace ros::service_traits { template <> struct MD5Sum { static const char* value(const foxglove_bridge::GenericService& m) { return m.md5sum.c_str(); } static const char* value() { return "*"; } }; template <> struct DataType { static const char* value(const foxglove_bridge::GenericService& m) { return m.type.c_str(); } static const char* value() { return "*"; } }; } // namespace ros::service_traits namespace ros::serialization { template <> struct Serializer { template inline static void write(Stream& stream, const foxglove_bridge::GenericService& m) { m.write(stream); } template inline static void read(Stream& stream, foxglove_bridge::GenericService& m) { m.read(stream); } inline static uint32_t serializedLength(const foxglove_bridge::GenericService& m) { return m.data.size(); } }; } // namespace ros::serialization ================================================ FILE: ros1_foxglove_bridge/include/foxglove_bridge/param_utils.hpp ================================================ #pragma once #include #include #include #include #include namespace foxglove_bridge { foxglove_ws::Parameter fromRosParam(const std::string& name, const XmlRpc::XmlRpcValue& value); XmlRpc::XmlRpcValue toRosParam(const foxglove_ws::ParameterValue& param); std::vector parseRegexPatterns(const std::vector& strings); } // namespace foxglove_bridge ================================================ FILE: ros1_foxglove_bridge/include/foxglove_bridge/service_utils.hpp ================================================ #pragma once #include #include namespace foxglove_bridge { /** * Opens a socket to the service server and retrieves the service type from the connection header. * This is necessary as the service type is not stored on the ROS master. */ std::string retrieveServiceType(const std::string& serviceName, std::chrono::milliseconds timeout_ms); } // namespace foxglove_bridge ================================================ FILE: ros1_foxglove_bridge/launch/foxglove_bridge.launch ================================================ $(arg topic_whitelist) $(arg param_whitelist) $(arg service_whitelist) $(arg client_topic_whitelist) $(arg capabilities) $(arg asset_uri_allowlist) ================================================ FILE: ros1_foxglove_bridge/src/param_utils.cpp ================================================ #include #include #include namespace foxglove_bridge { foxglove_ws::ParameterValue fromRosParam(const XmlRpc::XmlRpcValue& value) { const auto type = value.getType(); if (type == XmlRpc::XmlRpcValue::Type::TypeBoolean) { return foxglove_ws::ParameterValue(static_cast(value)); } else if (type == XmlRpc::XmlRpcValue::Type::TypeInt) { return foxglove_ws::ParameterValue(static_cast(static_cast(value))); } else if (type == XmlRpc::XmlRpcValue::Type::TypeDouble) { return foxglove_ws::ParameterValue(static_cast(value)); } else if (type == XmlRpc::XmlRpcValue::Type::TypeString) { return foxglove_ws::ParameterValue(static_cast(value)); } else if (type == XmlRpc::XmlRpcValue::Type::TypeStruct) { std::unordered_map paramMap; for (const auto& [elementName, elementVal] : value) { paramMap.insert({elementName, fromRosParam(elementVal)}); } return foxglove_ws::ParameterValue(paramMap); } else if (type == XmlRpc::XmlRpcValue::Type::TypeArray) { std::vector paramVec; for (int i = 0; i < value.size(); ++i) { paramVec.push_back(fromRosParam(value[i])); } return foxglove_ws::ParameterValue(paramVec); } else if (type == XmlRpc::XmlRpcValue::Type::TypeInvalid) { throw std::runtime_error("Parameter not set"); } else { throw std::runtime_error("Unsupported parameter type: " + std::to_string(type)); } } foxglove_ws::Parameter fromRosParam(const std::string& name, const XmlRpc::XmlRpcValue& value) { return foxglove_ws::Parameter(name, fromRosParam(value)); } XmlRpc::XmlRpcValue toRosParam(const foxglove_ws::ParameterValue& param) { const auto paramType = param.getType(); if (paramType == foxglove_ws::ParameterType::PARAMETER_BOOL) { return param.getValue(); } else if (paramType == foxglove_ws::ParameterType::PARAMETER_INTEGER) { return static_cast(param.getValue()); } else if (paramType == foxglove_ws::ParameterType::PARAMETER_DOUBLE) { return param.getValue(); } else if (paramType == foxglove_ws::ParameterType::PARAMETER_STRING) { return param.getValue(); } else if (paramType == foxglove_ws::ParameterType::PARAMETER_STRUCT) { XmlRpc::XmlRpcValue valueStruct; const auto& paramMap = param.getValue>(); for (const auto& [paramName, paramElement] : paramMap) { valueStruct[paramName] = toRosParam(paramElement); } return valueStruct; } else if (paramType == foxglove_ws::ParameterType::PARAMETER_ARRAY) { XmlRpc::XmlRpcValue arr; const auto vec = param.getValue>(); for (int i = 0; i < static_cast(vec.size()); ++i) { arr[i] = toRosParam(vec[i]); } return arr; } else { throw std::runtime_error("Unsupported parameter type"); } return XmlRpc::XmlRpcValue(); } std::vector parseRegexPatterns(const std::vector& patterns) { std::vector result; for (const auto& pattern : patterns) { try { result.push_back( std::regex(pattern, std::regex_constants::ECMAScript | std::regex_constants::icase)); } catch (...) { continue; } } return result; } } // namespace foxglove_bridge ================================================ FILE: ros1_foxglove_bridge/src/ros1_foxglove_bridge_node.cpp ================================================ #include #include int main(int argc, char** argv) { ros::init(argc, argv, "foxglove_bridge"); nodelet::Loader nodelet; nodelet::M_string remap(ros::names::getRemappings()); nodelet::V_string nargv; std::string nodelet_name = ros::this_node::getName(); if (nodelet.load(nodelet_name, "foxglove_bridge/foxglove_bridge_nodelet", remap, nargv)) { ros::spin(); return EXIT_SUCCESS; } else { return EXIT_FAILURE; } } ================================================ FILE: ros1_foxglove_bridge/src/ros1_foxglove_bridge_nodelet.cpp ================================================ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include namespace { inline std::unordered_set rpcValueToStringSet(const XmlRpc::XmlRpcValue& v) { std::unordered_set set; for (int i = 0; i < v.size(); ++i) { set.insert(v[i]); } return set; } } // namespace namespace foxglove_bridge { constexpr int DEFAULT_PORT = 8765; constexpr char DEFAULT_ADDRESS[] = "0.0.0.0"; constexpr int DEFAULT_MAX_UPDATE_MS = 5000; constexpr char ROS1_CHANNEL_ENCODING[] = "ros1"; constexpr uint32_t SUBSCRIPTION_QUEUE_LENGTH = 10; constexpr double MIN_UPDATE_PERIOD_MS = 100.0; constexpr uint32_t PUBLICATION_QUEUE_LENGTH = 10; constexpr int DEFAULT_SERVICE_TYPE_RETRIEVAL_TIMEOUT_MS = 250; constexpr int MAX_INVALID_PARAMS_TRACKED = 1000; using ConnectionHandle = websocketpp::connection_hdl; using TopicAndDatatype = std::pair; using SubscriptionsByClient = std::map>; using ClientPublications = std::unordered_map; using PublicationsByClient = std::map>; using foxglove_ws::isWhitelisted; class FoxgloveBridge : public nodelet::Nodelet { public: FoxgloveBridge() = default; virtual void onInit() { auto& nhp = getPrivateNodeHandle(); const auto address = nhp.param("address", DEFAULT_ADDRESS); const int port = nhp.param("port", DEFAULT_PORT); const auto send_buffer_limit = static_cast( nhp.param("send_buffer_limit", foxglove_ws::DEFAULT_SEND_BUFFER_LIMIT_BYTES)); const auto useTLS = nhp.param("tls", false); const auto certfile = nhp.param("certfile", ""); const auto keyfile = nhp.param("keyfile", ""); _maxUpdateMs = static_cast(nhp.param("max_update_ms", DEFAULT_MAX_UPDATE_MS)); const auto useCompression = nhp.param("use_compression", false); _useSimTime = nhp.param("/use_sim_time", false); const auto sessionId = nhp.param("/run_id", std::to_string(std::time(nullptr))); _capabilities = nhp.param>( "capabilities", std::vector(foxglove_ws::DEFAULT_CAPABILITIES.begin(), foxglove_ws::DEFAULT_CAPABILITIES.end())); _serviceRetrievalTimeoutMs = nhp.param("service_type_retrieval_timeout_ms", DEFAULT_SERVICE_TYPE_RETRIEVAL_TIMEOUT_MS); const auto topicWhitelistPatterns = nhp.param>("topic_whitelist", {".*"}); _topicWhitelistPatterns = parseRegexPatterns(topicWhitelistPatterns); if (topicWhitelistPatterns.size() != _topicWhitelistPatterns.size()) { ROS_ERROR("Failed to parse one or more topic whitelist patterns"); } const auto paramWhitelist = nhp.param>("param_whitelist", {".*"}); _paramWhitelistPatterns = parseRegexPatterns(paramWhitelist); if (paramWhitelist.size() != _paramWhitelistPatterns.size()) { ROS_ERROR("Failed to parse one or more param whitelist patterns"); } const auto serviceWhitelist = nhp.param>("service_whitelist", {".*"}); _serviceWhitelistPatterns = parseRegexPatterns(serviceWhitelist); if (serviceWhitelist.size() != _serviceWhitelistPatterns.size()) { ROS_ERROR("Failed to parse one or more service whitelist patterns"); } const auto clientTopicWhitelist = nhp.param>("client_topic_whitelist", {".*"}); const auto clientTopicWhitelistPatterns = parseRegexPatterns(clientTopicWhitelist); if (clientTopicWhitelist.size() != clientTopicWhitelistPatterns.size()) { ROS_ERROR("Failed to parse one or more service whitelist patterns"); } const auto assetUriAllowlist = nhp.param>( "asset_uri_allowlist", {"^package://(?:[-\\w%]+/" ")*[-\\w%.]+\\.(?:dae|fbx|glb|gltf|jpeg|jpg|mtl|obj|png|stl|tif|tiff|urdf|webp|xacro)$"}); _assetUriAllowlistPatterns = parseRegexPatterns(assetUriAllowlist); if (assetUriAllowlist.size() != _assetUriAllowlistPatterns.size()) { ROS_ERROR("Failed to parse one or more asset URI whitelist patterns"); } const char* rosDistro = std::getenv("ROS_DISTRO"); ROS_INFO("Starting foxglove_bridge (%s, %s@%s) with %s", rosDistro, foxglove::FOXGLOVE_BRIDGE_VERSION, foxglove::FOXGLOVE_BRIDGE_GIT_HASH, foxglove::WebSocketUserAgent()); try { foxglove_ws::ServerOptions serverOptions; serverOptions.capabilities = _capabilities; if (_useSimTime) { serverOptions.capabilities.push_back(foxglove_ws::CAPABILITY_TIME); } serverOptions.supportedEncodings = {ROS1_CHANNEL_ENCODING}; serverOptions.metadata = {{"ROS_DISTRO", rosDistro}}; serverOptions.sendBufferLimitBytes = send_buffer_limit; serverOptions.sessionId = sessionId; serverOptions.useTls = useTLS; serverOptions.certfile = certfile; serverOptions.keyfile = keyfile; serverOptions.useCompression = useCompression; serverOptions.clientTopicWhitelistPatterns = clientTopicWhitelistPatterns; const auto logHandler = std::bind(&FoxgloveBridge::logHandler, this, std::placeholders::_1, std::placeholders::_2); // Fetching of assets may be blocking, hence we fetch them in a separate thread. _fetchAssetQueue = std::make_unique(logHandler, 1 /* num_threads */); _server = foxglove_ws::ServerFactory::createServer( "foxglove_bridge", logHandler, serverOptions); foxglove_ws::ServerHandlers hdlrs; hdlrs.subscribeHandler = std::bind(&FoxgloveBridge::subscribe, this, std::placeholders::_1, std::placeholders::_2); hdlrs.unsubscribeHandler = std::bind(&FoxgloveBridge::unsubscribe, this, std::placeholders::_1, std::placeholders::_2); hdlrs.clientAdvertiseHandler = std::bind(&FoxgloveBridge::clientAdvertise, this, std::placeholders::_1, std::placeholders::_2); hdlrs.clientUnadvertiseHandler = std::bind(&FoxgloveBridge::clientUnadvertise, this, std::placeholders::_1, std::placeholders::_2); hdlrs.clientMessageHandler = std::bind(&FoxgloveBridge::clientMessage, this, std::placeholders::_1, std::placeholders::_2); hdlrs.parameterRequestHandler = std::bind(&FoxgloveBridge::getParameters, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); hdlrs.parameterChangeHandler = std::bind(&FoxgloveBridge::setParameters, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); hdlrs.parameterSubscriptionHandler = std::bind(&FoxgloveBridge::subscribeParameters, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); hdlrs.serviceRequestHandler = std::bind(&FoxgloveBridge::serviceRequest, this, std::placeholders::_1, std::placeholders::_2); hdlrs.subscribeConnectionGraphHandler = [this](bool subscribe) { _subscribeGraphUpdates = subscribe; }; if (hasCapability(foxglove_ws::CAPABILITY_ASSETS)) { hdlrs.fetchAssetHandler = [this](const std::string& uri, uint32_t requestId, foxglove_ws::ConnHandle hdl) { _fetchAssetQueue->addCallback( std::bind(&FoxgloveBridge::fetchAsset, this, uri, requestId, hdl)); }; } _server->setHandlers(std::move(hdlrs)); _server->start(address, static_cast(port)); xmlrpcServer.bind("paramUpdate", std::bind(&FoxgloveBridge::parameterUpdates, this, std::placeholders::_1, std::placeholders::_2)); xmlrpcServer.start(); updateAdvertisedTopicsAndServices(ros::TimerEvent()); if (_useSimTime) { _clockSubscription = getMTNodeHandle().subscribe( "/clock", 10, [&](const rosgraph_msgs::Clock::ConstPtr msg) { _server->broadcastTime(msg->clock.toNSec()); }); } } catch (const std::exception& err) { ROS_ERROR("Failed to start websocket server: %s", err.what()); // Rethrow exception such that the nodelet is unloaded. throw err; } }; virtual ~FoxgloveBridge() { xmlrpcServer.shutdown(); if (_server) { _server->stop(); } } private: struct PairHash { template std::size_t operator()(const std::pair& pair) const { return std::hash()(pair.first) ^ std::hash()(pair.second); } }; void subscribe(foxglove_ws::ChannelId channelId, ConnectionHandle clientHandle) { std::lock_guard lock(_subscriptionsMutex); auto it = _advertisedTopics.find(channelId); if (it == _advertisedTopics.end()) { const std::string errMsg = "Received subscribe request for unknown channel " + std::to_string(channelId); ROS_WARN_STREAM(errMsg); throw foxglove_ws::ChannelError(channelId, errMsg); } const auto& channel = it->second; const auto& topic = channel.topic; const auto& datatype = channel.schemaName; // Get client subscriptions for this channel or insert an empty map. auto [subscriptionsIt, firstSubscription] = _subscriptions.emplace(channelId, SubscriptionsByClient()); auto& subscriptionsByClient = subscriptionsIt->second; if (!firstSubscription && subscriptionsByClient.find(clientHandle) != subscriptionsByClient.end()) { const std::string errMsg = "Client is already subscribed to channel " + std::to_string(channelId); ROS_WARN_STREAM(errMsg); throw foxglove_ws::ChannelError(channelId, errMsg); } try { subscriptionsByClient.emplace( clientHandle, getMTNodeHandle().subscribe( topic, SUBSCRIPTION_QUEUE_LENGTH, std::bind(&FoxgloveBridge::rosMessageHandler, this, channelId, clientHandle, std::placeholders::_1))); if (firstSubscription) { ROS_INFO("Subscribed to topic \"%s\" (%s) on channel %d", topic.c_str(), datatype.c_str(), channelId); } else { ROS_INFO("Added subscriber #%zu to topic \"%s\" (%s) on channel %d", subscriptionsByClient.size(), topic.c_str(), datatype.c_str(), channelId); } } catch (const std::exception& ex) { const std::string errMsg = "Failed to subscribe to topic '" + topic + "' (" + datatype + "): " + ex.what(); ROS_ERROR_STREAM(errMsg); throw foxglove_ws::ChannelError(channelId, errMsg); } } void unsubscribe(foxglove_ws::ChannelId channelId, ConnectionHandle clientHandle) { std::lock_guard lock(_subscriptionsMutex); const auto channelIt = _advertisedTopics.find(channelId); if (channelIt == _advertisedTopics.end()) { const std::string errMsg = "Received unsubscribe request for unknown channel " + std::to_string(channelId); ROS_WARN_STREAM(errMsg); throw foxglove_ws::ChannelError(channelId, errMsg); } const auto& channel = channelIt->second; auto subscriptionsIt = _subscriptions.find(channelId); if (subscriptionsIt == _subscriptions.end()) { throw foxglove_ws::ChannelError(channelId, "Received unsubscribe request for channel " + std::to_string(channelId) + " that was not subscribed to "); } auto& subscriptionsByClient = subscriptionsIt->second; const auto clientSubscription = subscriptionsByClient.find(clientHandle); if (clientSubscription == subscriptionsByClient.end()) { throw foxglove_ws::ChannelError( channelId, "Received unsubscribe request for channel " + std::to_string(channelId) + "from a client that was not subscribed to this channel"); } subscriptionsByClient.erase(clientSubscription); if (subscriptionsByClient.empty()) { ROS_INFO("Unsubscribing from topic \"%s\" (%s) on channel %d", channel.topic.c_str(), channel.schemaName.c_str(), channelId); _subscriptions.erase(subscriptionsIt); } else { ROS_INFO("Removed one subscription from channel %d (%zu subscription(s) left)", channelId, subscriptionsByClient.size()); } } void clientAdvertise(const foxglove_ws::ClientAdvertisement& channel, ConnectionHandle clientHandle) { if (channel.encoding != ROS1_CHANNEL_ENCODING) { throw foxglove_ws::ClientChannelError( channel.channelId, "Unsupported encoding. Only '" + std::string(ROS1_CHANNEL_ENCODING) + "' encoding is supported at the moment."); } std::unique_lock lock(_publicationsMutex); // Get client publications or insert an empty map. auto [clientPublicationsIt, isFirstPublication] = _clientAdvertisedTopics.emplace(clientHandle, ClientPublications()); auto& clientPublications = clientPublicationsIt->second; if (!isFirstPublication && clientPublications.find(channel.channelId) != clientPublications.end()) { throw foxglove_ws::ClientChannelError( channel.channelId, "Received client advertisement from " + _server->remoteEndpointString(clientHandle) + " for channel " + std::to_string(channel.channelId) + " it had already advertised"); } const auto msgDescription = _rosTypeInfoProvider.getMessageDescription(channel.schemaName); if (!msgDescription) { throw foxglove_ws::ClientChannelError( channel.channelId, "Failed to retrieve type information of data type '" + channel.schemaName + "'. Unable to advertise topic " + channel.topic); } ros::AdvertiseOptions advertiseOptions; advertiseOptions.datatype = channel.schemaName; advertiseOptions.has_header = false; // TODO advertiseOptions.latch = false; advertiseOptions.md5sum = msgDescription->md5; advertiseOptions.message_definition = msgDescription->message_definition; advertiseOptions.queue_size = PUBLICATION_QUEUE_LENGTH; advertiseOptions.topic = channel.topic; auto publisher = getMTNodeHandle().advertise(advertiseOptions); if (publisher) { clientPublications.insert({channel.channelId, std::move(publisher)}); ROS_INFO("Client %s is advertising \"%s\" (%s) on channel %d", _server->remoteEndpointString(clientHandle).c_str(), channel.topic.c_str(), channel.schemaName.c_str(), channel.channelId); // Trigger topic discovery so other clients are immediately informed about this new topic. updateAdvertisedTopics(); } else { const auto errMsg = "Failed to create publisher for topic " + channel.topic + "(" + channel.schemaName + ")"; ROS_ERROR_STREAM(errMsg); throw foxglove_ws::ClientChannelError(channel.channelId, errMsg); } } void clientUnadvertise(foxglove_ws::ClientChannelId channelId, ConnectionHandle clientHandle) { std::unique_lock lock(_publicationsMutex); auto clientPublicationsIt = _clientAdvertisedTopics.find(clientHandle); if (clientPublicationsIt == _clientAdvertisedTopics.end()) { throw foxglove_ws::ClientChannelError( channelId, "Ignoring client unadvertisement from " + _server->remoteEndpointString(clientHandle) + " for unknown channel " + std::to_string(channelId) + ", client has no advertised topics"); } auto& clientPublications = clientPublicationsIt->second; auto channelPublicationIt = clientPublications.find(channelId); if (channelPublicationIt == clientPublications.end()) { throw foxglove_ws::ClientChannelError( channelId, "Ignoring client unadvertisement from " + _server->remoteEndpointString(clientHandle) + " for unknown channel " + std::to_string(channelId) + ", client has " + std::to_string(clientPublications.size()) + " advertised topic(s)"); } const auto& publisher = channelPublicationIt->second; ROS_INFO("Client %s is no longer advertising %s (%d subscribers) on channel %d", _server->remoteEndpointString(clientHandle).c_str(), publisher.getTopic().c_str(), publisher.getNumSubscribers(), channelId); clientPublications.erase(channelPublicationIt); if (clientPublications.empty()) { _clientAdvertisedTopics.erase(clientPublicationsIt); } } void clientMessage(const foxglove_ws::ClientMessage& clientMsg, ConnectionHandle clientHandle) { ros_babel_fish::BabelFishMessage::Ptr msg(new ros_babel_fish::BabelFishMessage); msg->read(clientMsg); const auto channelId = clientMsg.advertisement.channelId; std::shared_lock lock(_publicationsMutex); auto clientPublicationsIt = _clientAdvertisedTopics.find(clientHandle); if (clientPublicationsIt == _clientAdvertisedTopics.end()) { throw foxglove_ws::ClientChannelError( channelId, "Dropping client message from " + _server->remoteEndpointString(clientHandle) + " for unknown channel " + std::to_string(channelId) + ", client has no advertised topics"); } auto& clientPublications = clientPublicationsIt->second; auto channelPublicationIt = clientPublications.find(clientMsg.advertisement.channelId); if (channelPublicationIt == clientPublications.end()) { throw foxglove_ws::ClientChannelError( channelId, "Dropping client message from " + _server->remoteEndpointString(clientHandle) + " for unknown channel " + std::to_string(channelId) + ", client has " + std::to_string(clientPublications.size()) + " advertised topic(s)"); } try { channelPublicationIt->second.publish(msg); } catch (const std::exception& ex) { throw foxglove_ws::ClientChannelError(channelId, "Failed to publish message on topic '" + channelPublicationIt->second.getTopic() + "': " + ex.what()); } } void updateAdvertisedTopicsAndServices(const ros::TimerEvent&) { _updateTimer.stop(); if (!ros::ok()) { return; } const bool servicesEnabled = hasCapability(foxglove_ws::CAPABILITY_SERVICES); const bool querySystemState = servicesEnabled || _subscribeGraphUpdates; std::vector serviceNames; foxglove_ws::MapOfSets publishers, subscribers, services; // Retrieve system state from ROS master. if (querySystemState) { XmlRpc::XmlRpcValue params, result, payload; params[0] = this->getName(); if (ros::master::execute("getSystemState", params, result, payload, false) && static_cast(result[0]) == 1) { const auto& systemState = result[2]; const auto& publishersXmlRpc = systemState[0]; const auto& subscribersXmlRpc = systemState[1]; const auto& servicesXmlRpc = systemState[2]; for (int i = 0; i < servicesXmlRpc.size(); ++i) { const std::string& name = servicesXmlRpc[i][0]; if (isWhitelisted(name, _serviceWhitelistPatterns)) { serviceNames.push_back(name); services.emplace(name, rpcValueToStringSet(servicesXmlRpc[i][1])); } } for (int i = 0; i < publishersXmlRpc.size(); ++i) { const std::string& name = publishersXmlRpc[i][0]; if (isWhitelisted(name, _topicWhitelistPatterns)) { publishers.emplace(name, rpcValueToStringSet(publishersXmlRpc[i][1])); } } for (int i = 0; i < subscribersXmlRpc.size(); ++i) { const std::string& name = subscribersXmlRpc[i][0]; if (isWhitelisted(name, _topicWhitelistPatterns)) { subscribers.emplace(name, rpcValueToStringSet(subscribersXmlRpc[i][1])); } } } else { ROS_WARN("Failed to call getSystemState: %s", result.toXml().c_str()); } } updateAdvertisedTopics(); if (servicesEnabled) { updateAdvertisedServices(serviceNames); } if (_subscribeGraphUpdates) { _server->updateConnectionGraph(publishers, subscribers, services); } // Schedule the next update using truncated exponential backoff, between `MIN_UPDATE_PERIOD_MS` // and `_maxUpdateMs` _updateCount++; const auto nextUpdateMs = std::max( MIN_UPDATE_PERIOD_MS, static_cast(std::min(size_t(1) << _updateCount, _maxUpdateMs))); _updateTimer = getMTNodeHandle().createTimer( ros::Duration(nextUpdateMs / 1e3), &FoxgloveBridge::updateAdvertisedTopicsAndServices, this); } void updateAdvertisedTopics() { // Get the current list of visible topics and datatypes from the ROS graph // For this, we call the ros master's `getTopicTypes` method (See // https://wiki.ros.org/ROS/Master_API) which also includes topics without publisher(s) and only // subscriber(s). XmlRpc::XmlRpcValue request, response, topicNamesAndTypes; request[0] = ros::this_node::getName(); if (!ros::master::execute("getTopicTypes", request, response, topicNamesAndTypes, false)) { ROS_WARN("Failed to retrieve topics from ROS master."); return; } std::unordered_set latestTopics; latestTopics.reserve(topicNamesAndTypes.size()); for (int i = 0; i < topicNamesAndTypes.size(); ++i) { const std::string topicName = topicNamesAndTypes[i][0]; const std::string datatype = topicNamesAndTypes[i][1]; // Ignore the topic if it is not on the topic whitelist if (isWhitelisted(topicName, _topicWhitelistPatterns)) { latestTopics.emplace(topicName, datatype); } } if (const auto numIgnoredTopics = topicNamesAndTypes.size() - latestTopics.size()) { ROS_DEBUG( "%zu topics have been ignored as they do not match any pattern on the topic whitelist", numIgnoredTopics); } std::lock_guard lock(_subscriptionsMutex); // Remove channels for which the topic does not exist anymore std::vector channelIdsToRemove; for (auto channelIt = _advertisedTopics.begin(); channelIt != _advertisedTopics.end();) { const TopicAndDatatype topicAndDatatype = {channelIt->second.topic, channelIt->second.schemaName}; if (latestTopics.find(topicAndDatatype) == latestTopics.end()) { const auto channelId = channelIt->first; channelIdsToRemove.push_back(channelId); _subscriptions.erase(channelId); ROS_DEBUG("Removed channel %d for topic \"%s\" (%s)", channelId, topicAndDatatype.first.c_str(), topicAndDatatype.second.c_str()); channelIt = _advertisedTopics.erase(channelIt); } else { channelIt++; } } _server->removeChannels(channelIdsToRemove); // Add new channels for new topics std::vector channelsToAdd; for (const auto& topicAndDatatype : latestTopics) { if (std::find_if(_advertisedTopics.begin(), _advertisedTopics.end(), [topicAndDatatype](const auto& channelIdAndChannel) { const auto& channel = channelIdAndChannel.second; return channel.topic == topicAndDatatype.first && channel.schemaName == topicAndDatatype.second; }) != _advertisedTopics.end()) { continue; // Topic already advertised } foxglove_ws::ChannelWithoutId newChannel{}; newChannel.topic = topicAndDatatype.first; newChannel.schemaName = topicAndDatatype.second; newChannel.encoding = ROS1_CHANNEL_ENCODING; try { const auto msgDescription = _rosTypeInfoProvider.getMessageDescription(topicAndDatatype.second); if (msgDescription) { newChannel.schema = msgDescription->message_definition; } else { ROS_WARN("Could not find definition for type %s", topicAndDatatype.second.c_str()); // We still advertise the channel, but with an emtpy schema newChannel.schema = ""; } } catch (const std::exception& err) { ROS_WARN("Failed to add channel for topic \"%s\" (%s): %s", topicAndDatatype.first.c_str(), topicAndDatatype.second.c_str(), err.what()); continue; } channelsToAdd.push_back(newChannel); } const auto channelIds = _server->addChannels(channelsToAdd); for (size_t i = 0; i < channelsToAdd.size(); ++i) { const auto channelId = channelIds[i]; const auto& channel = channelsToAdd[i]; _advertisedTopics.emplace(channelId, channel); ROS_DEBUG("Advertising channel %d for topic \"%s\" (%s)", channelId, channel.topic.c_str(), channel.schemaName.c_str()); } } void updateAdvertisedServices(const std::vector& serviceNames) { std::unique_lock lock(_servicesMutex); // Remove advertisements for services that have been removed std::vector servicesToRemove; for (const auto& service : _advertisedServices) { const auto it = std::find_if(serviceNames.begin(), serviceNames.end(), [service](const auto& serviceName) { return serviceName == service.second.name; }); if (it == serviceNames.end()) { servicesToRemove.push_back(service.first); } } for (auto serviceId : servicesToRemove) { _advertisedServices.erase(serviceId); } _server->removeServices(servicesToRemove); // Advertise new services std::vector newServices; for (const auto& serviceName : serviceNames) { if (std::find_if(_advertisedServices.begin(), _advertisedServices.end(), [&serviceName](const auto& idWithService) { return idWithService.second.name == serviceName; }) != _advertisedServices.end()) { continue; // Already advertised } try { const auto serviceType = retrieveServiceType(serviceName, std::chrono::milliseconds(_serviceRetrievalTimeoutMs)); const auto srvDescription = _rosTypeInfoProvider.getServiceDescription(serviceType); foxglove_ws::ServiceWithoutId service; service.name = serviceName; service.type = serviceType; if (srvDescription) { service.requestSchema = srvDescription->request->message_definition; service.responseSchema = srvDescription->response->message_definition; } else { ROS_ERROR("Failed to retrieve type information for service '%s' of type '%s'", serviceName.c_str(), serviceType.c_str()); // We still advertise the channel, but with empty schema. service.requestSchema = ""; service.responseSchema = ""; } newServices.push_back(service); } catch (const std::exception& e) { ROS_ERROR("Failed to retrieve service type or service description of service %s: %s", serviceName.c_str(), e.what()); continue; } } const auto serviceIds = _server->addServices(newServices); for (size_t i = 0; i < serviceIds.size(); ++i) { _advertisedServices.emplace(serviceIds[i], newServices[i]); } } void getParameters(const std::vector& parameters, const std::optional& requestId, ConnectionHandle hdl) { const bool allParametersRequested = parameters.empty(); std::vector parameterNames = parameters; if (allParametersRequested) { if (!getMTNodeHandle().getParamNames(parameterNames)) { const auto errMsg = "Failed to retrieve parameter names"; ROS_ERROR_STREAM(errMsg); throw std::runtime_error(errMsg); } } bool success = true; std::vector params; std::vector invalidParams; for (const auto& paramName : parameterNames) { if (!isWhitelisted(paramName, _paramWhitelistPatterns)) { if (allParametersRequested) { continue; } else { ROS_ERROR("Parameter '%s' is not on the allowlist", paramName.c_str()); success = false; } } if (_invalidParams.find(paramName) != _invalidParams.end()) { continue; } try { XmlRpc::XmlRpcValue value; getMTNodeHandle().getParam(paramName, value); params.push_back(fromRosParam(paramName, value)); } catch (const std::exception& ex) { ROS_ERROR("Invalid parameter '%s': %s", paramName.c_str(), ex.what()); invalidParams.push_back(paramName); success = false; } catch (const XmlRpc::XmlRpcException& ex) { ROS_ERROR("Invalid parameter '%s': %s", paramName.c_str(), ex.getMessage().c_str()); invalidParams.push_back(paramName); success = false; } catch (...) { ROS_ERROR("Invalid parameter '%s'", paramName.c_str()); invalidParams.push_back(paramName); success = false; } } _server->publishParameterValues(hdl, params, requestId); if (!success) { for (std::string& param : invalidParams) { if (_invalidParams.size() < MAX_INVALID_PARAMS_TRACKED) { _invalidParams.insert(param); } } if (!invalidParams.empty()) { std::string errorMsg = "Failed to retrieve the following parameters: "; for (size_t i = 0; i < invalidParams.size(); i++) { errorMsg += invalidParams[i]; if (i < invalidParams.size() - 1) { errorMsg += ", "; } } throw std::runtime_error(errorMsg); } else { throw std::runtime_error("Failed to retrieve one or multiple parameters"); } } } void setParameters(const std::vector& parameters, const std::optional& requestId, ConnectionHandle hdl) { using foxglove_ws::ParameterType; auto nh = this->getMTNodeHandle(); bool success = true; for (const auto& param : parameters) { const auto paramName = param.getName(); if (!isWhitelisted(paramName, _paramWhitelistPatterns)) { ROS_ERROR("Parameter '%s' is not on the allowlist", paramName.c_str()); success = false; continue; } try { const auto paramType = param.getType(); const auto paramValue = param.getValue(); if (paramType == ParameterType::PARAMETER_NOT_SET) { nh.deleteParam(paramName); } else { nh.setParam(paramName, toRosParam(paramValue)); } } catch (const std::exception& ex) { ROS_ERROR("Failed to set parameter '%s': %s", paramName.c_str(), ex.what()); success = false; } catch (const XmlRpc::XmlRpcException& ex) { ROS_ERROR("Failed to set parameter '%s': %s", paramName.c_str(), ex.getMessage().c_str()); success = false; } catch (...) { ROS_ERROR("Failed to set parameter '%s'", paramName.c_str()); success = false; } } // If a request Id was given, send potentially updated parameters back to client if (requestId) { std::vector parameterNames(parameters.size()); for (size_t i = 0; i < parameters.size(); ++i) { parameterNames[i] = parameters[i].getName(); } getParameters(parameterNames, requestId, hdl); } if (!success) { throw std::runtime_error("Failed to set one or multiple parameters"); } } void subscribeParameters(const std::vector& parameters, foxglove_ws::ParameterSubscriptionOperation op, ConnectionHandle) { const auto opVerb = (op == foxglove_ws::ParameterSubscriptionOperation::SUBSCRIBE) ? "subscribe" : "unsubscribe"; bool success = true; for (const auto& paramName : parameters) { if (!isWhitelisted(paramName, _paramWhitelistPatterns)) { ROS_ERROR("Parameter '%s' is not allowlist", paramName.c_str()); continue; } XmlRpc::XmlRpcValue params, result, payload; params[0] = getName() + "2"; params[1] = xmlrpcServer.getServerURI(); params[2] = ros::names::resolve(paramName); const std::string opName = std::string(opVerb) + "Param"; if (ros::master::execute(opName, params, result, payload, false)) { ROS_DEBUG("%s '%s'", opName.c_str(), paramName.c_str()); } else { ROS_WARN("Failed to %s '%s': %s", opVerb, paramName.c_str(), result.toXml().c_str()); success = false; } } if (!success) { throw std::runtime_error("Failed to " + std::string(opVerb) + " one or multiple parameters."); } } void parameterUpdates(XmlRpc::XmlRpcValue& params, XmlRpc::XmlRpcValue& result) { result[0] = 1; result[1] = std::string(""); result[2] = 0; if (params.size() != 3) { ROS_ERROR("Parameter update called with invalid parameter size: %d", params.size()); return; } try { const std::string paramName = ros::names::clean(params[1]); const XmlRpc::XmlRpcValue paramValue = params[2]; const auto param = fromRosParam(paramName, paramValue); _server->updateParameterValues({param}); } catch (const std::exception& ex) { ROS_ERROR("Failed to update parameter: %s", ex.what()); } catch (const XmlRpc::XmlRpcException& ex) { ROS_ERROR("Failed to update parameter: %s", ex.getMessage().c_str()); } catch (...) { ROS_ERROR("Failed to update parameter"); } } void logHandler(foxglove_ws::WebSocketLogLevel level, char const* msg) { switch (level) { case foxglove_ws::WebSocketLogLevel::Debug: ROS_DEBUG("[WS] %s", msg); break; case foxglove_ws::WebSocketLogLevel::Info: ROS_INFO("[WS] %s", msg); break; case foxglove_ws::WebSocketLogLevel::Warn: ROS_WARN("[WS] %s", msg); break; case foxglove_ws::WebSocketLogLevel::Error: ROS_ERROR("[WS] %s", msg); break; case foxglove_ws::WebSocketLogLevel::Critical: ROS_FATAL("[WS] %s", msg); break; } } void rosMessageHandler( const foxglove_ws::ChannelId channelId, ConnectionHandle clientHandle, const ros::MessageEvent& msgEvent) { const auto& msg = msgEvent.getConstMessage(); const auto receiptTimeNs = msgEvent.getReceiptTime().toNSec(); _server->sendMessage(clientHandle, channelId, receiptTimeNs, msg->buffer(), msg->size()); } void serviceRequest(const foxglove_ws::ServiceRequest& request, ConnectionHandle clientHandle) { std::shared_lock lock(_servicesMutex); const auto serviceIt = _advertisedServices.find(request.serviceId); if (serviceIt == _advertisedServices.end()) { const auto errMsg = "Service with id " + std::to_string(request.serviceId) + " does not exist"; ROS_ERROR_STREAM(errMsg); throw foxglove_ws::ServiceError(request.serviceId, errMsg); } const auto& serviceName = serviceIt->second.name; const auto& serviceType = serviceIt->second.type; ROS_DEBUG("Received a service request for service %s (%s)", serviceName.c_str(), serviceType.c_str()); if (!ros::service::exists(serviceName, false)) { throw foxglove_ws::ServiceError(request.serviceId, "Service '" + serviceName + "' does not exist"); } const auto srvDescription = _rosTypeInfoProvider.getServiceDescription(serviceType); if (!srvDescription) { const auto errMsg = "Failed to retrieve type information for service " + serviceName + "(" + serviceType + ")"; ROS_ERROR_STREAM(errMsg); throw foxglove_ws::ServiceError(request.serviceId, errMsg); } GenericService genReq, genRes; genReq.type = genRes.type = serviceType; genReq.md5sum = genRes.md5sum = srvDescription->md5; genReq.data = request.data; if (ros::service::call(serviceName, genReq, genRes)) { foxglove_ws::ServiceResponse res; res.serviceId = request.serviceId; res.callId = request.callId; res.encoding = request.encoding; res.data = genRes.data; _server->sendServiceResponse(clientHandle, res); } else { throw foxglove_ws::ServiceError( request.serviceId, "Failed to call service " + serviceName + "(" + serviceType + ")"); } } void fetchAsset(const std::string& uri, uint32_t requestId, ConnectionHandle clientHandle) { foxglove_ws::FetchAssetResponse response; response.requestId = requestId; try { // We reject URIs that are not on the allowlist or that contain two consecutive dots. The // latter can be utilized to construct URIs for retrieving confidential files that should not // be accessible over the WebSocket connection. Example: // `package:///../../../secret.txt`. This is an extra security measure and should // not be necessary if the allowlist is strict enough. if (uri.find("..") != std::string::npos || !isWhitelisted(uri, _assetUriAllowlistPatterns)) { throw std::runtime_error("Asset URI not allowed: " + uri); } resource_retriever::Retriever resource_retriever; const resource_retriever::MemoryResource memoryResource = resource_retriever.get(uri); response.status = foxglove_ws::FetchAssetStatus::Success; response.errorMessage = ""; response.data.resize(memoryResource.size); std::memcpy(response.data.data(), memoryResource.data.get(), memoryResource.size); } catch (const std::exception& ex) { ROS_WARN("Failed to retrieve asset '%s': %s", uri.c_str(), ex.what()); response.status = foxglove_ws::FetchAssetStatus::Error; response.errorMessage = "Failed to retrieve asset " + uri; } if (_server) { _server->sendFetchAssetResponse(clientHandle, response); } } bool hasCapability(const std::string& capability) { return std::find(_capabilities.begin(), _capabilities.end(), capability) != _capabilities.end(); } std::unique_ptr> _server; ros_babel_fish::IntegratedDescriptionProvider _rosTypeInfoProvider; std::vector _topicWhitelistPatterns; std::vector _paramWhitelistPatterns; std::vector _serviceWhitelistPatterns; std::vector _assetUriAllowlistPatterns; ros::XMLRPCManager xmlrpcServer; std::unordered_map _advertisedTopics; std::unordered_map _subscriptions; std::unordered_map _advertisedServices; PublicationsByClient _clientAdvertisedTopics; std::mutex _subscriptionsMutex; std::shared_mutex _publicationsMutex; std::shared_mutex _servicesMutex; ros::Timer _updateTimer; size_t _maxUpdateMs = size_t(DEFAULT_MAX_UPDATE_MS); size_t _updateCount = 0; ros::Subscriber _clockSubscription; bool _useSimTime = false; std::vector _capabilities; int _serviceRetrievalTimeoutMs = DEFAULT_SERVICE_TYPE_RETRIEVAL_TIMEOUT_MS; std::atomic _subscribeGraphUpdates = false; std::unique_ptr _fetchAssetQueue; std::unordered_set _invalidParams; }; } // namespace foxglove_bridge PLUGINLIB_EXPORT_CLASS(foxglove_bridge::FoxgloveBridge, nodelet::Nodelet) ================================================ FILE: ros1_foxglove_bridge/src/service_utils.cpp ================================================ #include #include #include #include #include #include #include #include #include namespace foxglove_bridge { /** * Looks up the service server host & port and opens a TCP connection to it to retrieve the header * which contains the service type. * * The implementation is similar to how ROS does it under the hood when creating a service server * link: * https://github.com/ros/ros_comm/blob/845f74602c7464e08ef5ac6fd9e26c97d0fe42c9/clients/roscpp/src/libros/service_manager.cpp#L246-L261 * https://github.com/ros/ros_comm/blob/845f74602c7464e08ef5ac6fd9e26c97d0fe42c9/clients/roscpp/src/libros/service_server_link.cpp#L114-L130 */ std::string retrieveServiceType(const std::string& serviceName, std::chrono::milliseconds timeout) { std::string srvHost; uint32_t srvPort; if (!ros::ServiceManager::instance()->lookupService(serviceName, srvHost, srvPort)) { throw std::runtime_error("Failed to lookup service " + serviceName); } auto transport = boost::make_shared(&ros::PollManager::instance()->getPollSet()); auto connection = boost::make_shared(); ros::ConnectionManager::instance()->addConnection(connection); connection->initialize(transport, false, ros::HeaderReceivedFunc()); if (!transport->connect(srvHost, srvPort)) { throw std::runtime_error("Failed to connect to service server of service " + serviceName); } std::promise promise; auto future = promise.get_future(); connection->setHeaderReceivedCallback( [&promise](const ros::ConnectionPtr& conn, const ros::Header& header) { std::string serviceType; if (header.getValue("type", serviceType)) { promise.set_value(serviceType); } else { promise.set_exception(std::make_exception_ptr( std::runtime_error("Key 'type' not found in service connection header"))); } // Close connection since we don't need it any more. conn->drop(ros::Connection::DropReason::Destructing); return true; }); ros::M_string header; header["service"] = serviceName; header["md5sum"] = "*"; header["callerid"] = ros::this_node::getName(); header["persistent"] = "0"; header["probe"] = "1"; connection->writeHeader(header, [](const ros::ConnectionPtr&) {}); if (future.wait_for(timeout) != std::future_status::ready) { // Drop the connection here to prevent that the header-received callback is called after the // promise has already been destroyed. connection->drop(ros::Connection::DropReason::Destructing); throw std::runtime_error("Timed out when retrieving service type"); } return future.get(); } } // namespace foxglove_bridge ================================================ FILE: ros1_foxglove_bridge/tests/smoke.test ================================================ ['file://.*'] ================================================ FILE: ros1_foxglove_bridge/tests/smoke_test.cpp ================================================ #include #include #include #include #include #include #include #include #include #include constexpr char URI[] = "ws://localhost:9876"; // Binary representation of std_msgs/String for "hello world" constexpr uint8_t HELLO_WORLD_BINARY[] = {11, 0, 0, 0, 104, 101, 108, 108, 111, 32, 119, 111, 114, 108, 100}; constexpr auto ONE_SECOND = std::chrono::seconds(1); constexpr auto DEFAULT_TIMEOUT = std::chrono::seconds(8); class ParameterTest : public ::testing::Test { public: using PARAM_1_TYPE = std::string; inline static const std::string PARAM_1_NAME = "/node_1/string_param"; inline static const PARAM_1_TYPE PARAM_1_DEFAULT_VALUE = "hello"; using PARAM_2_TYPE = std::vector; inline static const std::string PARAM_2_NAME = "/node_2/int_array_param"; inline static const PARAM_2_TYPE PARAM_2_DEFAULT_VALUE = {1.2, 2.1, 3.3}; protected: void SetUp() override { _nh = ros::NodeHandle(); _nh.setParam(PARAM_1_NAME, PARAM_1_DEFAULT_VALUE); _nh.setParam(PARAM_2_NAME, PARAM_2_DEFAULT_VALUE); _wsClient = std::make_shared>(); ASSERT_EQ(std::future_status::ready, _wsClient->connect(URI).wait_for(DEFAULT_TIMEOUT)); } ros::NodeHandle _nh; std::shared_ptr> _wsClient; }; class ServiceTest : public ::testing::Test { public: inline static const std::string SERVICE_NAME = "/foo_service"; protected: void SetUp() override { _nh = ros::NodeHandle(); _service = _nh.advertiseService( SERVICE_NAME, [&](auto& req, auto& res) { res.message = "hello"; res.success = req.data; return true; }); } private: ros::NodeHandle _nh; ros::ServiceServer _service; }; TEST(SmokeTest, testConnection) { foxglove_ws::Client wsClient; EXPECT_EQ(std::future_status::ready, wsClient.connect(URI).wait_for(DEFAULT_TIMEOUT)); } TEST(SmokeTest, testSubscription) { // Publish a string message on a latched ros topic const std::string topic_name = "/pub_topic"; ros::NodeHandle nh; auto pub = nh.advertise(topic_name, 10, true); pub.publish(std::string("hello world")); // Connect a few clients and make sure that they receive the correct message const auto clientCount = 3; for (auto i = 0; i < clientCount; ++i) { // Set up a client and subscribe to the channel. auto client = std::make_shared>(); auto channelFuture = foxglove_ws::waitForChannel(client, topic_name); ASSERT_EQ(std::future_status::ready, client->connect(URI).wait_for(ONE_SECOND)); ASSERT_EQ(std::future_status::ready, channelFuture.wait_for(DEFAULT_TIMEOUT)); const foxglove_ws::Channel channel = channelFuture.get(); const foxglove_ws::SubscriptionId subscriptionId = 1; // Subscribe to the channel and confirm that the promise resolves auto msgFuture = waitForChannelMsg(client.get(), subscriptionId); client->subscribe({{subscriptionId, channel.id}}); ASSERT_EQ(std::future_status::ready, msgFuture.wait_for(ONE_SECOND)); const auto msgData = msgFuture.get(); ASSERT_EQ(sizeof(HELLO_WORLD_BINARY), msgData.size()); EXPECT_EQ(0, std::memcmp(HELLO_WORLD_BINARY, msgData.data(), msgData.size())); // Unsubscribe from the channel again. client->unsubscribe({subscriptionId}); } } TEST(SmokeTest, testSubscriptionParallel) { // Publish a string message on a latched ros topic const std::string topic_name = "/pub_topic"; ros::NodeHandle nh; auto pub = nh.advertise(topic_name, 10, true); pub.publish(std::string("hello world")); // Connect a few clients (in parallel) and make sure that they receive the correct message const foxglove_ws::SubscriptionId subscriptionId = 1; auto clients = { std::make_shared>(), std::make_shared>(), std::make_shared>(), }; std::vector>> futures; for (auto client : clients) { futures.push_back(waitForChannelMsg(client.get(), subscriptionId)); } for (auto client : clients) { auto channelFuture = foxglove_ws::waitForChannel(client, topic_name); ASSERT_EQ(std::future_status::ready, client->connect(URI).wait_for(ONE_SECOND)); ASSERT_EQ(std::future_status::ready, channelFuture.wait_for(DEFAULT_TIMEOUT)); const foxglove_ws::Channel channel = channelFuture.get(); client->subscribe({{subscriptionId, channel.id}}); } for (auto& future : futures) { ASSERT_EQ(std::future_status::ready, future.wait_for(DEFAULT_TIMEOUT)); auto msgData = future.get(); ASSERT_EQ(sizeof(HELLO_WORLD_BINARY), msgData.size()); EXPECT_EQ(0, std::memcmp(HELLO_WORLD_BINARY, msgData.data(), msgData.size())); } for (auto client : clients) { client->unsubscribe({subscriptionId}); } } TEST(SmokeTest, testPublishing) { foxglove_ws::Client wsClient; foxglove_ws::ClientAdvertisement advertisement; advertisement.channelId = 1; advertisement.topic = "/foo"; advertisement.encoding = "ros1"; advertisement.schemaName = "std_msgs/String"; // Set up a ROS node with a subscriber ros::NodeHandle nh; std::promise msgPromise; auto msgFuture = msgPromise.get_future(); auto subscriber = nh.subscribe( advertisement.topic, 10, [&msgPromise](const std_msgs::String::ConstPtr& msg) { msgPromise.set_value(msg->data); }); // Set up the client, advertise and publish the binary message ASSERT_EQ(std::future_status::ready, wsClient.connect(URI).wait_for(DEFAULT_TIMEOUT)); wsClient.advertise({advertisement}); std::this_thread::sleep_for(ONE_SECOND); wsClient.publish(advertisement.channelId, HELLO_WORLD_BINARY, sizeof(HELLO_WORLD_BINARY)); wsClient.unadvertise({advertisement.channelId}); // Ensure that we have received the correct message via our ROS subscriber const auto msgResult = msgFuture.wait_for(ONE_SECOND); ASSERT_EQ(std::future_status::ready, msgResult); EXPECT_EQ("hello world", msgFuture.get()); } TEST_F(ParameterTest, testGetAllParams) { const std::string requestId = "req-testGetAllParams"; auto future = foxglove_ws::waitForParameters(_wsClient, requestId); _wsClient->getParameters({}, requestId); ASSERT_EQ(std::future_status::ready, future.wait_for(DEFAULT_TIMEOUT)); std::vector params = future.get(); EXPECT_GE(params.size(), 2UL); } TEST_F(ParameterTest, testGetNonExistingParameters) { const std::string requestId = "req-testGetNonExistingParameters"; auto future = foxglove_ws::waitForParameters(_wsClient, requestId); _wsClient->getParameters( {"/foo_1/non_existing_parameter", "/foo_2/non_existing/nested_parameter"}, requestId); ASSERT_EQ(std::future_status::ready, future.wait_for(DEFAULT_TIMEOUT)); std::vector params = future.get(); EXPECT_TRUE(params.empty()); } TEST_F(ParameterTest, testGetParameters) { const std::string requestId = "req-testGetParameters"; auto future = foxglove_ws::waitForParameters(_wsClient, requestId); _wsClient->getParameters({PARAM_1_NAME, PARAM_2_NAME}, requestId); ASSERT_EQ(std::future_status::ready, future.wait_for(DEFAULT_TIMEOUT)); std::vector params = future.get(); EXPECT_EQ(2UL, params.size()); auto p1Iter = std::find_if(params.begin(), params.end(), [](const auto& param) { return param.getName() == PARAM_1_NAME; }); auto p2Iter = std::find_if(params.begin(), params.end(), [](const auto& param) { return param.getName() == PARAM_2_NAME; }); ASSERT_NE(p1Iter, params.end()); EXPECT_EQ(PARAM_1_DEFAULT_VALUE, p1Iter->getValue().getValue()); ASSERT_NE(p2Iter, params.end()); std::vector double_array_val; const auto array_params = p2Iter->getValue().getValue>(); for (const auto& paramValue : array_params) { double_array_val.push_back(paramValue.getValue()); } EXPECT_EQ(double_array_val, PARAM_2_DEFAULT_VALUE); } TEST_F(ParameterTest, testSetParameters) { const PARAM_1_TYPE newP1value = "world"; const std::vector newP2value = {4.1, 5.5, 6.6}; const std::vector parameters = { foxglove_ws::Parameter(PARAM_1_NAME, newP1value), foxglove_ws::Parameter(PARAM_2_NAME, newP2value), }; _wsClient->setParameters(parameters); const std::string requestId = "req-testSetParameters"; auto future = foxglove_ws::waitForParameters(_wsClient, requestId); _wsClient->getParameters({PARAM_1_NAME, PARAM_2_NAME}, requestId); ASSERT_EQ(std::future_status::ready, future.wait_for(DEFAULT_TIMEOUT)); std::vector params = future.get(); EXPECT_EQ(2UL, params.size()); auto p1Iter = std::find_if(params.begin(), params.end(), [](const auto& param) { return param.getName() == PARAM_1_NAME; }); auto p2Iter = std::find_if(params.begin(), params.end(), [](const auto& param) { return param.getName() == PARAM_2_NAME; }); ASSERT_NE(p1Iter, params.end()); EXPECT_EQ(newP1value, p1Iter->getValue().getValue()); ASSERT_NE(p2Iter, params.end()); std::vector double_array_val; const auto array_params = p2Iter->getValue().getValue>(); for (const auto& paramValue : array_params) { double_array_val.push_back(paramValue.getValue()); } const std::vector expected_value = {4.1, 5.5, 6.6}; EXPECT_EQ(double_array_val, expected_value); } TEST_F(ParameterTest, testSetParametersWithReqId) { const PARAM_1_TYPE newP1value = "world"; const std::vector parameters = { foxglove_ws::Parameter(PARAM_1_NAME, newP1value), }; const std::string requestId = "req-testSetParameters"; auto future = foxglove_ws::waitForParameters(_wsClient, requestId); _wsClient->setParameters(parameters, requestId); ASSERT_EQ(std::future_status::ready, future.wait_for(DEFAULT_TIMEOUT)); std::vector params = future.get(); EXPECT_EQ(1UL, params.size()); } TEST_F(ParameterTest, testUnsetParameter) { const std::vector parameters = { foxglove_ws::Parameter(PARAM_1_NAME), }; const std::string requestId = "req-testUnsetParameter"; auto future = foxglove_ws::waitForParameters(_wsClient, requestId); _wsClient->setParameters(parameters, requestId); ASSERT_EQ(std::future_status::ready, future.wait_for(DEFAULT_TIMEOUT)); std::vector params = future.get(); EXPECT_EQ(0UL, params.size()); } TEST_F(ParameterTest, testParameterSubscription) { auto future = foxglove_ws::waitForParameters(_wsClient); _wsClient->subscribeParameterUpdates({PARAM_1_NAME}); _wsClient->setParameters({foxglove_ws::Parameter(PARAM_1_NAME, "foo")}); ASSERT_EQ(std::future_status::ready, future.wait_for(DEFAULT_TIMEOUT)); std::vector params = future.get(); ASSERT_EQ(1UL, params.size()); EXPECT_EQ(params.front().getName(), PARAM_1_NAME); _wsClient->unsubscribeParameterUpdates({PARAM_1_NAME}); _wsClient->setParameters({foxglove_ws::Parameter(PARAM_1_NAME, "bar")}); future = foxglove_ws::waitForParameters(_wsClient); ASSERT_EQ(std::future_status::timeout, future.wait_for(ONE_SECOND)); } TEST_F(ParameterTest, testGetParametersParallel) { // Connect a few clients (in parallel) and make sure that they all receive parameters auto clients = { std::make_shared>(), std::make_shared>(), std::make_shared>(), }; std::vector>> futures; for (auto client : clients) { futures.push_back( std::async(std::launch::async, [client]() -> std::vector { if (std::future_status::ready == client->connect(URI).wait_for(DEFAULT_TIMEOUT)) { const std::string requestId = "req-123"; auto future = foxglove_ws::waitForParameters(client, requestId); client->getParameters({}, requestId); future.wait_for(DEFAULT_TIMEOUT); if (future.valid()) { return future.get(); } } return {}; })); } for (auto& future : futures) { ASSERT_EQ(std::future_status::ready, future.wait_for(DEFAULT_TIMEOUT)); std::vector parameters; EXPECT_NO_THROW(parameters = future.get()); EXPECT_GE(parameters.size(), 2UL); } } TEST_F(ServiceTest, testCallServiceParallel) { // Connect a few clients (in parallel) and make sure that they can all call the service auto clients = { std::make_shared>(), std::make_shared>(), std::make_shared>(), }; auto serviceFuture = foxglove_ws::waitForService(*clients.begin(), SERVICE_NAME); for (auto client : clients) { ASSERT_EQ(std::future_status::ready, client->connect(URI).wait_for(std::chrono::seconds(5))); } ASSERT_EQ(std::future_status::ready, serviceFuture.wait_for(std::chrono::seconds(5))); const foxglove_ws::Service service = serviceFuture.get(); foxglove_ws::ServiceRequest request; request.serviceId = service.id; request.callId = 123lu; request.encoding = "ros1"; request.data = {1}; // Serialized boolean "True" const std::vector expectedSerializedResponse = {1, 5, 0, 0, 0, 104, 101, 108, 108, 111}; std::vector> futures; for (auto client : clients) { futures.push_back(foxglove_ws::waitForServiceResponse(client)); client->sendServiceRequest(request); } for (auto& future : futures) { ASSERT_EQ(std::future_status::ready, future.wait_for(std::chrono::seconds(5))); foxglove_ws::ServiceResponse response; EXPECT_NO_THROW(response = future.get()); EXPECT_EQ(response.serviceId, request.serviceId); EXPECT_EQ(response.callId, request.callId); EXPECT_EQ(response.encoding, request.encoding); EXPECT_EQ(response.data, expectedSerializedResponse); } } TEST(FetchAssetTest, fetchExistingAsset) { auto wsClient = std::make_shared>(); EXPECT_EQ(std::future_status::ready, wsClient->connect(URI).wait_for(DEFAULT_TIMEOUT)); const auto millisSinceEpoch = std::chrono::duration_cast( std::chrono::system_clock::now().time_since_epoch()); const auto tmpFilePath = boost::filesystem::temp_directory_path() / std::to_string(millisSinceEpoch.count()); constexpr char content[] = "Hello, world"; FILE* tmpAssetFile = std::fopen(tmpFilePath.c_str(), "w"); std::fputs(content, tmpAssetFile); std::fclose(tmpAssetFile); const std::string uri = std::string("file://") + tmpFilePath.string(); const uint32_t requestId = 123; auto future = foxglove_ws::waitForFetchAssetResponse(wsClient); wsClient->fetchAsset(uri, requestId); ASSERT_EQ(std::future_status::ready, future.wait_for(DEFAULT_TIMEOUT)); const foxglove_ws::FetchAssetResponse response = future.get(); EXPECT_EQ(response.requestId, requestId); EXPECT_EQ(response.status, foxglove_ws::FetchAssetStatus::Success); // +1 since NULL terminator is not written to file. ASSERT_EQ(response.data.size() + 1ul, sizeof(content)); EXPECT_EQ(0, std::memcmp(content, response.data.data(), response.data.size())); std::remove(tmpFilePath.c_str()); } TEST(FetchAssetTest, fetchNonExistingAsset) { auto wsClient = std::make_shared>(); EXPECT_EQ(std::future_status::ready, wsClient->connect(URI).wait_for(DEFAULT_TIMEOUT)); const std::string assetId = "file:///foo/bar"; const uint32_t requestId = 456; auto future = foxglove_ws::waitForFetchAssetResponse(wsClient); wsClient->fetchAsset(assetId, requestId); ASSERT_EQ(std::future_status::ready, future.wait_for(DEFAULT_TIMEOUT)); const foxglove_ws::FetchAssetResponse response = future.get(); EXPECT_EQ(response.requestId, requestId); EXPECT_EQ(response.status, foxglove_ws::FetchAssetStatus::Error); EXPECT_FALSE(response.errorMessage.empty()); } // Run all the tests that were declared with TEST() int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "tester"); ros::NodeHandle nh; // Give the server some time to start std::this_thread::sleep_for(std::chrono::seconds(2)); ros::AsyncSpinner spinner(1); spinner.start(); const auto testResult = RUN_ALL_TESTS(); spinner.stop(); return testResult; } ================================================ FILE: scripts/format.py ================================================ import argparse import difflib import os import subprocess import sys from typing import List IGNORE_DIRS = ["build"] EXTENSIONS = [".cpp", ".hpp"] def main(dirs: List[str], fix: bool): changed_paths: List[str] = [] for root in dirs: for dirpath, dirnames, filenames in os.walk(root): # Filter out directories to skip dirnames[:] = filter(lambda d: d not in IGNORE_DIRS, dirnames) for name in filenames: path = os.path.join(dirpath, name) if any(name.endswith(ext) for ext in EXTENSIONS): if fix: subprocess.check_call(["clang-format", "-i", path]) continue stdout = ( subprocess.check_output(["clang-format", path]) .decode("utf-8") .splitlines() ) with open(path, "r") as f: orig = [line.rstrip("\n") for line in f] diff = difflib.unified_diff( orig, stdout, fromfile=path, tofile=f"clang-format {path}", # cspell:disable-line lineterm="", ) had_diff = False for line in diff: had_diff = True print(line) if had_diff: changed_paths.append(path) print("\n") if changed_paths: print(f"{len(changed_paths)} files need to be formatted:") for path in changed_paths: print(f" {path}") return 1 return 0 if __name__ == "__main__": parser = argparse.ArgumentParser( description="Run clang-format and display changed files." ) parser.add_argument( "dirs", help="List of directories to search", nargs="+") parser.add_argument("--fix", action="store_true") args = parser.parse_args() sys.exit(main(**vars(args)))