From 543402f8ef5bf73e433e1ff56ff361fb73c14103 Mon Sep 17 00:00:00 2001
From: Eric Cousineau <eric.cousineau@tri.global>
Date: Thu, 30 Mar 2023 17:30:19 -0400
Subject: [PATCH] Add example of PackageMap and an ament resource

---
 drake_ros_examples/BUILD.bazel                | 14 ++++++++-
 drake_ros_examples/CMakeLists.txt             | 13 ++++++++
 drake_ros_examples/WORKSPACE                  |  1 +
 .../test/parse_ros_model_test.py              | 30 +++++++++++++++++++
 4 files changed, 57 insertions(+), 1 deletion(-)
 create mode 100644 drake_ros_examples/test/parse_ros_model_test.py

diff --git a/drake_ros_examples/BUILD.bazel b/drake_ros_examples/BUILD.bazel
index affa3142..c40757b0 100644
--- a/drake_ros_examples/BUILD.bazel
+++ b/drake_ros_examples/BUILD.bazel
@@ -1,4 +1,6 @@
-py_test(
+load("@ros2//:ros_py.bzl", "ros_py_test")
+
+ros_py_test(
     name = "drake_ros_py_import_test",
     srcs = ["test/drake_ros_py_import_test.py"],
     main = "test/drake_ros_py_import_test.py",
@@ -17,3 +19,13 @@ py_library(
         "@rules_python//python/runfiles",
     ],
 )
+
+ros_py_test(
+    name = "parse_ros_model_test",
+    srcs = ["test/parse_ros_model_test.py"],
+    main = "test/parse_ros_model_test.py",
+    # TODO(eric.cousineau): Use a non-test resource and enable this test.
+    tags = ["manual"],
+    data = ["@ros2//:rviz_default_plugins_share"],
+    deps = ["@drake//bindings/pydrake"],
+)
diff --git a/drake_ros_examples/CMakeLists.txt b/drake_ros_examples/CMakeLists.txt
index ad2cadbf..93b86f76 100644
--- a/drake_ros_examples/CMakeLists.txt
+++ b/drake_ros_examples/CMakeLists.txt
@@ -22,6 +22,19 @@ add_subdirectory(examples/rs_flip_flop)
 if(BUILD_TESTING)
   find_package(ament_cmake_clang_format REQUIRED)
   find_package(ament_cmake_cpplint REQUIRED)
+  find_package(ament_cmake_test REQUIRED)
+  find_package(Python3 COMPONENTS Interpreter)
+
+  macro(add_python_test test_name test_file)
+    ament_add_test("${test_name}"
+      COMMAND
+        "${Python3_EXECUTABLE}"
+        "${CMAKE_CURRENT_SOURCE_DIR}/${test_file}"
+        "--junit-xml=${AMENT_TEST_RESULTS_DIR}/drake_ros_examples/${test_name}.xml"
+        "--junit-prefix=drake_ros_examples")
+  endmacro()
+
+  add_python_test(parse_ros_model_test test/parse_ros_model_test.py)
 
   ament_clang_format(CONFIG_FILE .clang-format)
   ament_cpplint()
diff --git a/drake_ros_examples/WORKSPACE b/drake_ros_examples/WORKSPACE
index f7c2b61a..72425e31 100644
--- a/drake_ros_examples/WORKSPACE
+++ b/drake_ros_examples/WORKSPACE
@@ -43,6 +43,7 @@ ROS_PACKAGES = DRAKE_ROS_REQUIRED_PACKAGES + [
     "rclcpp",
     "rclpy",
     "ros2cli_common_extensions",
+    "rviz_default_plugins",
     "rviz2",
     "tf2_ros",
     "tf2_ros_py",
diff --git a/drake_ros_examples/test/parse_ros_model_test.py b/drake_ros_examples/test/parse_ros_model_test.py
new file mode 100644
index 00000000..d06e1a77
--- /dev/null
+++ b/drake_ros_examples/test/parse_ros_model_test.py
@@ -0,0 +1,30 @@
+"""
+Shows how to locate and parse a simple model from the ROS 2 ament index.
+
+For more details on the ament index, please see:
+https://github.com/ament/ament_cmake/blob/master/ament_cmake_core/doc/resource_index.md
+"""  # noqa
+
+import sys
+
+import pytest
+
+from pydrake.multibody.parsing import Parser
+from pydrake.multibody.plant import MultibodyPlant
+
+
+def test_parse_model():
+    plant = MultibodyPlant(time_step=0.0)
+    parser = Parser(plant)
+    # TODO(eric.cousineau): Incorporate `AMENT_PREFIX_PATH` into
+    # PackageMap::PopulateFromRosPackagePath().
+    parser.package_map().PopulateFromEnvironment("AMENT_PREFIX_PATH")
+    # TODO(eric.cousineau): Use a non-test resource.
+    parser.AddModelsFromUrl(
+        "package://rviz_default_plugins/test_meshes/test.urdf"
+    )
+    plant.Finalize()
+
+
+if __name__ == '__main__':
+    sys.exit(pytest.main(sys.argv))