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