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 namespace move_group
52 {
53 // These capabilities are loaded unless listed in disable_capabilities
54 // clang-format off
55 static const char* DEFAULT_CAPABILITIES[] = {
56  "move_group/MoveGroupCartesianPathService",
57  "move_group/MoveGroupKinematicsService",
58  "move_group/MoveGroupExecuteTrajectoryAction",
59  "move_group/MoveGroupMoveAction",
60  "move_group/MoveGroupPickPlaceAction",
61  "move_group/MoveGroupPlanService",
62  "move_group/MoveGroupQueryPlannersService",
63  "move_group/MoveGroupStateValidationService",
64  "move_group/MoveGroupGetPlanningSceneService",
65  "move_group/ApplyPlanningSceneService",
66  "move_group/ClearOctomapService",
67 };
68 // clang-format on
69 
71 {
72 public:
73  MoveGroupExe(const moveit_cpp::MoveItCppPtr& moveit_cpp, const std::string& default_planning_pipeline, bool debug)
74  : node_handle_("~")
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  node_handle_.param("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
85  }
86 
88  {
89  capabilities_.clear();
90  context_.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  ROS_ERROR("No MoveGroup context created. Nothing will work.");
110  }
111 
112 private:
114  {
115  try
116  {
118  new pluginlib::ClassLoader<MoveGroupCapability>("moveit_ros_move_group", "move_group::MoveGroupCapability"));
119  }
121  {
122  ROS_FATAL_STREAM("Exception while creating plugin loader for move_group capabilities: " << ex.what());
123  return;
124  }
125 
126  std::set<std::string> capabilities;
127 
128  // add default capabilities
129  for (const char* capability : DEFAULT_CAPABILITIES)
130  capabilities.insert(capability);
131 
132  // add capabilities listed in ROS parameter
133  std::string capability_plugins;
134  if (node_handle_.getParam("capabilities", capability_plugins))
135  {
136  boost::char_separator<char> sep(" ");
137  boost::tokenizer<boost::char_separator<char> > tok(capability_plugins, sep);
138  capabilities.insert(tok.begin(), tok.end());
139  }
140 
141  // add capabilities configured for planning pipelines
142  for (const auto& pipeline_entry : context_->moveit_cpp_->getPlanningPipelines())
143  {
144  const auto& pipeline_name = pipeline_entry.first;
145  std::string pipeline_capabilities;
146  if (node_handle_.getParam("planning_pipelines/" + pipeline_name + "/capabilities", pipeline_capabilities))
147  {
148  boost::char_separator<char> sep(" ");
149  boost::tokenizer<boost::char_separator<char> > tok(pipeline_capabilities, sep);
150  capabilities.insert(tok.begin(), tok.end());
151  }
152  }
153 
154  // drop capabilities that have been explicitly disabled
155  if (node_handle_.getParam("disable_capabilities", capability_plugins))
156  {
157  boost::char_separator<char> sep(" ");
158  boost::tokenizer<boost::char_separator<char> > tok(capability_plugins, sep);
159  for (boost::tokenizer<boost::char_separator<char> >::iterator cap_name = tok.begin(); cap_name != tok.end();
160  ++cap_name)
161  capabilities.erase(*cap_name);
162  }
163 
164  for (const std::string& capability : capabilities)
165  {
166  try
167  {
168  printf(MOVEIT_CONSOLE_COLOR_CYAN "Loading '%s'...\n" MOVEIT_CONSOLE_COLOR_RESET, capability.c_str());
169  MoveGroupCapabilityPtr cap = capability_plugin_loader_->createUniqueInstance(capability);
170  cap->setContext(context_);
171  cap->initialize();
172  capabilities_.push_back(cap);
173  }
175  {
176  ROS_ERROR_STREAM("Exception while loading move_group capability '" << capability << "': " << ex.what());
177  }
178  }
179 
180  std::stringstream ss;
181  ss << std::endl;
182  ss << std::endl;
183  ss << "********************************************************" << std::endl;
184  ss << "* MoveGroup using: " << std::endl;
185  for (const MoveGroupCapabilityPtr& cap : capabilities_)
186  ss << "* - " << cap->getName() << std::endl;
187  ss << "********************************************************" << std::endl;
188  ROS_INFO_STREAM(ss.str());
189  }
190 
192  MoveGroupContextPtr context_;
193  std::shared_ptr<pluginlib::ClassLoader<MoveGroupCapability> > capability_plugin_loader_;
194  std::vector<MoveGroupCapabilityPtr> capabilities_;
195 };
196 } // namespace move_group
197 
198 int main(int argc, char** argv)
199 {
200  ros::init(argc, argv, move_group::NODE_NAME);
201 
203  spinner.start();
204 
205  // Load MoveItCpp parameters and check for valid planning pipeline configuration
206  ros::NodeHandle pnh("~");
207  moveit_cpp::MoveItCpp::Options moveit_cpp_options(pnh);
208 
209  // Prepare PlanningPipelineOptions
210  moveit_cpp_options.planning_pipeline_options.parent_namespace = pnh.getNamespace() + "/planning_pipelines";
211  XmlRpc::XmlRpcValue planning_pipeline_configs;
212  if (pnh.getParam("planning_pipelines", planning_pipeline_configs))
213  {
214  if (planning_pipeline_configs.getType() != XmlRpc::XmlRpcValue::TypeStruct)
215  {
216  ROS_ERROR("Failed to read parameter 'move_group/planning_pipelines'");
217  }
218  else
219  {
220  for (std::pair<const std::string, XmlRpc::XmlRpcValue>& config : planning_pipeline_configs)
221  {
222  moveit_cpp_options.planning_pipeline_options.pipeline_names.push_back(config.first);
223  }
224  }
225  }
226 
227  // Retrieve default planning pipeline
228  auto& pipeline_names = moveit_cpp_options.planning_pipeline_options.pipeline_names;
229  std::string default_planning_pipeline;
230  if (pnh.getParam("default_planning_pipeline", default_planning_pipeline))
231  {
232  // Ignore default_planning_pipeline if there is no known entry in pipeline_names
233  if (std::find(pipeline_names.begin(), pipeline_names.end(), default_planning_pipeline) == pipeline_names.end())
234  {
235  ROS_WARN("MoveGroup launched with ~default_planning_pipeline '%s' not configured in ~/planning_pipelines",
236  default_planning_pipeline.c_str());
237  default_planning_pipeline = ""; // reset invalid pipeline id
238  }
239  }
240  else
241  {
242  // Handle deprecated move_group.launch
243  ROS_WARN("MoveGroup launched without ~default_planning_pipeline specifying the namespace for the default "
244  "planning pipeline configuration");
245  }
246 
247  // If there is no valid default pipeline, either pick the first available one, or fall back to old behavior
248  if (default_planning_pipeline.empty())
249  {
250  if (!pipeline_names.empty())
251  {
252  ROS_WARN("Using default pipeline '%s'", pipeline_names[0].c_str());
253  default_planning_pipeline = pipeline_names[0];
254  }
255  else
256  {
257  ROS_WARN("Falling back to using the the move_group node namespace (deprecated behavior).");
258  moveit_cpp_options.planning_pipeline_options.pipeline_names = { default_planning_pipeline };
259  moveit_cpp_options.planning_pipeline_options.parent_namespace = pnh.getNamespace();
260  }
261 
262  // Reset invalid pipeline parameter for MGI requests
263  pnh.setParam("default_planning_pipeline", default_planning_pipeline);
264  }
265 
266  // Initialize MoveItCpp
267  const auto tf_buffer = std::make_shared<tf2_ros::Buffer>(ros::Duration(10.0));
268  const auto moveit_cpp = std::make_shared<moveit_cpp::MoveItCpp>(moveit_cpp_options, pnh, tf_buffer);
269  const auto planning_scene_monitor = moveit_cpp->getPlanningSceneMonitor();
270 
271  if (planning_scene_monitor->getPlanningScene())
272  {
273  bool debug = false;
274  for (int i = 1; i < argc; ++i)
275  if (strncmp(argv[i], "--debug", 7) == 0)
276  {
277  debug = true;
278  break;
279  }
280  if (debug)
281  ROS_INFO("MoveGroup debug mode is ON");
282  else
283  ROS_INFO("MoveGroup debug mode is OFF");
284 
285  move_group::MoveGroupExe mge(moveit_cpp, default_planning_pipeline, debug);
286 
287  planning_scene_monitor->publishDebugInformation(debug);
288 
289  mge.status();
290 
292  }
293  else
294  ROS_ERROR("Planning scene not configured");
295 
296  return 0;
297 }
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:73
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(args)
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:87
ros::NodeHandle::getParam
bool getParam(const std::string &key, bool &b) const
ROS_FATAL_STREAM
#define ROS_FATAL_STREAM(args)
node_name.h
ros::AsyncSpinner
main
int main(int argc, char **argv)
Definition: move_group.cpp:198
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:94
moveit_cpp
MOVEIT_CONSOLE_COLOR_GREEN
#define MOVEIT_CONSOLE_COLOR_GREEN
move_group::MoveGroupExe
Definition: move_group.cpp:70
moveit_cpp::MoveItCpp::PlanningPipelineOptions::pipeline_names
std::vector< std::string > pipeline_names
planning_scene_monitor.h
move_group::MoveGroupExe::node_handle_
ros::NodeHandle node_handle_
Definition: move_group.cpp:191
move_group::MoveGroupExe::capabilities_
std::vector< MoveGroupCapabilityPtr > capabilities_
Definition: move_group.cpp:194
ROS_ERROR
#define ROS_ERROR(...)
MOVEIT_CONSOLE_COLOR_BLUE
#define MOVEIT_CONSOLE_COLOR_BLUE
ROS_WARN
#define ROS_WARN(...)
ROBOT_DESCRIPTION
static const std::string ROBOT_DESCRIPTION
Definition: move_group.cpp:48
move_group::MoveGroupExe::context_
MoveGroupContextPtr context_
Definition: move_group.cpp:192
pluginlib::ClassLoader
move_group
Definition: capability_names.h:41
ROS_INFO_STREAM
#define ROS_INFO_STREAM(args)
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
transform_listener.h
tf_buffer
tf2_ros::Buffer * tf_buffer
ros::waitForShutdown
ROSCPP_DECL void waitForShutdown()
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
move_group::MoveGroupExe::configureCapabilities
void configureCapabilities()
Definition: move_group.cpp:113
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:193
ros::NodeHandle::getNamespace
const std::string & getNamespace() const
ROS_INFO
#define ROS_INFO(...)
console_colors.h
moveit_cpp.h
ros::Duration
capability_names.h
move_group::DEFAULT_CAPABILITIES
static const char * DEFAULT_CAPABILITIES[]
Definition: move_group.cpp:55
XmlRpc::XmlRpcValue
ros::NodeHandle


move_group
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Tue Jun 22 2021 06:00:00