[WIP] ROS 2 JSON publishing support

.devcontainer/Dockerfile

@@ -10,15 +10,16 @@ ENV CC=clang
 ENV CXX=clang++
 
 # Set environment and working directory
-ENV ROS_WS /ros2_ws
+ENV ROS_WS /ros_ws
 WORKDIR $ROS_WS
 
 # Install system dependencies
 RUN apt-get update && apt-get install -y --no-install-recommends \
   bash-completion \
   build-essential \
-  clang \
-  clang-format \
+  clang-15 \
+  clangd-15 \
+  clang-format-15 \
   curl \
   gdb \
   git \
@@ -33,6 +34,11 @@ RUN apt-get update && apt-get install -y --no-install-recommends \
   sudo \
   && rm -rf /var/lib/apt/lists/*
 
+# Set clang*-15 as the default
+RUN update-alternatives --install /usr/bin/clang clang /usr/bin/clang-15 100
+RUN update-alternatives --install /usr/bin/clangd clangd /usr/bin/clangd-15 100
+RUN update-alternatives --install /usr/bin/clang-format clang-format /usr/bin/clang-format-15 100
+
 # Add package.xml so we can install package dependencies
 COPY package.xml src/ros-foxglove-bridge/
 
@@ -60,11 +66,14 @@ RUN groupadd --gid $USER_GID $USERNAME \
     && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME \
     && chmod 0440 /etc/sudoers.d/$USERNAME
 
+# Make the workspace directory writable by the non-root user
+RUN chown -R $USERNAME:$USERNAME $ROS_WS
+
 USER $USERNAME
 
 # Add aliases to .bashrc
 RUN echo $'\
-alias ros2_build_debug="source /opt/ros/humble/setup.bash && colcon build --cmake-args -DCMAKE_BUILD_TYPE=Debug"\n\
-alias ros2_build_release="source /opt/ros/humble/setup.bash && colcon build --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo"\n\
+alias ros2_build_debug="source /opt/ros/$ROS_DISTRO/setup.bash && colcon build --cmake-args -DCMAKE_BUILD_TYPE=Debug -DCMAKE_EXPORT_COMPILE_COMMANDS:BOOL=ON"\n\
+alias ros2_build_release="source /opt/ros/$ROS_DISTRO/setup.bash && colcon build --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_EXPORT_COMPILE_COMMANDS:BOOL=ON"\n\
 alias ros2_foxglove_bridge="source /ros_ws/install_ros2/setup.bash && ros2 run foxglove_bridge foxglove_bridge --ros-args --log-level debug --log-level rcl:=INFO"\n\
 ' >> ~/.bashrc

.devcontainer/devcontainer.json

@@ -4,15 +4,19 @@
   "workspaceFolder": "/ros_ws/src/ros-foxglove-bridge",
   "workspaceMount": "source=${localWorkspaceFolder},target=/ros_ws/src/ros-foxglove-bridge,type=bind,consistency=cached",
   "dockerFile": "./Dockerfile",
+  "build": {
+    "args": {
+      "ROS_DISTRIBUTION": "humble"
+    }
+  },
   "context": "..",
   "forwardPorts": [
     8765
   ],
   "customizations": {
     "vscode": {
       "extensions": [
-        "ms-vscode.cpptools",
-        "xaver.clang-format",
+        "llvm-vs-code-extensions.vscode-clangd",
         "twxs.cmake"
       ],
       "settings": {

.vscode/settings.json

@@ -1,7 +1,7 @@
 // -*- jsonc -*-
 {
-  "C_Cpp.autoAddFileAssociations": false,
-  "editor.defaultFormatter": "xaver.clang-format",
+  "clangd.arguments": ["--compile-commands-dir=/ros_ws/build_ros2"],
+  "editor.defaultFormatter": "llvm-vs-code-extensions.vscode-clangd",
   "editor.formatOnSave": true,
   "files.eol": "\n",
   "files.insertFinalNewline": true,

CMakeLists.txt

@@ -142,12 +142,19 @@ elseif("$ENV{ROS_VERSION}" STREQUAL "2")
     set(ROS_BUILD_TYPE "ament_cmake")
 
     find_package(ament_cmake REQUIRED)
-    find_package(rosgraph_msgs REQUIRED)
+    find_package(ament_index_cpp REQUIRED)
     find_package(rclcpp REQUIRED)
+    find_package(rclcpp_action REQUIRED)
     find_package(rclcpp_components REQUIRED)
+    find_package(rcpputils REQUIRED)
     find_package(resource_retriever REQUIRED)
+    find_package(rosgraph_msgs REQUIRED)
+    find_package(rosidl_runtime_cpp REQUIRED)
+    find_package(rosidl_typesupport_cpp REQUIRED)
+    find_package(rosidl_typesupport_introspection_cpp REQUIRED)
 
     add_library(foxglove_bridge_component SHARED
+      ros2_foxglove_bridge/src/json_to_ros.cpp
       ros2_foxglove_bridge/src/message_definition_cache.cpp
       ros2_foxglove_bridge/src/param_utils.cpp
       ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp
@@ -160,8 +167,49 @@ elseif("$ENV{ROS_VERSION}" STREQUAL "2")
         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/ros2_foxglove_bridge/include>
         $<INSTALL_INTERFACE:include>
     )
+
+    # For ROS 2 Humble only, build and link the vendored ros2_babel_fish library
+    if("$ENV{ROS_DISTRO}" STREQUAL "humble")
+      message(STATUS "Building vendored ros2_babel_fish")
+      set(ROS2_BABEL_FISH_DIR ${CMAKE_CURRENT_SOURCE_DIR}/ros2_foxglove_bridge/src/ros2_babel_fish)
+      set(ROS2_BABEL_FISH_SOURCES
+        ${ROS2_BABEL_FISH_DIR}/detail/babel_fish_action_client.cpp
+        ${ROS2_BABEL_FISH_DIR}/detail/babel_fish_publisher.cpp
+        ${ROS2_BABEL_FISH_DIR}/detail/babel_fish_service.cpp
+        ${ROS2_BABEL_FISH_DIR}/detail/babel_fish_service_client.cpp
+        ${ROS2_BABEL_FISH_DIR}/detail/babel_fish_subscription.cpp
+        ${ROS2_BABEL_FISH_DIR}/idl/providers/local_type_support_provider.cpp
+        ${ROS2_BABEL_FISH_DIR}/idl/serialization.cpp
+        ${ROS2_BABEL_FISH_DIR}/idl/type_support_provider.cpp
+        ${ROS2_BABEL_FISH_DIR}/messages/array_message.cpp
+        ${ROS2_BABEL_FISH_DIR}/messages/compound_message.cpp
+        ${ROS2_BABEL_FISH_DIR}/messages/message.cpp
+        ${ROS2_BABEL_FISH_DIR}/babel_fish.cpp
+        ${ROS2_BABEL_FISH_DIR}/detail/topic.cpp
+      )
+
+      add_library(ros2_babel_fish STATIC ${ROS2_BABEL_FISH_SOURCES})
+      set_target_properties(ros2_babel_fish PROPERTIES POSITION_INDEPENDENT_CODE ON)
+      target_include_directories(ros2_babel_fish PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/ros2_foxglove_bridge/include)
+      target_compile_definitions(ros2_babel_fish PUBLIC RCLCPP_VERSION_MAJOR=${rclcpp_VERSION_MAJOR})
+      ament_target_dependencies(ros2_babel_fish
+        ament_index_cpp
+        rclcpp
+        rclcpp_action
+        rcpputils
+        rosidl_runtime_cpp
+        rosidl_typesupport_cpp
+        rosidl_typesupport_introspection_cpp
+      )
+
+      target_link_libraries(foxglove_bridge_component foxglove_bridge_base ros2_babel_fish)
+      target_compile_definitions(foxglove_bridge_component PUBLIC ENABLE_JSON_MESSAGES=1)
+    elseif(${rclcpp_VERSION_MAJOR} GREATER 20)
+      # NOTE: For ROS 2 Iron and later, we can use native rclcpp for dynamic message serialization
+      # ...
+    endif()
+
     ament_target_dependencies(foxglove_bridge_component ament_index_cpp rclcpp rclcpp_components resource_retriever rosgraph_msgs)
-    target_link_libraries(foxglove_bridge_component foxglove_bridge_base)
     rclcpp_components_register_nodes(foxglove_bridge_component "foxglove_bridge::FoxgloveBridge")
     enable_strict_compiler_warnings(foxglove_bridge_component)
     add_executable(foxglove_bridge
@@ -231,6 +279,13 @@ elseif(ROS_BUILD_TYPE STREQUAL "ament_cmake")
     target_link_libraries(base64_test foxglove_bridge_base)
     enable_strict_compiler_warnings(base64_test)
 
+    if("$ENV{ROS_DISTRO}" STREQUAL "humble")
+      ament_add_gtest(json_to_ros_test ros2_foxglove_bridge/tests/json_to_ros_test.cpp)
+      ament_target_dependencies(json_to_ros_test rclcpp rclcpp_components std_msgs)
+      target_link_libraries(json_to_ros_test foxglove_bridge_component)
+      enable_strict_compiler_warnings(json_to_ros_test)
+    endif()
+
     ament_add_gtest(smoke_test ros2_foxglove_bridge/tests/smoke_test.cpp)
     ament_target_dependencies(smoke_test rclcpp rclcpp_components std_msgs std_srvs)
     target_link_libraries(smoke_test foxglove_bridge_base)

Dockerfile.ros1

@@ -10,7 +10,7 @@ ENV CC=clang
 ENV CXX=clang++
 
 # Set environment and working directory
-ENV ROS_WS /ros1_ws
+ENV ROS_WS /ros_ws
 WORKDIR $ROS_WS
 
 # Add package.xml so we can install package dependencies.

Dockerfile.ros2

@@ -10,7 +10,7 @@ ENV CC=clang
 ENV CXX=clang++
 
 # Set environment and working directory
-ENV ROS_WS /ros2_ws
+ENV ROS_WS /ros_ws
 WORKDIR $ROS_WS
 
 # Install system dependencies

foxglove_bridge_base/include/foxglove_bridge/websocket_server.hpp

@@ -759,10 +759,11 @@ inline void Server<ServerConfiguration>::handleBinaryMessage(ConnHandle hdl, Mes
                                           length,
                                           data};
         _handlers.clientMessageHandler(clientMessage, hdl);
-      } catch (const ServiceError& e) {
+      } catch (ServiceError const& e) {
         sendStatusAndLogMsg(hdl, StatusLevel::Error, e.what());
-      } catch (...) {
-        sendStatusAndLogMsg(hdl, StatusLevel::Error, "callService: Failed to execute handler");
+      } catch (std::exception const& e) {
+        sendStatusAndLogMsg(hdl, StatusLevel::Error,
+                            std::string{"Failed to execute client message handler: "} + e.what());
       }
     } break;
     case ClientBinaryOpcode::SERVICE_CALL_REQUEST: {

ros2_foxglove_bridge/include/foxglove_bridge/json_to_ros.hpp

@@ -0,0 +1,21 @@
+#pragma once
+
+#include <optional>
+
+#include <ros2_babel_fish/babel_fish.hpp>
+
+namespace foxglove_bridge {
+
+/**
+ * Convert a JSON-serialized message with a given named schema to a ROS message
+ * using ros2_babel_fish. The output message is allocated as a shared pointer
+ * and assigned to the outputMessage argument. The return value is an optional
+ * exception, which if set indicates that an error occurred during the
+ * conversion and `outputMessage` is not valid.
+ */
+std::optional<std::exception> jsonMessageToRos(
+  const std::string_view jsonMessage, const std::string& schemaName,
+  ros2_babel_fish::BabelFish::SharedPtr babelFish,
+  ros2_babel_fish::CompoundMessage::SharedPtr& outputMessage);
+
+}  // namespace foxglove_bridge

ros2_foxglove_bridge/include/foxglove_bridge/ros2_foxglove_bridge.hpp

@@ -20,6 +20,10 @@
 #include <foxglove_bridge/server_factory.hpp>
 #include <foxglove_bridge/utils.hpp>
 
+#ifdef ENABLE_JSON_MESSAGES
+#include <ros2_babel_fish/babel_fish.hpp>
+#endif
+
 namespace foxglove_bridge {
 
 using ConnectionHandle = websocketpp::connection_hdl;
@@ -82,6 +86,9 @@ class FoxgloveBridge : public rclcpp::Node {
   std::atomic<bool> _subscribeGraphUpdates = false;
   bool _includeHidden = false;
   std::unique_ptr<foxglove::CallbackQueue> _fetchAssetQueue;
+#ifdef ENABLE_JSON_MESSAGES
+  ros2_babel_fish::BabelFish::SharedPtr _babelFish;
+#endif
 
   void subscribeConnectionGraph(bool subscribe);