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 
38 #include <tf/transform_listener.h>
41 #include <boost/algorithm/string/join.hpp>
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 planning_scene_monitor::PlanningSceneMonitorPtr& psm, bool debug) : node_handle_("~")
74  {
75  // if the user wants to be able to disable execution of paths, they can just set this ROS param to false
76  bool allow_trajectory_execution;
77  node_handle_.param("allow_trajectory_execution", allow_trajectory_execution, true);
78 
79  context_.reset(new MoveGroupContext(psm, allow_trajectory_execution, debug));
80 
81  // start the capabilities
83  }
84 
86  {
87  capabilities_.clear();
88  context_.reset();
90  }
91 
92  void status()
93  {
94  if (context_)
95  {
96  if (context_->status())
97  {
98  if (capabilities_.empty())
99  printf(MOVEIT_CONSOLE_COLOR_BLUE "\nmove_group is running but no capabilities are "
100  "loaded.\n\n" MOVEIT_CONSOLE_COLOR_RESET);
101  else
102  printf(MOVEIT_CONSOLE_COLOR_GREEN "\nYou can start planning now!\n\n" MOVEIT_CONSOLE_COLOR_RESET);
103  fflush(stdout);
104  }
105  }
106  else
107  ROS_ERROR("No MoveGroup context created. Nothing will work.");
108  }
109 
110 private:
112  {
113  try
114  {
116  new pluginlib::ClassLoader<MoveGroupCapability>("moveit_ros_move_group", "move_group::MoveGroupCapability"));
117  }
119  {
120  ROS_FATAL_STREAM("Exception while creating plugin loader for move_group capabilities: " << ex.what());
121  return;
122  }
123 
124  std::set<std::string> capabilities;
125 
126  // add default capabilities
127  for (size_t i = 0; i < sizeof(DEFAULT_CAPABILITIES) / sizeof(DEFAULT_CAPABILITIES[0]); ++i)
128  capabilities.insert(DEFAULT_CAPABILITIES[i]);
129 
130  // add capabilities listed in ROS parameter
131  std::string capability_plugins;
132  if (node_handle_.getParam("capabilities", capability_plugins))
133  {
134  boost::char_separator<char> sep(" ");
135  boost::tokenizer<boost::char_separator<char> > tok(capability_plugins, sep);
136  capabilities.insert(tok.begin(), tok.end());
137  }
138 
139  // drop capabilities that have been explicitly disabled
140  if (node_handle_.getParam("disable_capabilities", capability_plugins))
141  {
142  boost::char_separator<char> sep(" ");
143  boost::tokenizer<boost::char_separator<char> > tok(capability_plugins, sep);
144  for (boost::tokenizer<boost::char_separator<char> >::iterator cap_name = tok.begin(); cap_name != tok.end();
145  ++cap_name)
146  capabilities.erase(*cap_name);
147  }
148 
149  for (std::set<std::string>::iterator plugin = capabilities.cbegin(); plugin != capabilities.cend(); ++plugin)
150  {
151  try
152  {
153  printf(MOVEIT_CONSOLE_COLOR_CYAN "Loading '%s'...\n" MOVEIT_CONSOLE_COLOR_RESET, plugin->c_str());
154  MoveGroupCapability* cap = capability_plugin_loader_->createUnmanagedInstance(*plugin);
155  cap->setContext(context_);
156  cap->initialize();
157  capabilities_.push_back(MoveGroupCapabilityPtr(cap));
158  }
160  {
161  ROS_ERROR_STREAM("Exception while loading move_group capability '"
162  << *plugin << "': " << ex.what() << std::endl
163  << "Available capabilities: "
164  << boost::algorithm::join(capability_plugin_loader_->getDeclaredClasses(), ", "));
165  }
166  }
167 
168  std::stringstream ss;
169  ss << std::endl;
170  ss << std::endl;
171  ss << "********************************************************" << std::endl;
172  ss << "* MoveGroup using: " << std::endl;
173  for (std::size_t i = 0; i < capabilities_.size(); ++i)
174  ss << "* - " << capabilities_[i]->getName() << std::endl;
175  ss << "********************************************************" << std::endl;
176  ROS_INFO_STREAM(ss.str());
177  }
178 
180  MoveGroupContextPtr context_;
181  std::shared_ptr<pluginlib::ClassLoader<MoveGroupCapability> > capability_plugin_loader_;
182  std::vector<MoveGroupCapabilityPtr> capabilities_;
183 };
184 }
185 
186 int main(int argc, char** argv)
187 {
188  ros::init(argc, argv, move_group::NODE_NAME);
189 
191  spinner.start();
192 
194 
195  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor(
197 
198  if (planning_scene_monitor->getPlanningScene())
199  {
200  bool debug = false;
201  for (int i = 1; i < argc; ++i)
202  if (strncmp(argv[i], "--debug", 7) == 0)
203  {
204  debug = true;
205  break;
206  }
207  if (debug)
208  ROS_INFO("MoveGroup debug mode is ON");
209  else
210  ROS_INFO("MoveGroup debug mode is OFF");
211 
212  printf(MOVEIT_CONSOLE_COLOR_CYAN "Starting context monitors...\n" MOVEIT_CONSOLE_COLOR_RESET);
213  planning_scene_monitor->startSceneMonitor();
214  planning_scene_monitor->startWorldGeometryMonitor();
215  planning_scene_monitor->startStateMonitor();
216  printf(MOVEIT_CONSOLE_COLOR_CYAN "Context monitors started.\n" MOVEIT_CONSOLE_COLOR_RESET);
217 
218  move_group::MoveGroupExe mge(planning_scene_monitor, debug);
219 
220  planning_scene_monitor->publishDebugInformation(debug);
221 
222  mge.status();
223 
225  }
226  else
227  ROS_ERROR("Planning scene not configured");
228 
229  return 0;
230 }
MoveGroupContextPtr context_
Definition: move_group.cpp:180
#define MOVEIT_CONSOLE_COLOR_RESET
static const std::string NODE_NAME
Definition: node_name.h:44
static const char * DEFAULT_CAPABILITIES[]
Definition: move_group.cpp:55
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
#define MOVEIT_CONSOLE_COLOR_CYAN
std::string getName(void *handle)
MoveGroupExe(const planning_scene_monitor::PlanningSceneMonitorPtr &psm, bool debug)
Definition: move_group.cpp:73
void spinner()
std::shared_ptr< pluginlib::ClassLoader< MoveGroupCapability > > capability_plugin_loader_
Definition: move_group.cpp:181
int main(int argc, char **argv)
Definition: move_group.cpp:186
#define ROS_FATAL_STREAM(args)
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ros::NodeHandle node_handle_
Definition: move_group.cpp:179
std::vector< MoveGroupCapabilityPtr > capabilities_
Definition: move_group.cpp:182
static const std::string ROBOT_DESCRIPTION
Definition: move_group.cpp:48
#define ROS_INFO_STREAM(args)
#define MOVEIT_CONSOLE_COLOR_GREEN
bool getParam(const std::string &key, std::string &s) const
#define ROS_ERROR_STREAM(args)
#define ROS_ERROR(...)
#define MOVEIT_CONSOLE_COLOR_BLUE
ROSCPP_DECL void waitForShutdown()


move_group
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Thu Oct 18 2018 02:48:05