Support <gazebo_ros> in package.xml exports

ros_gz_sim/README.md

@@ -37,3 +37,77 @@ See more options with:
 ```
 ros2 run ros_gz_sim create --helpshort
 ```
+
+### Using `<gazebo_ros>` to export model paths in `package.xml`
+
+The `<gazebo_ros>` tag inside the `<export>` tag of a `package.xml` file can be
+used to add paths to `GZ_SIM_RESOURCE_PATH` and `GZ_SIM_SYSTEM_PLUGIN_PATH`,
+which are environment variables used to configure Gazebo search paths for
+resources (e.g. SDFormat files, meshes, etc) and plugins respectively.
+
+The values in the attributes `gazebo_model_path` and `gazebo_media_path` are
+appended to `GZ_SIM_RESOURCE_PATH`. The value of `plugin_path` is appended to
+`GZ_SIM_SYSTEM_PLUGIN_PATH`. See the
+[Finding resources](https://gazebosim.org/api/sim/8/resources.html) tutorial to
+learn more about these environment variables.
+
+The keyword `${prefix}` can be used when setting these values and it will be
+expanded to the package's share path (i.e., the value of
+`ros2 pkg prefix --share <package name>`)
+
+```xml
+<export>
+   <gazebo_ros gazebo_model_path="${prefix}/models"/>
+   <gazebo_ros gazebo_media_path="${prefix}/media"/>
+   <gazebo_ros plugin_path="${prefix}/plugins"/>
+</export>
+
+```
+
+Thus the required directory needs to be installed from `CMakeLists.txt`
+
+```cmake
+install(DIRECTORY models
+    DESTINATION share/${PROJECT_NAME})
+```
+
+In order to reference the models in a ROS package unambiguously, it is
+recommended to set the value of `gazebo_model_path` to be the parent
+of the `prefix`.
+
+```xml
+<export>
+   <gazebo_ros gazebo_model_path="${prefix}/../"/>
+</export>
+
+```
+
+Consider an example where we have a ROS package called `my_awesome_pkg`
+and it contains an SDFormat model cool `cool_robot`:
+
+```bash
+my_awesome_pkg
+├── models
+│   └── cool_robot
+│       ├── model.config
+│       └── model.sdf
+└── package.xml
+```
+
+With `gazebo_model_path="${prefix}/../` set up, we can
+reference the `cool_robot` model in a world file using the package name
+in the `uri`:
+
+```xml
+<sdf version="1.6">
+  <world name="default">
+    <include>
+      <uri>package://my_awesome_pkg/models/cool_robot</uri>
+    </include>
+  </world>
+</sdf>
+```
+
+However, if we set `gazebo_model_path=${prefix}/models`, we would
+need to reference `cool_robot` as `package://cool_robot`, which
+might have a name conflict with other models in the system.

ros_gz_sim/launch/gz_sim.launch.py.in

@@ -14,20 +14,99 @@
 
 """Launch Gazebo Sim with command line arguments."""
 
+import os
 from os import environ
 
+from ament_index_python.packages import get_package_share_directory
+from catkin_pkg.package import InvalidPackage, PACKAGE_MANIFEST_FILENAME, parse_package
+from ros2pkg.api import get_package_names
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, OpaqueFunction
 from launch.actions import ExecuteProcess, Shutdown
 from launch.substitutions import LaunchConfiguration
 
+# Copied from https://github.com/ros-simulation/gazebo_ros_pkgs/blob/79fd94c6da76781a91499bc0f54b70560b90a9d2/gazebo_ros/scripts/gazebo_ros_paths.py
+"""
+Search for model, plugin and media paths exported by packages.
+
+e.g.  <export>
+          <gazebo_ros gazebo_model_path="${prefix}/../"/>
+          <gazebo_ros gazebo_media_path="${prefix}/../"/>
+      </export>
+${prefix} is replaced by package's share directory in install.
+
+Thus the required directory needs to be installed from CMakeLists.txt
+e.g.  install(DIRECTORY models
+          DESTINATION share/${PROJECT_NAME})
+"""
+
+class GazeboRosPaths:
+
+    @staticmethod
+    def get_paths():
+        gazebo_model_path = []
+        gazebo_plugin_path = []
+        gazebo_media_path = []
+
+        for package_name in get_package_names():
+            package_share_path = get_package_share_directory(package_name)
+            package_file_path = os.path.join(package_share_path, PACKAGE_MANIFEST_FILENAME)
+            if os.path.isfile(package_file_path):
+                try:
+                    package = parse_package(package_file_path)
+                except InvalidPackage:
+                    continue
+                for export in package.exports:
+                    if export.tagname == 'gazebo_ros':
+                        if 'gazebo_model_path' in export.attributes:
+                            xml_path = export.attributes['gazebo_model_path']
+                            xml_path = xml_path.replace('${prefix}', package_share_path)
+                            gazebo_model_path.append(xml_path)
+                        if 'plugin_path' in export.attributes:
+                            xml_path = export.attributes['plugin_path']
+                            xml_path = xml_path.replace('${prefix}', package_share_path)
+                            gazebo_plugin_path.append(xml_path)
+                        if 'gazebo_media_path' in export.attributes:
+                            xml_path = export.attributes['gazebo_media_path']
+                            xml_path = xml_path.replace('${prefix}', package_share_path)
+                            gazebo_media_path.append(xml_path)
+
+        gazebo_model_path = os.pathsep.join(gazebo_model_path + gazebo_media_path)
+        gazebo_plugin_path = os.pathsep.join(gazebo_plugin_path)
+
+        return gazebo_model_path, gazebo_plugin_path
+
 def launch_gz(context, *args, **kwargs):
-    env = {'GZ_SIM_SYSTEM_PLUGIN_PATH':
-           ':'.join([environ.get('GZ_SIM_SYSTEM_PLUGIN_PATH', default=''),
-                     environ.get('LD_LIBRARY_PATH', default='')]),
-           'IGN_GAZEBO_SYSTEM_PLUGIN_PATH':  # TODO(CH3): To support pre-garden. Deprecated.
-                      ':'.join([environ.get('IGN_GAZEBO_SYSTEM_PLUGIN_PATH', default=''),
-                                environ.get('LD_LIBRARY_PATH', default='')])}
+    model_paths, plugin_paths = GazeboRosPaths.get_paths()
+
+    env = {
+        "GZ_SIM_SYSTEM_PLUGIN_PATH": os.pathsep.join(
+            [
+                environ.get("GZ_SIM_SYSTEM_PLUGIN_PATH", default=""),
+                environ.get("LD_LIBRARY_PATH", default=""),
+                plugin_paths,
+            ]
+        ),
+        "IGN_GAZEBO_SYSTEM_PLUGIN_PATH": os.pathsep.join(  # TODO(azeey): To support pre-garden. Deprecated.
+            [
+                environ.get("IGN_GAZEBO_SYSTEM_PLUGIN_PATH", default=""),
+                environ.get("LD_LIBRARY_PATH", default=""),
+                plugin_paths,
+            ]
+        ),
+        "GZ_SIM_RESOURCE_PATH": os.pathsep.join(
+            [
+                environ.get("GZ_SIM_RESOURCE_PATH", default=""),
+                model_paths,
+            ]
+        ),
+        "IGN_GAZEBO_RESOURCE_PATH": os.pathsep.join(  # TODO(azeey): To support pre-garden. Deprecated.
+            [
+                environ.get("IGN_GAZEBO_RESOURCE_PATH", default=""),
+                model_paths,
+            ]
+        ),
+    }
 
     gz_args = LaunchConfiguration('gz_args').perform(context)
     gz_version = LaunchConfiguration('gz_version').perform(context)