planning_request_adapter.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
00036 
00037 #include <moveit/planning_request_adapter/planning_request_adapter.h>
00038 #include <boost/bind.hpp>
00039 #include <algorithm>
00040 
00041 // we could really use some c++11 lambda functions here :)
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 // boost bind is not happy with overloading, so we add intermediate function objects
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   // if there are no adapters, run the planner directly
00157   if (adapters_.empty())
00158   {
00159     added_path_index.clear();
00160     return callPlannerInterfaceSolve(planner.get(), planning_scene, req, res);
00161   }
00162   else
00163   {
00164     // the index values added by each adapter
00165     std::vector<std::vector<std::size_t> > added_path_index_each(adapters_.size());
00166 
00167     // if there are adapters, construct a function pointer for each, in order,
00168     // so that in the end we have a nested sequence of function pointers that call the adapters in the correct order.
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     // merge the index values from each adapter
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 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Aug 27 2015 13:58:52