planning_request_adapter.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 <boost/bind.hpp>
39 #include <algorithm>
40 
41 // we could really use some c++11 lambda functions here :)
42 
44 {
45 namespace
46 {
47 bool callPlannerInterfaceSolve(const planning_interface::PlannerManager* planner,
48  const planning_scene::PlanningSceneConstPtr& planning_scene,
51 {
52  planning_interface::PlanningContextPtr context = planner->getPlanningContext(planning_scene, req, res.error_code_);
53  if (context)
54  return context->solve(res);
55  else
56  return false;
57 }
58 }
59 }
60 
62  const planning_interface::PlannerManagerPtr& planner, const planning_scene::PlanningSceneConstPtr& planning_scene,
64  std::vector<std::size_t>& added_path_index) const
65 {
66  return adaptAndPlan(boost::bind(&callPlannerInterfaceSolve, planner.get(), _1, _2, _3), planning_scene, req, res,
67  added_path_index);
68 }
69 
71  const planning_interface::PlannerManagerPtr& planner, const planning_scene::PlanningSceneConstPtr& planning_scene,
73 {
74  std::vector<std::size_t> dummy;
75  return adaptAndPlan(planner, planning_scene, req, res, dummy);
76 }
77 
79 {
80 namespace
81 {
82 // boost bind is not happy with overloading, so we add intermediate function objects
83 
84 bool callAdapter1(const PlanningRequestAdapter* adapter, const planning_interface::PlannerManagerPtr& planner,
85  const planning_scene::PlanningSceneConstPtr& planning_scene,
87  std::vector<std::size_t>& added_path_index)
88 {
89  try
90  {
91  return adapter->adaptAndPlan(planner, planning_scene, req, res, added_path_index);
92  }
93  catch (std::exception& ex)
94  {
95  logError("Exception caught executing *final* adapter '%s': %s", adapter->getDescription().c_str(), ex.what());
96  added_path_index.clear();
97  return callPlannerInterfaceSolve(planner.get(), planning_scene, req, res);
98  }
99 }
100 
101 bool callAdapter2(const PlanningRequestAdapter* adapter, const PlanningRequestAdapter::PlannerFn& planner,
102  const planning_scene::PlanningSceneConstPtr& planning_scene,
104  std::vector<std::size_t>& added_path_index)
105 {
106  try
107  {
108  return adapter->adaptAndPlan(planner, planning_scene, req, res, added_path_index);
109  }
110  catch (std::exception& ex)
111  {
112  logError("Exception caught executing *next* adapter '%s': %s", adapter->getDescription().c_str(), ex.what());
113  added_path_index.clear();
114  return planner(planning_scene, req, res);
115  }
116 }
117 }
118 }
119 
121  const planning_interface::PlannerManagerPtr& planner, const planning_scene::PlanningSceneConstPtr& planning_scene,
123 {
124  std::vector<std::size_t> dummy;
125  return adaptAndPlan(planner, planning_scene, req, res, dummy);
126 }
127 
129  const planning_interface::PlannerManagerPtr& planner, const planning_scene::PlanningSceneConstPtr& planning_scene,
131  std::vector<std::size_t>& added_path_index) const
132 {
133  // if there are no adapters, run the planner directly
134  if (adapters_.empty())
135  {
136  added_path_index.clear();
137  return callPlannerInterfaceSolve(planner.get(), planning_scene, req, res);
138  }
139  else
140  {
141  // the index values added by each adapter
142  std::vector<std::vector<std::size_t> > added_path_index_each(adapters_.size());
143 
144  // if there are adapters, construct a function pointer for each, in order,
145  // so that in the end we have a nested sequence of function pointers that call the adapters in the correct order.
146  PlanningRequestAdapter::PlannerFn fn = boost::bind(&callAdapter1, adapters_.back().get(), planner, _1, _2, _3,
147  boost::ref(added_path_index_each.back()));
148  for (int i = adapters_.size() - 2; i >= 0; --i)
149  fn = boost::bind(&callAdapter2, adapters_[i].get(), fn, _1, _2, _3, boost::ref(added_path_index_each[i]));
150  bool result = fn(planning_scene, req, res);
151  added_path_index.clear();
152 
153  // merge the index values from each adapter
154  for (std::size_t i = 0; i < added_path_index_each.size(); ++i)
155  for (std::size_t j = 0; j < added_path_index_each[i].size(); ++j)
156  {
157  for (std::size_t k = 0; k < added_path_index.size(); ++k)
158  if (added_path_index_each[i][j] <= added_path_index[k])
159  added_path_index[k]++;
160  added_path_index.push_back(added_path_index_each[i][j]);
161  }
162  std::sort(added_path_index.begin(), added_path_index.end());
163  return result;
164  }
165 }
virtual std::string getDescription() const
Get a short string that identifies the planning request adapter.
moveit_msgs::MoveItErrorCodes error_code_
virtual PlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr &planning_scene, const MotionPlanRequest &req, moveit_msgs::MoveItErrorCodes &error_code) const =0
Construct a planning context given the current scene and a planning request. If a problem is encounte...
bool adaptAndPlan(const planning_interface::PlannerManagerPtr &planner, const planning_scene::PlanningSceneConstPtr &planning_scene, const planning_interface::MotionPlanRequest &req, planning_interface::MotionPlanResponse &res) const
This namespace includes the central class for representing planning contexts.
moveit_msgs::MotionPlanRequest MotionPlanRequest
boost::function< bool(const planning_scene::PlanningSceneConstPtr &planning_scene, const planning_interface::MotionPlanRequest &req, planning_interface::MotionPlanResponse &res)> PlannerFn
Generic interface to adapting motion planning requests.
Base class for a MoveIt planner.
bool adaptAndPlan(const planning_interface::PlannerManagerPtr &planner, const planning_scene::PlanningSceneConstPtr &planning_scene, const planning_interface::MotionPlanRequest &req, planning_interface::MotionPlanResponse &res) const


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Jan 21 2018 03:54:29