Example showing usage of resource from an external ROS package in Drake

examples/external_ros_resources/README.md

@@ -0,0 +1,32 @@
+# Using External ROS Resources in Drake
+
+This example shows how to use external ROS resources, such as URDF files in Drake.
+It uses the [`ament_cmake`](https://index.ros.org/doc/ros2/Tutorials/Ament-CMake-Documentation/)
+build system and [`colcon`](https://colcon.readthedocs.io) command line tool, 
+with an installed instance of the Drake [binary packages](https://drake.mit.edu/from_binary.html).
+
+## Instructions
+
+To use `ament_cmake` and `colcon` from the ROS 2 Dashing package archive, install
+the required packages and configure your environment as follows:
+```
+sudo ../../scripts/setup/linux/ubuntu/bionic/install_prereqs --ros-dashing
+source /opt/ros/dashing/setup.bash
+```
+
+To build the `drake_example_pendulum` example:
+```
+colcon build --cmake-args "-DCMAKE_PREFIX_PATH=/path/to/drake;$CMAKE_PREFIX_PATH"
+```
+
+*If the Drake binary package is installed to `/opt/drake`, you may omit the
+`--cmake-args <args>` argument.*
+
+
+To run the `drake_example_pendulum` example, source the workspace and run the
+executable:
+
+```
+source install/setup.bash
+drake_example_pendulum
+```

examples/external_ros_resources/drake_example_pendulum/CMakeLists.txt

@@ -0,0 +1,41 @@
+# Copyright (c) 2021, Massachusetts Institute of Technology.
+# Copyright (c) 2021, Toyota Research Institute.
+# All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+cmake_minimum_required(VERSION 3.10)
+project(drake_example_pendulum)
+
+set(CMAKE_CXX_EXTENSIONS OFF)
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+find_package(ament_cmake CONFIG REQUIRED)
+find_package(drake CONFIG REQUIRED PATHS /opt/drake)
+
+add_executable(${PROJECT_NAME}
+  src/drake_example_pendulum.cc
+)
+
+target_link_libraries(${PROJECT_NAME} drake::drake)
+
+install(FILES LICENSE
+  DESTINATION share/doc/${PROJECT_NAME}
+)
+
+install(TARGETS ${PROJECT_NAME}
+  RUNTIME DESTINATION bin
+)
+
+ament_package()

examples/external_ros_resources/drake_example_pendulum/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>drake_example_pendulum</name>
+  <version>0.1.0</version>
+  <description>Example showing usage of resource from an external ROS package in Drake</description>
+  <author email="[email protected]">Drake Users</author>
+  <maintainer email="[email protected]">Drake Users</maintainer>
+  <license>Apache License 2.0</license>
+  <url type="website">https://drake.mit.edu</url>
+  <url type="bugtracker">https://github.com/RobotLocomotion/drake-ros/issues</url>
+  <url type="repository">https://github.com/RobotLocomotion/drake-ros</url>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <exec_depend>drake_example_pendulum_description</exec_depend>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

examples/external_ros_resources/drake_example_pendulum/src/drake_example_pendulum.cc

@@ -0,0 +1,87 @@
+/*****************************************************************************
+ * Copyright (c) 2017-2021, Massachusetts Institute of Technology.
+ * Copyright (c) 2017-2021, Toyota Research Institute.
+ * All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ *****************************************************************************/
+
+#include <iostream>
+
+#include <drake/geometry/drake_visualizer.h>
+#include <drake/multibody/parsing/parser.h>
+#include <drake/multibody/plant/multibody_plant.h>
+#include <drake/multibody/tree/revolute_joint.h>
+#include <drake/systems/analysis/simulator.h>
+#include <drake/systems/framework/diagram_builder.h>
+
+int main(int argc, char** argv) {
+  drake::systems::DiagramBuilder<double> builder;
+  auto [pendulum_plant, scene_graph] =
+      drake::multibody::AddMultibodyPlantSceneGraph(&builder, 0.0);
+  auto parser = drake::multibody::Parser(&pendulum_plant, &scene_graph);
+
+  // Populate from AMENT_PREFIX_PATH environment variable to find URDF files and
+  // their resources, such as meshes.
+  parser.package_map().PopulateFromEnvironment("AMENT_PREFIX_PATH");
+  const std::string pendulum_desc_package =
+      "drake_example_pendulum_description";
+
+  if (!parser.package_map().Contains(pendulum_desc_package))
+  {
+    std::cerr << "The package: '" << pendulum_desc_package
+              << "' could not be found. Have you sourced your ROS workspace?"
+              << std::endl;
+    return -1;
+  }
+
+  const std::string package_path =
+      parser.package_map().GetPath(pendulum_desc_package);
+  auto model_instance = parser.AddModelFromFile(
+      package_path + "/urdf/drake_example_pendulum.urdf");
+
+  const auto& base_link =
+      pendulum_plant.GetFrameByName("base_link", model_instance);
+  // Weld the base_link to the world so the pendulum doesn't fall forever.
+  pendulum_plant.WeldFrames(pendulum_plant.world_frame(), base_link, {});
+
+  pendulum_plant.Finalize();
+
+  // The following line can be commented out if Drake visualizer is not needed.
+  drake::geometry::DrakeVisualizer<double>::AddToBuilder(&builder, scene_graph);
+
+  auto diagram = builder.Build();
+
+  drake::systems::Simulator<double> simulator(*diagram);
+  simulator.set_target_realtime_rate(1.0);
+
+  drake::systems::Context<double>& pendulum_context =
+      diagram->GetMutableSubsystemContext(pendulum_plant,
+                                          &simulator.get_mutable_context());
+
+  drake::VectorX<double> joint_position(1);
+  joint_position << 0.5;
+  pendulum_plant.SetPositions(&pendulum_context, joint_position);
+
+  simulator.set_monitor([&pendulum = pendulum_plant](const auto& root_context) {
+    auto &context = pendulum.GetMyContextFromRoot(root_context);
+    std::cout << fmt::format("{:0.3f}: {}", context.get_time(),
+                             pendulum.GetPositions(context).transpose())
+              << std::endl;
+    return drake::systems::EventStatus::Succeeded();
+  });
+
+  simulator.AdvanceTo(10);
+  return 0;
+}

examples/external_ros_resources/drake_example_pendulum_description/CMakeLists.txt

@@ -0,0 +1,25 @@
+# Copyright (c) 2021, Massachusetts Institute of Technology.
+# Copyright (c) 2021, Toyota Research Institute.
+# All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+cmake_minimum_required(VERSION 3.10)
+project(drake_example_pendulum_description)
+
+find_package(ament_cmake CONFIG REQUIRED)
+
+install(DIRECTORY urdf meshes
+  DESTINATION share/${PROJECT_NAME})
+
+ament_package()

examples/external_ros_resources/drake_example_pendulum_description/meshes/base_link.mtl

@@ -0,0 +1,22 @@
+# Blender MTL File: 'drake_base_link.blend'
+# Material Count: 2
+
+newmtl Material.001
+Ns 323.999994
+Ka 1.000000 1.000000 1.000000
+Kd 0.800000 0.000000 0.025331
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.000000
+d 1.000000
+illum 2
+
+newmtl Material.002
+Ns 323.999994
+Ka 1.000000 1.000000 1.000000
+Kd 0.100046 0.420499 0.800000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.000000
+d 1.000000
+illum 2