00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <moveit/planning_request_adapter/planning_request_adapter.h>
00038 #include <boost/bind.hpp>
00039 #include <algorithm>
00040
00041
00042
00043 namespace planning_request_adapter
00044 {
00045 namespace
00046 {
00047 bool callPlannerInterfaceSolve(const planning_interface::PlannerManager* planner,
00048 const planning_scene::PlanningSceneConstPtr& planning_scene,
00049 const planning_interface::MotionPlanRequest& req,
00050 planning_interface::MotionPlanResponse& res)
00051 {
00052 planning_interface::PlanningContextPtr context = planner->getPlanningContext(planning_scene, req, res.error_code_);
00053 if (context)
00054 return context->solve(res);
00055 else
00056 return false;
00057 }
00058 }
00059 }
00060
00061 bool planning_request_adapter::PlanningRequestAdapter::adaptAndPlan(
00062 const planning_interface::PlannerManagerPtr& planner, const planning_scene::PlanningSceneConstPtr& planning_scene,
00063 const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
00064 std::vector<std::size_t>& added_path_index) const
00065 {
00066 return adaptAndPlan(boost::bind(&callPlannerInterfaceSolve, planner.get(), _1, _2, _3), planning_scene, req, res,
00067 added_path_index);
00068 }
00069
00070 bool planning_request_adapter::PlanningRequestAdapter::adaptAndPlan(
00071 const planning_interface::PlannerManagerPtr& planner, const planning_scene::PlanningSceneConstPtr& planning_scene,
00072 const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res) const
00073 {
00074 std::vector<std::size_t> dummy;
00075 return adaptAndPlan(planner, planning_scene, req, res, dummy);
00076 }
00077
00078 namespace planning_request_adapter
00079 {
00080 namespace
00081 {
00082
00083
00084 bool callAdapter1(const PlanningRequestAdapter* adapter, const planning_interface::PlannerManagerPtr& planner,
00085 const planning_scene::PlanningSceneConstPtr& planning_scene,
00086 const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
00087 std::vector<std::size_t>& added_path_index)
00088 {
00089 try
00090 {
00091 return adapter->adaptAndPlan(planner, planning_scene, req, res, added_path_index);
00092 }
00093 catch (std::runtime_error& ex)
00094 {
00095 logError("Exception caught executing *final* adapter '%s': %s", adapter->getDescription().c_str(), ex.what());
00096 added_path_index.clear();
00097 return callPlannerInterfaceSolve(planner.get(), planning_scene, req, res);
00098 }
00099 catch (...)
00100 {
00101 logError("Exception caught executing *final* adapter '%s'", adapter->getDescription().c_str());
00102 added_path_index.clear();
00103 return callPlannerInterfaceSolve(planner.get(), planning_scene, req, res);
00104 }
00105 }
00106
00107 bool callAdapter2(const PlanningRequestAdapter* adapter, const PlanningRequestAdapter::PlannerFn& planner,
00108 const planning_scene::PlanningSceneConstPtr& planning_scene,
00109 const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
00110 std::vector<std::size_t>& added_path_index)
00111 {
00112 try
00113 {
00114 return adapter->adaptAndPlan(planner, planning_scene, req, res, added_path_index);
00115 }
00116 catch (std::runtime_error& ex)
00117 {
00118 logError("Exception caught executing *next* adapter '%s': %s", adapter->getDescription().c_str(), ex.what());
00119 added_path_index.clear();
00120 return planner(planning_scene, req, res);
00121 }
00122 catch (...)
00123 {
00124 logError("Exception caught executing *next* adapter '%s'", adapter->getDescription().c_str());
00125 added_path_index.clear();
00126 return planner(planning_scene, req, res);
00127 }
00128 }
00129 }
00130 }
00131
00132 bool planning_request_adapter::PlanningRequestAdapterChain::adaptAndPlan(
00133 const planning_interface::PlannerManagerPtr& planner, const planning_scene::PlanningSceneConstPtr& planning_scene,
00134 const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res) const
00135 {
00136 std::vector<std::size_t> dummy;
00137 return adaptAndPlan(planner, planning_scene, req, res, dummy);
00138 }
00139
00140 bool planning_request_adapter::PlanningRequestAdapterChain::adaptAndPlan(
00141 const planning_interface::PlannerManagerPtr& planner, const planning_scene::PlanningSceneConstPtr& planning_scene,
00142 const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
00143 std::vector<std::size_t>& added_path_index) const
00144 {
00145
00146 if (adapters_.empty())
00147 {
00148 added_path_index.clear();
00149 return callPlannerInterfaceSolve(planner.get(), planning_scene, req, res);
00150 }
00151 else
00152 {
00153
00154 std::vector<std::vector<std::size_t> > added_path_index_each(adapters_.size());
00155
00156
00157
00158 PlanningRequestAdapter::PlannerFn fn = boost::bind(&callAdapter1, adapters_.back().get(), planner, _1, _2, _3,
00159 boost::ref(added_path_index_each.back()));
00160 for (int i = adapters_.size() - 2; i >= 0; --i)
00161 fn = boost::bind(&callAdapter2, adapters_[i].get(), fn, _1, _2, _3, boost::ref(added_path_index_each[i]));
00162 bool result = fn(planning_scene, req, res);
00163 added_path_index.clear();
00164
00165
00166 for (std::size_t i = 0; i < added_path_index_each.size(); ++i)
00167 for (std::size_t j = 0; j < added_path_index_each[i].size(); ++j)
00168 {
00169 for (std::size_t k = 0; k < added_path_index.size(); ++k)
00170 if (added_path_index_each[i][j] <= added_path_index[k])
00171 added_path_index[k]++;
00172 added_path_index.push_back(added_path_index_each[i][j]);
00173 }
00174 std::sort(added_path_index.begin(), added_path_index.end());
00175 return result;
00176 }
00177 }