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