sbpl_meta_interface.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 
00036 #include <sbpl_interface/sbpl_meta_interface.h>
00037 #include <sbpl_interface/sbpl_interface.h>
00038 #include <planning_models/conversions.h>
00039 
00040 namespace sbpl_interface {
00041 
00042 SBPLMetaInterface::SBPLMetaInterface(const planning_models::RobotModelConstPtr& kmodel)
00043 {
00044   sbpl_interface_first_.reset(new sbpl_interface::SBPLInterface(kmodel));
00045   sbpl_interface_second_.reset(new sbpl_interface::SBPLInterface(kmodel));
00046 }
00047 
00048 bool SBPLMetaInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
00049                               const moveit_msgs::GetMotionPlan::Request &req,
00050                               moveit_msgs::GetMotionPlan::Response &res)
00051 {
00052   first_ok_ = false;
00053   first_done_ = false;
00054   second_ok_ = false;
00055   second_done_ = false;
00056 
00057   PlanningParameters param_bfs;
00058   param_bfs.use_bfs_ = true;
00059   PlanningParameters param_no_bfs;
00060   param_no_bfs.use_bfs_ = false;
00061   moveit_msgs::GetMotionPlan::Response res1, res2;
00062   boost::thread thread1(boost::bind(&SBPLMetaInterface::runSolver, this, true,
00063                                     boost::cref(planning_scene), boost::cref(req), boost::ref(res1), param_bfs));
00064   boost::thread thread2(boost::bind(&SBPLMetaInterface::runSolver, this, false,
00065                                     boost::cref(planning_scene), boost::cref(req), boost::ref(res2), param_no_bfs));
00066   boost::mutex::scoped_lock lock(planner_done_mutex_);
00067   planner_done_condition_.wait(lock);
00068 
00069   if(first_done_) {
00070     std::cerr << "FIRST DONE" << std::endl;
00071     if(first_ok_) {
00072       std::cerr << "First ok, interrupting second" << std::endl;
00073       if(!second_done_) {
00074         thread2.interrupt();
00075         thread2.join();
00076       }
00077     } else {
00078       if(!second_done_) {
00079         planner_done_condition_.wait(lock);
00080       }
00081     }
00082   }
00083   if(second_done_) {
00084     std::cerr << "Second done" << std::endl;
00085     if(second_ok_) {
00086       std::cerr << "Second ok, interrupting first" << std::endl;
00087       if(!first_done_) {
00088         thread1.interrupt();
00089         thread1.join();
00090       }
00091     } else {
00092       if(!first_done_) {
00093         planner_done_condition_.wait(lock);
00094       }
00095     }
00096   }
00097 
00098   if(!first_ok_ && !second_ok_) {
00099     std::cerr << "Both planners failed" << std::endl;
00100     res = res1;
00101     return false;
00102   }
00103   if(!first_ok_ && second_ok_) {
00104     std::cerr << "Sbpl interface no bfs reports time " << sbpl_interface_second_->getLastPlanningStatistics().total_planning_time_ << std::endl;
00105     last_planning_statistics_ = sbpl_interface_second_->getLastPlanningStatistics();
00106     res = res2;
00107     return true;
00108   } else if(first_ok_ && !second_ok_) {
00109     std::cerr << "Sbpl interface bfs reports time " << sbpl_interface_first_->getLastPlanningStatistics().total_planning_time_ << std::endl;
00110     last_planning_statistics_ = sbpl_interface_first_->getLastPlanningStatistics();
00111     res = res1;
00112     return true;
00113   }
00114   std::cerr << "Sbpl interface bfs reports time " << sbpl_interface_first_->getLastPlanningStatistics().total_planning_time_ << std::endl;
00115   std::cerr << "Sbpl interface no bfs reports time " << sbpl_interface_second_->getLastPlanningStatistics().total_planning_time_ << std::endl;
00116   if(sbpl_interface_first_->getLastPlanningStatistics().total_planning_time_ <
00117      sbpl_interface_second_->getLastPlanningStatistics().total_planning_time_) {
00118     last_planning_statistics_ = sbpl_interface_first_->getLastPlanningStatistics();
00119     res = res1;
00120   } else {
00121     last_planning_statistics_ = sbpl_interface_second_->getLastPlanningStatistics();
00122     res = res2;
00123   }
00124   return true;
00125 }
00126 
00127 void SBPLMetaInterface::runSolver(bool use_first,
00128                                   const planning_scene::PlanningSceneConstPtr& planning_scene,
00129                                   const moveit_msgs::GetMotionPlan::Request &req,
00130                                   moveit_msgs::GetMotionPlan::Response &res,
00131                                   const PlanningParameters& params)
00132 {
00133   try {
00134     if(use_first) {
00135       std::cerr << "Running first planner" << std::endl;
00136       first_ok_ = sbpl_interface_first_->solve(planning_scene,
00137                                                req,
00138                                                res,
00139                                                params);
00140       first_done_ = true;
00141     } else {
00142       std::cerr << "Running second planner" << std::endl;
00143       second_ok_ = sbpl_interface_second_->solve(planning_scene,
00144                                                  req,
00145                                                  res,
00146                                                  params);
00147       second_done_ = true;
00148     }
00149     planner_done_condition_.notify_all();
00150   } catch(...) {
00151     std::cerr << "Interruption requested\n";
00152   }
00153 }
00154 
00155 
00156 }


sbpl_interface
Author(s): Gil Jones
autogenerated on Sun Jan 17 2016 12:57:03