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 bool PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
61  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 
70 bool PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
71  const planning_scene::PlanningSceneConstPtr& planning_scene,
74 {
75  std::vector<std::size_t> dummy;
76  return adaptAndPlan(planner, planning_scene, req, res, dummy);
77 }
78 
79 namespace
80 {
81 // boost bind is not happy with overloading, so we add intermediate function objects
82 
83 bool callAdapter1(const PlanningRequestAdapter* adapter, const planning_interface::PlannerManagerPtr& planner,
84  const planning_scene::PlanningSceneConstPtr& planning_scene,
86  std::vector<std::size_t>& added_path_index)
87 {
88  try
89  {
90  return adapter->adaptAndPlan(planner, planning_scene, req, res, added_path_index);
91  }
92  catch (std::exception& ex)
93  {
94  ROS_ERROR_NAMED("planning_request_adapter", "Exception caught executing *final* adapter '%s': %s",
95  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  ROS_ERROR_NAMED("planning_request_adapter", "Exception caught executing *next* adapter '%s': %s",
113  adapter->getDescription().c_str(), ex.what());
114  added_path_index.clear();
115  return planner(planning_scene, req, res);
116  }
117 }
118 }
119 
120 bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
121  const planning_scene::PlanningSceneConstPtr& planning_scene,
124 {
125  std::vector<std::size_t> dummy;
126  return adaptAndPlan(planner, planning_scene, req, res, dummy);
127 }
128 
129 bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
130  const planning_scene::PlanningSceneConstPtr& planning_scene,
133  std::vector<std::size_t>& added_path_index) const
134 {
135  // if there are no adapters, run the planner directly
136  if (adapters_.empty())
137  {
138  added_path_index.clear();
139  return callPlannerInterfaceSolve(planner.get(), planning_scene, req, res);
140  }
141  else
142  {
143  // the index values added by each adapter
144  std::vector<std::vector<std::size_t> > added_path_index_each(adapters_.size());
145 
146  // if there are adapters, construct a function pointer for each, in order,
147  // so that in the end we have a nested sequence of function pointers that call the adapters in the correct order.
148  PlanningRequestAdapter::PlannerFn fn = boost::bind(&callAdapter1, adapters_.back().get(), planner, _1, _2, _3,
149  boost::ref(added_path_index_each.back()));
150  for (int i = adapters_.size() - 2; i >= 0; --i)
151  fn = boost::bind(&callAdapter2, adapters_[i].get(), fn, _1, _2, _3, boost::ref(added_path_index_each[i]));
152  bool result = fn(planning_scene, req, res);
153  added_path_index.clear();
154 
155  // merge the index values from each adapter
156  for (std::size_t i = 0; i < added_path_index_each.size(); ++i)
157  for (std::size_t j = 0; j < added_path_index_each[i].size(); ++j)
158  {
159  for (std::size_t k = 0; k < added_path_index.size(); ++k)
160  if (added_path_index_each[i][j] <= added_path_index[k])
161  added_path_index[k]++;
162  added_path_index.push_back(added_path_index_each[i][j]);
163  }
164  std::sort(added_path_index.begin(), added_path_index.end());
165  return result;
166  }
167 }
168 
169 } // end of namespace planning_request_adapter
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.
#define ROS_ERROR_NAMED(name,...)
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 Wed Jul 10 2019 04:03:05