Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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