Add RGBD example

.github/config_display.sh

@@ -0,0 +1,7 @@
+#!/bin/sh
+set -x
+sudo apt-get update && sudo apt-get install libgl1-mesa-glx xvfb -y
+Xvfb :1 -screen 0 1024x768x24 > /dev/null 2>&1 &
+# give xvfb some time to start
+sleep 3
+set +x

.github/workflows/bazelized_drake_ros.yml

@@ -69,7 +69,7 @@ jobs:
         run: export ROS_DISTRO=humble; bazel build //...
         working-directory: drake_ros
       - name: Test drake_ros
-        run: export ROS_DISTRO=humble; bazel test //...
+        run: ../.github/config_display.sh; export ROS_DISTRO=humble; bazel test //... --test_env=DISPLAY=":1.0"
         working-directory: drake_ros
       - name: Clean up drake_ros
         run: bazel clean
@@ -87,7 +87,7 @@ jobs:
         run: bazel build //...
         working-directory: drake_ros_examples
       - name: Test drake_ros_examples
-        run: bazel test //...
+        run: ../.github/config_display.sh; bazel test //... --test_env=DISPLAY=":1.0"
         working-directory: drake_ros_examples
       - name: Clean up drake_ros_examples
         run: bazel clean

.github/workflows/main.yml

@@ -23,6 +23,7 @@ jobs:
       image: robotlocomotion/drake:${{matrix.os_code_name}}
     env:
       PYTHONPATH:
+      DISPLAY: ':1.0'
     steps:
       - uses: actions/checkout@v4
       - name: Simplify apt upgrades
@@ -33,6 +34,8 @@ jobs:
           use-ros2-testing: "true"
       - name: Cope with Python 2 pollution
         run: apt-get update && apt-get install -y python-is-python3
+      - name: Configure display
+        run: .github/config_display.sh
       - name: Build and test all packages
         uses: ros-tooling/[email protected]
         with:
@@ -51,6 +54,7 @@ jobs:
       image: robotlocomotion/drake:${{matrix.os_code_name}}
     env:
       PYTHONPATH:
+      DISPLAY: ':1.0'
     steps:
       - uses: actions/checkout@v4
       - name: Simplify apt upgrades
@@ -80,5 +84,6 @@ jobs:
           colcon build --event-handlers console_cohesion+
       - name: Test all packages
         run: |
+          .github/config_display.sh
           . /opt/ros/${{matrix.ros_distro}}/setup.sh
           colcon test --event-handlers console_cohesion+

drake_ros/CMakeLists.txt

@@ -22,6 +22,7 @@ find_package(rclcpp REQUIRED)
 find_package(rosidl_runtime_c REQUIRED)
 find_package(rosidl_typesupport_cpp REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(tf2_eigen REQUIRED)
 find_package(visualization_msgs REQUIRED)
@@ -87,6 +88,7 @@ ament_export_dependencies(geometry_msgs)
 ament_export_dependencies(rclcpp)
 ament_export_dependencies(rosidl_runtime_c)
 ament_export_dependencies(rosidl_typesupport_cpp)
+ament_export_dependencies(sensor_msgs)
 ament_export_dependencies(tf2_eigen)
 ament_export_dependencies(tf2_ros)
 ament_export_dependencies(visualization_msgs)

drake_ros/core/BUILD.bazel

@@ -10,6 +10,7 @@ cc_library(
         "@ros2//:rclcpp_cc",
         "@ros2//:rosidl_runtime_c_cc",
         "@ros2//:rosidl_typesupport_cpp_cc",
+        "@ros2//:sensor_msgs_cc",
     ],
 )
 
@@ -34,6 +35,8 @@ cc_library(
         "@drake//multibody/math:spatial_algebra",
         "@drake//systems/framework:diagram_builder",
         "@drake//systems/framework:leaf_system",
+        "@drake//systems/sensors:camera_info",
+        "@drake//systems/sensors:rgbd_sensor",
     ],
 )
 
@@ -53,6 +56,44 @@ ros_cc_test(
     ],
 )
 
+ros_cc_test(
+    name = "test_camera_info_system",
+    size = "small",
+    srcs = ["test/test_camera_info_system.cc"],
+    rmw_implementation = "rmw_cyclonedds_cpp",
+    deps = [
+        ":core",
+        "@com_google_googletest//:gtest_main",
+        "@drake//geometry/render_vtk:factory",
+        "@drake//systems/analysis:simulator",
+        "@drake//systems/framework:diagram_builder",
+        "@drake//systems/sensors:camera_info",
+        "@ros2//:rclcpp_cc",
+        "@ros2//:sensor_msgs_cc",
+        "@ros2//resources/rmw_isolation:rmw_isolation_cc",
+    ],
+)
+
+ros_cc_test(
+    name = "test_rgbd_system",
+    size = "small",
+    srcs = ["test/test_rgbd_system.cc"],
+    rmw_implementation = "rmw_cyclonedds_cpp",
+    deps = [
+        ":core",
+        "@com_google_googletest//:gtest_main",
+        "@drake//geometry/render_vtk:factory",
+        "@drake//multibody/plant:multibody_plant_config_functions",
+        "@drake//systems/analysis:simulator",
+        "@drake//systems/framework:diagram_builder",
+        "@drake//systems/sensors:camera_info",
+        "@drake//systems/sensors:rgbd_sensor",
+        "@ros2//:rclcpp_cc",
+        "@ros2//:sensor_msgs_cc",
+        "@ros2//resources/rmw_isolation:rmw_isolation_cc",
+    ],
+)
+
 ros_cc_test(
     name = "test_clock_system",
     size = "small",

drake_ros/core/CMakeLists.txt

@@ -1,10 +1,12 @@
 set(HEADERS
+  "camera_info_system.h"
   "clock_system.h"
   "drake_ros.h"
   "geometry_conversions.h"
   "geometry_conversions_pybind.h"
   "ros_idl_pybind.h"
   "publisher.h"
+  "rgbd_system.h"
   "ros_interface_system.h"
   "ros_publisher_system.h"
   "ros_subscriber_system.h"
@@ -20,10 +22,12 @@ foreach(hdr ${HEADERS})
 endforeach()
 
 add_library(drake_ros_core
+  camera_info_system.cc
   clock_system.cc
   drake_ros.cc
   geometry_conversions.cc
   publisher.cc
+  rgbd_system.cc
   ros_interface_system.cc
   ros_publisher_system.cc
   ros_subscriber_system.cc
@@ -37,6 +41,7 @@ target_link_libraries(drake_ros_core PUBLIC
   rclcpp::rclcpp
   rosidl_runtime_c::rosidl_runtime_c
   rosidl_typesupport_cpp::rosidl_typesupport_cpp
+  ${sensor_msgs_TARGETS}
 )
 
 target_include_directories(drake_ros_core
@@ -88,6 +93,17 @@ if(BUILD_TESTING)
   ament_add_gtest(test_geometry_conversions test/test_geometry_conversions.cc)
   target_link_libraries(test_geometry_conversions drake_ros_core)
 
+
+  ament_add_gtest(test_rgbd_system test/test_rgbd_system.cc)
+  target_compile_definitions(test_rgbd_system
+    PRIVATE
+    # We do not expose `rmw_isoliation` via CMake.
+    _TEST_DISABLE_RMW_ISOLATION
+  )
+  target_link_libraries(test_rgbd_system
+    drake_ros_core
+  )
+
   ament_add_gtest(test_clock_system test/test_clock_system.cc)
   target_compile_definitions(test_clock_system
     PRIVATE
@@ -97,4 +113,14 @@ if(BUILD_TESTING)
   target_link_libraries(test_clock_system
     drake_ros_core
   )
+
+  ament_add_gtest(test_camera_info_system test/test_camera_info_system.cc)
+  target_compile_definitions(test_camera_info_system
+    PRIVATE
+    # We do not expose `rmw_isoliation` via CMake.
+    _TEST_DISABLE_RMW_ISOLATION
+  )
+  target_link_libraries(test_camera_info_system
+    drake_ros_core
+  )
 endif()

drake_ros/core/camera_info_system.cc

@@ -0,0 +1,85 @@
+#include "drake_ros/core/camera_info_system.h"
+
+#include <rclcpp/time.hpp>
+
+using drake_ros::core::CameraInfoSystem;
+using drake_ros::core::RosPublisherSystem;
+
+CameraInfoSystem::CameraInfoSystem() : camera_info(1, 1, 1, 1, 0.5, 0.5) {
+  DeclareAbstractOutputPort("CameraInfoSystem",
+                            &CameraInfoSystem::CalcCameraInfo);
+}
+
+CameraInfoSystem::~CameraInfoSystem() {}
+
+void CameraInfoSystem::CalcCameraInfo(
+    const drake::systems::Context<double>& context,
+    sensor_msgs::msg::CameraInfo* output_value) const {
+  rclcpp::Time now{0, 0, RCL_ROS_TIME};
+  now += rclcpp::Duration::from_seconds(context.get_time());
+  output_value->header.stamp = now;
+  output_value->header.frame_id = "CartPole/camera_optical";
+
+  output_value->height = this->camera_info.height();
+  output_value->width = this->camera_info.width();
+  output_value->distortion_model = "plumb_bob";
+
+  // distortion isn't supported. Setting this values to zero
+  output_value->d.push_back(0);
+  output_value->d.push_back(0);
+  output_value->d.push_back(0);
+  output_value->d.push_back(0);
+  output_value->d.push_back(0);
+
+  output_value->r[0] = 1;
+  output_value->r[1] = 0;
+  output_value->r[2] = 0;
+  output_value->r[3] = 0;
+  output_value->r[4] = 1;
+  output_value->r[5] = 0;
+  output_value->r[6] = 0;
+  output_value->r[7] = 0;
+  output_value->r[8] = 1;
+
+  //      │ f_x  0    c_x │
+  //  K = │ 0    f_y  c_y │
+  //      │ 0    0    1   │
+
+  //      │ f_x  0    c_x Tx│
+  //  P = │ 0    f_y  c_y Ty│
+  //      │ 0    0    1   0│
+  output_value->k[0] = this->camera_info.focal_x();
+  output_value->k[2] = this->camera_info.center_x();
+  output_value->k[4] = this->camera_info.focal_y();
+  output_value->k[5] = this->camera_info.center_y();
+  output_value->k[8] = 1.0;
+
+  output_value->p[0] = this->camera_info.focal_x();
+  output_value->p[2] = this->camera_info.center_x();
+  output_value->p[3] = 0;
+  output_value->p[5] = this->camera_info.focal_y();
+  output_value->p[6] = this->camera_info.center_y();
+  output_value->p[10] = 1.0;
+}
+
+void CameraInfoSystem::SetCameraInfo(const CameraInfo& _camera_info) {
+  this->camera_info = _camera_info;
+}
+
+std::tuple<CameraInfoSystem*, RosPublisherSystem*>
+CameraInfoSystem::AddToBuilder(
+    drake::systems::DiagramBuilder<double>* builder, DrakeRos* ros,
+    const std::string& topic_name, const rclcpp::QoS& qos,
+    const std::unordered_set<drake::systems::TriggerType>& publish_triggers,
+    double publish_period) {
+  auto* camera_info_system = builder->AddSystem<CameraInfoSystem>();
+
+  auto* pub_system =
+      builder->AddSystem(RosPublisherSystem::Make<sensor_msgs::msg::CameraInfo>(
+          topic_name, qos, ros, publish_triggers, publish_period));
+
+  builder->Connect(camera_info_system->get_output_port(),
+                   pub_system->get_input_port());
+
+  return {camera_info_system, pub_system};
+}

drake_ros/core/camera_info_system.h

@@ -0,0 +1,53 @@
+#pragma once
+
+#include <memory>
+#include <string>
+#include <tuple>
+#include <unordered_set>
+
+#include <drake/systems/framework/diagram_builder.h>
+#include <drake/systems/framework/leaf_system.h>
+#include <drake/systems/sensors/camera_info.h>
+#include <drake_ros/core/ros_publisher_system.h>
+#include <rclcpp/qos.hpp>
+#include <sensor_msgs/msg/camera_info.hpp>
+
+using drake::systems::sensors::CameraInfo;
+
+namespace drake_ros::core {
+/** A system that converts drake's camera info to a sensor_msgs/msg/CameraInfo
+ * message.
+ */
+class CameraInfoSystem : public drake::systems::LeafSystem<double> {
+ public:
+  /** A constructor for the camera info system.
+   */
+  CameraInfoSystem();
+
+  ~CameraInfoSystem() override;
+
+  void SetCameraInfo(const CameraInfo& _camera_info);
+
+  /** Add a CameraInfoSystem and RosPublisherSystem to a diagram builder.
+   *
+   * This adds both a CameraInfoSystem and a RosPublisherSystem that publishes
+   * time to a `/image/camera_info` topic. All nodes should have their
+   * `use_sim_time` parameter set to `True` so they use the published topic as
+   * their source of time.
+   */
+  static std::tuple<CameraInfoSystem*, RosPublisherSystem*> AddToBuilder(
+      drake::systems::DiagramBuilder<double>* builder, DrakeRos* ros,
+      const std::string& topic_name = "/image/camera_info",
+      const rclcpp::QoS& qos = rclcpp::SystemDefaultsQoS(),
+      const std::unordered_set<drake::systems::TriggerType>& publish_triggers =
+          RosPublisherSystem::kDefaultTriggerTypes,
+      double publish_period = 0.0);
+
+ private:
+  CameraInfo camera_info;
+
+ protected:
+  void CalcCameraInfo(const drake::systems::Context<double>& context,
+                      sensor_msgs::msg::CameraInfo* output_value) const;
+};
+}  // namespace drake_ros::core