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(const planning_interface::PlannerManagerPtr &planner,
00062 const planning_scene::PlanningSceneConstPtr& planning_scene,
00063 const planning_interface::MotionPlanRequest &req,
00064 planning_interface::MotionPlanResponse &res,
00065 std::vector<std::size_t> &added_path_index) const
00066 {
00067 return adaptAndPlan(boost::bind(&callPlannerInterfaceSolve, planner.get(), _1, _2, _3), planning_scene, req, res, added_path_index);
00068 }
00069
00070 bool planning_request_adapter::PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerManagerPtr &planner,
00071 const planning_scene::PlanningSceneConstPtr& planning_scene,
00072 const planning_interface::MotionPlanRequest &req,
00073 planning_interface::MotionPlanResponse &res) const
00074 {
00075 std::vector<std::size_t> dummy;
00076 return adaptAndPlan(planner, planning_scene, req, res, dummy);
00077 }
00078
00079 namespace planning_request_adapter
00080 {
00081
00082 namespace
00083 {
00084
00085
00086
00087 bool callAdapter1(const PlanningRequestAdapter *adapter,
00088 const planning_interface::PlannerManagerPtr &planner,
00089 const planning_scene::PlanningSceneConstPtr& planning_scene,
00090 const planning_interface::MotionPlanRequest &req,
00091 planning_interface::MotionPlanResponse &res,
00092 std::vector<std::size_t> &added_path_index)
00093 {
00094 try
00095 {
00096 return adapter->adaptAndPlan(planner, planning_scene, req, res, added_path_index);
00097 }
00098 catch(std::runtime_error &ex)
00099 {
00100 logError("Exception caught executing *final* adapter '%s': %s", adapter->getDescription().c_str(), ex.what());
00101 added_path_index.clear();
00102 return callPlannerInterfaceSolve(planner.get(), planning_scene, req, res);
00103 }
00104 catch(...)
00105 {
00106 logError("Exception caught executing *final* adapter '%s'", adapter->getDescription().c_str());
00107 added_path_index.clear();
00108 return callPlannerInterfaceSolve(planner.get(), planning_scene, req, res);
00109 }
00110 }
00111
00112 bool callAdapter2(const PlanningRequestAdapter *adapter,
00113 const PlanningRequestAdapter::PlannerFn &planner,
00114 const planning_scene::PlanningSceneConstPtr& planning_scene,
00115 const planning_interface::MotionPlanRequest &req,
00116 planning_interface::MotionPlanResponse &res,
00117 std::vector<std::size_t> &added_path_index)
00118 {
00119 try
00120 {
00121 return adapter->adaptAndPlan(planner, planning_scene, req, res, added_path_index);
00122 }
00123 catch(std::runtime_error &ex)
00124 {
00125 logError("Exception caught executing *next* adapter '%s': %s", adapter->getDescription().c_str(), ex.what());
00126 added_path_index.clear();
00127 return planner(planning_scene, req, res);
00128 }
00129 catch(...)
00130 {
00131 logError("Exception caught executing *next* adapter '%s'", adapter->getDescription().c_str());
00132 added_path_index.clear();
00133 return planner(planning_scene, req, res);
00134 }
00135 }
00136
00137 }
00138
00139 }
00140
00141 bool planning_request_adapter::PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::PlannerManagerPtr &planner,
00142 const planning_scene::PlanningSceneConstPtr& planning_scene,
00143 const planning_interface::MotionPlanRequest &req,
00144 planning_interface::MotionPlanResponse &res) const
00145 {
00146 std::vector<std::size_t> dummy;
00147 return adaptAndPlan(planner, planning_scene, req, res, dummy);
00148 }
00149
00150 bool planning_request_adapter::PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::PlannerManagerPtr &planner,
00151 const planning_scene::PlanningSceneConstPtr& planning_scene,
00152 const planning_interface::MotionPlanRequest &req,
00153 planning_interface::MotionPlanResponse &res,
00154 std::vector<std::size_t> &added_path_index) const
00155 {
00156
00157 if (adapters_.empty())
00158 {
00159 added_path_index.clear();
00160 return callPlannerInterfaceSolve(planner.get(), planning_scene, req, res);
00161 }
00162 else
00163 {
00164
00165 std::vector<std::vector<std::size_t> > added_path_index_each(adapters_.size());
00166
00167
00168
00169 PlanningRequestAdapter::PlannerFn fn = boost::bind(&callAdapter1, adapters_.back().get(), planner, _1, _2, _3, boost::ref(added_path_index_each.back()));
00170 for (int i = adapters_.size() - 2 ; i >= 0 ; --i)
00171 fn = boost::bind(&callAdapter2, adapters_[i].get(), fn, _1, _2, _3, boost::ref(added_path_index_each[i]));
00172 bool result = fn(planning_scene, req, res);
00173 added_path_index.clear();
00174
00175
00176 for (std::size_t i = 0 ; i < added_path_index_each.size() ; ++i)
00177 for (std::size_t j = 0 ; j < added_path_index_each[i].size() ; ++j)
00178 {
00179 for (std::size_t k = 0 ; k < added_path_index.size() ; ++k)
00180 if (added_path_index_each[i][j] <= added_path_index[k])
00181 added_path_index[k]++;
00182 added_path_index.push_back(added_path_index_each[i][j]);
00183 }
00184 std::sort(added_path_index.begin(), added_path_index.end());
00185 return result;
00186 }
00187 }