Skip to content

Commit c0e7a98

Browse files
ahcordeEricCousineau-TRI
ahcorde
authored and
EricCousineau-TRI
committed
Add RGBD example
1 parent 423dd8f commit c0e7a98

27 files changed

+2025
-2
lines changed

‎.github/config_display.sh

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
#!/bin/sh
2+
set -x
3+
sudo apt-get update && sudo apt-get install libgl1-mesa-glx xvfb -y
4+
Xvfb :1 -screen 0 1024x768x24 > /dev/null 2>&1 &
5+
# give xvfb some time to start
6+
sleep 3
7+
set +x

‎.github/workflows/bazelized_drake_ros.yml

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -69,7 +69,7 @@ jobs:
6969
run: export ROS_DISTRO=humble; bazel build //...
7070
working-directory: drake_ros
7171
- name: Test drake_ros
72-
run: export ROS_DISTRO=humble; bazel test //...
72+
run: ../.github/config_display.sh; export ROS_DISTRO=humble; bazel test //... --test_env=DISPLAY=":1.0"
7373
working-directory: drake_ros
7474
- name: Clean up drake_ros
7575
run: bazel clean
@@ -87,7 +87,7 @@ jobs:
8787
run: bazel build //...
8888
working-directory: drake_ros_examples
8989
- name: Test drake_ros_examples
90-
run: bazel test //...
90+
run: ../.github/config_display.sh; bazel test //... --test_env=DISPLAY=":1.0"
9191
working-directory: drake_ros_examples
9292
- name: Clean up drake_ros_examples
9393
run: bazel clean

‎.github/workflows/main.yml

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@ jobs:
2323
image: robotlocomotion/drake:${{matrix.os_code_name}}
2424
env:
2525
PYTHONPATH:
26+
DISPLAY: ':1.0'
2627
steps:
2728
- uses: actions/checkout@v4
2829
- name: Simplify apt upgrades
@@ -33,6 +34,8 @@ jobs:
3334
use-ros2-testing: "true"
3435
- name: Cope with Python 2 pollution
3536
run: apt-get update && apt-get install -y python-is-python3
37+
- name: Configure display
38+
run: .github/config_display.sh
3639
- name: Build and test all packages
3740
uses: ros-tooling/[email protected]
3841
with:
@@ -51,6 +54,7 @@ jobs:
5154
image: robotlocomotion/drake:${{matrix.os_code_name}}
5255
env:
5356
PYTHONPATH:
57+
DISPLAY: ':1.0'
5458
steps:
5559
- uses: actions/checkout@v4
5660
- name: Simplify apt upgrades
@@ -80,5 +84,6 @@ jobs:
8084
colcon build --event-handlers console_cohesion+
8185
- name: Test all packages
8286
run: |
87+
.github/config_display.sh
8388
. /opt/ros/${{matrix.ros_distro}}/setup.sh
8489
colcon test --event-handlers console_cohesion+

‎drake_ros/CMakeLists.txt

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,7 @@ find_package(rclcpp REQUIRED)
2222
find_package(rosidl_runtime_c REQUIRED)
2323
find_package(rosidl_typesupport_cpp REQUIRED)
2424
find_package(geometry_msgs REQUIRED)
25+
find_package(sensor_msgs REQUIRED)
2526
find_package(tf2_ros REQUIRED)
2627
find_package(tf2_eigen REQUIRED)
2728
find_package(visualization_msgs REQUIRED)
@@ -87,6 +88,7 @@ ament_export_dependencies(geometry_msgs)
8788
ament_export_dependencies(rclcpp)
8889
ament_export_dependencies(rosidl_runtime_c)
8990
ament_export_dependencies(rosidl_typesupport_cpp)
91+
ament_export_dependencies(sensor_msgs)
9092
ament_export_dependencies(tf2_eigen)
9193
ament_export_dependencies(tf2_ros)
9294
ament_export_dependencies(visualization_msgs)

‎drake_ros/core/BUILD.bazel

Lines changed: 41 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,7 @@ cc_library(
1010
"@ros2//:rclcpp_cc",
1111
"@ros2//:rosidl_runtime_c_cc",
1212
"@ros2//:rosidl_typesupport_cpp_cc",
13+
"@ros2//:sensor_msgs_cc",
1314
],
1415
)
1516

@@ -34,6 +35,8 @@ cc_library(
3435
"@drake//multibody/math:spatial_algebra",
3536
"@drake//systems/framework:diagram_builder",
3637
"@drake//systems/framework:leaf_system",
38+
"@drake//systems/sensors:camera_info",
39+
"@drake//systems/sensors:rgbd_sensor",
3740
],
3841
)
3942

@@ -53,6 +56,44 @@ ros_cc_test(
5356
],
5457
)
5558

59+
ros_cc_test(
60+
name = "test_camera_info_system",
61+
size = "small",
62+
srcs = ["test/test_camera_info_system.cc"],
63+
rmw_implementation = "rmw_cyclonedds_cpp",
64+
deps = [
65+
":core",
66+
"@com_google_googletest//:gtest_main",
67+
"@drake//geometry/render_vtk:factory",
68+
"@drake//systems/analysis:simulator",
69+
"@drake//systems/framework:diagram_builder",
70+
"@drake//systems/sensors:camera_info",
71+
"@ros2//:rclcpp_cc",
72+
"@ros2//:sensor_msgs_cc",
73+
"@ros2//resources/rmw_isolation:rmw_isolation_cc",
74+
],
75+
)
76+
77+
ros_cc_test(
78+
name = "test_rgbd_system",
79+
size = "small",
80+
srcs = ["test/test_rgbd_system.cc"],
81+
rmw_implementation = "rmw_cyclonedds_cpp",
82+
deps = [
83+
":core",
84+
"@com_google_googletest//:gtest_main",
85+
"@drake//geometry/render_vtk:factory",
86+
"@drake//multibody/plant:multibody_plant_config_functions",
87+
"@drake//systems/analysis:simulator",
88+
"@drake//systems/framework:diagram_builder",
89+
"@drake//systems/sensors:camera_info",
90+
"@drake//systems/sensors:rgbd_sensor",
91+
"@ros2//:rclcpp_cc",
92+
"@ros2//:sensor_msgs_cc",
93+
"@ros2//resources/rmw_isolation:rmw_isolation_cc",
94+
],
95+
)
96+
5697
ros_cc_test(
5798
name = "test_clock_system",
5899
size = "small",

‎drake_ros/core/CMakeLists.txt

Lines changed: 26 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,12 @@
11
set(HEADERS
2+
"camera_info_system.h"
23
"clock_system.h"
34
"drake_ros.h"
45
"geometry_conversions.h"
56
"geometry_conversions_pybind.h"
67
"ros_idl_pybind.h"
78
"publisher.h"
9+
"rgbd_system.h"
810
"ros_interface_system.h"
911
"ros_publisher_system.h"
1012
"ros_subscriber_system.h"
@@ -20,10 +22,12 @@ foreach(hdr ${HEADERS})
2022
endforeach()
2123

2224
add_library(drake_ros_core
25+
camera_info_system.cc
2326
clock_system.cc
2427
drake_ros.cc
2528
geometry_conversions.cc
2629
publisher.cc
30+
rgbd_system.cc
2731
ros_interface_system.cc
2832
ros_publisher_system.cc
2933
ros_subscriber_system.cc
@@ -37,6 +41,7 @@ target_link_libraries(drake_ros_core PUBLIC
3741
rclcpp::rclcpp
3842
rosidl_runtime_c::rosidl_runtime_c
3943
rosidl_typesupport_cpp::rosidl_typesupport_cpp
44+
${sensor_msgs_TARGETS}
4045
)
4146

4247
target_include_directories(drake_ros_core
@@ -88,6 +93,17 @@ if(BUILD_TESTING)
8893
ament_add_gtest(test_geometry_conversions test/test_geometry_conversions.cc)
8994
target_link_libraries(test_geometry_conversions drake_ros_core)
9095

96+
97+
ament_add_gtest(test_rgbd_system test/test_rgbd_system.cc)
98+
target_compile_definitions(test_rgbd_system
99+
PRIVATE
100+
# We do not expose `rmw_isoliation` via CMake.
101+
_TEST_DISABLE_RMW_ISOLATION
102+
)
103+
target_link_libraries(test_rgbd_system
104+
drake_ros_core
105+
)
106+
91107
ament_add_gtest(test_clock_system test/test_clock_system.cc)
92108
target_compile_definitions(test_clock_system
93109
PRIVATE
@@ -97,4 +113,14 @@ if(BUILD_TESTING)
97113
target_link_libraries(test_clock_system
98114
drake_ros_core
99115
)
116+
117+
ament_add_gtest(test_camera_info_system test/test_camera_info_system.cc)
118+
target_compile_definitions(test_camera_info_system
119+
PRIVATE
120+
# We do not expose `rmw_isoliation` via CMake.
121+
_TEST_DISABLE_RMW_ISOLATION
122+
)
123+
target_link_libraries(test_camera_info_system
124+
drake_ros_core
125+
)
100126
endif()

‎drake_ros/core/camera_info_system.cc

Lines changed: 85 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,85 @@
1+
#include "drake_ros/core/camera_info_system.h"
2+
3+
#include <rclcpp/time.hpp>
4+
5+
using drake_ros::core::CameraInfoSystem;
6+
using drake_ros::core::RosPublisherSystem;
7+
8+
CameraInfoSystem::CameraInfoSystem() : camera_info(1, 1, 1, 1, 0.5, 0.5) {
9+
DeclareAbstractOutputPort("CameraInfoSystem",
10+
&CameraInfoSystem::CalcCameraInfo);
11+
}
12+
13+
CameraInfoSystem::~CameraInfoSystem() {}
14+
15+
void CameraInfoSystem::CalcCameraInfo(
16+
const drake::systems::Context<double>& context,
17+
sensor_msgs::msg::CameraInfo* output_value) const {
18+
rclcpp::Time now{0, 0, RCL_ROS_TIME};
19+
now += rclcpp::Duration::from_seconds(context.get_time());
20+
output_value->header.stamp = now;
21+
output_value->header.frame_id = "CartPole/camera_optical";
22+
23+
output_value->height = this->camera_info.height();
24+
output_value->width = this->camera_info.width();
25+
output_value->distortion_model = "plumb_bob";
26+
27+
// distortion isn't supported. Setting this values to zero
28+
output_value->d.push_back(0);
29+
output_value->d.push_back(0);
30+
output_value->d.push_back(0);
31+
output_value->d.push_back(0);
32+
output_value->d.push_back(0);
33+
34+
output_value->r[0] = 1;
35+
output_value->r[1] = 0;
36+
output_value->r[2] = 0;
37+
output_value->r[3] = 0;
38+
output_value->r[4] = 1;
39+
output_value->r[5] = 0;
40+
output_value->r[6] = 0;
41+
output_value->r[7] = 0;
42+
output_value->r[8] = 1;
43+
44+
// │ f_x 0 c_x │
45+
// K = │ 0 f_y c_y │
46+
// │ 0 0 1 │
47+
48+
// │ f_x 0 c_x Tx│
49+
// P = │ 0 f_y c_y Ty│
50+
// │ 0 0 1 0│
51+
output_value->k[0] = this->camera_info.focal_x();
52+
output_value->k[2] = this->camera_info.center_x();
53+
output_value->k[4] = this->camera_info.focal_y();
54+
output_value->k[5] = this->camera_info.center_y();
55+
output_value->k[8] = 1.0;
56+
57+
output_value->p[0] = this->camera_info.focal_x();
58+
output_value->p[2] = this->camera_info.center_x();
59+
output_value->p[3] = 0;
60+
output_value->p[5] = this->camera_info.focal_y();
61+
output_value->p[6] = this->camera_info.center_y();
62+
output_value->p[10] = 1.0;
63+
}
64+
65+
void CameraInfoSystem::SetCameraInfo(const CameraInfo& _camera_info) {
66+
this->camera_info = _camera_info;
67+
}
68+
69+
std::tuple<CameraInfoSystem*, RosPublisherSystem*>
70+
CameraInfoSystem::AddToBuilder(
71+
drake::systems::DiagramBuilder<double>* builder, DrakeRos* ros,
72+
const std::string& topic_name, const rclcpp::QoS& qos,
73+
const std::unordered_set<drake::systems::TriggerType>& publish_triggers,
74+
double publish_period) {
75+
auto* camera_info_system = builder->AddSystem<CameraInfoSystem>();
76+
77+
auto* pub_system =
78+
builder->AddSystem(RosPublisherSystem::Make<sensor_msgs::msg::CameraInfo>(
79+
topic_name, qos, ros, publish_triggers, publish_period));
80+
81+
builder->Connect(camera_info_system->get_output_port(),
82+
pub_system->get_input_port());
83+
84+
return {camera_info_system, pub_system};
85+
}

‎drake_ros/core/camera_info_system.h

Lines changed: 53 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,53 @@
1+
#pragma once
2+
3+
#include <memory>
4+
#include <string>
5+
#include <tuple>
6+
#include <unordered_set>
7+
8+
#include <drake/systems/framework/diagram_builder.h>
9+
#include <drake/systems/framework/leaf_system.h>
10+
#include <drake/systems/sensors/camera_info.h>
11+
#include <drake_ros/core/ros_publisher_system.h>
12+
#include <rclcpp/qos.hpp>
13+
#include <sensor_msgs/msg/camera_info.hpp>
14+
15+
using drake::systems::sensors::CameraInfo;
16+
17+
namespace drake_ros::core {
18+
/** A system that converts drake's camera info to a sensor_msgs/msg/CameraInfo
19+
* message.
20+
*/
21+
class CameraInfoSystem : public drake::systems::LeafSystem<double> {
22+
public:
23+
/** A constructor for the camera info system.
24+
*/
25+
CameraInfoSystem();
26+
27+
~CameraInfoSystem() override;
28+
29+
void SetCameraInfo(const CameraInfo& _camera_info);
30+
31+
/** Add a CameraInfoSystem and RosPublisherSystem to a diagram builder.
32+
*
33+
* This adds both a CameraInfoSystem and a RosPublisherSystem that publishes
34+
* time to a `/image/camera_info` topic. All nodes should have their
35+
* `use_sim_time` parameter set to `True` so they use the published topic as
36+
* their source of time.
37+
*/
38+
static std::tuple<CameraInfoSystem*, RosPublisherSystem*> AddToBuilder(
39+
drake::systems::DiagramBuilder<double>* builder, DrakeRos* ros,
40+
const std::string& topic_name = "/image/camera_info",
41+
const rclcpp::QoS& qos = rclcpp::SystemDefaultsQoS(),
42+
const std::unordered_set<drake::systems::TriggerType>& publish_triggers =
43+
RosPublisherSystem::kDefaultTriggerTypes,
44+
double publish_period = 0.0);
45+
46+
private:
47+
CameraInfo camera_info;
48+
49+
protected:
50+
void CalcCameraInfo(const drake::systems::Context<double>& context,
51+
sensor_msgs::msg::CameraInfo* output_value) const;
52+
};
53+
} // namespace drake_ros::core

0 commit comments

Comments
 (0)