Add features to PackageMap (RobotLocomotion#15064)

- Makes PackageMap copy/move/assignable
- Changes PackageMap::Add to allow duplicate (package, path) to be added without throwing
- Adds PackageMap::AddMap to combine package maps
- Adds PackageMap::GetPackageNames to enumerate the packages mapped by the map

bindings/pydrake/multibody/parsing_py.cc

@@ -31,9 +31,12 @@ PYBIND11_MODULE(parsing, m) {
         .def(py::init<>(), cls_doc.ctor.doc)
         .def("Add", &Class::Add, py::arg("package_name"),
             py::arg("package_path"), cls_doc.Add.doc)
+        .def("AddMap", &Class::AddMap, py::arg("other_map"), cls_doc.AddMap.doc)
         .def("Contains", &Class::Contains, py::arg("package_name"),
             cls_doc.Contains.doc)
         .def("size", &Class::size, cls_doc.size.doc)
+        .def("GetPackageNames", &Class::GetPackageNames,
+            cls_doc.GetPackageNames.doc)
         .def("GetPath", &Class::GetPath, py::arg("package_name"),
             cls_doc.GetPath.doc)
         .def("AddPackageXml", &Class::AddPackageXml, py::arg("filename"),

bindings/pydrake/multibody/test/parsing_test.py

@@ -27,14 +27,19 @@ class TestParsing(unittest.TestCase):
 
     def test_package_map(self):
         dut = PackageMap()
+        dut2 = PackageMap()
         tmpdir = os.environ.get('TEST_TMPDIR')
         model = FindResourceOrThrow(
             "drake/examples/atlas/urdf/atlas_minimal_contact.urdf")
 
-        # Simple coverage test for Add, Contains, size, GetPath, AddPackageXml.
+        # Simple coverage test for Add, AddMap, Contains, size,
+        # GetPackageNames, GetPath, AddPackageXml.
         dut.Add(package_name="root", package_path=tmpdir)
-        self.assertEqual(dut.size(), 1)
+        dut2.Add(package_name="root", package_path=tmpdir)
+        dut.AddMap(dut2)
         self.assertTrue(dut.Contains(package_name="root"))
+        self.assertEqual(dut.size(), 1)
+        self.assertEqual(dut.GetPackageNames(), ["root"])
         self.assertEqual(dut.GetPath(package_name="root"), tmpdir)
         dut.AddPackageXml(filename=FindResourceOrThrow(
             "drake/multibody/parsing/test/box_package/package.xml"))

multibody/parsing/BUILD.bazel

@@ -377,6 +377,7 @@ drake_cc_googletest(
         ":package_map",
         "//common:filesystem",
         "//common:find_resource",
+        "//common/test_utilities:expect_throws_message",
     ],
 )
 

multibody/parsing/package_map.cc

@@ -13,6 +13,7 @@
 #include "drake/common/drake_throw.h"
 #include "drake/common/filesystem.h"
 #include "drake/common/text_logging.h"
+#include "drake/common/unused.h"
 
 namespace drake {
 namespace multibody {
@@ -25,13 +26,18 @@ using tinyxml2::XMLElement;
 PackageMap::PackageMap() {}
 
 void PackageMap::Add(const string& package_name, const string& package_path) {
-  DRAKE_THROW_UNLESS(map_.count(package_name) == 0);
-  if (!filesystem::is_directory(package_path)) {
-    throw std::runtime_error(
-        "Could not add package://" + package_name + " to the search path "
-        "because directory " + package_path + " does not exist");
+  if (!AddPackageIfNew(package_name, package_path)) {
+    throw std::runtime_error(fmt::format(
+        "PackageMap already contains package \"{}\" with path \"{}\" that "
+        "conflicts with provided path \"{}\"",
+        package_name, GetPath(package_name), package_path));
+  }
+}
+
+void PackageMap::AddMap(const PackageMap& other_map) {
+  for (const auto& [package_name, path] : other_map.map_) {
+    Add(package_name, path);
   }
-  map_.insert(make_pair(package_name, package_path));
 }
 
 bool PackageMap::Contains(const string& package_name) const {
@@ -50,6 +56,16 @@ int PackageMap::size() const {
   return map_.size();
 }
 
+std::vector<std::string> PackageMap::GetPackageNames() const {
+  std::vector<std::string> package_names;
+  package_names.reserve(map_.size());
+  for (const auto& [package_name, path] : map_) {
+    unused(path);
+    package_names.push_back(package_name);
+  }
+  return package_names;
+}
+
 const string& PackageMap::GetPath(const string& package_name) const {
   DRAKE_DEMAND(Contains(package_name));
   return map_.at(package_name);
@@ -128,15 +144,20 @@ string GetPackageName(const string& package_xml_file) {
 
 }  // namespace
 
-void PackageMap::AddPackageIfNew(const string& package_name,
+bool PackageMap::AddPackageIfNew(const string& package_name,
     const string& path) {
   DRAKE_DEMAND(!package_name.empty());
   DRAKE_DEMAND(!path.empty());
   // Don't overwrite entries in the map.
   if (!Contains(package_name)) {
     drake::log()->trace(
         "PackageMap: Adding package://{}: {}", package_name, path);
-    Add(package_name, path);
+    if (!filesystem::is_directory(path)) {
+      throw std::runtime_error(
+          "Could not add package://" + package_name + " to the search path "
+          "because directory " + path + " does not exist");
+    }
+    map_.insert(make_pair(package_name, path));
   } else {
     // Don't warn if we've found the same path with a different spelling.
     const string existing_path = GetPath(package_name);
@@ -145,8 +166,10 @@ void PackageMap::AddPackageIfNew(const string& package_name,
           "PackageMap is ignoring newly-found path \"{}\" for package \"{}\""
           " and will continue using the previously-known path at \"{}\".",
           path, package_name, existing_path);
+      return false;
     }
   }
+  return true;
 }
 
 void PackageMap::PopulateUpstreamToDrakeHelper(

multibody/parsing/package_map.h

@@ -2,6 +2,7 @@
 
 #include <map>
 #include <string>
+#include <vector>
 
 #include "drake/common/drake_copyable.h"
 
@@ -11,18 +12,22 @@ namespace multibody {
 /// Maps ROS package names to their full path on the local file system. It is
 /// used by the SDF and URDF parsers when parsing files that reference ROS
 /// packages for resources like mesh files.
-class PackageMap {
+class PackageMap final {
  public:
-  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(PackageMap)
+  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(PackageMap)
 
   /// A constructor that initializes an empty map.
   PackageMap();
 
   /// Adds package @p package_name and its path, @p package_path.
-  /// Throws if @p package_name is already present in this PackageMap, or
-  /// if @p package_path does not exist.
+  /// Throws if @p package_name is already present in this PackageMap with a
+  /// different path, or if @p package_path does not exist.
   void Add(const std::string& package_name, const std::string& package_path);
 
+  /// Adds package->path mappings from another PackageMap @p other_map. Throws
+  /// if the other PackageMap contains the same package with a different path.
+  void AddMap(const PackageMap& other_map);
+
   /// Returns true if and only if this PackageMap contains @p package_name.
   bool Contains(const std::string& package_name) const;
 
@@ -33,6 +38,10 @@ class PackageMap {
   /// Returns the number of entries in this PackageMap.
   int size() const;
 
+  /// Returns the package names in this PackageMap. The order of package names
+  /// returned is unspecified.
+  std::vector<std::string> GetPackageNames() const;
+
   /// Obtains the path associated with package @p package_name. Aborts if no
   /// package named @p package_name exists in this PackageMap.
   const std::string& GetPath(const std::string& package_name) const;
@@ -47,6 +56,8 @@ class PackageMap {
   /// directories, this method adds a new entry into this PackageMap where the
   /// key is the package name as specified within `package.xml` and the
   /// directory's path is the value.
+  /// If a package already known by the PackageMap is found again with a
+  /// conflicting path, a warning is logged and the original path is kept.
   void PopulateFromFolder(const std::string& path);
 
   /// Obtains one or more paths from environment variable
@@ -58,6 +69,10 @@ class PackageMap {
   /// separating them using the ':' symbol. For example, the environment
   /// variable can contain [path 1]:[path 2]:[path 3] to search three different
   /// paths.
+  /// If a package already known by the PackageMap is found again with a
+  /// conflicting path, a warning is logged and the original path is kept. This
+  /// accomodates the expected behavior using ROS_PACKAGE_PATH, where a package
+  /// path corresponds to the "highest" overlay in which that package is found.
   void PopulateFromEnvironment(const std::string& environment_variable);
 
   /// Crawls up the directory tree from @p model_file to `drake`
@@ -80,11 +95,10 @@ class PackageMap {
   // three different paths.
   void CrawlForPackages(const std::string& path);
 
-  // This method is the same as Add() except it first checks to ensure that
-  // package_name is not already in this PackageMap. If it was already present
-  // with a different path, then this method prints a warning and returns
-  // without adding the new path.
-  void AddPackageIfNew(const std::string& package_name,
+  // This method is the same as Add() except if package_name is already present
+  // with a different path, then this method prints a warning and returns false
+  // without adding the new path. Returns true otherwise.
+  bool AddPackageIfNew(const std::string& package_name,
       const std::string& path);
 
   // Recursively searches up the directory path searching for package.xml files.