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/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