diff --git a/moveit_py/moveit/__init__.py b/moveit_py/moveit/__init__.py index 0f44d4a088..d7c49b2af2 100644 --- a/moveit_py/moveit/__init__.py +++ b/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 diff --git a/moveit_py/moveit/utils.py b/moveit_py/moveit/utils.py index a40cd12284..18bc7ae553 100644 --- a/moveit_py/moveit/utils.py +++ b/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." + ] diff --git a/moveit_py/moveit/utils.pyi b/moveit_py/moveit/utils.pyi index 1b43712276..d840a3ffed 100644 --- a/moveit_py/moveit/utils.pyi +++ b/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]: ... diff --git a/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp index 82e05c762f..ed43b05e24 100644 --- a/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp +++ b/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& 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(); 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(), + py::arg("launch_params_filepaths") = + utils.attr("get_launch_params_filepaths")().cast>(), py::arg("config_dict") = py::none(), py::arg("provide_planning_service") = true, py::return_value_policy::take_ownership, R"(