planning_pipeline.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 <moveit_msgs/DisplayTrajectory.h>
42 #include <visualization_msgs/MarkerArray.h>
43 #include <boost/tokenizer.hpp>
44 #include <boost/algorithm/string/join.hpp>
45 #include <sstream>
46 
47 const std::string planning_pipeline::PlanningPipeline::DISPLAY_PATH_TOPIC = "display_planned_path";
48 const std::string planning_pipeline::PlanningPipeline::MOTION_PLAN_REQUEST_TOPIC = "motion_plan_request";
49 const std::string planning_pipeline::PlanningPipeline::MOTION_CONTACTS_TOPIC = "display_contacts";
50 
51 planning_pipeline::PlanningPipeline::PlanningPipeline(const robot_model::RobotModelConstPtr& model,
52  const ros::NodeHandle& nh,
53  const std::string& planner_plugin_param_name,
54  const std::string& adapter_plugins_param_name)
55  : nh_(nh), kmodel_(model)
56 {
57  std::string planner;
58  if (nh_.getParam(planner_plugin_param_name, planner))
59  planner_plugin_name_ = planner;
60 
61  std::string adapters;
62  if (nh_.getParam(adapter_plugins_param_name, adapters))
63  {
64  boost::char_separator<char> sep(" ");
65  boost::tokenizer<boost::char_separator<char> > tok(adapters, sep);
66  for (boost::tokenizer<boost::char_separator<char> >::iterator beg = tok.begin(); beg != tok.end(); ++beg)
67  adapter_plugin_names_.push_back(*beg);
68  }
69 
70  configure();
71 }
72 
73 planning_pipeline::PlanningPipeline::PlanningPipeline(const robot_model::RobotModelConstPtr& model,
74  const ros::NodeHandle& nh, const std::string& planner_plugin_name,
75  const std::vector<std::string>& adapter_plugin_names)
76  : nh_(nh), planner_plugin_name_(planner_plugin_name), adapter_plugin_names_(adapter_plugin_names), kmodel_(model)
77 {
78  configure();
79 }
80 
82 {
83  check_solution_paths_ = false; // this is set to true below
85  display_computed_motion_plans_ = false; // this is set to true below
86 
87  // load the planning plugin
88  try
89  {
91  "moveit_core", "planning_interface::PlannerManager"));
92  }
94  {
95  ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what());
96  }
97 
98  std::vector<std::string> classes;
100  classes = planner_plugin_loader_->getDeclaredClasses();
101  if (planner_plugin_name_.empty() && classes.size() == 1)
102  {
103  planner_plugin_name_ = classes[0];
104  ROS_INFO("No '~planning_plugin' parameter specified, but only '%s' planning plugin is available. Using that one.",
105  planner_plugin_name_.c_str());
106  }
107  if (planner_plugin_name_.empty() && classes.size() > 1)
108  {
109  planner_plugin_name_ = classes[0];
110  ROS_INFO("Multiple planning plugins available. You should specify the '~planning_plugin' parameter. Using '%s' for "
111  "now.",
112  planner_plugin_name_.c_str());
113  }
114  try
115  {
116  planner_instance_.reset(planner_plugin_loader_->createUnmanagedInstance(planner_plugin_name_));
117  if (!planner_instance_->initialize(kmodel_, nh_.getNamespace()))
118  throw std::runtime_error("Unable to initialize planning plugin");
119  ROS_INFO_STREAM("Using planning interface '" << planner_instance_->getDescription() << "'");
120  }
122  {
123  ROS_ERROR_STREAM("Exception while loading planner '"
124  << planner_plugin_name_ << "': " << ex.what() << std::endl
125  << "Available plugins: " << boost::algorithm::join(classes, ", "));
126  }
127 
128  // load the planner request adapters
129  if (!adapter_plugin_names_.empty())
130  {
131  std::vector<planning_request_adapter::PlanningRequestAdapterConstPtr> ads;
132  try
133  {
135  "moveit_core", "planning_request_adapter::PlanningRequestAdapter"));
136  }
138  {
139  ROS_ERROR_STREAM("Exception while creating planning plugin loader " << ex.what());
140  }
141 
143  for (std::size_t i = 0; i < adapter_plugin_names_.size(); ++i)
144  {
145  planning_request_adapter::PlanningRequestAdapterConstPtr ad;
146  try
147  {
148  ad.reset(adapter_plugin_loader_->createUnmanagedInstance(adapter_plugin_names_[i]));
149  }
151  {
152  ROS_ERROR_STREAM("Exception while loading planning adapter plugin '" << adapter_plugin_names_[i]
153  << "': " << ex.what());
154  }
155  if (ad)
156  ads.push_back(ad);
157  }
158  if (!ads.empty())
159  {
161  for (std::size_t i = 0; i < ads.size(); ++i)
162  {
163  ROS_INFO_STREAM("Using planning request adapter '" << ads[i]->getDescription() << "'");
164  adapter_chain_->addAdapter(ads[i]);
165  }
166  }
167  }
169  checkSolutionPaths(true);
170 }
171 
173 {
174  if (display_computed_motion_plans_ && !flag)
176  else if (!display_computed_motion_plans_ && flag)
177  display_path_publisher_ = nh_.advertise<moveit_msgs::DisplayTrajectory>(DISPLAY_PATH_TOPIC, 10, true);
179 }
180 
182 {
183  if (publish_received_requests_ && !flag)
185  else if (!publish_received_requests_ && flag)
186  received_request_publisher_ = nh_.advertise<moveit_msgs::MotionPlanRequest>(MOTION_PLAN_REQUEST_TOPIC, 10, true);
188 }
189 
191 {
192  if (check_solution_paths_ && !flag)
194  else if (!check_solution_paths_ && flag)
195  contacts_publisher_ = nh_.advertise<visualization_msgs::MarkerArray>(MOTION_CONTACTS_TOPIC, 100, true);
196  check_solution_paths_ = flag;
197 }
198 
199 bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene,
202 {
203  std::vector<std::size_t> dummy;
204  return generatePlan(planning_scene, req, res, dummy);
205 }
206 
207 bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene,
210  std::vector<std::size_t>& adapter_added_state_index) const
211 {
212  // broadcast the request we are about to work on, if needed
215  adapter_added_state_index.clear();
216 
217  if (!planner_instance_)
218  {
219  ROS_ERROR("No planning plugin loaded. Cannot plan.");
220  return false;
221  }
222 
223  bool solved = false;
224  try
225  {
226  if (adapter_chain_)
227  {
228  solved = adapter_chain_->adaptAndPlan(planner_instance_, planning_scene, req, res, adapter_added_state_index);
229  if (!adapter_added_state_index.empty())
230  {
231  std::stringstream ss;
232  for (std::size_t i = 0; i < adapter_added_state_index.size(); ++i)
233  ss << adapter_added_state_index[i] << " ";
234  ROS_INFO("Planning adapters have added states at index positions: [ %s]", ss.str().c_str());
235  }
236  }
237  else
238  {
239  planning_interface::PlanningContextPtr context =
240  planner_instance_->getPlanningContext(planning_scene, req, res.error_code_);
241  solved = context ? context->solve(res) : false;
242  }
243  }
244  catch (std::exception& ex)
245  {
246  ROS_ERROR("Exception caught: '%s'", ex.what());
247  return false;
248  }
249  bool valid = true;
250 
251  if (solved && res.trajectory_)
252  {
253  std::size_t state_count = res.trajectory_->getWayPointCount();
254  ROS_DEBUG_STREAM("Motion planner reported a solution path with " << state_count << " states");
256  {
257  std::vector<std::size_t> index;
258  if (!planning_scene->isPathValid(*res.trajectory_, req.path_constraints, req.group_name, false, &index))
259  {
260  // check to see if there is any problem with the states that are found to be invalid
261  // they are considered ok if they were added by a planning request adapter
262  bool problem = false;
263  for (std::size_t i = 0; i < index.size() && !problem; ++i)
264  {
265  bool found = false;
266  for (std::size_t j = 0; j < adapter_added_state_index.size(); ++j)
267  if (index[i] == adapter_added_state_index[j])
268  {
269  found = true;
270  break;
271  }
272  if (!found)
273  problem = true;
274  }
275  if (problem)
276  {
277  if (index.size() == 1 && index[0] == 0) // ignore cases when the robot starts at invalid location
278  ROS_DEBUG("It appears the robot is starting at an invalid state, but that is ok.");
279  else
280  {
281  valid = false;
282  res.error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN;
283 
284  // display error messages
285  std::stringstream ss;
286  for (std::size_t i = 0; i < index.size(); ++i)
287  ss << index[i] << " ";
288  ROS_ERROR_STREAM("Computed path is not valid. Invalid states at index locations: [ "
289  << ss.str() << "] out of " << state_count
290  << ". Explanations follow in command line. Contacts are published on "
292 
293  // call validity checks in verbose mode for the problematic states
294  visualization_msgs::MarkerArray arr;
295  for (std::size_t i = 0; i < index.size(); ++i)
296  {
297  // check validity with verbose on
298  const robot_state::RobotState& kstate = res.trajectory_->getWayPoint(index[i]);
299  planning_scene->isStateValid(kstate, req.path_constraints, req.group_name, true);
300 
301  // compute the contacts if any
304  c_req.contacts = true;
305  c_req.max_contacts = 10;
306  c_req.max_contacts_per_pair = 3;
307  c_req.verbose = false;
308  planning_scene->checkCollision(c_req, c_res, kstate);
309  if (c_res.contact_count > 0)
310  {
311  visualization_msgs::MarkerArray arr_i;
312  collision_detection::getCollisionMarkersFromContacts(arr_i, planning_scene->getPlanningFrame(),
313  c_res.contacts);
314  arr.markers.insert(arr.markers.end(), arr_i.markers.begin(), arr_i.markers.end());
315  }
316  }
317  ROS_ERROR_STREAM("Completed listing of explanations for invalid states.");
318  if (!arr.markers.empty())
320  }
321  }
322  else
323  ROS_DEBUG("Planned path was found to be valid, except for states that were added by planning request "
324  "adapters, but that is ok.");
325  }
326  else
327  ROS_DEBUG("Planned path was found to be valid when rechecked");
328  }
329  }
330 
331  // display solution path if needed
332  if (display_computed_motion_plans_ && solved)
333  {
334  moveit_msgs::DisplayTrajectory disp;
335  disp.model_id = kmodel_->getName();
336  disp.trajectory.resize(1);
337  res.trajectory_->getRobotTrajectoryMsg(disp.trajectory[0]);
338  robot_state::robotStateToRobotStateMsg(res.trajectory_->getFirstWayPoint(), disp.trajectory_start);
340  }
341 
342  return solved && valid;
343 }
344 
346 {
347  if (planner_instance_)
348  planner_instance_->terminate();
349 }
robot_trajectory::RobotTrajectoryPtr trajectory_
static const std::string MOTION_PLAN_REQUEST_TOPIC
When motion planning requests are received and they are supposed to be automatically published...
void publish(const boost::shared_ptr< M > &message) const
void getCollisionMarkersFromContacts(visualization_msgs::MarkerArray &arr, const std::string &frame_id, const CollisionResult::ContactMap &con, const std_msgs::ColorRGBA &color, const ros::Duration &lifetime, const double radius=0.035)
bool generatePlan(const planning_scene::PlanningSceneConstPtr &planning_scene, const planning_interface::MotionPlanRequest &req, planning_interface::MotionPlanResponse &res) const
Call the motion planner plugin and the sequence of planning request adapters (if any).
std::string resolveName(const std::string &name, bool remap=true) const
std::unique_ptr< planning_request_adapter::PlanningRequestAdapterChain > adapter_chain_
void publishReceivedRequests(bool flag)
Pass a flag telling the pipeline whether or not to publish the received motion planning requests on M...
void displayComputedMotionPlans(bool flag)
Pass a flag telling the pipeline whether or not to publish the computed motion plans on DISPLAY_PATH_...
moveit_msgs::MoveItErrorCodes error_code_
void terminate() const
Request termination, if a generatePlan() function is currently computing plans.
unsigned int index
#define ROS_FATAL_STREAM(args)
std::vector< std::string > adapter_plugin_names_
#define ROS_INFO(...)
robot_model::RobotModelConstPtr kmodel_
const std::string & getNamespace() const
void checkSolutionPaths(bool flag)
Pass a flag telling the pipeline whether or not to re-check the solution paths reported by the planne...
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
#define ROS_DEBUG_STREAM(args)
bool check_solution_paths_
Flag indicating whether the reported plans should be checked once again, by the planning pipeline its...
moveit_msgs::MotionPlanRequest MotionPlanRequest
static const std::string DISPLAY_PATH_TOPIC
When motion plans are computed and they are supposed to be automatically displayed, they are sent to this topic (moveit_msgs::DisplauTrajectory)
std::unique_ptr< pluginlib::ClassLoader< planning_interface::PlannerManager > > planner_plugin_loader_
#define ROS_INFO_STREAM(args)
bool getParam(const std::string &key, std::string &s) const
bool display_computed_motion_plans_
Flag indicating whether motion plans should be published as a moveit_msgs::DisplayTrajectory.
planning_interface::PlannerManagerPtr planner_instance_
#define ROS_ERROR_STREAM(args)
#define ROS_ERROR(...)
static const std::string MOTION_CONTACTS_TOPIC
When contacts are found in the solution path reported by a planner, they can be published as markers ...
#define ROS_DEBUG(...)
std::unique_ptr< pluginlib::ClassLoader< planning_request_adapter::PlanningRequestAdapter > > adapter_plugin_loader_
PlanningPipeline(const robot_model::RobotModelConstPtr &model, const ros::NodeHandle &nh=ros::NodeHandle("~"), const std::string &planning_plugin_param_name="planning_plugin", const std::string &adapter_plugins_param_name="request_adapters")
Given a robot model (model), a node handle (nh), initialize the planning pipeline.


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Sun Oct 18 2020 13:17:33