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 <functional>
39 #include <algorithm>
40 
42 {
43 namespace
44 {
45 bool callPlannerInterfaceSolve(const planning_interface::PlannerManager& planner,
46  const planning_scene::PlanningSceneConstPtr& planning_scene,
49 {
50  planning_interface::PlanningContextPtr context = planner.getPlanningContext(planning_scene, req, res.error_code_);
51  if (context)
52  return context->solve(res);
53  else
54  return false;
55 }
56 
57 bool callAdapter(const PlanningRequestAdapter& adapter, const PlanningRequestAdapter::PlannerFn& planner,
58  const planning_scene::PlanningSceneConstPtr& planning_scene,
60  std::vector<std::size_t>& added_path_index)
61 {
62  try
63  {
64  return adapter.adaptAndPlan(planner, planning_scene, req, res, added_path_index);
65  }
66  catch (std::exception& ex)
67  {
68  ROS_ERROR_NAMED("planning_request_adapter",
69  "Exception caught executing adapter '%s': %s\nSkipping adapter instead.",
70  adapter.getDescription().c_str(), ex.what());
71  added_path_index.clear();
72  return planner(planning_scene, req, res);
73  }
74 }
75 
76 } // namespace
77 
78 bool PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
79  const planning_scene::PlanningSceneConstPtr& planning_scene,
82  std::vector<std::size_t>& added_path_index) const
83 {
84  return adaptAndPlan(
85  [&planner](const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req,
87  return callPlannerInterfaceSolve(*planner, scene, req, res);
88  },
89  planning_scene, req, res, added_path_index);
90 }
91 
92 bool PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
93  const planning_scene::PlanningSceneConstPtr& planning_scene,
96 {
97  std::vector<std::size_t> dummy;
98  return adaptAndPlan(planner, planning_scene, req, res, dummy);
99 }
100 
101 bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
102  const planning_scene::PlanningSceneConstPtr& planning_scene,
105 {
106  std::vector<std::size_t> dummy;
107  return adaptAndPlan(planner, planning_scene, req, res, dummy);
108 }
109 
110 bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
111  const planning_scene::PlanningSceneConstPtr& planning_scene,
114  std::vector<std::size_t>& added_path_index) const
115 {
116  // if there are no adapters, run the planner directly
117  if (adapters_.empty())
118  {
119  added_path_index.clear();
120  return callPlannerInterfaceSolve(*planner, planning_scene, req, res);
121  }
122  else
123  {
124  // the index values added by each adapter
125  std::vector<std::vector<std::size_t> > added_path_index_each(adapters_.size());
126 
127  // if there are adapters, construct a function for each, in order,
128  // so that in the end we have a nested sequence of functions that calls all adapters
129  // and eventually the planner in the correct order.
130  PlanningRequestAdapter::PlannerFn fn = [&planner = *planner](const planning_scene::PlanningSceneConstPtr& scene,
133  return callPlannerInterfaceSolve(planner, scene, req, res);
134  };
135 
136  for (int i = adapters_.size() - 1; i >= 0; --i)
137  {
138  fn = [&adapter = *adapters_[i], fn, &added_path_index = added_path_index_each[i]](
139  const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req,
141  return callAdapter(adapter, fn, scene, req, res, added_path_index);
142  };
143  }
144 
145  bool result = fn(planning_scene, req, res);
146  added_path_index.clear();
147 
148  // merge the index values from each adapter
149  for (std::vector<std::size_t>& added_states_by_each_adapter : added_path_index_each)
150  for (std::size_t& added_index : added_states_by_each_adapter)
151  {
152  for (std::size_t& index_in_path : added_path_index)
153  if (added_index <= index_in_path)
154  index_in_path++;
155  added_path_index.push_back(added_index);
156  }
157  std::sort(added_path_index.begin(), added_path_index.end());
158  return result;
159  }
160 }
161 
162 } // end of namespace planning_request_adapter
planning_interface::PlannerManager::getPlanningContext
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...
planning_request_adapter::PlanningRequestAdapterChain::adaptAndPlan
bool adaptAndPlan(const planning_interface::PlannerManagerPtr &planner, const planning_scene::PlanningSceneConstPtr &planning_scene, const planning_interface::MotionPlanRequest &req, planning_interface::MotionPlanResponse &res) const
Definition: planning_request_adapter.cpp:133
planning_interface::MotionPlanResponse
Definition: planning_response.h:78
planning_interface::MotionPlanResponse::error_code_
moveit_msgs::MoveItErrorCodes error_code_
Definition: planning_response.h:120
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
planning_request_adapter.h
planning_request_adapter::PlanningRequestAdapter::adaptAndPlan
bool adaptAndPlan(const planning_interface::PlannerManagerPtr &planner, const planning_scene::PlanningSceneConstPtr &planning_scene, const planning_interface::MotionPlanRequest &req, planning_interface::MotionPlanResponse &res) const
Definition: planning_request_adapter.cpp:124
planning_interface::MotionPlanRequest
moveit_msgs::MotionPlanRequest MotionPlanRequest
Definition: planning_request.h:77
planning_request_adapter::PlanningRequestAdapterChain::adapters_
std::vector< PlanningRequestAdapterConstPtr > adapters_
Definition: planning_request_adapter.h:151
planning_request_adapter::PlanningRequestAdapter::PlannerFn
boost::function< bool(const planning_scene::PlanningSceneConstPtr &, const planning_interface::MotionPlanRequest &, planning_interface::MotionPlanResponse &)> PlannerFn
Definition: planning_request_adapter.h:86
planning_request_adapter
Generic interface to adapting motion planning requests.
Definition: planning_request_adapter.h:45
planning_scene
This namespace includes the central class for representing planning contexts.
Definition: planning_interface.h:45
planning_interface::PlannerManager
Base class for a MoveIt planner.
Definition: planning_interface.h:150


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jun 15 2022 02:24:55