ompl_interface.h
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2011, 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 #ifndef MOVEIT_OMPL_INTERFACE_OMPL_INTERFACE_
00038 #define MOVEIT_OMPL_INTERFACE_OMPL_INTERFACE_
00039 
00040 #include <moveit/ompl_interface/planning_context_manager.h>
00041 #include <moveit/ompl_interface/constraints_library.h>
00042 #include <moveit/constraint_samplers/constraint_sampler_manager.h>
00043 #include <moveit/constraint_sampler_manager_loader/constraint_sampler_manager_loader.h>
00044 #include <moveit/planning_interface/planning_interface.h>
00045 #include <moveit_msgs/MotionPlanRequest.h>
00046 #include <moveit_msgs/MotionPlanResponse.h>
00047 #include <string>
00048 #include <map>
00049 #include <ros/ros.h>
00050 
00052 namespace ompl_interface
00053 {
00056 class OMPLInterface
00057 {
00058 public:
00061   OMPLInterface(const robot_model::RobotModelConstPtr& kmodel, const ros::NodeHandle& nh = ros::NodeHandle("~"));
00062 
00067   OMPLInterface(const robot_model::RobotModelConstPtr& kmodel,
00068                 const planning_interface::PlannerConfigurationMap& pconfig,
00069                 const ros::NodeHandle& nh = ros::NodeHandle("~"));
00070 
00071   virtual ~OMPLInterface();
00072 
00075   void setPlannerConfigurations(const planning_interface::PlannerConfigurationMap& pconfig);
00076 
00079   const planning_interface::PlannerConfigurationMap& getPlannerConfigurations() const
00080   {
00081     return context_manager_.getPlannerConfigurations();
00082   }
00083 
00084   ModelBasedPlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene,
00085                                                   const planning_interface::MotionPlanRequest& req) const;
00086   ModelBasedPlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene,
00087                                                   const planning_interface::MotionPlanRequest& req,
00088                                                   moveit_msgs::MoveItErrorCodes& error_code) const;
00089 
00090   ModelBasedPlanningContextPtr getPlanningContext(const std::string& config,
00091                                                   const std::string& factory_type = "") const;
00092 
00093   ModelBasedPlanningContextPtr getLastPlanningContext() const
00094   {
00095     return context_manager_.getLastPlanningContext();
00096   }
00097 
00098   const PlanningContextManager& getPlanningContextManager() const
00099   {
00100     return context_manager_;
00101   }
00102 
00103   PlanningContextManager& getPlanningContextManager()
00104   {
00105     return context_manager_;
00106   }
00107 
00108   ConstraintsLibrary& getConstraintsLibrary()
00109   {
00110     return *constraints_library_;
00111   }
00112 
00113   const ConstraintsLibrary& getConstraintsLibrary() const
00114   {
00115     return *constraints_library_;
00116   }
00117 
00118   constraint_samplers::ConstraintSamplerManager& getConstraintSamplerManager()
00119   {
00120     return *constraint_sampler_manager_;
00121   }
00122 
00123   const constraint_samplers::ConstraintSamplerManager& getConstraintSamplerManager() const
00124   {
00125     return *constraint_sampler_manager_;
00126   }
00127 
00128   void useConstraintsApproximations(bool flag)
00129   {
00130     use_constraints_approximations_ = flag;
00131   }
00132 
00133   bool isUsingConstraintsApproximations() const
00134   {
00135     return use_constraints_approximations_;
00136   }
00137 
00138   void loadConstraintApproximations(const std::string& path);
00139 
00140   void saveConstraintApproximations(const std::string& path);
00141 
00142   bool simplifySolutions() const
00143   {
00144     return simplify_solutions_;
00145   }
00146 
00147   void simplifySolutions(bool flag)
00148   {
00149     simplify_solutions_ = flag;
00150   }
00151 
00154   bool saveConstraintApproximations();
00155 
00158   bool loadConstraintApproximations();
00159 
00161   void printStatus();
00162 
00163 protected:
00165   void loadPlannerConfigurations();
00166 
00168   void loadConstraintSamplers();
00169 
00170   void configureContext(const ModelBasedPlanningContextPtr& context) const;
00171 
00173   ModelBasedPlanningContextPtr prepareForSolve(const planning_interface::MotionPlanRequest& req,
00174                                                const planning_scene::PlanningSceneConstPtr& planning_scene,
00175                                                moveit_msgs::MoveItErrorCodes* error_code, unsigned int* attempts,
00176                                                double* timeout) const;
00177 
00178   ros::NodeHandle nh_;  
00179 
00181   robot_model::RobotModelConstPtr kmodel_;
00182 
00183   constraint_samplers::ConstraintSamplerManagerPtr constraint_sampler_manager_;
00184 
00185   PlanningContextManager context_manager_;
00186 
00187   ConstraintsLibraryPtr constraints_library_;
00188   bool use_constraints_approximations_;
00189 
00190   bool simplify_solutions_;
00191 
00192 private:
00193   constraint_sampler_manager_loader::ConstraintSamplerManagerLoaderPtr constraint_sampler_manager_loader_;
00194 };
00195 }
00196 
00197 #endif


ompl
Author(s): Ioan Sucan
autogenerated on Mon Nov 26 2018 03:23:00