diff --git a/.gitignore b/.gitignore
index 7c1f968a..993ce587 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 build
 bazel-*
 MODULE.bazel.lock
+*.egg-info
diff --git a/xacro/__init__.py b/xacro/__init__.py
index cb008458..1fb691a0 100644
--- a/xacro/__init__.py
+++ b/xacro/__init__.py
@@ -1040,12 +1040,13 @@ def parse(inp, filename=None):
             f.close()
 
 
-def process_doc(doc, mappings=None, **kwargs):
+def process_doc(doc, mappings=None, extra_find_pkgs={}, **kwargs):
     global verbosity
     verbosity = kwargs.get('verbosity', verbosity)
 
     # set substitution args
     substitution_args_context['arg'] = {} if mappings is None else mappings
+    substitution_args_context['find'] = extra_find_pkgs
 
     # if not yet defined: initialize filestack
     if not filestack:
@@ -1064,7 +1065,7 @@ def process_doc(doc, mappings=None, **kwargs):
 
     # reset substitution args
     substitution_args_context['arg'] = {}
-
+    substitution_args_context['find'] = {}
 
 def open_output(output_filename):
     if output_filename is None:
diff --git a/xacro/substitution_args.py b/xacro/substitution_args.py
index 789f312a..3a2a5dd9 100644
--- a/xacro/substitution_args.py
+++ b/xacro/substitution_args.py
@@ -53,7 +53,6 @@ class ArgException(SubstitutionException):
     """Exception for missing $(arg) values."""
     pass
 
-
 def _eval_env(name):
     """
     Returns the environment variable value or throws exception.
@@ -152,7 +151,12 @@ def _find(resolved, a, args, context):
     if len(args) != 1:
         raise SubstitutionException(
             '$(find pkg) accepts exactly one argument [%s]' % a)
-    return resolved.replace('$(%s)' % a, _eval_find(args[0]))
+    pkg_name_str = args[0]
+    if "find" in context and pkg_name_str in context["find"]:
+        pkg_path_str = context["find"][pkg_name_str]
+    else:
+        pkg_path_str = _eval_find(args[0])
+    return resolved.replace('$(%s)' % a, pkg_path_str)
 
 
 def _eval_arg(name, args):