planning_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 /* Author: Ioan Sucan */
00036 
00037 #include <moveit/planning_interface/planning_interface.h>
00038 #include <boost/thread/mutex.hpp>
00039 #include <set>
00040 
00041 namespace
00042 {
00043 // keep track of currently active contexts
00044 struct ActiveContexts
00045 {
00046   boost::mutex mutex_;
00047   std::set<planning_interface::PlanningContext*> contexts_;
00048 };
00049 
00050 static ActiveContexts& getActiveContexts()
00051 {
00052   static ActiveContexts ac;
00053   return ac;
00054 }
00055 }
00056 
00057 planning_interface::PlanningContext::PlanningContext(const std::string &name, const std::string &group) :
00058   name_(name),
00059   group_(group)
00060 {
00061   ActiveContexts &ac = getActiveContexts();
00062   boost::mutex::scoped_lock _(ac.mutex_);
00063   ac.contexts_.insert(this);
00064 }
00065 
00066 planning_interface::PlanningContext::~PlanningContext()
00067 {
00068   ActiveContexts &ac = getActiveContexts();
00069   boost::mutex::scoped_lock _(ac.mutex_);
00070   ac.contexts_.erase(this);
00071 }
00072 
00073 void planning_interface::PlanningContext::setPlanningScene(const planning_scene::PlanningSceneConstPtr &planning_scene)
00074 {
00075   planning_scene_ = planning_scene;
00076 }
00077 
00078 void planning_interface::PlanningContext::setMotionPlanRequest(const MotionPlanRequest &request)
00079 {
00080   request_ = request;
00081   if (request_.allowed_planning_time <= 0.0)
00082   {
00083     logInform("The timeout for planning must be positive (%lf specified). Assuming one second instead.", request_.allowed_planning_time);
00084     request_.allowed_planning_time = 1.0;
00085   }
00086   if (request_.num_planning_attempts < 0)
00087     logError("The number of desired planning attempts should be positive. Assuming one attempt.");
00088   request_.num_planning_attempts = std::max(1, request_.num_planning_attempts);
00089 }
00090 
00091 bool planning_interface::PlannerManager::initialize(const robot_model::RobotModelConstPtr &, const std::string &)
00092 {
00093   return true;
00094 }
00095 
00096 std::string planning_interface::PlannerManager::getDescription() const
00097 {
00098   return "";
00099 }
00100 
00101 planning_interface::PlanningContextPtr planning_interface::PlannerManager::getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene,
00102                                                                                               const MotionPlanRequest &req) const
00103 {
00104   moveit_msgs::MoveItErrorCodes dummy;
00105   return getPlanningContext(planning_scene, req, dummy);
00106 }
00107 
00108 void planning_interface::PlannerManager::getPlanningAlgorithms(std::vector<std::string> &algs) const
00109 {
00110   // nothing by default
00111   algs.clear();
00112 }
00113 
00114 void planning_interface::PlannerManager::setPlannerConfigurations(const PlannerConfigurationMap &pcs)
00115 {
00116   config_settings_ = pcs;
00117 }
00118 
00119 void planning_interface::PlannerManager::terminate() const
00120 {
00121   ActiveContexts &ac = getActiveContexts();
00122   boost::mutex::scoped_lock _(ac.mutex_);
00123   for (std::set<PlanningContext*>::iterator it = ac.contexts_.begin() ; it != ac.contexts_.end() ; ++it)
00124     (*it)->terminate();
00125 }


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