From 636b98849127ef6dd29b104e1ab421b278b93ee2 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Tue, 20 Jun 2023 22:20:17 +0200 Subject: [PATCH] Fix moveit_py rclcpp::init() (#2223) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Fix moveit_py rclcpp::init() Rclcpp has been initialized without args which was problematic for some use cases like clock simulation. Parameters like use_sim_time:=true need to be passed to rclcpp, also NodeOptions access the global rcl state on construction. Co-authored-by: Jafar Uruç --- .../moveit_ros/moveit_cpp/moveit_cpp.cpp | 44 ++++++++++--------- 1 file changed, 24 insertions(+), 20 deletions(-) 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 b18afa9ec3..82e05c762f 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 @@ -58,40 +58,44 @@ void init_moveit_py(py::module& m) const py::object& config_dict, bool provide_planning_service) { static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_cpp_initializer"); - if (!rclcpp::ok()) - { - RCLCPP_INFO(LOGGER, "Initialize rclcpp"); - rclcpp::init(0, nullptr); - } - - RCLCPP_INFO(LOGGER, "Initialize node parameters"); - rclcpp::NodeOptions node_options; - // This section is used to load the appropriate node parameters before spinning a moveit_cpp instance // Priority is given to parameters supplied directly via a config_dict, followed by launch parameters // and finally no supplied parameters. + std::vector launch_arguments; if (!config_dict.is(py::none())) { auto utils = py::module::import("moveit.utils"); // TODO (peterdavidfagan): replace python method with C++ method std::string params_filepath = - utils.attr("create_params_file_from_dict")(config_dict, "moveit_py").cast(); - RCLCPP_INFO(LOGGER, "params_filepath: %s", params_filepath.c_str()); - node_options.allow_undeclared_parameters(true) - .automatically_declare_parameters_from_overrides(true) - .arguments({ "--ros-args", "--params-file", params_filepath }); + 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()) { - node_options.allow_undeclared_parameters(true) - .automatically_declare_parameters_from_overrides(true) - .arguments({ "--ros-args", "--params-file", launch_params_filepath }); + launch_arguments = { "--ros-args", "--params-file", launch_params_filepath }; } - else + + // Initialize ROS, pass launch arguments with rclcpp::init() + if (!rclcpp::ok()) { - // TODO (peterdavidfagan): consider failing initialization if no params file is provided - node_options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true); + RCLCPP_INFO(LOGGER, "Initialize rclcpp"); + std::vector chars; + chars.reserve(launch_arguments.size()); + for (const auto& arg : launch_arguments) + { + chars.push_back(arg.c_str()); + } + + rclcpp::init(launch_arguments.size(), chars.data()); } + + // Build NodeOptions + RCLCPP_INFO(LOGGER, "Initialize node parameters"); + rclcpp::NodeOptions node_options; + node_options.allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true) + .arguments(launch_arguments); + RCLCPP_INFO(LOGGER, "Initialize node and executor"); rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared(node_name, "", node_options); std::shared_ptr executor =