plan_execution.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #ifndef MOVEIT_PLAN_EXECUTION_PLAN_EXECUTION_
38 #define MOVEIT_PLAN_EXECUTION_PLAN_EXECUTION_
39 
47 
49 namespace plan_execution
50 {
52 
54 {
55 public:
56  struct Options
57  {
59  {
60  }
61 
63  bool replan_;
64 
67  unsigned int replan_attempts_;
68 
70  double replan_delay_;
71 
74 
84  boost::function<bool(ExecutableMotionPlan& plan_to_update, const std::pair<int, int>& trajectory_index)>
86 
87  boost::function<void()> before_plan_callback_;
88  boost::function<void()> before_execution_callback_;
89  boost::function<void()> done_callback_;
90  };
91 
92  PlanExecution(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
93  const trajectory_execution_manager::TrajectoryExecutionManagerPtr& trajectory_execution);
95 
96  const planning_scene_monitor::PlanningSceneMonitorPtr& getPlanningSceneMonitor() const
97  {
99  }
100 
101  const trajectory_execution_manager::TrajectoryExecutionManagerPtr& getTrajectoryExecutionManager() const
102  {
104  }
105 
107  {
109  return trajectory_monitor_->getSamplingFrequency();
110  else
111  return 0.0;
112  }
113 
115  {
117  trajectory_monitor_->setSamplingFrequency(freq);
118  }
119 
120  void setMaxReplanAttempts(unsigned int attempts)
121  {
122  default_max_replan_attempts_ = attempts;
123  }
124 
125  unsigned int getMaxReplanAttempts() const
126  {
128  }
129 
130  void planAndExecute(ExecutableMotionPlan& plan, const Options& opt);
131  void planAndExecute(ExecutableMotionPlan& plan, const moveit_msgs::PlanningScene& scene_diff, const Options& opt);
132 
137  moveit_msgs::MoveItErrorCodes executeAndMonitor(ExecutableMotionPlan& plan);
138 
139  void stop();
140 
141  std::string getErrorCodeString(const moveit_msgs::MoveItErrorCodes& error_code);
142 
143 private:
144  void planAndExecuteHelper(ExecutableMotionPlan& plan, const Options& opt);
145  bool isRemainingPathValid(const ExecutableMotionPlan& plan);
146  bool isRemainingPathValid(const ExecutableMotionPlan& plan, const std::pair<int, int>& path_segment);
147 
150  void successfulTrajectorySegmentExecution(const ExecutableMotionPlan* plan, std::size_t index);
151 
153  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
154  trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_;
155  planning_scene_monitor::TrajectoryMonitorPtr trajectory_monitor_;
156 
158 
161 
164 
167 };
168 }
169 #endif
double getTrajectoryStateRecordingFrequency() const
void planAndExecuteHelper(ExecutableMotionPlan &plan, const Options &opt)
boost::function< bool(ExecutableMotionPlan &plan_to_update, const std::pair< int, int > &trajectory_index)> repair_plan_callback_
unsigned int getMaxReplanAttempts() const
planning_scene_monitor::TrajectoryMonitorPtr trajectory_monitor_
trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_
void planningSceneUpdatedCallback(const planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type)
void setTrajectoryStateRecordingFrequency(double freq)
boost::function< bool(ExecutableMotionPlan &plan)> ExecutableMotionPlanComputationFn
The signature of a function that can compute a motion plan.
unsigned int index
void planAndExecute(ExecutableMotionPlan &plan, const Options &opt)
boost::function< void()> done_callback_
ExecutableMotionPlanComputationFn plan_callback_
Callback for computing motion plans. This callback must always be specified.
boost::function< void()> before_plan_callback_
PlanExecution(const planning_scene_monitor::PlanningSceneMonitorPtr &planning_scene_monitor, const trajectory_execution_manager::TrajectoryExecutionManagerPtr &trajectory_execution)
double replan_delay_
The amount of time to wait in between replanning attempts (in seconds)
bool isRemainingPathValid(const ExecutableMotionPlan &plan)
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_
bool replan_
Flag indicating whether replanning is allowed.
void setMaxReplanAttempts(unsigned int attempts)
const planning_scene_monitor::PlanningSceneMonitorPtr & getPlanningSceneMonitor() const
This namespace includes functionality specific to the execution and monitoring of motion plans...
MOVEIT_CLASS_FORWARD(PlanExecution)
A generic representation on what a computed motion plan looks like.
void successfulTrajectorySegmentExecution(const ExecutableMotionPlan *plan, std::size_t index)
unsigned int default_max_replan_attempts_
boost::function< void()> before_execution_callback_
const trajectory_execution_manager::TrajectoryExecutionManagerPtr & getTrajectoryExecutionManager() const
DynamicReconfigureImpl * reconfigure_impl_
std::string getErrorCodeString(const moveit_msgs::MoveItErrorCodes &error_code)
void doneWithTrajectoryExecution(const moveit_controller_manager::ExecutionStatus &status)
moveit_msgs::MoveItErrorCodes executeAndMonitor(ExecutableMotionPlan &plan)
Execute and monitor a previously created plan.


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Sun Oct 18 2020 13:17:33