(moveit_py) node can have multiple param files (moveit#2393)

A node is often started with multiple param files. Using `list.index` only returns the first occurrence. The new code searches for all occurrences.

moveit_py/moveit/__init__.py

@@ -1,3 +1,3 @@
 from moveit.core import *
 from moveit.planning import *
-from moveit.utils import get_launch_params_filepath
+from moveit.utils import get_launch_params_filepaths

moveit_py/moveit/utils.py

@@ -32,6 +32,8 @@
 #
 # Author: Peter David Fagan
 
+from typing import List, Optional
+
 import sys
 import traceback
 import yaml
@@ -47,12 +49,17 @@ def create_params_file_from_dict(params, node_name):
         return param_file_path
 
 
-def get_launch_params_filepath():
+def get_launch_params_filepaths(cli_args: Optional[List[str]] = None) -> List[str]:
     """
-    A utility that returns the path value after argument --params-file.
+    A utility that returns the path value after the --params-file arguments.
     """
-    try:
+    if cli_args is None:
         cli_args = sys.argv
-        return sys.argv[sys.argv.index("--params-file") + 1]
-    except ValueError:
-        return "Failed to parse params file path from command line arguments. Check that --params-file command line argument is specified."
+
+    try:
+        indexes = [i for i, v in enumerate(cli_args) if v == "--params-file"]
+        return [cli_args[i + 1] for i in indexes]
+    except IndexError:
+        return [
+            "Failed to parse params file paths from command line arguments. Check that --params-file command line argument is specified."
+        ]

moveit_py/moveit/utils.pyi

@@ -1,2 +1,4 @@
+from typing import List, Optional
+
 def create_params_file_from_dict(params, node_name): ...
-def get_launch_params_filepath(): ...
+def get_launch_params_filepaths(cli_args: Optional[List[str]] = ...) -> List[str]: ...

moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp

@@ -54,7 +54,7 @@ void init_moveit_py(py::module& m)
   The MoveItPy class is the main interface to the MoveIt Python API. It is a wrapper around the MoveIt C++ API.
 									     )")
 
-      .def(py::init([](const std::string& node_name, const std::string& launch_params_filepath,
+      .def(py::init([](const std::string& node_name, const std::vector<std::string>& launch_params_filepaths,
                        const py::object& config_dict, bool provide_planning_service) {
              static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_cpp_initializer");
 
@@ -70,9 +70,14 @@ void init_moveit_py(py::module& m)
                    utils.attr("create_params_file_from_dict")(config_dict, node_name).cast<std::string>();
                launch_arguments = { "--ros-args", "--params-file", params_filepath };
              }
-             else if (!launch_params_filepath.empty())
+             else if (!launch_params_filepaths.empty())
              {
-               launch_arguments = { "--ros-args", "--params-file", launch_params_filepath };
+               launch_arguments = { "--ros-args" };
+               for (const auto& launch_params_filepath : launch_params_filepaths)
+               {
+                 launch_arguments.push_back("--params-file");
+                 launch_arguments.push_back(launch_params_filepath);
+               }
              }
 
              // Initialize ROS, pass launch arguments with rclcpp::init()
@@ -125,7 +130,8 @@ void init_moveit_py(py::module& m)
              return moveit_cpp_ptr;
            }),
            py::arg("node_name") = "moveit_py",
-           py::arg("launch_params_filepath") = utils.attr("get_launch_params_filepath")().cast<std::string>(),
+           py::arg("launch_params_filepaths") =
+               utils.attr("get_launch_params_filepaths")().cast<std::vector<std::string>>(),
            py::arg("config_dict") = py::none(), py::arg("provide_planning_service") = true,
            py::return_value_policy::take_ownership,
            R"(