plan_with_sensing.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_WITH_SENSING_
38 #define MOVEIT_PLAN_EXECUTION_PLAN_WITH_SENSING_
39 
43 
47 
48 #include <memory>
49 
50 namespace plan_execution
51 {
52 MOVEIT_CLASS_FORWARD(PlanWithSensing);
53 
55 {
56 public:
57  PlanWithSensing(const trajectory_execution_manager::TrajectoryExecutionManagerPtr& trajectory_execution);
59 
60  const trajectory_execution_manager::TrajectoryExecutionManagerPtr& getTrajectoryExecutionManager() const
61  {
63  }
64 
65  bool computePlan(ExecutableMotionPlan& plan, const ExecutableMotionPlanComputationFn& motion_planner,
66  unsigned int max_look_attempts, double max_safe_path_cost);
67 
68  double getMaxSafePathCost() const
69  {
71  }
72 
73  void setMaxSafePathCost(double max_safe_path_cost)
74  {
75  default_max_safe_path_cost_ = max_safe_path_cost;
76  }
77 
78  void setMaxLookAttempts(unsigned int attempts)
79  {
80  default_max_look_attempts_ = attempts;
81  }
82 
83  unsigned int getMaxLookAttempts() const
84  {
86  }
87 
88  unsigned int getMaxCostSources() const
89  {
90  return max_cost_sources_;
91  }
92 
93  void setMaxCostSources(unsigned int value)
94  {
95  max_cost_sources_ = value;
96  }
97 
99  {
101  }
102 
104  {
106  }
107 
108  void setBeforeLookCallback(const boost::function<void()>& callback)
109  {
110  before_look_callback_ = callback;
111  }
112 
113  void displayCostSources(bool flag);
114 
115 private:
116  bool lookAt(const std::set<collision_detection::CostSource>& cost_sources, const std::string& frame_id);
117 
119  trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_;
120 
121  std::unique_ptr<pluginlib::ClassLoader<moveit_sensor_manager::MoveItSensorManager> > sensor_manager_loader_;
122  moveit_sensor_manager::MoveItSensorManagerPtr sensor_manager_;
125 
127  unsigned int max_cost_sources_;
128 
131 
132  boost::function<void()> before_look_callback_;
133 
136 };
137 }
138 #endif
void setMaxSafePathCost(double max_safe_path_cost)
bool lookAt(const std::set< collision_detection::CostSource > &cost_sources, const std::string &frame_id)
unsigned int getMaxCostSources() const
void setMaxCostSources(unsigned int value)
unsigned int getMaxLookAttempts() const
std::unique_ptr< pluginlib::ClassLoader< moveit_sensor_manager::MoveItSensorManager > > sensor_manager_loader_
boost::function< bool(ExecutableMotionPlan &plan)> ExecutableMotionPlanComputationFn
The signature of a function that can compute a motion plan.
double getDiscardOverlappingCostSources() const
const trajectory_execution_manager::TrajectoryExecutionManagerPtr & getTrajectoryExecutionManager() const
moveit_sensor_manager::MoveItSensorManagerPtr sensor_manager_
bool computePlan(ExecutableMotionPlan &plan, const ExecutableMotionPlanComputationFn &motion_planner, unsigned int max_look_attempts, double max_safe_path_cost)
boost::function< void()> before_look_callback_
void setDiscardOverlappingCostSources(double value)
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 setBeforeLookCallback(const boost::function< void()> &callback)
PlanWithSensing(const trajectory_execution_manager::TrajectoryExecutionManagerPtr &trajectory_execution)
void setMaxLookAttempts(unsigned int attempts)
DynamicReconfigureImpl * reconfigure_impl_
trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_


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