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