From b0b75133ad6de545f6634e5380878c4b706e15dc Mon Sep 17 00:00:00 2001 From: Ian McMahon Date: Fri, 23 Oct 2020 18:06:00 -0400 Subject: [PATCH] Adds realsense-ros dependency for URDF and meshes (#14203) This commit provides Drake the ability to fetch the realsense-ros upstream repository during build: IntelRealSense/realsense-ros: - It gives Drake access to the realsense d415 xacro and mesh files - After the download, xacro files are modified to cleanly stitch into a URDF in a Drake environment (rather than their native ROS environment) - A test is provided that is closely based (copied + slightly modified) on the YCB meshes simple parsing tests --- manipulation/models/BUILD.bazel | 1 + .../models/realsense2_description/BUILD.bazel | 46 +++++++++++ .../test/realsense_parse_test.cc | 82 +++++++++++++++++++ tools/workspace/default.bzl | 3 + .../workspace/intel_realsense_ros/BUILD.bazel | 5 ++ tools/workspace/intel_realsense_ros/files.bzl | 12 +++ .../intel_realsense_ros/package.BUILD.bazel | 23 ++++++ .../intel_realsense_ros/repository.bzl | 24 ++++++ 8 files changed, 196 insertions(+) create mode 100644 manipulation/models/realsense2_description/BUILD.bazel create mode 100644 manipulation/models/realsense2_description/test/realsense_parse_test.cc create mode 100644 tools/workspace/intel_realsense_ros/BUILD.bazel create mode 100644 tools/workspace/intel_realsense_ros/files.bzl create mode 100644 tools/workspace/intel_realsense_ros/package.BUILD.bazel create mode 100644 tools/workspace/intel_realsense_ros/repository.bzl diff --git a/manipulation/models/BUILD.bazel b/manipulation/models/BUILD.bazel index e5cdd89b512b..b0c8c68561ac 100644 --- a/manipulation/models/BUILD.bazel +++ b/manipulation/models/BUILD.bazel @@ -11,6 +11,7 @@ install( "//manipulation/models/franka_description:install_data", "//manipulation/models/iiwa_description:install_data", "//manipulation/models/jaco_description:install_data", + "//manipulation/models/realsense2_description:install_data", "//manipulation/models/wsg_50_description:install_data", "//manipulation/models/ycb:install_data", ], diff --git a/manipulation/models/realsense2_description/BUILD.bazel b/manipulation/models/realsense2_description/BUILD.bazel new file mode 100644 index 000000000000..b9e7d24f658a --- /dev/null +++ b/manipulation/models/realsense2_description/BUILD.bazel @@ -0,0 +1,46 @@ +# -*- python -*- + +load("@drake//tools/lint:lint.bzl", "add_lint_tests") +load("@drake//tools/install:install_data.bzl", "install_data") +load( + "@drake//tools/skylark:drake_cc.bzl", + "drake_cc_googletest", +) +load("@drake//tools/workspace:forward_files.bzl", "forward_files") +load( + "@drake//tools/workspace/intel_realsense_ros:files.bzl", + "realsense2_description_files", +) + +# This package is public so that other packages can refer to +# individual files in models from their bazel rules. +package( + default_visibility = ["//visibility:public"], +) + +_REALSENSE2_DESCRIPTION_FILES = forward_files( + srcs = ["@intel_realsense_ros//:" + + x for x in realsense2_description_files()], + dest_prefix = "", + strip_prefix = "@intel_realsense_ros//:realsense2_description/", + visibility = ["//visibility:private"], +) + +install_data( + extra_prod_models = _REALSENSE2_DESCRIPTION_FILES, +) + +drake_cc_googletest( + name = "realsense_parse_test", + data = [":models"], + deps = [ + "//common:find_resource", + "//geometry:geometry_visualization", + "//geometry:scene_graph", + "//multibody/parsing", + "//systems/analysis:simulator", + "//systems/framework:diagram_builder", + ], +) + +add_lint_tests() diff --git a/manipulation/models/realsense2_description/test/realsense_parse_test.cc b/manipulation/models/realsense2_description/test/realsense_parse_test.cc new file mode 100644 index 000000000000..e8ffdbba3cd3 --- /dev/null +++ b/manipulation/models/realsense2_description/test/realsense_parse_test.cc @@ -0,0 +1,82 @@ +/* +@file +Parses and visualizes Realsense d415. + +If you wish to visualize this mesh, in its own terminal run: + + $ bazel-bin/tools/drake_visualizer + +In a new terminal, run example of showing a Realsense d415: + + $ bazel run \ + //manipulation/models/realsense2_description:realsense_parse_test -- \ + --visualize=true + +*/ + +#include + +#include +#include +#include + +#include "drake/common/find_resource.h" +#include "drake/geometry/geometry_visualization.h" +#include "drake/geometry/scene_graph.h" +#include "drake/multibody/parsing/parser.h" +#include "drake/systems/analysis/simulator.h" +#include "drake/systems/framework/diagram_builder.h" + +DEFINE_bool( + visualize, false, "Publish model to Drake Visualizer."); + +namespace drake { +namespace manipulation { +namespace { + +using geometry::ConnectDrakeVisualizer; +using geometry::SceneGraph; +using multibody::AddMultibodyPlantSceneGraph; +using multibody::Parser; +using systems::DiagramBuilder; +using systems::Simulator; + +class ParseTest : public testing::TestWithParam {}; + +TEST_P(ParseTest, ParsesUrdfAndVisualizes) { + const std::string object_name = GetParam(); + const std::string filename = FindResourceOrThrow(fmt::format( + "drake/manipulation/models/realsense2_description/urdf/{}.urdf", + object_name)); + + DiagramBuilder builder; + auto [plant, scene_graph] = AddMultibodyPlantSceneGraph(&builder, 0.0); + // Record the default number of models before a new model is added. + const int default_num_models = plant.num_model_instances(); + + // Check to ensure URDF is parsable. + EXPECT_NO_THROW(Parser(&plant).AddModelFromFile(filename)); + + // Ensure there was exactly one model instance added for the new model. + EXPECT_EQ(plant.num_model_instances() - default_num_models, 1); + + // Visualize via publishing, if requested. + if (FLAGS_visualize) { + ConnectDrakeVisualizer(&builder, scene_graph); + plant.Finalize(); + auto diagram = builder.Build(); + drake::log()->info("Visualize: {}", object_name); + Simulator simulator(*diagram); + simulator.Initialize(); + diagram->Publish(simulator.get_context()); + } +} + +// Note: We use a test suite here, even if currently for only one value, to +// allow for easily adding more models under test in the future without +// rewriting the test code. +INSTANTIATE_TEST_SUITE_P(RealsenseD415, ParseTest, testing::Values("d415")); + +} // namespace +} // namespace manipulation +} // namespace drake diff --git a/tools/workspace/default.bzl b/tools/workspace/default.bzl index ce37deff4608..f287c3028d5f 100644 --- a/tools/workspace/default.bzl +++ b/tools/workspace/default.bzl @@ -34,6 +34,7 @@ load("@drake//tools/workspace/gtest:repository.bzl", "gtest_repository") load("@drake//tools/workspace/gurobi:repository.bzl", "gurobi_repository") load("@drake//tools/workspace/ibex:repository.bzl", "ibex_repository") load("@drake//tools/workspace/ignition_math:repository.bzl", "ignition_math_repository") # noqa +load("@drake//tools/workspace/intel_realsense_ros:repository.bzl", "intel_realsense_ros_repository") # noqa load("@drake//tools/workspace/ipopt:repository.bzl", "ipopt_repository") load("@drake//tools/workspace/jsoncpp:repository.bzl", "jsoncpp_repository") load("@drake//tools/workspace/lapack:repository.bzl", "lapack_repository") @@ -161,6 +162,8 @@ def add_default_repositories(excludes = [], mirrors = DEFAULT_MIRRORS): ibex_repository(name = "ibex") if "ignition_math" not in excludes: ignition_math_repository(name = "ignition_math", mirrors = mirrors) + if "intel_realsense_ros" not in excludes: + intel_realsense_ros_repository(name = "intel_realsense_ros", mirrors = mirrors) # noqa if "ipopt" not in excludes: ipopt_repository(name = "ipopt") if "jsoncpp" not in excludes: diff --git a/tools/workspace/intel_realsense_ros/BUILD.bazel b/tools/workspace/intel_realsense_ros/BUILD.bazel new file mode 100644 index 000000000000..7198e3bb9b53 --- /dev/null +++ b/tools/workspace/intel_realsense_ros/BUILD.bazel @@ -0,0 +1,5 @@ +# -*- python -*- + +load("//tools/lint:lint.bzl", "add_lint_tests") + +add_lint_tests() diff --git a/tools/workspace/intel_realsense_ros/files.bzl b/tools/workspace/intel_realsense_ros/files.bzl new file mode 100644 index 000000000000..4a38ff61f1a9 --- /dev/null +++ b/tools/workspace/intel_realsense_ros/files.bzl @@ -0,0 +1,12 @@ +# -*- python -*- + +# List files to be exported from the upstream +# IntelRealsense/realsense-ros repository + +def realsense2_description_files(): + return [ + "realsense2_description/LICENSE", + "realsense2_description/meshes/d415.stl", + "realsense2_description/package.xml", + "realsense2_description/urdf/d415.urdf", + ] diff --git a/tools/workspace/intel_realsense_ros/package.BUILD.bazel b/tools/workspace/intel_realsense_ros/package.BUILD.bazel new file mode 100644 index 000000000000..e7ceed8ead32 --- /dev/null +++ b/tools/workspace/intel_realsense_ros/package.BUILD.bazel @@ -0,0 +1,23 @@ +# -*- python -*- + +load("@drake//tools/workspace/ros_xacro:defs.bzl", "xacro_file") + +licenses(["notice"]) # Apache-2.0 + +package(default_visibility = ["//visibility:public"]) + +xacro_file( + name = "realsense2_description/urdf/d415.urdf", + src = "realsense2_description/urdf/test_d415_camera.urdf.xacro", + data = [ + "realsense2_description/urdf/_d415.urdf.xacro", + "realsense2_description/urdf/_materials.urdf.xacro", + "realsense2_description/urdf/_usb_plug.urdf.xacro", + ], +) + +exports_files( + srcs = glob([ + "realsense2_description/**", + ], allow_empty = False), +) diff --git a/tools/workspace/intel_realsense_ros/repository.bzl b/tools/workspace/intel_realsense_ros/repository.bzl new file mode 100644 index 000000000000..280cf7aa6fef --- /dev/null +++ b/tools/workspace/intel_realsense_ros/repository.bzl @@ -0,0 +1,24 @@ +# -*- python -*- + +load("@drake//tools/workspace:github.bzl", "github_archive") + +def intel_realsense_ros_repository( + name, + mirrors = None): + github_archive( + name = name, + repository = "IntelRealSense/realsense-ros", + # N.B. Even though 2.2.x series is not the highest-numbered release, we + # are using it here because it aligns with the ROS Melodic version + # released for Ubuntu 18.04. + commit = "2.2.17", + sha256 = "babffbd3b2d20c94431436fc50606fddf8712dd7e9392d9da3fd228b432f9cfe", # noqa + build_file = "@drake//tools/workspace/intel_realsense_ros:package.BUILD.bazel", # noqa + patch_cmds = [ + "cp LICENSE realsense2_description/", + "sed -i.orig -e 's|$(find realsense2_description)/urdf/||' realsense2_description/urdf/*.xacro", # noqa + "sed -i -e 's|$(arg use_nominal_extrinsics)|false|' realsense2_description/urdf/*.xacro", # noqa + "sed -i -e 's|$(arg add_plug)|false|' realsense2_description/urdf/*.xacro", # noqa + ], + mirrors = mirrors, + )