moveit2
The MoveIt Motion Planning Framework for ROS 2.
move_group.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
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 Willow Garage 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: Ioan Sucan */
36 
39 #include <tf2_ros/transform_listener.h>
42 #include <boost/tokenizer.hpp>
45 #include <memory>
46 #include <set>
47 
48 static const std::string ROBOT_DESCRIPTION =
49  "robot_description"; // name of the robot description (a param name, so it can be changed externally)
50 
51 static const rclcpp::Logger LOGGER = rclcpp::get_logger("move_group.move_group");
52 
53 namespace move_group
54 {
55 // These capabilities are loaded unless listed in disable_capabilities
56 // clang-format off
57 static const char* DEFAULT_CAPABILITIES[] = {
58  "move_group/MoveGroupCartesianPathService",
59  "move_group/MoveGroupKinematicsService",
60  "move_group/MoveGroupExecuteTrajectoryAction",
61  "move_group/MoveGroupMoveAction",
62  "move_group/MoveGroupPlanService",
63  "move_group/MoveGroupQueryPlannersService",
64  "move_group/MoveGroupStateValidationService",
65  "move_group/MoveGroupGetPlanningSceneService",
66  "move_group/ApplyPlanningSceneService",
67  "move_group/ClearOctomapService",
68 };
69 // clang-format on
70 
72 {
73 public:
74  MoveGroupExe(const moveit_cpp::MoveItCppPtr& moveit_cpp, const std::string& default_planning_pipeline, bool debug)
75  {
76  // if the user wants to be able to disable execution of paths, they can just set this ROS param to false
77  bool allow_trajectory_execution;
78  moveit_cpp->getNode()->get_parameter_or("allow_trajectory_execution", allow_trajectory_execution, true);
79 
80  context_ =
81  std::make_shared<MoveGroupContext>(moveit_cpp, default_planning_pipeline, allow_trajectory_execution, debug);
82 
83  // start the capabilities
84  configureCapabilities();
85  }
86 
88  {
89  capabilities_.clear();
90  context_.reset();
91  capability_plugin_loader_.reset();
92  }
93 
94  void status()
95  {
96  if (context_)
97  {
98  if (context_->status())
99  {
100  if (capabilities_.empty())
101  printf(MOVEIT_CONSOLE_COLOR_BLUE "\nmove_group is running but no capabilities are "
102  "loaded.\n\n" MOVEIT_CONSOLE_COLOR_RESET);
103  else
104  printf(MOVEIT_CONSOLE_COLOR_GREEN "\nYou can start planning now!\n\n" MOVEIT_CONSOLE_COLOR_RESET);
105  fflush(stdout);
106  }
107  }
108  else
109  RCLCPP_ERROR(LOGGER, "No MoveGroup context created. Nothing will work.");
110  }
111 
112  MoveGroupContextPtr getContext()
113  {
114  return context_;
115  }
116 
117 private:
118  void configureCapabilities()
119  {
120  try
121  {
122  capability_plugin_loader_ = std::make_shared<pluginlib::ClassLoader<MoveGroupCapability>>(
123  "moveit_ros_move_group", "move_group::MoveGroupCapability");
124  }
125  catch (pluginlib::PluginlibException& ex)
126  {
127  RCLCPP_FATAL_STREAM(LOGGER, "Exception while creating plugin loader for move_group capabilities: " << ex.what());
128  return;
129  }
130 
131  std::set<std::string> capabilities;
132 
133  // add default capabilities
134  for (const char* capability : DEFAULT_CAPABILITIES)
135  capabilities.insert(capability);
136 
137  // add capabilities listed in ROS parameter
138  std::string capability_plugins;
139  if (context_->moveit_cpp_->getNode()->get_parameter("capabilities", capability_plugins))
140  {
141  boost::char_separator<char> sep(" ");
142  boost::tokenizer<boost::char_separator<char>> tok(capability_plugins, sep);
143  capabilities.insert(tok.begin(), tok.end());
144  }
145 
146  // add capabilities configured for planning pipelines
147  for (const auto& pipeline_entry : context_->moveit_cpp_->getPlanningPipelines())
148  {
149  const auto& pipeline_name = pipeline_entry.first;
150  std::string pipeline_capabilities;
151  if (context_->moveit_cpp_->getNode()->get_parameter("planning_pipelines/" + pipeline_name + "/capabilities",
152  pipeline_capabilities))
153  {
154  boost::char_separator<char> sep(" ");
155  boost::tokenizer<boost::char_separator<char>> tok(pipeline_capabilities, sep);
156  capabilities.insert(tok.begin(), tok.end());
157  }
158  }
159 
160  // drop capabilities that have been explicitly disabled
161  if (context_->moveit_cpp_->getNode()->get_parameter("disable_capabilities", capability_plugins))
162  {
163  boost::char_separator<char> sep(" ");
164  boost::tokenizer<boost::char_separator<char>> tok(capability_plugins, sep);
165  for (boost::tokenizer<boost::char_separator<char>>::iterator cap_name = tok.begin(); cap_name != tok.end();
166  ++cap_name)
167  capabilities.erase(*cap_name);
168  }
169 
170  for (const std::string& capability : capabilities)
171  {
172  try
173  {
174  printf(MOVEIT_CONSOLE_COLOR_CYAN "Loading '%s'...\n" MOVEIT_CONSOLE_COLOR_RESET, capability.c_str());
175  MoveGroupCapabilityPtr cap = capability_plugin_loader_->createUniqueInstance(capability);
176  cap->setContext(context_);
177  cap->initialize();
178  capabilities_.push_back(cap);
179  }
180  catch (pluginlib::PluginlibException& ex)
181  {
182  RCLCPP_ERROR_STREAM(LOGGER,
183  "Exception while loading move_group capability '" << capability << "': " << ex.what());
184  }
185  }
186 
187  std::stringstream ss;
188  ss << '\n';
189  ss << '\n';
190  ss << "********************************************************" << '\n';
191  ss << "* MoveGroup using: " << '\n';
192  for (const MoveGroupCapabilityPtr& cap : capabilities_)
193  ss << "* - " << cap->getName() << '\n';
194  ss << "********************************************************" << '\n';
195  RCLCPP_INFO(LOGGER, "%s", ss.str().c_str());
196  }
197 
198  MoveGroupContextPtr context_;
199  std::shared_ptr<pluginlib::ClassLoader<MoveGroupCapability>> capability_plugin_loader_;
200  std::vector<MoveGroupCapabilityPtr> capabilities_;
201 };
202 } // namespace move_group
203 
204 int main(int argc, char** argv)
205 {
206  rclcpp::init(argc, argv);
207 
208  rclcpp::NodeOptions opt;
209  opt.allow_undeclared_parameters(true);
210  opt.automatically_declare_parameters_from_overrides(true);
211  rclcpp::Node::SharedPtr nh = rclcpp::Node::make_shared("move_group", opt);
212  moveit_cpp::MoveItCpp::Options moveit_cpp_options(nh);
213 
214  // Prepare PlanningPipelineOptions
215  moveit_cpp_options.planning_pipeline_options.parent_namespace = nh->get_effective_namespace() + ".planning_pipelines";
216  std::vector<std::string> planning_pipeline_configs;
217  if (nh->get_parameter("planning_pipelines", planning_pipeline_configs))
218  {
219  if (planning_pipeline_configs.empty())
220  {
221  RCLCPP_ERROR(LOGGER, "Failed to read parameter 'move_group.planning_pipelines'");
222  }
223  else
224  {
225  for (const auto& config : planning_pipeline_configs)
226  {
227  moveit_cpp_options.planning_pipeline_options.pipeline_names.push_back(config);
228  }
229  }
230  }
231 
232  // Retrieve default planning pipeline
233  auto& pipeline_names = moveit_cpp_options.planning_pipeline_options.pipeline_names;
234  std::string default_planning_pipeline;
235  if (nh->get_parameter("default_planning_pipeline", default_planning_pipeline))
236  {
237  // Ignore default_planning_pipeline if there is no matching entry in pipeline_names
238  if (std::find(pipeline_names.begin(), pipeline_names.end(), default_planning_pipeline) == pipeline_names.end())
239  {
240  RCLCPP_WARN(LOGGER,
241  "MoveGroup launched with ~default_planning_pipeline '%s' not configured in ~planning_pipelines",
242  default_planning_pipeline.c_str());
243  default_planning_pipeline = ""; // reset invalid pipeline id
244  }
245  }
246  else if (pipeline_names.size() > 1) // only warn if there are multiple pipelines to choose from
247  {
248  // Handle deprecated move_group.launch
249  RCLCPP_WARN(LOGGER,
250  "MoveGroup launched without ~default_planning_pipeline specifying the namespace for the default "
251  "planning pipeline configuration");
252  }
253 
254  // If there is no valid default pipeline, either pick the first available one, or fall back to old behavior
255  if (default_planning_pipeline.empty())
256  {
257  if (!pipeline_names.empty())
258  {
259  RCLCPP_WARN(LOGGER, "Using default pipeline '%s'", pipeline_names[0].c_str());
260  default_planning_pipeline = pipeline_names[0];
261  }
262  else
263  {
264  RCLCPP_WARN(LOGGER, "Falling back to using the the move_group node namespace (deprecated behavior).");
265  default_planning_pipeline = "move_group";
266  moveit_cpp_options.planning_pipeline_options.pipeline_names = { default_planning_pipeline };
267  moveit_cpp_options.planning_pipeline_options.parent_namespace = nh->get_effective_namespace();
268  }
269 
270  // Reset invalid pipeline parameter for MGI requests
271  nh->set_parameter(rclcpp::Parameter("default_planning_pipeline", default_planning_pipeline));
272  }
273 
274  // Initialize MoveItCpp
275  const auto tf_buffer = std::make_shared<tf2_ros::Buffer>(nh->get_clock(), tf2::durationFromSec(10.0));
276  const auto moveit_cpp = std::make_shared<moveit_cpp::MoveItCpp>(nh, moveit_cpp_options, tf_buffer);
277  const auto planning_scene_monitor = moveit_cpp->getPlanningSceneMonitor();
278 
279  if (planning_scene_monitor->getPlanningScene())
280  {
281  bool debug = false;
282  for (int i = 1; i < argc; ++i)
283  if (strncmp(argv[i], "--debug", 7) == 0)
284  {
285  debug = true;
286  break;
287  }
288  debug = true;
289  if (debug)
290  RCLCPP_INFO(LOGGER, "MoveGroup debug mode is ON");
291  else
292  RCLCPP_INFO(LOGGER, "MoveGroup debug mode is OFF");
293 
294  rclcpp::executors::MultiThreadedExecutor executor;
295 
296  move_group::MoveGroupExe mge(moveit_cpp, default_planning_pipeline, debug);
297 
298  bool monitor_dynamics;
299  if (nh->get_parameter("monitor_dynamics", monitor_dynamics) && monitor_dynamics)
300  {
301  RCLCPP_INFO(LOGGER, "MoveGroup monitors robot dynamics (higher load)");
302  planning_scene_monitor->getStateMonitor()->enableCopyDynamics(true);
303  }
304 
305  planning_scene_monitor->publishDebugInformation(debug);
306 
307  mge.status();
308  executor.add_node(nh);
309  executor.spin();
310 
311  rclcpp::shutdown();
312  }
313  else
314  RCLCPP_ERROR(LOGGER, "Planning scene not configured");
315 
316  return 0;
317 }
MoveGroupContextPtr getContext()
Definition: move_group.cpp:112
MoveGroupExe(const moveit_cpp::MoveItCppPtr &moveit_cpp, const std::string &default_planning_pipeline, bool debug)
Definition: move_group.cpp:74
#define MOVEIT_CONSOLE_COLOR_BLUE
#define MOVEIT_CONSOLE_COLOR_GREEN
#define MOVEIT_CONSOLE_COLOR_RESET
#define MOVEIT_CONSOLE_COLOR_CYAN
int main(int argc, char **argv)
Definition: move_group.cpp:204
const rclcpp::Logger LOGGER
Definition: async_test.h:31
Parameter container for initializing MoveItCpp.
Definition: moveit_cpp.h:99
PlanningPipelineOptions planning_pipeline_options
Definition: moveit_cpp.h:107
std::vector< std::string > pipeline_names
Definition: moveit_cpp.h:93