From c0e7a98f8a7a554426c4f5a4a6aad8e75e7be292 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 10 Jun 2024 11:24:10 -0400 Subject: [PATCH] Add RGBD example --- .github/config_display.sh | 7 + .github/workflows/bazelized_drake_ros.yml | 4 +- .github/workflows/main.yml | 5 + drake_ros/CMakeLists.txt | 2 + drake_ros/core/BUILD.bazel | 41 +++ drake_ros/core/CMakeLists.txt | 26 ++ drake_ros/core/camera_info_system.cc | 85 +++++ drake_ros/core/camera_info_system.h | 53 ++++ drake_ros/core/rgbd_system.cc | 296 ++++++++++++++++++ drake_ros/core/rgbd_system.h | 100 ++++++ .../core/test/test_camera_info_system.cc | 79 +++++ drake_ros/core/test/test_rgbd_system.cc | 126 ++++++++ drake_ros/drake_ros/core/__init__.py | 4 + drake_ros/drake_ros/core/cc_pybind.cc | 109 +++++++ drake_ros/package.xml | 1 + drake_ros_examples/CMakeLists.txt | 2 + drake_ros_examples/examples/rgbd/BUILD.bazel | 74 +++++ .../examples/rgbd/CMakeLists.txt | 31 ++ drake_ros_examples/examples/rgbd/README.md | 44 +++ .../examples/rgbd/camera_info.py | 54 ++++ .../examples/rgbd/launch/rgbd_launch.py | 30 ++ drake_ros_examples/examples/rgbd/rgbd.cc | 242 ++++++++++++++ drake_ros_examples/examples/rgbd/rgbd.py | 185 +++++++++++ drake_ros_examples/examples/rgbd/rgbd.rviz | 273 ++++++++++++++++ drake_ros_examples/examples/rgbd/rgbd.sdf | 123 ++++++++ .../examples/rgbd/test/rgbd_test.py | 28 ++ drake_ros_examples/package.xml | 3 + 27 files changed, 2025 insertions(+), 2 deletions(-) create mode 100755 .github/config_display.sh create mode 100644 drake_ros/core/camera_info_system.cc create mode 100644 drake_ros/core/camera_info_system.h create mode 100644 drake_ros/core/rgbd_system.cc create mode 100644 drake_ros/core/rgbd_system.h create mode 100644 drake_ros/core/test/test_camera_info_system.cc create mode 100644 drake_ros/core/test/test_rgbd_system.cc create mode 100644 drake_ros_examples/examples/rgbd/BUILD.bazel create mode 100644 drake_ros_examples/examples/rgbd/CMakeLists.txt create mode 100644 drake_ros_examples/examples/rgbd/README.md create mode 100644 drake_ros_examples/examples/rgbd/camera_info.py create mode 100644 drake_ros_examples/examples/rgbd/launch/rgbd_launch.py create mode 100644 drake_ros_examples/examples/rgbd/rgbd.cc create mode 100644 drake_ros_examples/examples/rgbd/rgbd.py create mode 100644 drake_ros_examples/examples/rgbd/rgbd.rviz create mode 100644 drake_ros_examples/examples/rgbd/rgbd.sdf create mode 100644 drake_ros_examples/examples/rgbd/test/rgbd_test.py diff --git a/.github/config_display.sh b/.github/config_display.sh new file mode 100755 index 00000000..73caaf6b --- /dev/null +++ b/.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 diff --git a/.github/workflows/bazelized_drake_ros.yml b/.github/workflows/bazelized_drake_ros.yml index 038aa9b6..e842c8d1 100755 --- a/.github/workflows/bazelized_drake_ros.yml +++ b/.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 diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index e7172006..c97e3a28 100644 --- a/.github/workflows/main.yml +++ b/.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/action-ros-ci@v0.3 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+ diff --git a/drake_ros/CMakeLists.txt b/drake_ros/CMakeLists.txt index aa83f42e..3362547e 100644 --- a/drake_ros/CMakeLists.txt +++ b/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) diff --git a/drake_ros/core/BUILD.bazel b/drake_ros/core/BUILD.bazel index f54440fe..57eb47eb 100644 --- a/drake_ros/core/BUILD.bazel +++ b/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", diff --git a/drake_ros/core/CMakeLists.txt b/drake_ros/core/CMakeLists.txt index 2afd1834..cc253631 100644 --- a/drake_ros/core/CMakeLists.txt +++ b/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() diff --git a/drake_ros/core/camera_info_system.cc b/drake_ros/core/camera_info_system.cc new file mode 100644 index 00000000..bcb22f63 --- /dev/null +++ b/drake_ros/core/camera_info_system.cc @@ -0,0 +1,85 @@ +#include "drake_ros/core/camera_info_system.h" + +#include + +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& 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::AddToBuilder( + drake::systems::DiagramBuilder* builder, DrakeRos* ros, + const std::string& topic_name, const rclcpp::QoS& qos, + const std::unordered_set& publish_triggers, + double publish_period) { + auto* camera_info_system = builder->AddSystem(); + + auto* pub_system = + builder->AddSystem(RosPublisherSystem::Make( + 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}; +} diff --git a/drake_ros/core/camera_info_system.h b/drake_ros/core/camera_info_system.h new file mode 100644 index 00000000..6c2f60a3 --- /dev/null +++ b/drake_ros/core/camera_info_system.h @@ -0,0 +1,53 @@ +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +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 { + 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 AddToBuilder( + drake::systems::DiagramBuilder* builder, DrakeRos* ros, + const std::string& topic_name = "/image/camera_info", + const rclcpp::QoS& qos = rclcpp::SystemDefaultsQoS(), + const std::unordered_set& publish_triggers = + RosPublisherSystem::kDefaultTriggerTypes, + double publish_period = 0.0); + + private: + CameraInfo camera_info; + + protected: + void CalcCameraInfo(const drake::systems::Context& context, + sensor_msgs::msg::CameraInfo* output_value) const; +}; +} // namespace drake_ros::core diff --git a/drake_ros/core/rgbd_system.cc b/drake_ros/core/rgbd_system.cc new file mode 100644 index 00000000..65c0e06a --- /dev/null +++ b/drake_ros/core/rgbd_system.cc @@ -0,0 +1,296 @@ +#include "drake_ros/core/rgbd_system.h" + +#include +#include +#include +#include +#include + +#include + +using drake::systems::sensors::Image; +using drake::systems::sensors::ImageRgba8U; +using drake_ros::core::RGBDSystem; +using drake_ros::core::RosPublisherSystem; + +using drake::Value; +using drake::math::RigidTransformd; +using drake::math::RollPitchYawd; +using drake::systems::Context; +using drake::systems::EventStatus; +using drake::systems::PublishEvent; +using drake::systems::TriggerType; + +RGBDSystem::RGBDSystem() { + DeclareAbstractOutputPort("rgbd_color", &RGBDSystem::CalcColorImage); + DeclareAbstractOutputPort("rgbd_depth", &RGBDSystem::CalcDepthImage); +} + +RGBDSystem::~RGBDSystem() {} + +void RGBDSystem::CalcColorImage(const drake::systems::Context& context, + sensor_msgs::msg::Image* color_value) const { + color_value->header.frame_id = "CartPole/camera_optical"; + + drake_time = rclcpp::Time{0, 0, RCL_ROS_TIME}; + drake_time += rclcpp::Duration::from_seconds(context.get_time()); + color_value->header.stamp = drake_time; + + color_value->height = image_msgs.height; + color_value->width = image_msgs.width; + color_value->step = image_msgs.step; + color_value->encoding = image_msgs.encoding; + color_value->data.resize(image_msgs.data.size()); + memcpy(&color_value->data[0], &image_msgs.data[0], image_msgs.data.size()); +} + +void RGBDSystem::CalcDepthImage( + const drake::systems::Context& /*context*/, + sensor_msgs::msg::Image* depth_value) const { + depth_value->header.frame_id = "CartPole/camera_optical"; + depth_value->header.stamp = drake_time; + + depth_value->height = depth_msgs.height; + depth_value->width = depth_msgs.width; + depth_value->step = depth_msgs.step; + depth_value->encoding = depth_msgs.encoding; + depth_value->data.resize(depth_msgs.data.size()); + + float* depth_row = reinterpret_cast(&depth_msgs.data[0]); + float* depth_row_value = reinterpret_cast(&depth_value->data[0]); + + for (size_t x = 0; x < depth_value->data.size() / 4; ++x) { + if (std::numeric_limits::infinity() == depth_row[x]) { + depth_row_value[x] = 0.0f; + } else { + depth_row_value[x] = depth_row[x]; + } + } +} + +const InputPort& RGBDSystem::DeclareDepthInputPort( + PixelType pixel_type, std::string port_name, double publish_period, + double start_time) { + switch (pixel_type) { + case PixelType::kRgb8U: + break; + case PixelType::kBgr8U: + break; + case PixelType::kRgba8U: { + break; + } + case PixelType::kBgra8U: + break; + case PixelType::kDepth16U: { + return this->template DeclareDepthInputPort( + std::move(port_name), publish_period, start_time); + } + case PixelType::kDepth32F: { + return this->template DeclareDepthInputPort( + std::move(port_name), publish_period, start_time); + } + case PixelType::kLabel16I: { + break; + } + case PixelType::kGrey8U: { + break; + } + default: + break; + } + throw std::logic_error(fmt::format( + "RGBDSystem::DeclareDepthInputPort does not support pixel_type={}", + static_cast(pixel_type))); +} + +template +const InputPort& RGBDSystem::DeclareDepthInputPort( + std::string port_name, double publish_period, double start_time) { + // Test to confirm valid pixel type. + static_assert( + kPixelType == PixelType::kDepth32F || kPixelType == PixelType::kDepth16U, + "ImageWriter: the only supported pixel types are: kDepth32F " + "and kDepth16U"); + + if (publish_period <= 0) { + throw std::logic_error("RGBDSystem: publish period must be positive"); + } + + // Now configure the system for the valid port declaration. + const auto& port = + DeclareAbstractInputPort(port_name, Value>()); + // There is no DeclarePeriodicPublishEvent that accepts a lambda, so we must + // use the advanced API to add our event. + PublishEvent event( + TriggerType::kPeriodic, + [port_index = port.get_index()](const System& system, + const Context& context, + const PublishEvent&) { + const auto& self = dynamic_cast(system); + self.PubDepth(context, port_index); + return EventStatus::Succeeded(); + }); + DeclarePeriodicEvent>(publish_period, start_time, event); + port_info_.emplace_back("depth", kPixelType); + return port; +} + +const InputPort& RGBDSystem::DeclareImageInputPort( + PixelType pixel_type, std::string port_name, double publish_period, + double start_time) { + switch (pixel_type) { + case PixelType::kRgb8U: + break; + case PixelType::kBgr8U: + break; + case PixelType::kRgba8U: { + return this->template DeclareImageInputPort( + std::move(port_name), publish_period, start_time); + } + case PixelType::kBgra8U: + break; + case PixelType::kDepth16U: { + break; + } + case PixelType::kDepth32F: { + break; + } + case PixelType::kLabel16I: { + break; + } + case PixelType::kGrey8U: { + return this->template DeclareImageInputPort( + std::move(port_name), publish_period, start_time); + } + default: + break; + } + throw std::logic_error(fmt::format( + "RGBDSystem::DeclareImageInputPort does not support pixel_type={}", + static_cast(pixel_type))); +} + +template +const InputPort& RGBDSystem::DeclareImageInputPort( + std::string port_name, double publish_period, double start_time) { + // Test to confirm valid pixel type. + static_assert( + kPixelType == PixelType::kRgba8U || kPixelType == PixelType::kGrey8U, + "RGBDSystem color image: the only supported pixel types are: kRgba8U " + "and kGrey8U"); + + if (publish_period <= 0) { + throw std::logic_error("RGBDSystem: publish period must be positive"); + } + + // Now configure the system for the valid port declaration. + const auto& port = + DeclareAbstractInputPort(port_name, Value>()); + // There is no DeclarePeriodicPublishEvent that accepts a lambda, so we must + // use the advanced API to add our event. + PublishEvent event( + TriggerType::kPeriodic, + [port_index = port.get_index()](const System& system, + const Context& context, + const PublishEvent&) { + const auto& self = dynamic_cast(system); + self.PubImage(context, port_index); + return EventStatus::Succeeded(); + }); + DeclarePeriodicEvent>(publish_period, start_time, event); + port_info_.emplace_back("color", kPixelType); + return port; +} + +template +void RGBDSystem::PubImage(const Context& context, int index) const { + const auto& port = get_input_port(index); + const Image& image = port.Eval>(context); + + int channels = 4; + std::string encoding = "rgba8"; + + if (kPixelType == PixelType::kRgba8U) { + channels = 4; + encoding = "rgba8"; + } else if (kPixelType == PixelType::kGrey8U) { + channels = 1; + encoding = "mono8"; + } + + image_msgs.data.resize(image.width() * image.height() * channels); + image_msgs.height = image.height(); + image_msgs.width = image.width(); + image_msgs.step = image.width() * channels; + image_msgs.encoding = encoding; + memcpy(&image_msgs.data[0], image.at(0, 0), image_msgs.data.size()); +} + +template +void RGBDSystem::PubDepth(const Context& context, int index) const { + const auto& port = get_input_port(index); + const Image& image = port.Eval>(context); + + std::string encoding = "32FC1"; + int octets = sizeof(float); + + if (kPixelType == PixelType::kDepth32F) { + encoding = "32FC1"; + octets = sizeof(float); + } else if (kPixelType == PixelType::kDepth16U) { + encoding = "16UC1"; + octets = sizeof(uint16_t); + } + + depth_msgs.data.resize(image.width() * image.height() * octets); + depth_msgs.height = image.height(); + depth_msgs.width = image.width(); + depth_msgs.step = image.width() * octets; + depth_msgs.encoding = encoding; + memcpy(&depth_msgs.data[0], image.at(0, 0), depth_msgs.data.size()); +} + +template const InputPort& +RGBDSystem::DeclareImageInputPort(std::string port_name, + double publish_period, + double start_time); +template const InputPort& +RGBDSystem::DeclareImageInputPort(std::string port_name, + double publish_period, + double start_time); + +template void RGBDSystem::PubImage( + const Context& context, int index) const; +template void RGBDSystem::PubImage( + const Context& context, int index) const; + +template const InputPort& +RGBDSystem::DeclareDepthInputPort(std::string port_name, + double publish_period, + double start_time); +template const InputPort& +RGBDSystem::DeclareDepthInputPort(std::string port_name, + double publish_period, + double start_time); + +template void RGBDSystem::PubDepth( + const Context& context, int index) const; +template void RGBDSystem::PubDepth( + const Context& context, int index) const; + +std::pair RGBDSystem::AddToBuilder( + drake::systems::DiagramBuilder* builder, DrakeRos* ros, + const std::string& color_topic_name, const std::string& depth_topic_name, + const rclcpp::QoS& qos, + const std::unordered_set& publish_triggers, + double publish_period) { + auto* pub_color_system = + builder->AddSystem(RosPublisherSystem::Make( + color_topic_name, qos, ros, publish_triggers, publish_period)); + + auto* pub_depth_system = + builder->AddSystem(RosPublisherSystem::Make( + depth_topic_name, qos, ros, publish_triggers, publish_period)); + + return {pub_color_system, pub_depth_system}; +} diff --git a/drake_ros/core/rgbd_system.h b/drake_ros/core/rgbd_system.h new file mode 100644 index 00000000..2ee60eda --- /dev/null +++ b/drake_ros/core/rgbd_system.h @@ -0,0 +1,100 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "drake/systems/sensors/rgbd_sensor.h" +#include +#include +#include +#include +#include + +using drake::systems::Context; +using drake::systems::InputPort; +using drake::systems::sensors::PixelType; +using drake::systems::sensors::RgbdSensor; + +namespace drake_ros::core { + +/** A system that converts drake's RGBD camera to a sensor_msgs/msg/Image + * messages. + */ +class RGBDSystem : public drake::systems::LeafSystem { + public: + /** A constructor for the rbgd system. + */ + RGBDSystem(); + + ~RGBDSystem() override; + + const InputPort& DeclareImageInputPort(PixelType pixel_type, + std::string port_name, + double publish_period, + double start_time); + + template + const InputPort& DeclareImageInputPort(std::string port_name, + double publish_period, + double start_time); + + const InputPort& DeclareDepthInputPort(PixelType pixel_type, + std::string port_name, + double publish_period, + double start_time); + template + const InputPort& DeclareDepthInputPort(std::string port_name, + double publish_period, + double start_time); + + // The per-input port data. + struct RgbdSensorPortInfo { + RgbdSensorPortInfo(std::string format_in, PixelType pixel_type_in) + : format(std::move(format_in)), pixel_type(pixel_type_in) {} + const std::string format; + const PixelType pixel_type; + }; + + template + void PubImage(const Context&, int) const; + + template + void PubDepth(const Context&, int) const; + + // For each input port, this stores the corresponding image data. It is an + // invariant that port_info_.size() == num_input_ports(). + std::vector port_info_; + + /** Add a RGBDSystem and RosPublisherSystem to a diagram builder. + * + * This adds both a RGBDSystem and a RosPublisherSystem that publishes + * time to a `/image` and `depth` topics. 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::pair AddToBuilder( + drake::systems::DiagramBuilder* builder, DrakeRos* ros, + const std::string& topic_name = "/image", + const std::string& depth_topic_name = "/depth", + const rclcpp::QoS& qos = rclcpp::SensorDataQoS(), + const std::unordered_set& publish_triggers = + RosPublisherSystem::kDefaultTriggerTypes, + double publish_period = 0.0); + + private: + mutable sensor_msgs::msg::Image image_msgs; + mutable sensor_msgs::msg::Image depth_msgs; + mutable rclcpp::Time drake_time; + + protected: + void CalcColorImage(const drake::systems::Context& context, + sensor_msgs::msg::Image* color_value) const; + + void CalcDepthImage(const drake::systems::Context& context, + sensor_msgs::msg::Image* depth_value) const; +}; +} // namespace drake_ros::core diff --git a/drake_ros/core/test/test_camera_info_system.cc b/drake_ros/core/test/test_camera_info_system.cc new file mode 100644 index 00000000..812478a4 --- /dev/null +++ b/drake_ros/core/test/test_camera_info_system.cc @@ -0,0 +1,79 @@ + +#include "drake/geometry/render_vtk/factory.h" +#include +#include +#include +#include + +#include "drake_ros/core/camera_info_system.h" +#include "drake_ros/core/drake_ros.h" +#include "drake_ros/core/ros_interface_system.h" + +using drake_ros::core::CameraInfoSystem; +using drake_ros::core::DrakeRos; +using drake_ros::core::RosInterfaceSystem; + +using drake::geometry::render::ColorRenderCamera; + +TEST(Integration, camera_info_system) { + drake_ros::core::init(0, nullptr); + + drake::systems::DiagramBuilder builder; + + // Create a Drake system to interface with ROS + auto ros_interface_system = builder.AddSystem( + std::make_unique("test_node")); + + const ColorRenderCamera color_camera{ + {"renderer", {640, 480, M_PI_4}, {0.01, 10.0}, {}}, false}; + + auto camera_info_system = CameraInfoSystem::AddToBuilder( + &builder, ros_interface_system->get_ros_interface()); + + std::get<0>(camera_info_system) + ->SetCameraInfo(color_camera.core().intrinsics()); + + auto diagram = builder.Build(); + auto context = diagram->CreateDefaultContext(); + + auto simulator = std::make_unique>( + *diagram, std::move(context)); + simulator->set_target_realtime_rate(1.0); + simulator->Initialize(); + + auto& simulator_context = simulator->get_mutable_context(); + + // Don't need to rclcpp::init because DrakeRos uses global rclcpp::Context by + // default + uint msg_counter = 0; + auto direct_ros_node = rclcpp::Node::make_shared("subscriber_node"); + auto subscription_color = + direct_ros_node->create_subscription( + "/image/camera_info", rclcpp::SystemDefaultsQoS(), + [&](const sensor_msgs::msg::CameraInfo::SharedPtr) { + msg_counter++; + }); + + simulator->AdvanceTo(simulator_context.get_time() + 1.0); + rclcpp::spin_some(direct_ros_node); + EXPECT_GE(msg_counter, 1u); + + drake_ros::core::shutdown(); +} + +// Only available in Bazel. +#ifndef _TEST_DISABLE_RMW_ISOLATION +#include "rmw_isolation/rmw_isolation.h" + +int main(int argc, char* argv[]) { + const char* TEST_TMPDIR = std::getenv("TEST_TMPDIR"); + if (TEST_TMPDIR != nullptr) { + std::string ros_home = std::string(TEST_TMPDIR) + "/.ros"; + setenv("ROS_HOME", ros_home.c_str(), 1); + ros2::isolate_rmw_by_path(argv[0], TEST_TMPDIR); + } + + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} +#endif diff --git a/drake_ros/core/test/test_rgbd_system.cc b/drake_ros/core/test/test_rgbd_system.cc new file mode 100644 index 00000000..65caa737 --- /dev/null +++ b/drake_ros/core/test/test_rgbd_system.cc @@ -0,0 +1,126 @@ + +#include "drake/geometry/render_vtk/factory.h" +#include +#include +#include +#include +#include + +#include "drake_ros/core/drake_ros.h" +#include "drake_ros/core/rgbd_system.h" +#include "drake_ros/core/ros_interface_system.h" + +using drake::multibody::AddMultibodyPlantSceneGraph; +using drake_ros::core::DrakeRos; +using drake_ros::core::RGBDSystem; +using drake_ros::core::RosInterfaceSystem; + +using drake::geometry::RenderEngineVtkParams; +using drake::geometry::render::ColorRenderCamera; +using drake::geometry::render::DepthRenderCamera; + +using drake::math::RigidTransformd; + +TEST(Integration, rgbd_system) { + drake_ros::core::init(0, nullptr); + + drake::systems::DiagramBuilder builder; + + // Make and add the cart_pole model. + auto [plant, scene_graph] = AddMultibodyPlantSceneGraph(&builder, 0); + + scene_graph.AddRenderer("renderer", + MakeRenderEngineVtk(RenderEngineVtkParams())); + + // Create a Drake system to interface with ROS + auto ros_interface_system = builder.AddSystem( + std::make_unique("test_node")); + + const ColorRenderCamera color_camera{ + {"renderer", {640, 480, M_PI_4}, {0.01, 10.0}, {}}, false}; + const DepthRenderCamera depth_camera{color_camera.core(), {0.01, 10.0}}; + const RigidTransformd X_WB; + + const auto world_id = scene_graph.world_frame_id(); + RgbdSensor* camera = + builder.AddSystem(world_id, X_WB, color_camera, depth_camera); + builder.Connect(scene_graph.get_query_output_port(), + camera->query_object_input_port()); + + const double image_publish_period = 1. / 30; + RGBDSystem* rgbd_publisher{nullptr}; + rgbd_publisher = builder.template AddSystem(); + + const auto& rgbd_port = + rgbd_publisher->DeclareImageInputPort( + "color", image_publish_period, 0.); + builder.Connect(camera->color_image_output_port(), rgbd_port); + + const auto& depth_port = + rgbd_publisher->DeclareDepthInputPort( + "depth", image_publish_period, 0.); + builder.Connect(camera->depth_image_32F_output_port(), depth_port); + + auto [pub_color_system, pub_depth_system] = RGBDSystem::AddToBuilder( + &builder, ros_interface_system->get_ros_interface()); + + builder.Connect(rgbd_publisher->GetOutputPort("rgbd_color"), + pub_color_system->get_input_port()); + + builder.Connect(rgbd_publisher->GetOutputPort("rgbd_depth"), + pub_depth_system->get_input_port()); + + plant.Finalize(); + auto diagram = builder.Build(); + auto context = diagram->CreateDefaultContext(); + + auto simulator = std::make_unique>( + *diagram, std::move(context)); + simulator->set_target_realtime_rate(1.0); + simulator->Initialize(); + + auto& simulator_context = simulator->get_mutable_context(); + + // Don't need to rclcpp::init because DrakeRos uses global rclcpp::Context by + // default + uint color_msg_counter = 0; + uint depth_msg_counter = 0; + auto direct_ros_node = rclcpp::Node::make_shared("subscriber_node"); + auto subscription_color = + direct_ros_node->create_subscription( + "/image", rclcpp::SensorDataQoS(), + [&](const sensor_msgs::msg::Image::SharedPtr) { + color_msg_counter++; + }); + + auto subscription_depth = + direct_ros_node->create_subscription( + "/depth", rclcpp::SensorDataQoS(), + [&](const sensor_msgs::msg::Image::SharedPtr) { + depth_msg_counter++; + }); + + simulator->AdvanceTo(simulator_context.get_time() + 1.0); + rclcpp::spin_some(direct_ros_node); + EXPECT_GE(color_msg_counter, 1u); + EXPECT_GE(depth_msg_counter, 1u); + + drake_ros::core::shutdown(); +} + +// Only available in Bazel. +#ifndef _TEST_DISABLE_RMW_ISOLATION +#include "rmw_isolation/rmw_isolation.h" + +int main(int argc, char* argv[]) { + const char* TEST_TMPDIR = std::getenv("TEST_TMPDIR"); + if (TEST_TMPDIR != nullptr) { + std::string ros_home = std::string(TEST_TMPDIR) + "/.ros"; + setenv("ROS_HOME", ros_home.c_str(), 1); + ros2::isolate_rmw_by_path(argv[0], TEST_TMPDIR); + } + + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} +#endif diff --git a/drake_ros/drake_ros/core/__init__.py b/drake_ros/drake_ros/core/__init__.py index 2de48bf9..88dde46a 100644 --- a/drake_ros/drake_ros/core/__init__.py +++ b/drake_ros/drake_ros/core/__init__.py @@ -1,5 +1,6 @@ """Python wrapper for drake_ros.core.""" +from drake_ros._cc.core import CameraInfoSystem from drake_ros._cc.core import ClockSystem from drake_ros._cc.core import init from drake_ros._cc.core import CppNode @@ -8,6 +9,7 @@ from drake_ros._cc.core import Isometry3ToRosPose from drake_ros._cc.core import Isometry3ToRosTransform from drake_ros._cc.core import QuaternionToRosQuaternion +from drake_ros._cc.core import RGBDSystem from drake_ros._cc.core import RigidTransformToRosPose from drake_ros._cc.core import RigidTransformToRosTransform from drake_ros._cc.core import RosAccelToSpatialAcceleration @@ -107,12 +109,14 @@ def _make_ros_subscriber_system( __all__ = [ + 'CameraInfoSystem', 'ClockSystem', 'DrakeRosInterface', 'Isometry3ToRosPose', 'Isometry3ToRosTransform', 'PySerializer', 'QuaternionToRosQuaternion', + 'RGBDSystem', 'RigidTransformToRosPose', 'RigidTransformToRosTransform', 'RosAccelToSpatialAcceleration', diff --git a/drake_ros/drake_ros/core/cc_pybind.cc b/drake_ros/drake_ros/core/cc_pybind.cc index c56f9914..774eeaa7 100644 --- a/drake_ros/drake_ros/core/cc_pybind.cc +++ b/drake_ros/drake_ros/core/cc_pybind.cc @@ -6,11 +6,13 @@ #include #include +#include "drake_ros/core/camera_info_system.h" #include "drake_ros/core/clock_system.h" #include "drake_ros/core/drake_ros.h" #include "drake_ros/core/geometry_conversions.h" #include "drake_ros/core/geometry_conversions_pybind.h" #include "drake_ros/core/qos_pybind.h" +#include "drake_ros/core/rgbd_system.h" #include "drake_ros/core/ros_interface_system.h" #include "drake_ros/core/ros_publisher_system.h" #include "drake_ros/core/ros_subscriber_system.h" @@ -20,9 +22,11 @@ namespace drake_ros { namespace drake_ros_py DRAKE_ROS_NO_EXPORT { +using drake_ros::core::CameraInfoSystem; using drake_ros::core::ClockSystem; using drake_ros::core::DrakeRos; using drake_ros::core::init; +using drake_ros::core::RGBDSystem; using drake_ros::core::RosInterfaceSystem; using drake_ros::core::RosPublisherSystem; using drake_ros::core::RosSubscriberSystem; @@ -34,6 +38,29 @@ using drake::systems::TriggerType; using py_rvp = pybind11::return_value_policy; +// Implementation for `overload_cast_explicit`. We must use this structure so +// that we can constrain what is inferred. Otherwise, the ambiguity confuses +// the compiler. +template +struct overload_cast_impl { + auto operator()(Return (*func)(Args...)) const { return func; } + + template + auto operator()(Return (Class::*method)(Args...)) const { + return method; + } + + template + auto operator()(Return (Class::*method)(Args...) const) const { + return method; + } +}; + +/// Provides option to provide explicit signature when +/// `py::overload_cast` fails to infer the Return argument. +template +constexpr auto overload_cast_explicit = overload_cast_impl{}; + // A (de)serialization interface implementation for Python ROS messages // that can be overriden from Python itself. class PySerializerInterface : public py::wrapper { @@ -166,6 +193,35 @@ void DefCore(py::module m) { return self.get_node().shared_from_this(); }); + py::class_>(m, "CameraInfoSystem") + .def_static( + "AddToBuilder", + [](drake::systems::DiagramBuilder* builder, DrakeRos* ros, + const std::string& topic_name, const QoS& qos, + const std::unordered_set& + pub_triggers, + double publish_period) { + auto [camera_info_system, pub_system] = + CameraInfoSystem::AddToBuilder(builder, ros, topic_name, qos, + pub_triggers, publish_period); + + py::object py_builder = py::cast(builder, py_rvp::reference); + py::list result; + result.append(py::cast(camera_info_system, + py_rvp::reference_internal, py_builder)); + result.append( + py::cast(pub_system, py_rvp::reference_internal, py_builder)); + return result; + }, + py::arg("builder"), py::arg("ros"), py::kw_only(), + py::arg("topic_name") = std::string{"/image/camera_info"}, + py::arg("qos") = drake_ros::QoS(rclcpp::SensorDataQoS()), + py::arg("publish_triggers") = + std::unordered_set{ + RosPublisherSystem::kDefaultTriggerTypes}, + py::arg("publish_period") = 0.0) + .def("set_camera_info", &CameraInfoSystem::SetCameraInfo); + py::class_>(m, "ClockSystem") .def_static( "AddToBuilder", @@ -193,6 +249,59 @@ void DefCore(py::module m) { RosPublisherSystem::kDefaultTriggerTypes}, py::arg("publish_period") = 0.0); + py::class_>(m, "RGBDSystem") + .def(py::init<>()) + .def( + "DeclareImageInputPort", + [](RGBDSystem& self, drake::systems::sensors::PixelType pixel_type, + std::string port_name, double publish_period, + double start_time) -> const drake::systems::InputPort& { + return self.DeclareImageInputPort(pixel_type, std::move(port_name), + publish_period, start_time); + }, + py::arg("pixel_type"), py::arg("port_name"), + py::arg("publish_period"), py::arg("start_time"), + py_rvp::reference_internal) + .def( + "DeclareDepthInputPort", + [](RGBDSystem& self, drake::systems::sensors::PixelType pixel_type, + std::string port_name, double publish_period, + double start_time) -> const drake::systems::InputPort& { + return self.DeclareDepthInputPort(pixel_type, std::move(port_name), + publish_period, start_time); + }, + py::arg("pixel_type"), py::arg("port_name"), + py::arg("publish_period"), py::arg("start_time"), + py_rvp::reference_internal) + .def_static( + "AddToBuilder", + [](drake::systems::DiagramBuilder* builder, DrakeRos* ros, + const std::string& topic_name, const std::string& depth_topic_name, + const QoS& qos, + const std::unordered_set& + pub_triggers, + double publish_period) { + auto [rgba_system, pub_system] = RGBDSystem::AddToBuilder( + builder, ros, topic_name, depth_topic_name, qos, pub_triggers, + publish_period); + + py::object py_builder = py::cast(builder, py_rvp::reference); + py::list result; + result.append( + py::cast(rgba_system, py_rvp::reference_internal, py_builder)); + result.append( + py::cast(pub_system, py_rvp::reference_internal, py_builder)); + return result; + }, + py::arg("builder"), py::arg("ros"), py::kw_only(), + py::arg("topic_name") = std::string{"/image"}, + py::arg("depth_topic_name") = std::string{"/depth"}, + py::arg("qos") = drake_ros::QoS(rclcpp::SystemDefaultsQoS()), + py::arg("publish_triggers") = + std::unordered_set{ + RosPublisherSystem::kDefaultTriggerTypes}, + py::arg("publish_period") = 0.0); + m.def( "init", [](std::vector args) { diff --git a/drake_ros/package.xml b/drake_ros/package.xml index c6593146..fad5c7db 100644 --- a/drake_ros/package.xml +++ b/drake_ros/package.xml @@ -23,6 +23,7 @@ rosidl_runtime_c rosidl_typesupport_cpp geometry_msgs + sensor_msgs tf2_eigen visualization_msgs diff --git a/drake_ros_examples/CMakeLists.txt b/drake_ros_examples/CMakeLists.txt index af64c646..f631f444 100644 --- a/drake_ros_examples/CMakeLists.txt +++ b/drake_ros_examples/CMakeLists.txt @@ -11,6 +11,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake_ros REQUIRED) +find_package(ament_index_cpp REQUIRED) find_package(drake REQUIRED) find_package(drake_ros REQUIRED) find_package(gflags REQUIRED COMPONENTS shared) @@ -18,6 +19,7 @@ find_package(gflags REQUIRED COMPONENTS shared) add_subdirectory(examples/hydroelastic) add_subdirectory(examples/iiwa_manipulator) add_subdirectory(examples/multirobot) +add_subdirectory(examples/rgbd) add_subdirectory(examples/rs_flip_flop) if(BUILD_TESTING) diff --git a/drake_ros_examples/examples/rgbd/BUILD.bazel b/drake_ros_examples/examples/rgbd/BUILD.bazel new file mode 100644 index 00000000..0522f492 --- /dev/null +++ b/drake_ros_examples/examples/rgbd/BUILD.bazel @@ -0,0 +1,74 @@ +# -*- mode: python -*- +# vi: set ft=python : + +load("@ros2//:ament_index.bzl", "ament_index_share_files") +load("@ros2//:ros_cc.bzl", "ros_cc_binary") +load("@ros2//:ros_py.bzl", "ros_py_binary", "ros_py_test") + +# We use `ament_index_share_files` so we can write code that can find +# resources both when built by CMake and by Bazel. +ament_index_share_files( + name = "rgbd_share", + package_name = "drake_ros_examples", + srcs = [":rgbd.sdf"], + strip_prefix = "examples/", +) + +ros_cc_binary( + name = "rgbd", + srcs = ["rgbd.cc"], + data = [ + ":rgbd_share", + ], + local_defines = ["BAZEL"], + rmw_implementation = "rmw_cyclonedds_cpp", + visibility = ["//visibility:public"], + deps = [ + "@drake//geometry:drake_visualizer", + "@drake//geometry/render_vtk:factory", + "@drake//multibody/parsing:parser", + "@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", + "@drake//visualization:visualization_config", + "@drake//visualization:visualization_config_functions", + "@drake_ros_repo//:drake_ros", + "@drake_ros_repo//core", + "@drake_ros_repo//viz", + "@gflags", + "@ros2//:rclcpp_cc", + "@ros2//:sensor_msgs_cc", + ], +) + +ros_py_binary( + name = "rgbd_py", + srcs = ["rgbd.py"], + data = [ + ":rgbd_share", + ], + main = "rgbd.py", + rmw_implementation = "rmw_cyclonedds_cpp", + visibility = ["//visibility:public"], + deps = [ + "@drake//bindings/pydrake", + "@drake_ros_repo//:drake_ros_py", + "@ros2//:rclpy_py", + ], +) + +ros_py_test( + name = "rgbd_test", + srcs = ["test/rgbd_test.py"], + data = [ + ":rgbd", + ":rgbd_py", + ], + main = "test/rgbd_test.py", + rmw_implementation = "rmw_cyclonedds_cpp", + deps = [ + "@ros2//resources/bazel_ros_env:bazel_ros_env_py", + ], +) diff --git a/drake_ros_examples/examples/rgbd/CMakeLists.txt b/drake_ros_examples/examples/rgbd/CMakeLists.txt new file mode 100644 index 00000000..b5aba9a4 --- /dev/null +++ b/drake_ros_examples/examples/rgbd/CMakeLists.txt @@ -0,0 +1,31 @@ +add_executable(rgbd rgbd.cc) +target_link_libraries(rgbd + ament_index_cpp::ament_index_cpp + drake::drake + drake_ros::drake_ros_core + drake_ros::drake_ros_viz + gflags_shared +) + +install( + TARGETS + rgbd + DESTINATION lib/${PROJECT_NAME} +) + +install(FILES + rgbd.sdf + rgbd.rviz + DESTINATION share/${PROJECT_NAME}/rgbd/ +) + +install( + PROGRAMS rgbd.py + DESTINATION lib/${PROJECT_NAME} +) + +install( + DIRECTORY launch + DESTINATION share/${PROJECT_NAME} + FILES_MATCHING PATTERN "*_launch.py" +) diff --git a/drake_ros_examples/examples/rgbd/README.md b/drake_ros_examples/examples/rgbd/README.md new file mode 100644 index 00000000..0f187617 --- /dev/null +++ b/drake_ros_examples/examples/rgbd/README.md @@ -0,0 +1,44 @@ +# RGBD camera + +## Overview + +The `rgbd` executable and `rgbd.py` script enable a cartpole simulation with a RGBD sensor looking at it + +The simulation publishes the following topics: + + * `/tf` (all scene frames) + * `/color/image_raw` + * `/color/camera_info` + * `/depth/image_raw` + * `/depth/camera_info` + +## How To + +```sh +# Using Colcon/CMake +ros2 run rviz2 rviz2 -d $(ros2 pkg prefix drake_ros_examples)/share/drake_ros_examples/rgbd/rgbd.rviz + +# Using bazel +bazel run @ros2//:rviz2 -- -d `pwd`/examples/rgbd/rgbd.rviz +``` + +In a separate terminal, run either the C++ executable or the Python script. + +# Using Colcon/CMake +# C++ +ros2 run drake_ros_examples rgbd +# Python +ros2 run drake_ros_examples rgbd.py + +# Using bazel +# C++ +bazel run //examples/rgbd:rgbd +# Python +bazel run //examples/rgbd:rgbd_py +``` + +Or use ROS 2 Launch: + +```bash +ros2 launch drake_ros_examples rgba_launch.py +``` diff --git a/drake_ros_examples/examples/rgbd/camera_info.py b/drake_ros_examples/examples/rgbd/camera_info.py new file mode 100644 index 00000000..71d23dc6 --- /dev/null +++ b/drake_ros_examples/examples/rgbd/camera_info.py @@ -0,0 +1,54 @@ +#!/usr/bin/env python3 + +import numpy as np + +import drake_ros.core +from drake_ros.core import ClockSystem +from drake_ros.core import CameraInfoSystem +from drake_ros.core import RosInterfaceSystem + +from pydrake.common import FindResourceOrThrow +from pydrake.multibody.parsing import Parser +from pydrake.systems.analysis import Simulator +from pydrake.systems.framework import DiagramBuilder +from pydrake.systems.sensors import CameraInfo + + +def main(): + # Create a Drake diagram + builder = DiagramBuilder() + # Initialise the ROS infrastructure + drake_ros.core.init() + # Create a Drake system to interface with ROS + sys_ros_interface = builder.AddSystem(RosInterfaceSystem('camera_info')) + ClockSystem.AddToBuilder(builder, sys_ros_interface.get_ros_interface()) + camera_info_system = CameraInfoSystem.AddToBuilder(builder, sys_ros_interface.get_ros_interface()) + + intrinsics = CameraInfo( + width=640, + height=480, + fov_y=np.pi/4, + ) + + camera_info_system[0].set_camera_info(intrinsics) + + # Build the complete system from the diagram + diagram = builder.Build() + + # Create a simulator for the system + simulator = Simulator(diagram) + simulator.Initialize() + simulator_context = simulator.get_mutable_context() + simulator.set_target_realtime_rate(1.0) + + # Step the simulator in 0.1s intervals + step = 0.1 + while simulator_context.get_time() < float('inf'): + next_time = min( + simulator_context.get_time() + step, float('inf'), + ) + simulator.AdvanceTo(next_time) + + +if __name__ == '__main__': + main() diff --git a/drake_ros_examples/examples/rgbd/launch/rgbd_launch.py b/drake_ros_examples/examples/rgbd/launch/rgbd_launch.py new file mode 100644 index 00000000..67c9e2f4 --- /dev/null +++ b/drake_ros_examples/examples/rgbd/launch/rgbd_launch.py @@ -0,0 +1,30 @@ +#!/bin/python +import os + +from ament_index_python.packages import get_package_prefix +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ExecuteProcess + +import launch_ros.actions +import launch_ros.descriptions + + +def generate_launch_description(): + rviz_config_file = os.path.join(get_package_share_directory('drake_ros_examples'), + 'rgbd/rgbd.rviz') + + rviz_node = ExecuteProcess( + cmd=['rviz2', '-d', rviz_config_file], + output='screen' + ) + + rgbd_node = ExecuteProcess( + cmd=[os.path.join(get_package_prefix('drake_ros_examples'), 'lib', + 'drake_ros_examples', 'rgbd')], + ) + + return LaunchDescription([ + rviz_node, + rgbd_node, + ]) diff --git a/drake_ros_examples/examples/rgbd/rgbd.cc b/drake_ros_examples/examples/rgbd/rgbd.cc new file mode 100644 index 00000000..70d4edee --- /dev/null +++ b/drake_ros_examples/examples/rgbd/rgbd.cc @@ -0,0 +1,242 @@ +#include + +#include "drake/common/drake_assert.h" +#include "drake/geometry/drake_visualizer.h" +#include "drake/geometry/render_vtk/factory.h" +#include "drake/geometry/scene_graph.h" +#include "drake/math/rigid_transform.h" +#include "drake/multibody/parsing/parser.h" +#include "drake/multibody/plant/multibody_plant.h" +#include "drake/multibody/tree/prismatic_joint.h" +#include "drake/multibody/tree/revolute_joint.h" +#include "drake/systems/analysis/simulator.h" +#include "drake/systems/framework/diagram_builder.h" +#include "drake/systems/sensors/image.h" +#include "drake/systems/sensors/pixel_types.h" +#include "drake/systems/sensors/rgbd_sensor.h" +#include "drake/visualization/visualization_config_functions.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using drake::geometry::SceneGraph; + +// "multibody" namespace is ambiguous here without "drake::". +using drake::multibody::AddMultibodyPlantSceneGraph; +using drake::multibody::MultibodyPlant; +using drake::multibody::Parser; +using drake::multibody::PrismaticJoint; +using drake::multibody::RevoluteJoint; + +using drake_ros::core::CameraInfoSystem; +using drake_ros::core::ClockSystem; +using drake_ros::core::DrakeRos; +using drake_ros::core::RGBDSystem; +using drake_ros::core::RosInterfaceSystem; +using drake_ros::viz::RvizVisualizer; + +using drake::geometry::RenderEngineVtkParams; +using drake::geometry::render::ColorRenderCamera; +using drake::geometry::render::DepthRenderCamera; + +using drake::math::RigidTransformd; +using drake::math::RollPitchYawd; +using Eigen::Vector3d; + +using drake::systems::TriggerType; +using drake::systems::sensors::PixelType; +using drake::systems::sensors::RgbdSensor; + +DEFINE_double(target_realtime_rate, 1.0, + "Desired rate relative to real time. See documentation for " + "Simulator::set_target_realtime_rate() for details."); + +DEFINE_double(simulation_time, 100.0, + "Desired duration of the simulation in seconds."); + +DEFINE_double(time_step, 0, + "If greater than zero, the plant is modeled as a system with " + "discrete updates and period equal to this time_step. " + "If 0, the plant is modeled as a continuous system."); + +RigidTransformd ParseCameraPose(const std::string& input_str) { + const char delimiter = ','; + std::vector xyzrpy_numeric; + + size_t pos = 0; + size_t next = 0; + while ((next = input_str.find(delimiter, pos)) != std::string::npos) { + xyzrpy_numeric.push_back(std::stod(input_str.substr(pos, next))); + pos = next + 1; + } + xyzrpy_numeric.push_back(std::stod(input_str.substr(pos, next))); + DRAKE_DEMAND(xyzrpy_numeric.size() == 6); + + const RigidTransformd X_WB( + RollPitchYawd{xyzrpy_numeric[3], xyzrpy_numeric[4], xyzrpy_numeric[5]}, + Vector3d(xyzrpy_numeric[0], xyzrpy_numeric[1], xyzrpy_numeric[2])); + + return X_WB; +} + +int do_main() { + drake::systems::DiagramBuilder builder; + + // Make and add the cart_pole model. + auto [cart_pole, scene_graph] = + AddMultibodyPlantSceneGraph(&builder, FLAGS_time_step); + + scene_graph.AddRenderer("renderer", + MakeRenderEngineVtk(RenderEngineVtkParams())); + + auto parser = Parser(&cart_pole); + + const auto package_path = + ament_index_cpp::get_package_share_directory("drake_ros_examples"); + parser.package_map().Add("drake_ros_examples", package_path); + std::filesystem::path fs_path{ + parser.package_map().GetPath("drake_ros_examples")}; + + const std::string sdf_url = (fs_path / "rgbd" / "rgbd.sdf").string(); + Parser(&cart_pole, &scene_graph).AddModels(sdf_url); + + drake_ros::core::init(); + auto ros_interface_system = builder.AddSystem( + std::make_unique("cart_pole")); + + const double image_publish_period = 1. / 25.0; + + auto camera_info_system = CameraInfoSystem::AddToBuilder( + &builder, ros_interface_system->get_ros_interface(), "/color/camera_info", + rclcpp::SystemDefaultsQoS(), {TriggerType::kPeriodic}, + image_publish_period); + + auto depth_camera_info_system = CameraInfoSystem::AddToBuilder( + &builder, ros_interface_system->get_ros_interface(), "/depth/camera_info", + rclcpp::SystemDefaultsQoS(), {TriggerType::kPeriodic}, + image_publish_period); + + // The size of the image is small because the performance of drake with + // images/depth images is not good. Vtk and Gl perform similar. 640x480 is + // generating ~4-5 fps + const ColorRenderCamera color_camera{ + {"renderer", {320, 240, M_PI_4}, {0.01, 10.0}, {}}, false}; + const DepthRenderCamera depth_camera{color_camera.core(), {0.01, 10.0}}; + const RigidTransformd X_WB = + ParseCameraPose("0.0, 1.0, 0.0, 1.57, 3.14, 0.0"); + + std::get<0>(camera_info_system) + ->SetCameraInfo(color_camera.core().intrinsics()); + std::get<0>(depth_camera_info_system) + ->SetCameraInfo(depth_camera.core().intrinsics()); + + const auto world_id = scene_graph.world_frame_id(); + RgbdSensor* camera = + builder.AddSystem(world_id, X_WB, color_camera, depth_camera); + builder.Connect(scene_graph.get_query_output_port(), + camera->query_object_input_port()); + + RGBDSystem* rgbd_publisher{nullptr}; + rgbd_publisher = builder.template AddSystem(); + + const auto& rgbd_port = + rgbd_publisher->DeclareImageInputPort( + "color", image_publish_period, 0.); + builder.Connect(camera->color_image_output_port(), rgbd_port); + + const auto& depth_port = + rgbd_publisher->DeclareDepthInputPort( + "depth", image_publish_period, 0.); + builder.Connect(camera->depth_image_32F_output_port(), depth_port); + + const double viz_dt = 1 / 60.0; + auto rviz_visualizer = builder.AddSystem( + ros_interface_system->get_ros_interface(), + drake_ros::viz::RvizVisualizerParams{ + {TriggerType::kPeriodic}, viz_dt, true}); + builder.Connect(scene_graph.get_query_output_port(), + rviz_visualizer->get_graph_query_input_port()); + + rviz_visualizer->RegisterMultibodyPlant(&cart_pole); + + // Add a TF2 broadcaster to provide task frame information + auto scene_tf_broadcaster = + builder.AddSystem( + ros_interface_system->get_ros_interface(), + drake_ros::tf2::SceneTfBroadcasterParams{ + {TriggerType::kPeriodic}, viz_dt, "/tf"}); + builder.Connect(scene_graph.get_query_output_port(), + scene_tf_broadcaster->get_graph_query_input_port()); + + auto [pub_color_system, pub_depth_system] = RGBDSystem::AddToBuilder( + &builder, ros_interface_system->get_ros_interface(), "/color/image_raw", + "/depth/image_raw", rclcpp::SensorDataQoS(), {TriggerType::kPeriodic}, + image_publish_period); + + builder.Connect(rgbd_publisher->GetOutputPort("rgbd_color"), + pub_color_system->get_input_port()); + + builder.Connect(rgbd_publisher->GetOutputPort("rgbd_depth"), + pub_depth_system->get_input_port()); + + // Now the model is complete. + cart_pole.Finalize(); + + ClockSystem::AddToBuilder(&builder, ros_interface_system->get_ros_interface(), + "/clock"); + + auto diagram = builder.Build(); + + // Create a context for this system: + std::unique_ptr> diagram_context = + diagram->CreateDefaultContext(); + diagram->SetDefaultContext(diagram_context.get()); + drake::systems::Context& cart_pole_context = + diagram->GetMutableSubsystemContext(cart_pole, diagram_context.get()); + + // There is no input actuation in this example for the passive dynamics. + cart_pole.get_actuation_input_port().FixValue(&cart_pole_context, 0.); + + // Get joints so that we can set initial conditions. + const PrismaticJoint& cart_slider = + cart_pole.GetJointByName("CartSlider"); + const RevoluteJoint& pole_pin = + cart_pole.GetJointByName("PolePin"); + + // Set initial state. + cart_slider.set_translation(&cart_pole_context, 0.0); + pole_pin.set_angle(&cart_pole_context, 2.0); + + drake::systems::Simulator simulator(*diagram, + std::move(diagram_context)); + + simulator.set_publish_every_time_step(false); + simulator.set_target_realtime_rate(FLAGS_target_realtime_rate); + simulator.Initialize(); + + auto& simulator_context = simulator.get_mutable_context(); + + // Step the simulator in 0.1s intervals + constexpr double kStep{0.01}; + while (simulator_context.get_time() < FLAGS_simulation_time) { + const double next_time = + std::min(FLAGS_simulation_time, simulator_context.get_time() + kStep); + simulator.AdvanceTo(next_time); + } + + return 0; +} + +int main(int argc, char* argv[]) { + gflags::SetUsageMessage( + "A simple cart pole demo using Drake's MultibodyPlant," + "publishing images and camera info in ROS 2."); + gflags::ParseCommandLineFlags(&argc, &argv, true); + return do_main(); +} diff --git a/drake_ros_examples/examples/rgbd/rgbd.py b/drake_ros_examples/examples/rgbd/rgbd.py new file mode 100644 index 00000000..6309c1f4 --- /dev/null +++ b/drake_ros_examples/examples/rgbd/rgbd.py @@ -0,0 +1,185 @@ +#!/usr/bin/env python3 +import argparse +import numpy as np +import os + +from ament_index_python.packages import get_package_share_directory + +import drake_ros.core +from drake_ros.core import ClockSystem +from drake_ros.core import CameraInfoSystem +from drake_ros.core import RGBDSystem +from drake_ros.core import RosInterfaceSystem + +from pydrake.geometry import (ClippingRange, ColorRenderCamera, DepthRange, DepthRenderCamera, + MakeRenderEngineGl, RenderCameraCore, RenderEngineGlParams) +from pydrake.math import RigidTransform, RollPitchYaw +from pydrake.multibody.parsing import Parser +from pydrake.multibody.plant import AddMultibodyPlant, MultibodyPlantConfig +from pydrake.systems.analysis import Simulator +from pydrake.systems.framework import DiagramBuilder, TriggerType +from pydrake.systems.sensors import CameraInfo, PixelType, RgbdSensor +from pydrake.visualization import ( + ColorizeDepthImage, + ColorizeLabelImage, +) + +from rclpy import qos + +def xyz_rpy_deg(xyz, rpy_deg): + """Shorthand for defining a pose.""" + rpy_deg = np.asarray(rpy_deg) + return RigidTransform(RollPitchYaw(rpy_deg * np.pi / 180), xyz) + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument( + '--simulation_time', + type=float, + default=float('inf'), + help='How many seconds to run the simulation') + args = parser.parse_args() + + # Create a Drake diagram + builder = DiagramBuilder() + + # Add a multibody plant and a scene graph to hold the robots + plant, scene_graph = AddMultibodyPlant( + MultibodyPlantConfig(time_step=0.001), + builder, + ) + + renderer_name = "renderer" + scene_graph.AddRenderer( + renderer_name, MakeRenderEngineGl(RenderEngineGlParams())) + parser = Parser(plant) + + package_path = get_package_share_directory("drake_ros_examples") + parser.package_map().Add("drake_ros_examples", package_path) + fs_path = parser.package_map().GetPath("drake_ros_examples") + sdf_url = os.path.join(fs_path, 'rgbd', 'rgbd.sdf') + + Parser(plant, scene_graph).AddModels(sdf_url) + + # Initialise the ROS infrastructure + drake_ros.core.init() + # Create a Drake system to interface with ROS + sys_ros_interface = builder.AddSystem(RosInterfaceSystem('cart_pole')) + ClockSystem.AddToBuilder(builder, sys_ros_interface.get_ros_interface()) + + camera_info_system = CameraInfoSystem.AddToBuilder( + builder, + sys_ros_interface.get_ros_interface(), + topic_name='/color/camera_info', + publish_period = 1. / 10., + publish_triggers={TriggerType.kPeriodic}) + + depth_camera_info_system = CameraInfoSystem.AddToBuilder( + builder, + sys_ros_interface.get_ros_interface(), + topic_name='/depth/camera_info', + publish_period = 1. / 10., + publish_triggers={TriggerType.kPeriodic}) + + intrinsics = CameraInfo( + width=320, + height=240, + fov_y=np.pi/4, + ) + + core = RenderCameraCore( + renderer_name, + intrinsics, + ClippingRange(0.01, 10.0), + RigidTransform(), + ) + + color_camera = ColorRenderCamera(core, show_window=False) + depth_camera = DepthRenderCamera(core, DepthRange(0.01, 10.0)) + + camera_info_system[0].set_camera_info(intrinsics) + depth_camera_info_system[0].set_camera_info(intrinsics) + + world_id = plant.GetBodyFrameIdOrThrow(plant.world_body().index()) + X_WB = xyz_rpy_deg([0.0, 1.0, 0.0], [90, 180, 0.0]) + sensor = RgbdSensor( + world_id, + X_PB=X_WB, + color_camera=color_camera, + depth_camera=depth_camera, + ) + + builder.AddSystem(sensor) + builder.Connect( + scene_graph.get_query_output_port(), + sensor.query_object_input_port(), + ) + + colorize_depth = builder.AddSystem(ColorizeDepthImage()) + colorize_label = builder.AddSystem(ColorizeLabelImage()) + colorize_label.background_color.set([0,0,0]) + builder.Connect(sensor.GetOutputPort("depth_image_32f"), + colorize_depth.GetInputPort("depth_image_32f")) + builder.Connect(sensor.GetOutputPort("label_image"), + colorize_label.GetInputPort("label_image")) + + rgbd_publisher = builder.AddSystem(RGBDSystem()) + + image_publish_period = 1. / 30. + rgbd_port = rgbd_publisher.DeclareImageInputPort( + PixelType.kRgba8U, "color", image_publish_period, 0.) + builder.Connect(sensor.color_image_output_port(), rgbd_port) + + depth_port = rgbd_publisher.DeclareDepthInputPort( + PixelType.kDepth32F, "depth", image_publish_period, 0.) + builder.Connect(sensor.depth_image_32F_output_port(), depth_port) + + [pub_color_system, pub_depth_system] = RGBDSystem.AddToBuilder( + builder, sys_ros_interface.get_ros_interface(), + topic_name="/color/image_raw", + depth_topic_name='/depth/image_raw', + publish_period = 1. / 25., + publish_triggers={TriggerType.kPeriodic}, + qos=qos.qos_profile_sensor_data) + + builder.Connect(rgbd_publisher.GetOutputPort("rgbd_color"), + pub_color_system.get_input_port()) + + builder.Connect(rgbd_publisher.GetOutputPort("rgbd_depth"), + pub_depth_system.get_input_port()) + + plant.Finalize() + + # Build the complete system from the diagram + diagram = builder.Build() + + diagram_context = diagram.CreateDefaultContext() + cart_pole_context = plant.GetMyMutableContextFromRoot(diagram_context) + + plant.get_actuation_input_port().FixValue(cart_pole_context, 0) + + cart_slider = plant.GetJointByName("CartSlider") + pole_pin = plant.GetJointByName("PolePin") + + cart_slider.set_translation(context=cart_pole_context, translation=0.0) + pole_pin.set_angle(context=cart_pole_context, angle=2.0) + + # Create a simulator for the system + simulator = Simulator(diagram, diagram_context) + simulator.Initialize() + simulator_context = simulator.get_mutable_context() + simulator.set_publish_every_time_step(False) + simulator.set_target_realtime_rate(1.0) + + # Step the simulator in 0.1s intervals + step = 0.1 + while simulator_context.get_time() < args.simulation_time: + next_time = min( + simulator_context.get_time() + step, args.simulation_time, + ) + simulator.AdvanceTo(next_time) + + +if __name__ == '__main__': + main() diff --git a/drake_ros_examples/examples/rgbd/rgbd.rviz b/drake_ros_examples/examples/rgbd/rgbd.rviz new file mode 100644 index 00000000..6cfc56b6 --- /dev/null +++ b/drake_ros_examples/examples/rgbd/rgbd.rviz @@ -0,0 +1,273 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Camera1/Topic1 + - /Camera2/Topic1 + - /DepthCloud1/Auto Size1 + Splitter Ratio: 0.6238244771957397 + Tree Height: 339 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /Interact1 + - /Measure1 + - /Publish Point1 + - /2D Goal Pose1 + - /2D Pose Estimate1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: true + Name: Time + SyncMode: 1 + SyncSource: DepthCloud +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/Camera + Enabled: true + Far Plane Distance: 100 + Image Rendering: background and overlay + Name: Camera + Overlay Alpha: 0.5 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /color/image_raw + Value: true + Visibility: + Camera: true + DepthCloud: true + Grid: true + MarkerArray: true + TF: true + Value: true + Zoom Factor: 1 + - Class: rviz_default_plugins/Camera + Enabled: true + Far Plane Distance: 100 + Image Rendering: background and overlay + Name: Camera + Overlay Alpha: 0.5 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /depth/image_raw + Value: true + Visibility: + Camera: true + DepthCloud: true + Grid: true + MarkerArray: true + TF: true + Value: true + Zoom Factor: 1 + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + CartPole/Cart: + Value: true + CartPole/Cart/16: + Value: true + CartPole/Pole: + Value: true + CartPole/Pole/24: + Value: true + CartPole/camera_optical: + Value: true + CartPole/camera_optical/23: + Value: true + world: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + world: + CartPole/Cart: + {} + CartPole/Cart/16: + {} + CartPole/Pole: + {} + CartPole/Pole/24: + {} + CartPole/camera_optical: + {} + CartPole/camera_optical/23: + {} + Update Interval: 0 + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + plant: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /scene_markers/visual + Value: true + - Alpha: 1 + Auto Size: + Auto Size Factor: 1 + Value: true + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/DepthCloud + Color: 255; 255; 255 + Color Image Topic: /color/image_raw + Color Transformer: RGB8 + Color Transport Hint: raw + Decay Time: 0 + Depth Map Topic: /depth/image_raw + Depth Map Transport Hint: raw + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: DepthCloud + Occlusion Compensation: + Occlusion Time-Out: 30 + Value: false + Position Transformer: XYZ + Queue Size: 5 + Selectable: true + Size (Pixels): 3 + Style: Flat Squares + Topic Filter: true + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: goal_pose + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: initialpose + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 5.136790752410889 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.5503758192062378 + Y: -0.7238019108772278 + Z: 0.3039085865020752 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.10979700833559036 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.3554128408432007 + Saved: ~ +Window Geometry: + Camera: + collapsed: false + Displays: + collapsed: false + Height: 995 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001fb0000034afc020000000ffb0000001200530065006c0065006300740069006f006e000000003d0000007f0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001de000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000221000000a90000002800fffffffb0000000a0049006d0061006700650100000194000000bc0000000000000000fb0000000c00430061006d00650072006101000002d0000000b70000002800fffffffb0000000a0049006d0061006700650100000210000001050000000000000000fb0000000a0049006d0061006700650100000256000000bf0000000000000000fb0000000c00430061006d0065007200610100000202000000a20000000000000000fb0000000c00430061006d00650072006101000002aa000000dd0000000000000000000000010000010f0000034afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000034a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000039fc0100000002fb0000000800540069006d00650100000000000007800000031f00fffffffb0000000800540069006d006501000000000000045000000000000000000000046a0000034a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 1920 + Y: 24 diff --git a/drake_ros_examples/examples/rgbd/rgbd.sdf b/drake_ros_examples/examples/rgbd/rgbd.sdf new file mode 100644 index 00000000..1c1653ca --- /dev/null +++ b/drake_ros_examples/examples/rgbd/rgbd.sdf @@ -0,0 +1,123 @@ + + + + + + + 10.0 + + + 1.0e-20 + 1.0e-20 + 1.0e-20 + 0 + 0 + 0 + + + + + + 0.24 0.12 0.12 + + + + + + + 0.24 0.12 0.12 + + + + + + + 0.0 1.0 0.0 1.57 3.14 0.0 + + + + + + 0 0 -0.5 0 0 0 + + 1.0 + + + 1.0e-20 + 1.0e-20 + 1.0e-20 + 0 + 0 + 0 + + + + + + 0.05 + + + + + + + 0.05 + + + + + 0 0 0.25 0 0 0 + + + 0.025 + 0.5 + + + + + 0 0 0.25 0 0 0 + + + 0.025 + 0.5 + + + + + + world + Cart + + 1.0 0.0 0.0 + + + + + world + camera_optical + + + + + 0 0 0.5 0 0 0 + Cart + Pole + + 0.0 -1.0 0.0 + + + 0 + + + + + diff --git a/drake_ros_examples/examples/rgbd/test/rgbd_test.py b/drake_ros_examples/examples/rgbd/test/rgbd_test.py new file mode 100644 index 00000000..a0c22e45 --- /dev/null +++ b/drake_ros_examples/examples/rgbd/test/rgbd_test.py @@ -0,0 +1,28 @@ +import os +from subprocess import run + +from bazel_ros_env import ( + Rlocation, + make_bazel_runfiles_env, + make_unique_ros_isolation_env, +) + + +def make_env(): + env = dict(os.environ) + env.update(make_bazel_runfiles_env()) + env.update(make_unique_ros_isolation_env()) + return env + + +def main(): + env = make_env() + cc_bin = Rlocation("drake_ros_examples/examples/rgbd/rgbd") + py_bin = Rlocation("drake_ros_examples/examples/rgbd/rgbd_py") + run([cc_bin, "--simulation_time=0.01"], env=env, check=True) + run([py_bin, "--simulation_time=0.01"], env=env, check=True) + print("[ Done ]") + + +if __name__ == '__main__': + main() diff --git a/drake_ros_examples/package.xml b/drake_ros_examples/package.xml index 9ba3bd55..8d86f02b 100644 --- a/drake_ros_examples/package.xml +++ b/drake_ros_examples/package.xml @@ -10,10 +10,13 @@ ament_cmake_ros + ament_index_cpp drake_ros std_msgs libgflags-dev + rclcpp_components + ament_cmake_clang_format ament_lint_auto ament_lint_common