moveit2
The MoveIt Motion Planning Framework for ROS 2.
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 
44 #include <pluginlib/class_loader.hpp>
45 #include <rclcpp/logger.hpp>
46 
47 #include <atomic>
48 
50 namespace plan_execution
51 {
52 MOVEIT_CLASS_FORWARD(PlanExecution); // Defines PlanExecutionPtr, ConstPtr, WeakPtr... etc
53 
55 {
56 public:
57  struct Options
58  {
60  {
61  }
62 
64  bool replan;
65 
68  unsigned int replan_attemps;
69 
71  double replan_delay;
72 
75 
85  std::function<bool(ExecutableMotionPlan& plan_to_update, const std::pair<int, int>& trajectory_index)>
87 
88  std::function<void()> before_plan_callback_;
89  std::function<void()> before_execution_callback_;
90  std::function<void()> done_callback_;
91  };
92 
93  PlanExecution(const rclcpp::Node::SharedPtr& node,
94  const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
95  const trajectory_execution_manager::TrajectoryExecutionManagerPtr& trajectory_execution);
97 
98  const planning_scene_monitor::PlanningSceneMonitorPtr& getPlanningSceneMonitor() const
99  {
100  return planning_scene_monitor_;
101  }
102 
103  const trajectory_execution_manager::TrajectoryExecutionManagerPtr& getTrajectoryExecutionManager() const
104  {
105  return trajectory_execution_manager_;
106  }
107 
109  {
110  if (trajectory_monitor_)
111  {
112  return trajectory_monitor_->getSamplingFrequency();
113  }
114  else
115  {
116  return 0.0;
117  }
118  }
119 
121  {
122  if (trajectory_monitor_)
123  trajectory_monitor_->setSamplingFrequency(freq);
124  }
125 
126  void setMaxReplanAttempts(unsigned int attempts)
127  {
128  default_max_replan_attempts_ = attempts;
129  }
130 
131  unsigned int getMaxReplanAttempts() const
132  {
133  return default_max_replan_attempts_;
134  }
135 
136  void planAndExecute(ExecutableMotionPlan& plan, const Options& opt);
137  void planAndExecute(ExecutableMotionPlan& plan, const moveit_msgs::msg::PlanningScene& scene_diff, const Options& opt);
138 
143  moveit_msgs::msg::MoveItErrorCodes executeAndMonitor(ExecutableMotionPlan& plan, bool reset_preempted = true);
144 
145  void stop();
146 
147 private:
148  void planAndExecuteHelper(ExecutableMotionPlan& plan, const Options& opt);
149  bool isRemainingPathValid(const ExecutableMotionPlan& plan, const std::pair<int, int>& path_segment);
150 
151  void planningSceneUpdatedCallback(const planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type);
152  void doneWithTrajectoryExecution(const moveit_controller_manager::ExecutionStatus& status);
153  void successfulTrajectorySegmentExecution(const ExecutableMotionPlan& plan, std::size_t index);
154 
155  const rclcpp::Node::SharedPtr node_;
156  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
157  trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_;
158  planning_scene_monitor::TrajectoryMonitorPtr trajectory_monitor_;
159 
160  unsigned int default_max_replan_attempts_;
161 
162  class
163  {
164  private:
165  std::atomic<bool> preemption_requested_{ false };
166 
167  public:
171  inline bool checkAndClear()
172  {
173  return preemption_requested_.exchange(false);
174  }
175  inline void request()
176  {
177  preemption_requested_.store(true);
178  }
179  } preempt_;
180 
181  bool new_scene_update_;
182 
183  bool execution_complete_;
184  bool path_became_invalid_;
185 
186  rclcpp::Logger logger_;
187 
188  // class DynamicReconfigureImpl;
189  // DynamicReconfigureImpl* reconfigure_impl_;
190 };
191 } // namespace plan_execution
const planning_scene_monitor::PlanningSceneMonitorPtr & getPlanningSceneMonitor() const
void planAndExecute(ExecutableMotionPlan &plan, const Options &opt)
double getTrajectoryStateRecordingFrequency() const
PlanExecution(const rclcpp::Node::SharedPtr &node, const planning_scene_monitor::PlanningSceneMonitorPtr &planning_scene_monitor, const trajectory_execution_manager::TrajectoryExecutionManagerPtr &trajectory_execution)
moveit_msgs::msg::MoveItErrorCodes executeAndMonitor(ExecutableMotionPlan &plan, bool reset_preempted=true)
Execute and monitor a previously created plan.
const trajectory_execution_manager::TrajectoryExecutionManagerPtr & getTrajectoryExecutionManager() const
void setTrajectoryStateRecordingFrequency(double freq)
void setMaxReplanAttempts(unsigned int attempts)
unsigned int getMaxReplanAttempts() const
planning_interface::MotionPlanResponse plan(std::shared_ptr< moveit_cpp::PlanningComponent > &planning_component, std::shared_ptr< moveit_cpp::PlanningComponent::PlanRequestParameters > &single_plan_parameters, std::shared_ptr< moveit_cpp::PlanningComponent::MultiPipelinePlanRequestParameters > &multi_plan_parameters, std::shared_ptr< planning_scene::PlanningScene > &planning_scene, std::optional< const moveit::planning_pipeline_interfaces::SolutionSelectionFunction > solution_selection_function, std::optional< moveit::planning_pipeline_interfaces::StoppingCriterionFunction > stopping_criterion_callback)
This namespace includes functionality specific to the execution and monitoring of motion plans.
std::function< bool(ExecutableMotionPlan &)> ExecutableMotionPlanComputationFn
The signature of a function that can compute a motion plan.
MOVEIT_CLASS_FORWARD(PlanExecution)
A generic representation on what a computed motion plan looks like.
double replan_delay
The amount of time to wait in between replanning attempts (in seconds)
ExecutableMotionPlanComputationFn plan_callback
Callback for computing motion plans. This callback must always be specified.
std::function< bool(ExecutableMotionPlan &plan_to_update, const std::pair< int, int > &trajectory_index)> repair_plan_callback_
std::function< void()> before_execution_callback_
bool replan
Flag indicating whether replanning is allowed.
std::function< void()> before_plan_callback_
std::function< void()> done_callback_