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


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Mon Apr 23 2018 10:25:14