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 
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 constexpr char LOGNAME[] = "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/MoveGroupPickPlaceAction",
63  "move_group/MoveGroupPlanService",
64  "move_group/MoveGroupQueryPlannersService",
65  "move_group/MoveGroupStateValidationService",
66  "move_group/MoveGroupGetPlanningSceneService",
67  "move_group/ApplyPlanningSceneService",
68  "move_group/ClearOctomapService",
69 };
70 // clang-format on
71 
73 {
74 public:
75  MoveGroupExe(const moveit_cpp::MoveItCppPtr& moveit_cpp, const std::string& default_planning_pipeline, bool debug)
76  : node_handle_("~")
77  {
78  // if the user wants to be able to disable execution of paths, they can just set this ROS param to false
79  bool allow_trajectory_execution;
80  node_handle_.param("allow_trajectory_execution", allow_trajectory_execution, true);
81 
82  context_ =
83  std::make_shared<MoveGroupContext>(moveit_cpp, default_planning_pipeline, allow_trajectory_execution, debug);
84 
85  // start the capabilities
87  }
88 
90  {
91  capabilities_.clear();
92  context_.reset();
94  }
95 
96  void status()
97  {
98  if (context_)
99  {
100  if (context_->status())
101  {
102  if (capabilities_.empty())
103  printf(MOVEIT_CONSOLE_COLOR_BLUE "\nmove_group is running but no capabilities are "
104  "loaded.\n\n" MOVEIT_CONSOLE_COLOR_RESET);
105  else
106  printf(MOVEIT_CONSOLE_COLOR_GREEN "\nYou can start planning now!\n\n" MOVEIT_CONSOLE_COLOR_RESET);
107  fflush(stdout);
108  }
109  }
110  else
111  ROS_ERROR_NAMED(LOGNAME, "No MoveGroup context created. Nothing will work.");
112  }
113 
114 private:
116  {
117  try
118  {
119  capability_plugin_loader_ = std::make_shared<pluginlib::ClassLoader<MoveGroupCapability>>(
120  "moveit_ros_move_group", "move_group::MoveGroupCapability");
121  }
123  {
125  "Exception while creating plugin loader for move_group capabilities: " << ex.what());
126  return;
127  }
128 
129  std::set<std::string> capabilities;
130 
131  // add default capabilities
132  for (const char* capability : DEFAULT_CAPABILITIES)
133  capabilities.insert(capability);
134 
135  // add capabilities listed in ROS parameter
136  std::string capability_plugins;
137  if (node_handle_.getParam("capabilities", capability_plugins))
138  {
139  boost::char_separator<char> sep(" ");
140  boost::tokenizer<boost::char_separator<char>> tok(capability_plugins, sep);
141  capabilities.insert(tok.begin(), tok.end());
142  }
143 
144  // add capabilities configured for planning pipelines
145  for (const auto& pipeline_entry : context_->moveit_cpp_->getPlanningPipelines())
146  {
147  const auto& pipeline_name = pipeline_entry.first;
148  std::string pipeline_capabilities;
149  if (node_handle_.getParam("planning_pipelines/" + pipeline_name + "/capabilities", pipeline_capabilities))
150  {
151  boost::char_separator<char> sep(" ");
152  boost::tokenizer<boost::char_separator<char>> tok(pipeline_capabilities, sep);
153  capabilities.insert(tok.begin(), tok.end());
154  }
155  }
156 
157  // drop capabilities that have been explicitly disabled
158  if (node_handle_.getParam("disable_capabilities", capability_plugins))
159  {
160  boost::char_separator<char> sep(" ");
161  boost::tokenizer<boost::char_separator<char>> tok(capability_plugins, sep);
162  for (boost::tokenizer<boost::char_separator<char>>::iterator cap_name = tok.begin(); cap_name != tok.end();
163  ++cap_name)
164  capabilities.erase(*cap_name);
165  }
166 
167  for (const std::string& capability : capabilities)
168  {
169  try
170  {
171  printf(MOVEIT_CONSOLE_COLOR_CYAN "Loading '%s'...\n" MOVEIT_CONSOLE_COLOR_RESET, capability.c_str());
172  MoveGroupCapabilityPtr cap = capability_plugin_loader_->createUniqueInstance(capability);
173  cap->setContext(context_);
174  cap->initialize();
175  capabilities_.push_back(cap);
176  }
178  {
180  "Exception while loading move_group capability '" << capability << "': " << ex.what());
181  }
182  }
183 
184  std::stringstream ss;
185  ss << std::endl;
186  ss << std::endl;
187  ss << "********************************************************" << std::endl;
188  ss << "* MoveGroup using: " << std::endl;
189  for (const MoveGroupCapabilityPtr& cap : capabilities_)
190  ss << "* - " << cap->getName() << std::endl;
191  ss << "********************************************************" << std::endl;
192  ROS_INFO_STREAM_NAMED(LOGNAME, ss.str());
193  }
194 
196  MoveGroupContextPtr context_;
197  std::shared_ptr<pluginlib::ClassLoader<MoveGroupCapability>> capability_plugin_loader_;
198  std::vector<MoveGroupCapabilityPtr> capabilities_;
199 };
200 } // namespace move_group
201 
202 int main(int argc, char** argv)
203 {
204  ros::init(argc, argv, move_group::NODE_NAME);
205 
207  spinner.start();
208 
209  // Load MoveItCpp parameters and check for valid planning pipeline configuration
210  ros::NodeHandle pnh("~");
211  moveit_cpp::MoveItCpp::Options moveit_cpp_options(pnh);
212 
213  // Prepare PlanningPipelineOptions
214  moveit_cpp_options.planning_pipeline_options.parent_namespace = pnh.getNamespace() + "/planning_pipelines";
215  XmlRpc::XmlRpcValue planning_pipeline_configs;
216  if (pnh.getParam("planning_pipelines", planning_pipeline_configs))
217  {
218  if (planning_pipeline_configs.getType() != XmlRpc::XmlRpcValue::TypeStruct)
219  {
220  ROS_ERROR_STREAM_NAMED(LOGNAME, "Parameter '" << moveit_cpp_options.planning_pipeline_options.parent_namespace
221  << "' is expected to be a dictionary of pipeline configurations.");
222  }
223  else
224  {
225  for (std::pair<const std::string, XmlRpc::XmlRpcValue>& config : planning_pipeline_configs)
226  {
227  if (config.second.getType() != XmlRpc::XmlRpcValue::TypeStruct ||
228  std::find_if(config.second.begin(), config.second.end(),
229  [](std::pair<const std::string, XmlRpc::XmlRpcValue>& item) {
230  return item.first == "planning_plugin";
231  }) == config.second.end())
232  ROS_ERROR_STREAM_NAMED(LOGNAME, "Planning pipeline parameter '~planning_pipelines/"
233  << config.first << "/planning_plugin' doesn't exist");
234  else
235  moveit_cpp_options.planning_pipeline_options.pipeline_names.push_back(config.first);
236  }
237  if (planning_pipeline_configs.size() && moveit_cpp_options.planning_pipeline_options.pipeline_names.empty())
238  ROS_WARN_STREAM_NAMED(LOGNAME, "No valid planning pipelines found in "
239  << moveit_cpp_options.planning_pipeline_options.parent_namespace
240  << ". Did you use a namespace, e.g. planning_pipelines/ompl/ ?");
241  }
242  }
243 
244  // Retrieve default planning pipeline
245  auto& pipeline_names = moveit_cpp_options.planning_pipeline_options.pipeline_names;
246  std::string default_planning_pipeline;
247  if (pnh.getParam("default_planning_pipeline", default_planning_pipeline))
248  {
249  // Ignore default_planning_pipeline if there is no matching entry in pipeline_names
250  if (std::find(pipeline_names.begin(), pipeline_names.end(), default_planning_pipeline) == pipeline_names.end())
251  {
253  "MoveGroup launched with ~default_planning_pipeline '%s' not configured in ~planning_pipelines",
254  default_planning_pipeline.c_str());
255  default_planning_pipeline = ""; // reset invalid pipeline id
256  }
257  }
258  else if (pipeline_names.size() > 1) // only warn if there are multiple pipelines to choose from
259  {
260  // Handle deprecated move_group.launch
262  "MoveGroup launched without ~default_planning_pipeline specifying the namespace for the default "
263  "planning pipeline configuration");
264  }
265 
266  // If there is no valid default pipeline, either pick the first available one, or fall back to old behavior
267  if (default_planning_pipeline.empty())
268  {
269  if (!pipeline_names.empty())
270  {
271  ROS_WARN_NAMED(LOGNAME, "Using default pipeline '%s'", pipeline_names[0].c_str());
272  default_planning_pipeline = pipeline_names[0];
273  }
274  else
275  {
276  ROS_WARN_NAMED(LOGNAME, "Falling back to using the move_group node's namespace (deprecated Melodic behavior).");
277  moveit_cpp_options.planning_pipeline_options.pipeline_names = { default_planning_pipeline };
278  moveit_cpp_options.planning_pipeline_options.parent_namespace = pnh.getNamespace();
279  }
280 
281  // Reset invalid pipeline parameter for MGI requests
282  pnh.setParam("default_planning_pipeline", default_planning_pipeline);
283  }
284 
285  // Initialize MoveItCpp
286  const auto tf_buffer = std::make_shared<tf2_ros::Buffer>(ros::Duration(10.0));
287  const auto moveit_cpp = std::make_shared<moveit_cpp::MoveItCpp>(moveit_cpp_options, pnh, tf_buffer);
288  const auto planning_scene_monitor = moveit_cpp->getPlanningSceneMonitor();
289 
290  if (planning_scene_monitor->getPlanningScene())
291  {
292  bool debug = false;
293  for (int i = 1; i < argc; ++i)
294  if (strncmp(argv[i], "--debug", 7) == 0)
295  {
296  debug = true;
297  break;
298  }
299  if (debug)
300  ROS_INFO_NAMED(LOGNAME, "MoveGroup debug mode is ON");
301  else
302  ROS_INFO_NAMED(LOGNAME, "MoveGroup debug mode is OFF");
303 
304  move_group::MoveGroupExe mge(moveit_cpp, default_planning_pipeline, debug);
305 
306  if (pnh.param<bool>("monitor_dynamics", false))
307  {
308  ROS_INFO_NAMED(LOGNAME, "MoveGroup monitors robot dynamics (higher load)");
309  planning_scene_monitor->getStateMonitor()->enableCopyDynamics(true);
310  }
311  planning_scene_monitor->publishDebugInformation(debug);
312 
313  mge.status();
314 
316  }
317  else
318  ROS_ERROR_NAMED(LOGNAME, "Planning scene not configured");
319 
320  return 0;
321 }
planning_scene_monitor
ros::NodeHandle::setParam
void setParam(const std::string &key, bool b) const
moveit_cpp::MoveItCpp::Options
move_group::MoveGroupExe::MoveGroupExe
MoveGroupExe(const moveit_cpp::MoveItCppPtr &moveit_cpp, const std::string &default_planning_pipeline, bool debug)
Definition: move_group.cpp:75
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
move_group::MoveGroupExe::~MoveGroupExe
~MoveGroupExe()
Definition: move_group.cpp:89
ros::NodeHandle::getParam
bool getParam(const std::string &key, bool &b) const
node_name.h
ros::AsyncSpinner
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
main
int main(int argc, char **argv)
Definition: move_group.cpp:202
MOVEIT_CONSOLE_COLOR_RESET
#define MOVEIT_CONSOLE_COLOR_RESET
move_group_capability.h
XmlRpc::XmlRpcValue::TypeStruct
TypeStruct
pluginlib::PluginlibException
spinner
void spinner()
move_group::MoveGroupExe::status
void status()
Definition: move_group.cpp:96
moveit_cpp
MOVEIT_CONSOLE_COLOR_GREEN
#define MOVEIT_CONSOLE_COLOR_GREEN
move_group::MoveGroupExe
Definition: move_group.cpp:72
moveit_cpp::MoveItCpp::PlanningPipelineOptions::pipeline_names
std::vector< std::string > pipeline_names
ROS_INFO_NAMED
#define ROS_INFO_NAMED(name,...)
ROS_FATAL_STREAM_NAMED
#define ROS_FATAL_STREAM_NAMED(name, args)
planning_scene_monitor.h
ROS_ERROR_STREAM_NAMED
#define ROS_ERROR_STREAM_NAMED(name, args)
move_group::MoveGroupExe::node_handle_
ros::NodeHandle node_handle_
Definition: move_group.cpp:195
move_group::MoveGroupExe::capabilities_
std::vector< MoveGroupCapabilityPtr > capabilities_
Definition: move_group.cpp:198
MOVEIT_CONSOLE_COLOR_BLUE
#define MOVEIT_CONSOLE_COLOR_BLUE
ROBOT_DESCRIPTION
static const std::string ROBOT_DESCRIPTION
Definition: move_group.cpp:48
move_group::MoveGroupExe::context_
MoveGroupContextPtr context_
Definition: move_group.cpp:196
move_group
Definition: capability_names.h:41
XmlRpc::XmlRpcValue::getType
const Type & getType() const
moveit_cpp::MoveItCpp::Options::planning_pipeline_options
PlanningPipelineOptions planning_pipeline_options
MOVEIT_CONSOLE_COLOR_CYAN
#define MOVEIT_CONSOLE_COLOR_CYAN
ROS_WARN_STREAM_NAMED
#define ROS_WARN_STREAM_NAMED(name, args)
transform_listener.h
LOGNAME
constexpr char LOGNAME[]
Definition: move_group.cpp:51
tf_buffer
tf2_ros::Buffer * tf_buffer
ros::waitForShutdown
ROSCPP_DECL void waitForShutdown()
ROS_WARN_NAMED
#define ROS_WARN_NAMED(name,...)
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
ROS_INFO_STREAM_NAMED
#define ROS_INFO_STREAM_NAMED(name, args)
move_group::MoveGroupExe::configureCapabilities
void configureCapabilities()
Definition: move_group.cpp:115
moveit_cpp::MoveItCpp::PlanningPipelineOptions::parent_namespace
std::string parent_namespace
move_group::NODE_NAME
static const std::string NODE_NAME
Definition: node_name.h:75
move_group::MoveGroupExe::capability_plugin_loader_
std::shared_ptr< pluginlib::ClassLoader< MoveGroupCapability > > capability_plugin_loader_
Definition: move_group.cpp:197
ros::NodeHandle::getNamespace
const std::string & getNamespace() const
console_colors.h
moveit_cpp.h
ros::Duration
capability_names.h
move_group::DEFAULT_CAPABILITIES
static const char * DEFAULT_CAPABILITIES[]
Definition: move_group.cpp:57
XmlRpc::XmlRpcValue
ros::NodeHandle


move_group
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Sun Sep 18 2022 02:27:14