plan_execution.h
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 #ifndef MOVEIT_PLAN_EXECUTION_PLAN_EXECUTION_
00038 #define MOVEIT_PLAN_EXECUTION_PLAN_EXECUTION_
00039 
00040 #include <moveit/plan_execution/plan_representation.h>
00041 #include <moveit/trajectory_execution_manager/trajectory_execution_manager.h>
00042 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
00043 #include <moveit/planning_scene_monitor/trajectory_monitor.h>
00044 #include <moveit/sensor_manager/sensor_manager.h>
00045 #include <pluginlib/class_loader.h>
00046 #include <boost/scoped_ptr.hpp>
00047 
00049 namespace plan_execution
00050 {
00051 
00052 class PlanExecution
00053 {
00054 public:
00055 
00056   struct Options
00057   {
00058     Options() : replan_(false),
00059                 replan_attempts_(0),
00060                 replan_delay_(0.0)
00061     {
00062     }
00063 
00065     bool replan_;
00066 
00068     unsigned int replan_attempts_;
00069 
00071     double replan_delay_;
00072 
00074     ExecutableMotionPlanComputationFn plan_callback_;
00075 
00081     boost::function<bool(ExecutableMotionPlan &plan_to_update,
00082                          const std::pair<int, int> &trajectory_index)> repair_plan_callback_;
00083 
00084     boost::function<void()> before_plan_callback_;
00085     boost::function<void()> before_execution_callback_;
00086     boost::function<void()> done_callback_;
00087   };
00088 
00089   PlanExecution(const planning_scene_monitor::PlanningSceneMonitorPtr &planning_scene_monitor,
00090                 const trajectory_execution_manager::TrajectoryExecutionManagerPtr& trajectory_execution);
00091   ~PlanExecution();
00092 
00093   const planning_scene_monitor::PlanningSceneMonitorPtr& getPlanningSceneMonitor() const
00094   {
00095     return planning_scene_monitor_;
00096   }
00097 
00098   const trajectory_execution_manager::TrajectoryExecutionManagerPtr& getTrajectoryExecutionManager() const
00099   {
00100     return trajectory_execution_manager_;
00101   }
00102 
00103   double getTrajectoryStateRecordingFrequency() const
00104   {
00105     if (trajectory_monitor_)
00106       return trajectory_monitor_->getSamplingFrequency();
00107     else
00108       return 0.0;
00109   }
00110 
00111   void setTrajectoryStateRecordingFrequency(double freq)
00112   {
00113     if (trajectory_monitor_)
00114       trajectory_monitor_->setSamplingFrequency(freq);
00115   }
00116 
00117   void setMaxReplanAttempts(unsigned int attempts)
00118   {
00119     default_max_replan_attempts_ = attempts;
00120   }
00121 
00122   unsigned int getMaxReplanAttempts() const
00123   {
00124     return default_max_replan_attempts_;
00125   }
00126 
00127   void planAndExecute(ExecutableMotionPlan &plan, const Options &opt);
00128   void planAndExecute(ExecutableMotionPlan &plan, const moveit_msgs::PlanningScene &scene_diff, const Options &opt);
00129 
00130   void stop();
00131 
00132   std::string getErrorCodeString(const moveit_msgs::MoveItErrorCodes& error_code);
00133 
00134 private:
00135 
00136   void planAndExecuteHelper(ExecutableMotionPlan &plan, const Options &opt);
00137   moveit_msgs::MoveItErrorCodes executeAndMonitor(const ExecutableMotionPlan &plan);
00138   bool isRemainingPathValid(const ExecutableMotionPlan &plan);
00139   bool isRemainingPathValid(const ExecutableMotionPlan &plan, const std::pair<int, int> &path_segment);
00140 
00141   void planningSceneUpdatedCallback(const planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type);
00142   void doneWithTrajectoryExecution(const moveit_controller_manager::ExecutionStatus &status);
00143   void successfulTrajectorySegmentExecution(const ExecutableMotionPlan *plan, std::size_t index);
00144 
00145   ros::NodeHandle node_handle_;
00146   planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
00147   trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_;
00148   planning_scene_monitor::TrajectoryMonitorPtr trajectory_monitor_;
00149 
00150   unsigned int default_max_replan_attempts_;
00151 
00152   bool preempt_requested_;
00153   bool new_scene_update_;
00154 
00155   bool execution_complete_;
00156   bool path_became_invalid_;
00157 
00158   class DynamicReconfigureImpl;
00159   DynamicReconfigureImpl *reconfigure_impl_;
00160 };
00161 
00162 typedef boost::shared_ptr<PlanExecution> PlanExecutionPtr;
00163 typedef boost::shared_ptr<const PlanExecution> PlanExecutionConstPtr;
00164 
00165 }
00166 #endif


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Aug 26 2015 12:43:30