pick_place.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_PICK_PLACE_PICK_PLACE_
38 #define MOVEIT_PICK_PLACE_PICK_PLACE_
39 
45 #include <moveit_msgs/PickupAction.h>
46 #include <moveit_msgs/PlaceAction.h>
47 #include <boost/noncopyable.hpp>
48 #include <memory>
49 
50 namespace pick_place
51 {
52 MOVEIT_CLASS_FORWARD(PickPlace);
53 
55 {
56 public:
57  PickPlacePlanBase(const PickPlaceConstPtr& pick_place, const std::string& name);
59 
60  const std::vector<ManipulationPlanPtr>& getSuccessfulManipulationPlans() const
61  {
63  }
64  const std::vector<ManipulationPlanPtr>& getFailedManipulationPlans() const
65  {
67  }
68 
69  const moveit_msgs::MoveItErrorCodes& getErrorCode() const
70  {
71  return error_code_;
72  }
73 
74 protected:
75  void initialize();
76  void waitForPipeline(const ros::WallTime& endtime);
77  void foundSolution();
78  void emptyQueue();
79 
80  PickPlaceConstPtr pick_place_;
82 
84  bool done_;
86  boost::condition_variable done_condition_;
87  boost::mutex done_mutex_;
88  moveit_msgs::MoveItErrorCodes error_code_;
89 };
90 
92 
94 {
95 public:
96  PickPlan(const PickPlaceConstPtr& pick_place);
97  bool plan(const planning_scene::PlanningSceneConstPtr& planning_scene, const moveit_msgs::PickupGoal& goal);
98 };
99 
101 
103 {
104 public:
105  PlacePlan(const PickPlaceConstPtr& pick_place);
106  bool plan(const planning_scene::PlanningSceneConstPtr& planning_scene, const moveit_msgs::PlaceGoal& goal);
107 };
108 
109 class PickPlace : private boost::noncopyable, public std::enable_shared_from_this<PickPlace>
110 {
111 public:
112  static const std::string DISPLAY_PATH_TOPIC;
113  static const std::string DISPLAY_GRASP_TOPIC;
114 
115  // the amount of time (maximum) to wait for achieving a grasp posture
116  static const double DEFAULT_GRASP_POSTURE_COMPLETION_DURATION; // seconds
117 
118  PickPlace(const planning_pipeline::PlanningPipelinePtr& planning_pipeline);
119 
120  const constraint_samplers::ConstraintSamplerManagerPtr& getConstraintsSamplerManager() const
121  {
122  return constraint_sampler_manager_loader_->getConstraintSamplerManager();
123  }
124 
125  const planning_pipeline::PlanningPipelinePtr& getPlanningPipeline() const
126  {
127  return planning_pipeline_;
128  }
129 
130  const robot_model::RobotModelConstPtr& getRobotModel() const
131  {
132  return planning_pipeline_->getRobotModel();
133  }
134 
136  PickPlanPtr planPick(const planning_scene::PlanningSceneConstPtr& planning_scene,
137  const moveit_msgs::PickupGoal& goal) const;
138 
140  PlacePlanPtr planPlace(const planning_scene::PlanningSceneConstPtr& planning_scene,
141  const moveit_msgs::PlaceGoal& goal) const;
142 
143  void displayComputedMotionPlans(bool flag);
144  void displayProcessedGrasps(bool flag);
145 
146  void visualizePlan(const ManipulationPlanPtr& plan) const;
147 
148  void visualizeGrasp(const ManipulationPlanPtr& plan) const;
149 
150  void visualizeGrasps(const std::vector<ManipulationPlanPtr>& plans) const;
151 
152 private:
154  planning_pipeline::PlanningPipelinePtr planning_pipeline_;
159 
160  constraint_sampler_manager_loader::ConstraintSamplerManagerLoaderPtr constraint_sampler_manager_loader_;
161 };
162 }
163 
164 #endif
ros::Publisher display_path_publisher_
Definition: pick_place.h:157
constraint_sampler_manager_loader::ConstraintSamplerManagerLoaderPtr constraint_sampler_manager_loader_
Definition: pick_place.h:160
PickPlaceConstPtr pick_place_
Definition: pick_place.h:80
const std::vector< ManipulationPlanPtr > & getSuccessfulManipulationPlans() const
Definition: pick_place.h:60
static const std::string DISPLAY_GRASP_TOPIC
Definition: pick_place.h:113
ros::NodeHandle nh_
Definition: pick_place.h:153
const constraint_samplers::ConstraintSamplerManagerPtr & getConstraintsSamplerManager() const
Definition: pick_place.h:120
boost::condition_variable done_condition_
Definition: pick_place.h:86
Represent the sequence of steps that are executed for a manipulation plan.
ros::Publisher grasps_publisher_
Definition: pick_place.h:158
static const std::string DISPLAY_PATH_TOPIC
Definition: pick_place.h:112
planning_pipeline::PlanningPipelinePtr planning_pipeline_
Definition: pick_place.h:154
moveit_msgs::MoveItErrorCodes error_code_
Definition: pick_place.h:88
ManipulationPipeline pipeline_
Definition: pick_place.h:81
void waitForPipeline(const ros::WallTime &endtime)
Definition: pick_place.cpp:87
const std::vector< ManipulationPlanPtr > & getFailedManipulationPlans() const
const std::vector< ManipulationPlanPtr > & getFailedManipulationPlans() const
Definition: pick_place.h:64
MOVEIT_CLASS_FORWARD(ManipulationPlanSharedData)
const robot_model::RobotModelConstPtr & getRobotModel() const
Definition: pick_place.h:130
bool display_computed_motion_plans_
Definition: pick_place.h:155
const planning_pipeline::PlanningPipelinePtr & getPlanningPipeline() const
Definition: pick_place.h:125
const std::vector< ManipulationPlanPtr > & getSuccessfulManipulationPlans() const
const moveit_msgs::MoveItErrorCodes & getErrorCode() const
Definition: pick_place.h:69
PickPlacePlanBase(const PickPlaceConstPtr &pick_place, const std::string &name)
Definition: pick_place.cpp:53
static const double DEFAULT_GRASP_POSTURE_COMPLETION_DURATION
Definition: pick_place.h:116


manipulation
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Sun Oct 18 2020 13:18:24