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