pick_place.h
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2012, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
00036 
00037 #ifndef MOVEIT_PICK_PLACE_PICK_PLACE_
00038 #define MOVEIT_PICK_PLACE_PICK_PLACE_
00039 
00040 #include <moveit/macros/class_forward.h>
00041 #include <moveit/pick_place/manipulation_pipeline.h>
00042 #include <moveit/pick_place/pick_place_params.h>
00043 #include <moveit/constraint_sampler_manager_loader/constraint_sampler_manager_loader.h>
00044 #include <moveit/planning_pipeline/planning_pipeline.h>
00045 #include <moveit_msgs/PickupAction.h>
00046 #include <moveit_msgs/PlaceAction.h>
00047 #include <boost/noncopyable.hpp>
00048 #include <boost/enable_shared_from_this.hpp>
00049 
00050 namespace pick_place
00051 {
00052 MOVEIT_CLASS_FORWARD(PickPlace);
00053 
00054 class PickPlacePlanBase
00055 {
00056 public:
00057   PickPlacePlanBase(const PickPlaceConstPtr& pick_place, const std::string& name);
00058   ~PickPlacePlanBase();
00059 
00060   const std::vector<ManipulationPlanPtr>& getSuccessfulManipulationPlans() const
00061   {
00062     return pipeline_.getSuccessfulManipulationPlans();
00063   }
00064   const std::vector<ManipulationPlanPtr>& getFailedManipulationPlans() const
00065   {
00066     return pipeline_.getFailedManipulationPlans();
00067   }
00068 
00069   const moveit_msgs::MoveItErrorCodes& getErrorCode() const
00070   {
00071     return error_code_;
00072   }
00073 
00074 protected:
00075   void initialize();
00076   void waitForPipeline(const ros::WallTime& endtime);
00077   void foundSolution();
00078   void emptyQueue();
00079 
00080   PickPlaceConstPtr pick_place_;
00081   ManipulationPipeline pipeline_;
00082 
00083   double last_plan_time_;
00084   bool done_;
00085   bool pushed_all_poses_;
00086   boost::condition_variable done_condition_;
00087   boost::mutex done_mutex_;
00088   moveit_msgs::MoveItErrorCodes error_code_;
00089 };
00090 
00091 MOVEIT_CLASS_FORWARD(PickPlan);
00092 
00093 class PickPlan : public PickPlacePlanBase
00094 {
00095 public:
00096   PickPlan(const PickPlaceConstPtr& pick_place);
00097   bool plan(const planning_scene::PlanningSceneConstPtr& planning_scene, const moveit_msgs::PickupGoal& goal);
00098 };
00099 
00100 MOVEIT_CLASS_FORWARD(PlacePlan);
00101 
00102 class PlacePlan : public PickPlacePlanBase
00103 {
00104 public:
00105   PlacePlan(const PickPlaceConstPtr& pick_place);
00106   bool plan(const planning_scene::PlanningSceneConstPtr& planning_scene, const moveit_msgs::PlaceGoal& goal);
00107 };
00108 
00109 class PickPlace : private boost::noncopyable, public boost::enable_shared_from_this<PickPlace>
00110 {
00111 public:
00112   static const std::string DISPLAY_PATH_TOPIC;
00113   static const std::string DISPLAY_GRASP_TOPIC;
00114 
00115   // the amount of time (maximum) to wait for achieving a grasp posture
00116   static const double DEFAULT_GRASP_POSTURE_COMPLETION_DURATION;  // seconds
00117 
00118   PickPlace(const planning_pipeline::PlanningPipelinePtr& planning_pipeline);
00119 
00120   const constraint_samplers::ConstraintSamplerManagerPtr& getConstraintsSamplerManager() const
00121   {
00122     return constraint_sampler_manager_loader_->getConstraintSamplerManager();
00123   }
00124 
00125   const planning_pipeline::PlanningPipelinePtr& getPlanningPipeline() const
00126   {
00127     return planning_pipeline_;
00128   }
00129 
00130   const robot_model::RobotModelConstPtr& getRobotModel() const
00131   {
00132     return planning_pipeline_->getRobotModel();
00133   }
00134 
00136   PickPlanPtr planPick(const planning_scene::PlanningSceneConstPtr& planning_scene,
00137                        const moveit_msgs::PickupGoal& goal) const;
00138 
00140   PlacePlanPtr planPlace(const planning_scene::PlanningSceneConstPtr& planning_scene,
00141                          const moveit_msgs::PlaceGoal& goal) const;
00142 
00143   void displayComputedMotionPlans(bool flag);
00144   void displayProcessedGrasps(bool flag);
00145 
00146   void visualizePlan(const ManipulationPlanPtr& plan) const;
00147 
00148   void visualizeGrasp(const ManipulationPlanPtr& plan) const;
00149 
00150   void visualizeGrasps(const std::vector<ManipulationPlanPtr>& plans) const;
00151 
00152 private:
00153   ros::NodeHandle nh_;
00154   planning_pipeline::PlanningPipelinePtr planning_pipeline_;
00155   bool display_computed_motion_plans_;
00156   bool display_grasps_;
00157   ros::Publisher display_path_publisher_;
00158   ros::Publisher grasps_publisher_;
00159 
00160   constraint_sampler_manager_loader::ConstraintSamplerManagerLoaderPtr constraint_sampler_manager_loader_;
00161 };
00162 }
00163 
00164 #endif


manipulation
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Mon Jul 24 2017 02:21:50