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 
41 #include <boost/tokenizer.hpp>
44 #include <memory>
45 #include <set>
46 
47 static const std::string ROBOT_DESCRIPTION =
48  "robot_description"; // name of the robot description (a param name, so it can be changed externally)
49 
50 namespace move_group
51 {
52 // These capabilities are loaded unless listed in disable_capabilities
53 // clang-format off
54 static const char* DEFAULT_CAPABILITIES[] = {
55  "move_group/MoveGroupCartesianPathService",
56  "move_group/MoveGroupKinematicsService",
57  "move_group/MoveGroupExecuteTrajectoryAction",
58  "move_group/MoveGroupMoveAction",
59  "move_group/MoveGroupPickPlaceAction",
60  "move_group/MoveGroupPlanService",
61  "move_group/MoveGroupQueryPlannersService",
62  "move_group/MoveGroupStateValidationService",
63  "move_group/MoveGroupGetPlanningSceneService",
64  "move_group/ApplyPlanningSceneService",
65  "move_group/ClearOctomapService",
66 };
67 // clang-format on
68 
70 {
71 public:
72  MoveGroupExe(const planning_scene_monitor::PlanningSceneMonitorPtr& psm, bool debug) : node_handle_("~")
73  {
74  // if the user wants to be able to disable execution of paths, they can just set this ROS param to false
75  bool allow_trajectory_execution;
76  node_handle_.param("allow_trajectory_execution", allow_trajectory_execution, true);
77 
78  context_.reset(new MoveGroupContext(psm, allow_trajectory_execution, debug));
79 
80  // start the capabilities
82  }
83 
85  {
86  capabilities_.clear();
87  context_.reset();
89  }
90 
91  void status()
92  {
93  if (context_)
94  {
95  if (context_->status())
96  {
97  if (capabilities_.empty())
98  printf(MOVEIT_CONSOLE_COLOR_BLUE "\nmove_group is running but no capabilities are "
99  "loaded.\n\n" MOVEIT_CONSOLE_COLOR_RESET);
100  else
101  printf(MOVEIT_CONSOLE_COLOR_GREEN "\nYou can start planning now!\n\n" MOVEIT_CONSOLE_COLOR_RESET);
102  fflush(stdout);
103  }
104  }
105  else
106  ROS_ERROR("No MoveGroup context created. Nothing will work.");
107  }
108 
109 private:
111  {
112  try
113  {
115  new pluginlib::ClassLoader<MoveGroupCapability>("moveit_ros_move_group", "move_group::MoveGroupCapability"));
116  }
118  {
119  ROS_FATAL_STREAM("Exception while creating plugin loader for move_group capabilities: " << ex.what());
120  return;
121  }
122 
123  std::set<std::string> capabilities;
124 
125  // add default capabilities
126  for (size_t i = 0; i < sizeof(DEFAULT_CAPABILITIES) / sizeof(DEFAULT_CAPABILITIES[0]); ++i)
127  capabilities.insert(DEFAULT_CAPABILITIES[i]);
128 
129  // add capabilities listed in ROS parameter
130  std::string capability_plugins;
131  if (node_handle_.getParam("capabilities", capability_plugins))
132  {
133  boost::char_separator<char> sep(" ");
134  boost::tokenizer<boost::char_separator<char> > tok(capability_plugins, sep);
135  capabilities.insert(tok.begin(), tok.end());
136  }
137 
138  // drop capabilities that have been explicitly disabled
139  if (node_handle_.getParam("disable_capabilities", capability_plugins))
140  {
141  boost::char_separator<char> sep(" ");
142  boost::tokenizer<boost::char_separator<char> > tok(capability_plugins, sep);
143  for (boost::tokenizer<boost::char_separator<char> >::iterator cap_name = tok.begin(); cap_name != tok.end();
144  ++cap_name)
145  capabilities.erase(*cap_name);
146  }
147 
148  for (std::set<std::string>::iterator plugin = capabilities.cbegin(); plugin != capabilities.cend(); ++plugin)
149  {
150  try
151  {
152  printf(MOVEIT_CONSOLE_COLOR_CYAN "Loading '%s'...\n" MOVEIT_CONSOLE_COLOR_RESET, plugin->c_str());
153  MoveGroupCapabilityPtr cap = capability_plugin_loader_->createUniqueInstance(*plugin);
154  cap->setContext(context_);
155  cap->initialize();
156  capabilities_.push_back(cap);
157  }
159  {
160  ROS_ERROR_STREAM("Exception while loading move_group capability '" << *plugin << "': " << ex.what());
161  }
162  }
163 
164  std::stringstream ss;
165  ss << std::endl;
166  ss << std::endl;
167  ss << "********************************************************" << std::endl;
168  ss << "* MoveGroup using: " << std::endl;
169  for (std::size_t i = 0; i < capabilities_.size(); ++i)
170  ss << "* - " << capabilities_[i]->getName() << std::endl;
171  ss << "********************************************************" << std::endl;
172  ROS_INFO_STREAM(ss.str());
173  }
174 
176  MoveGroupContextPtr context_;
177  std::shared_ptr<pluginlib::ClassLoader<MoveGroupCapability> > capability_plugin_loader_;
178  std::vector<MoveGroupCapabilityPtr> capabilities_;
179 };
180 }
181 
182 int main(int argc, char** argv)
183 {
184  ros::init(argc, argv, move_group::NODE_NAME);
185 
187  spinner.start();
188  ros::NodeHandle nh;
189 
190  std::shared_ptr<tf2_ros::Buffer> tf_buffer = std::make_shared<tf2_ros::Buffer>(ros::Duration(10.0));
191  std::shared_ptr<tf2_ros::TransformListener> tfl = std::make_shared<tf2_ros::TransformListener>(*tf_buffer, nh);
192 
193  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor(
195 
196  if (planning_scene_monitor->getPlanningScene())
197  {
198  bool debug = false;
199  for (int i = 1; i < argc; ++i)
200  if (strncmp(argv[i], "--debug", 7) == 0)
201  {
202  debug = true;
203  break;
204  }
205  if (debug)
206  ROS_INFO("MoveGroup debug mode is ON");
207  else
208  ROS_INFO("MoveGroup debug mode is OFF");
209 
210  printf(MOVEIT_CONSOLE_COLOR_CYAN "Starting planning scene monitors...\n" MOVEIT_CONSOLE_COLOR_RESET);
211  planning_scene_monitor->startSceneMonitor();
212  planning_scene_monitor->startWorldGeometryMonitor();
213  planning_scene_monitor->startStateMonitor();
214  printf(MOVEIT_CONSOLE_COLOR_CYAN "Planning scene monitors started.\n" MOVEIT_CONSOLE_COLOR_RESET);
215 
216  move_group::MoveGroupExe mge(planning_scene_monitor, debug);
217 
218  planning_scene_monitor->publishDebugInformation(debug);
219 
220  mge.status();
221 
223  }
224  else
225  ROS_ERROR("Planning scene not configured");
226 
227  return 0;
228 }
MoveGroupContextPtr context_
Definition: move_group.cpp:176
#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:54
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
#define MOVEIT_CONSOLE_COLOR_CYAN
ROSCONSOLE_CONSOLE_IMPL_DECL std::string getName(void *handle)
MoveGroupExe(const planning_scene_monitor::PlanningSceneMonitorPtr &psm, bool debug)
Definition: move_group.cpp:72
void spinner()
std::shared_ptr< pluginlib::ClassLoader< MoveGroupCapability > > capability_plugin_loader_
Definition: move_group.cpp:177
bool param(const std::string &param_name, T &param_val, const T &default_val) const
int main(int argc, char **argv)
Definition: move_group.cpp:182
#define ROS_FATAL_STREAM(args)
#define ROS_INFO(...)
bool getParam(const std::string &key, std::string &s) const
ros::NodeHandle node_handle_
Definition: move_group.cpp:175
std::vector< MoveGroupCapabilityPtr > capabilities_
Definition: move_group.cpp:178
static const std::string ROBOT_DESCRIPTION
Definition: move_group.cpp:47
tf2_ros::Buffer * tf_buffer
#define ROS_INFO_STREAM(args)
#define MOVEIT_CONSOLE_COLOR_GREEN
#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 Fri Feb 8 2019 04:08:16