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 #pragma once
38 
46 
47 #include <atomic>
48 
50 namespace plan_execution
51 {
52 MOVEIT_CLASS_FORWARD(PlanExecution); // Defines PlanExecutionPtr, ConstPtr, WeakPtr... etc
53 
54 class PlanExecution
55 {
56 public:
57  struct Options
58  {
59  Options() : replan_(false), replan_attempts_(0), replan_delay_(0.0)
60  {
61  }
62 
64  bool replan_;
65 
68  unsigned int replan_attempts_;
69 
71  double replan_delay_;
72 
75 
85  boost::function<bool(ExecutableMotionPlan& plan_to_update, const std::pair<int, int>& trajectory_index)>
87 
88  boost::function<void()> before_plan_callback_;
89  boost::function<void()> before_execution_callback_;
90  boost::function<void()> done_callback_;
91  };
92 
93  PlanExecution(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
94  const trajectory_execution_manager::TrajectoryExecutionManagerPtr& trajectory_execution);
96 
97  const planning_scene_monitor::PlanningSceneMonitorPtr& getPlanningSceneMonitor() const
98  {
100  }
101 
102  const trajectory_execution_manager::TrajectoryExecutionManagerPtr& getTrajectoryExecutionManager() const
103  {
105  }
106 
108  {
110  return trajectory_monitor_->getSamplingFrequency();
111  else
112  return 0.0;
113  }
114 
115  void setTrajectoryStateRecordingFrequency(double freq)
116  {
118  trajectory_monitor_->setSamplingFrequency(freq);
119  }
120 
121  void setMaxReplanAttempts(unsigned int attempts)
122  {
123  default_max_replan_attempts_ = attempts;
124  }
125 
126  unsigned int getMaxReplanAttempts() const
127  {
129  }
130 
131  void planAndExecute(ExecutableMotionPlan& plan, const Options& opt);
132  void planAndExecute(ExecutableMotionPlan& plan, const moveit_msgs::PlanningScene& scene_diff, const Options& opt);
133 
138  moveit_msgs::MoveItErrorCodes executeAndMonitor(ExecutableMotionPlan& plan, bool reset_preempted = true);
139 
140  void stop();
141 
142 private:
143  void planAndExecuteHelper(ExecutableMotionPlan& plan, const Options& opt);
144  bool isRemainingPathValid(const ExecutableMotionPlan& plan, const std::pair<int, int>& path_segment);
145 
148  void successfulTrajectorySegmentExecution(const ExecutableMotionPlan& plan, std::size_t index);
149 
151  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
152  trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_;
153  planning_scene_monitor::TrajectoryMonitorPtr trajectory_monitor_;
154 
155  unsigned int default_max_replan_attempts_;
156 
157  class
158  {
159  private:
160  std::atomic<bool> preemption_requested{ false };
161 
162  public:
166  inline bool checkAndClear()
167  {
168  return preemption_requested.exchange(false);
169  }
170  inline void request()
171  {
172  preemption_requested.store(true);
173  }
174  } preempt_;
175 
176  bool new_scene_update_;
177 
178  bool execution_complete_;
180 
181  class DynamicReconfigureImpl;
183 };
184 } // namespace plan_execution
plan_execution::PlanExecution::~PlanExecution
~PlanExecution()
Definition: plan_execution.cpp:98
planning_scene_monitor
Definition: current_state_monitor.h:47
plan_execution::PlanExecution::reconfigure_impl_
DynamicReconfigureImpl * reconfigure_impl_
Definition: plan_execution.h:213
sensor_manager.h
plan_execution::PlanExecution::getTrajectoryStateRecordingFrequency
double getTrajectoryStateRecordingFrequency() const
Definition: plan_execution.h:139
plan_execution::MOVEIT_CLASS_FORWARD
MOVEIT_CLASS_FORWARD(PlanExecution)
plan_execution::PlanExecution::Options
Definition: plan_execution.h:89
plan_execution::PlanExecution::setMaxReplanAttempts
void setMaxReplanAttempts(unsigned int attempts)
Definition: plan_execution.h:153
plan_execution::PlanExecution::preemption_requested
std::atomic< bool > preemption_requested
Definition: plan_execution.h:192
plan_execution::PlanExecution::Options::replan_
bool replan_
Flag indicating whether replanning is allowed.
Definition: plan_execution.h:96
plan_execution::PlanExecution::default_max_replan_attempts_
unsigned int default_max_replan_attempts_
Definition: plan_execution.h:187
plan_execution::PlanExecution::DynamicReconfigureImpl
Definition: plan_execution.cpp:84
plan_execution::PlanExecution::new_scene_update_
bool new_scene_update_
Definition: plan_execution.h:208
planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType
SceneUpdateType
Definition: planning_scene_monitor.h:96
plan_execution::PlanExecution::trajectory_execution_manager_
trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_
Definition: plan_execution.h:184
plan_execution::PlanExecution::planAndExecuteHelper
void planAndExecuteHelper(ExecutableMotionPlan &plan, const Options &opt)
Definition: plan_execution.cpp:134
plan_execution::PlanExecution
Definition: plan_execution.h:86
plan_execution::PlanExecution::Options::repair_plan_callback_
boost::function< bool(ExecutableMotionPlan &plan_to_update, const std::pair< int, int > &trajectory_index)> repair_plan_callback_
Definition: plan_execution.h:118
planning_scene_monitor.h
plan_execution::PlanExecution::doneWithTrajectoryExecution
void doneWithTrajectoryExecution(const moveit_controller_manager::ExecutionStatus &status)
Definition: plan_execution.cpp:471
plan_execution::PlanExecution::Options::plan_callback_
ExecutableMotionPlanComputationFn plan_callback_
Callback for computing motion plans. This callback must always be specified.
Definition: plan_execution.h:106
class_loader.hpp
plan_execution::PlanExecution::Options::replan_delay_
double replan_delay_
The amount of time to wait in between replanning attempts (in seconds)
Definition: plan_execution.h:103
plan_execution::PlanExecution::Options::before_plan_callback_
boost::function< void()> before_plan_callback_
Definition: plan_execution.h:120
plan_execution::PlanExecution::trajectory_monitor_
planning_scene_monitor::TrajectoryMonitorPtr trajectory_monitor_
Definition: plan_execution.h:185
plan_execution::PlanExecution::Options::replan_attempts_
unsigned int replan_attempts_
Definition: plan_execution.h:100
plan_execution::PlanExecution::planningSceneUpdatedCallback
void planningSceneUpdatedCallback(const planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type)
Definition: plan_execution.cpp:463
plan_execution::PlanExecution::Options::Options
Options()
Definition: plan_execution.h:91
plan_execution::PlanExecution::Options::done_callback_
boost::function< void()> done_callback_
Definition: plan_execution.h:122
plan_execution::PlanExecution::node_handle_
ros::NodeHandle node_handle_
Definition: plan_execution.h:182
plan_execution
This namespace includes functionality specific to the execution and monitoring of motion plans.
Definition: plan_execution.h:50
trajectory_monitor.h
plan_execution::PlanExecution::planning_scene_monitor_
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_
Definition: plan_execution.h:183
plan_execution::PlanExecution::stop
void stop()
Definition: plan_execution.cpp:103
plan_execution::PlanExecution::setTrajectoryStateRecordingFrequency
void setTrajectoryStateRecordingFrequency(double freq)
Definition: plan_execution.h:147
plan_execution::PlanExecution::Options::before_execution_callback_
boost::function< void()> before_execution_callback_
Definition: plan_execution.h:121
plan_execution::ExecutableMotionPlan
A generic representation on what a computed motion plan looks like.
Definition: plan_representation.h:105
class_forward.h
trajectory_execution_manager.h
plan_execution::PlanExecution::getPlanningSceneMonitor
const planning_scene_monitor::PlanningSceneMonitorPtr & getPlanningSceneMonitor() const
Definition: plan_execution.h:129
plan_execution::PlanExecution::executeAndMonitor
moveit_msgs::MoveItErrorCodes executeAndMonitor(ExecutableMotionPlan &plan, bool reset_preempted=true)
Execute and monitor a previously created plan.
Definition: plan_execution.cpp:287
plan_execution::PlanExecution::PlanExecution
PlanExecution(const planning_scene_monitor::PlanningSceneMonitorPtr &planning_scene_monitor, const trajectory_execution_manager::TrajectoryExecutionManagerPtr &trajectory_execution)
Definition: plan_execution.cpp:74
plan_execution::PlanExecution::preempt_
class plan_execution::PlanExecution::@0 preempt_
plan_execution::PlanExecution::planAndExecute
void planAndExecute(ExecutableMotionPlan &plan, const Options &opt)
Definition: plan_execution.cpp:108
plan_execution::PlanExecution::getMaxReplanAttempts
unsigned int getMaxReplanAttempts() const
Definition: plan_execution.h:158
plan_execution::ExecutableMotionPlanComputationFn
boost::function< bool(ExecutableMotionPlan &)> ExecutableMotionPlanComputationFn
The signature of a function that can compute a motion plan.
Definition: plan_representation.h:120
moveit_controller_manager::ExecutionStatus
plan_execution::PlanExecution::path_became_invalid_
bool path_became_invalid_
Definition: plan_execution.h:211
plan_representation.h
plan_execution::PlanExecution::successfulTrajectorySegmentExecution
void successfulTrajectorySegmentExecution(const ExecutableMotionPlan &plan, std::size_t index)
Definition: plan_execution.cpp:477
ros::NodeHandle
plan_execution::PlanExecution::getTrajectoryExecutionManager
const trajectory_execution_manager::TrajectoryExecutionManagerPtr & getTrajectoryExecutionManager() const
Definition: plan_execution.h:134
plan_execution::PlanExecution::isRemainingPathValid
bool isRemainingPathValid(const ExecutableMotionPlan &plan, const std::pair< int, int > &path_segment)
Definition: plan_execution.cpp:245
plan_execution::PlanExecution::execution_complete_
bool execution_complete_
Definition: plan_execution.h:210


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Sun Mar 3 2024 03:24:15