moveit2
The MoveIt Motion Planning Framework for ROS 2.
moveit_cpp.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2022, Peter David Fagan
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Peter David Fagan */
36 
37 #include "moveit_cpp.h"
38 #include <moveit/utils/logger.hpp>
39 
40 namespace moveit_py
41 {
42 namespace bind_moveit_cpp
43 {
44 rclcpp::Logger getLogger()
45 {
46  return moveit::getLogger("moveit_cpp_initializer");
47 }
48 
49 std::shared_ptr<moveit_cpp::PlanningComponent>
50 getPlanningComponent(std::shared_ptr<moveit_cpp::MoveItCpp>& moveit_cpp_ptr, const std::string& planning_component)
51 {
52  return std::make_shared<moveit_cpp::PlanningComponent>(planning_component, moveit_cpp_ptr);
53 }
54 
55 void initMoveitPy(py::module& m)
56 {
57  auto utils = py::module::import("moveit.utils");
58 
59  py::class_<moveit_cpp::MoveItCpp, std::shared_ptr<moveit_cpp::MoveItCpp>>(m, "MoveItPy", R"(
60  The MoveItPy class is the main interface to the MoveIt Python API. It is a wrapper around the MoveIt C++ API.
61  )")
62 
63  .def(py::init([](const std::string& node_name, const std::vector<std::string>& launch_params_filepaths,
64  const py::object& config_dict, bool provide_planning_service) {
65  // This section is used to load the appropriate node parameters before spinning a moveit_cpp instance
66  // Priority is given to parameters supplied directly via a config_dict, followed by launch parameters
67  // and finally no supplied parameters.
68  std::vector<std::string> launch_arguments;
69  if (!config_dict.is(py::none()))
70  {
71  auto utils = py::module::import("moveit.utils");
72  // TODO (peterdavidfagan): replace python method with C++ method
73  std::string params_filepath =
74  utils.attr("create_params_file_from_dict")(config_dict, node_name).cast<std::string>();
75  launch_arguments = { "--ros-args", "--params-file", params_filepath };
76  }
77  else if (!launch_params_filepaths.empty())
78  {
79  launch_arguments = { "--ros-args" };
80  for (const auto& launch_params_filepath : launch_params_filepaths)
81  {
82  launch_arguments.push_back("--params-file");
83  launch_arguments.push_back(launch_params_filepath);
84  }
85  }
86 
87  // Initialize ROS, pass launch arguments with rclcpp::init()
88  if (!rclcpp::ok())
89  {
90  std::vector<const char*> chars;
91  chars.reserve(launch_arguments.size());
92  for (const auto& arg : launch_arguments)
93  {
94  chars.push_back(arg.c_str());
95  }
96 
97  rclcpp::init(launch_arguments.size(), chars.data());
98  RCLCPP_INFO(getLogger(), "Initialize rclcpp");
99  }
100 
101  // Build NodeOptions
102  RCLCPP_INFO(getLogger(), "Initialize node parameters");
103  rclcpp::NodeOptions node_options;
104  node_options.allow_undeclared_parameters(true)
105  .automatically_declare_parameters_from_overrides(true)
106  .arguments(launch_arguments);
107 
108  RCLCPP_INFO(getLogger(), "Initialize node and executor");
109  rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared(node_name, "", node_options);
110  std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> executor =
111  std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
112 
113  RCLCPP_INFO(getLogger(), "Spin separate thread");
114  auto spin_node = [node, executor]() {
115  executor->add_node(node);
116  executor->spin();
117  };
118  std::thread execution_thread(spin_node);
119  execution_thread.detach();
120 
121  auto custom_deleter = [executor](moveit_cpp::MoveItCpp* moveit_cpp) {
122  executor->cancel();
123  rclcpp::shutdown();
124  delete moveit_cpp;
125  };
126 
127  std::shared_ptr<moveit_cpp::MoveItCpp> moveit_cpp_ptr(new moveit_cpp::MoveItCpp(node), custom_deleter);
128 
129  if (provide_planning_service)
130  {
131  moveit_cpp_ptr->getPlanningSceneMonitorNonConst()->providePlanningSceneService();
132  };
133 
134  return moveit_cpp_ptr;
135  }),
136  py::arg("node_name") = "moveit_py",
137  py::arg("launch_params_filepaths") =
138  utils.attr("get_launch_params_filepaths")().cast<std::vector<std::string>>(),
139  py::arg("config_dict") = py::none(), py::arg("provide_planning_service") = true,
140  py::return_value_policy::take_ownership,
141  R"(
142  Initialize moveit_cpp node and the planning scene service.
143  )")
144  .def("execute",
145  py::overload_cast<const robot_trajectory::RobotTrajectoryPtr&, const std::vector<std::string>&>(
147  py::arg("robot_trajectory"), py::arg("controllers"), py::call_guard<py::gil_scoped_release>(),
148  R"(
149  Execute a trajectory (planning group is inferred from robot trajectory object).
150  )")
151  .def("get_planning_component", &moveit_py::bind_moveit_cpp::getPlanningComponent,
152  py::arg("planning_component_name"), py::return_value_policy::take_ownership,
153  R"(
154  Creates a planning component instance.
155  Args:
156  planning_component_name (str): The name of the planning component.
157  Returns:
158  :py:class:`moveit_py.planning.PlanningComponent`: A planning component instance corresponding to the provided plan component name.
159  )")
160 
161  .def(
162  "shutdown", [](std::shared_ptr<moveit_cpp::MoveItCpp>& /*moveit_cpp*/) { rclcpp::shutdown(); },
163  R"(
164  Shutdown the moveit_cpp node.
165  )")
166 
167  .def("get_planning_scene_monitor", &moveit_cpp::MoveItCpp::getPlanningSceneMonitorNonConst,
168  py::return_value_policy::reference,
169  R"(
170  Returns the planning scene monitor.
171  )")
172 
173  .def("get_trajactory_execution_manager", &moveit_cpp::MoveItCpp::getTrajectoryExecutionManagerNonConst,
174  py::return_value_policy::reference,
175  R"(
176  Returns the trajectory execution manager.
177  )")
178 
179  .def("get_robot_model", &moveit_cpp::MoveItCpp::getRobotModel, py::return_value_policy::reference,
180  R"(
181  Returns robot model.
182  )");
183 }
184 } // namespace bind_moveit_cpp
185 } // namespace moveit_py
moveit_controller_manager::ExecutionStatus execute(const robot_trajectory::RobotTrajectoryPtr &robot_trajectory, bool blocking, const std::vector< std::string > &controllers=std::vector< std::string >())
Execute a trajectory on the planning group specified by the robot's trajectory using the trajectory e...
Definition: moveit_cpp.cpp:197
moveit::core::RobotModelConstPtr getRobotModel() const
Get the RobotModel object.
Definition: moveit_cpp.cpp:140
planning_scene_monitor::PlanningSceneMonitorPtr getPlanningSceneMonitorNonConst()
Definition: moveit_cpp.cpp:181
trajectory_execution_manager::TrajectoryExecutionManagerPtr getTrajectoryExecutionManagerNonConst()
Definition: moveit_cpp.cpp:191
std::shared_ptr< moveit_cpp::PlanningComponent > getPlanningComponent(std::shared_ptr< moveit_cpp::MoveItCpp > &moveit_cpp_ptr, const std::string &planning_component)
Definition: moveit_cpp.cpp:50
rclcpp::Logger getLogger()
Definition: moveit_cpp.cpp:44
void initMoveitPy(py::module &m)
Definition: moveit_cpp.cpp:55
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.
Definition: logger.cpp:79