object_manipulator: object_manipulator: object

00001 /*********************************************************************
00002 *
00003 *  Copyright (c) 2009, Willow Garage, Inc.
00004 *  All rights reserved.
00005 *
00006 *  Redistribution and use in source and binary forms, with or without
00007 *  modification, are permitted provided that the following conditions
00008 *  are met:
00009 *
00010 *   * Redistributions of source code must retain the above copyright
00011 *     notice, this list of conditions and the following disclaimer.
00012 *   * Redistributions in binary form must reproduce the above
00013 *     copyright notice, this list of conditions and the following
00014 *     disclaimer in the documentation and/or other materials provided
00015 *     with the distribution.
00016 *   * Neither the name of the Willow Garage nor the names of its
00017 *     contributors may be used to endorse or promote products derived
00018 *     from this software without specific prior written permission.
00019 *
00020 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 *  POSSIBILITY OF SUCH DAMAGE.
00032 *********************************************************************/
00033 
00034 // Author(s): Matei Ciocarlie
00035 
00036 #include "object_manipulator/object_manipulator.h"
00037 
00038 #include <algorithm>
00039 
00040 //#define PROF_ENABLED
00041 //#include <profiling/profiling.h>
00042 //PROF_DECLARE(TOTAL_PICKUP_TIMER);
00043 
00044 //#include <demo_synchronizer/synchronizer_client.h>
00045 
00046 #include <object_manipulation_msgs/tools.h>
00047 
00048 //old style executors
00049 #include "object_manipulator/grasp_execution/grasp_executor_with_approach.h"
00050 #include "object_manipulator/grasp_execution/reactive_grasp_executor.h"
00051 #include "object_manipulator/grasp_execution/unsafe_grasp_executor.h"
00052 #include "object_manipulator/place_execution/place_executor.h"
00053 
00054 //new style executors
00055 #include "object_manipulator/grasp_execution/approach_lift_grasp.h"
00056 #include "object_manipulator/place_execution/descend_retreat_place.h"
00057 
00058 #include "object_manipulator/tools/grasp_marker_publisher.h"
00059 #include "object_manipulator/tools/exceptions.h"
00060 
00061 using object_manipulation_msgs::GraspableObject;
00062 using object_manipulation_msgs::PickupGoal;
00063 using object_manipulation_msgs::PickupResult;
00064 using object_manipulation_msgs::PickupFeedback;
00065 using object_manipulation_msgs::PlaceGoal;
00066 using object_manipulation_msgs::PlaceResult;
00067 using object_manipulation_msgs::PlaceFeedback;
00068 using object_manipulation_msgs::ManipulationResult;
00069 using object_manipulation_msgs::GraspResult;
00070 using object_manipulation_msgs::PlaceLocationResult;
00071 using object_manipulation_msgs::getGraspResultInfo;
00072 using object_manipulation_msgs::getPlaceLocationResultInfo;
00073 using object_manipulation_msgs::Grasp;
00074 using object_manipulation_msgs::GraspPlanningAction;
00075 
00076 namespace object_manipulator {
00077 
00078 ObjectManipulator::ObjectManipulator() :
00079   priv_nh_("~"),
00080   root_nh_(""),
00081   grasp_planning_actions_("", "", false, false),
00082   marker_pub_(NULL)
00083 {
00084   bool publish_markers = true;
00085   if (publish_markers)
00086   {
00087     marker_pub_ = new GraspMarkerPublisher();
00088   }
00089 
00090   //old style executors
00091   grasp_executor_with_approach_ = new GraspExecutorWithApproach(marker_pub_);
00092   reactive_grasp_executor_ = new ReactiveGraspExecutor(marker_pub_);
00093   unsafe_grasp_executor_ = new UnsafeGraspExecutor(marker_pub_);
00094   place_executor_ = new PlaceExecutor(marker_pub_);
00095   reactive_place_executor_ = new ReactivePlaceExecutor(marker_pub_);
00096 
00097   //new syle executors
00098   
00099   grasp_tester_with_approach_ = new GraspTesterWithApproach;
00100   grasp_tester_with_approach_->setMarkerPublisher(marker_pub_);
00101   unsafe_grasp_tester_ = new UnsafeGraspTester;
00102   unsafe_grasp_tester_->setMarkerPublisher(marker_pub_);
00103   standard_grasp_performer_ = new StandardGraspPerformer;
00104   standard_grasp_performer_->setMarkerPublisher(marker_pub_);
00105   reactive_grasp_performer_ = new ReactiveGraspPerformer;
00106   reactive_grasp_performer_->setMarkerPublisher(marker_pub_);
00107   unsafe_grasp_performer_ = new UnsafeGraspPerformer;
00108   unsafe_grasp_performer_->setMarkerPublisher(marker_pub_);
00109  
00110   standard_place_tester_ = new StandardPlaceTester;
00111   standard_place_tester_->setMarkerPublisher(marker_pub_);
00112   standard_place_performer_ = new StandardPlacePerformer;
00113   standard_place_performer_->setMarkerPublisher(marker_pub_);
00114   reactive_place_performer_ = new ReactivePlacePerformer;
00115   reactive_place_performer_->setMarkerPublisher(marker_pub_);
00116 
00117   priv_nh_.param<std::string>("default_cluster_planner", default_cluster_planner_, "default_cluster_planner");
00118   priv_nh_.param<std::string>("default_database_planner", default_database_planner_, "default_database_planner");
00119   priv_nh_.param<std::string>("default_probabilistic_planner", default_probabilistic_planner_,
00120     "default_probabilistic_planner");
00121   priv_nh_.param<bool>("use_probabilistic_grasp_planner", use_probabilistic_planner_, false);
00122   priv_nh_.param<bool>("randomize_grasps", randomize_grasps_, false);
00123 
00124   ROS_INFO("Object manipulator ready. Default cluster planner: %s. Default database planner: %s.", 
00125            default_cluster_planner_.c_str(), default_database_planner_.c_str());
00126   if(use_probabilistic_planner_)
00127   {
00128     ROS_INFO("Probabilistic planner enabled");
00129   }
00130   ROS_INFO_NAMED("manipulation","Object manipulator ready");
00131 }
00132 
00133 ObjectManipulator::~ObjectManipulator()
00134 {
00135   delete marker_pub_;
00136 
00137   //old style executors
00138   delete grasp_executor_with_approach_;
00139   delete reactive_grasp_executor_;
00140   delete unsafe_grasp_executor_;
00141   delete place_executor_;
00142   delete reactive_place_executor_;
00143 
00144   //new style executors
00145   delete grasp_tester_with_approach_;
00146   delete unsafe_grasp_tester_;
00147   delete standard_grasp_performer_;
00148   delete reactive_grasp_performer_;
00149   delete unsafe_grasp_performer_;
00150 
00151   delete standard_place_tester_;
00152   delete standard_place_performer_;
00153   delete reactive_place_performer_;
00154 }
00155 
00156 void ObjectManipulator::graspFeedback(
00157                                  actionlib::SimpleActionServer<object_manipulation_msgs::PickupAction> *action_server,
00158                                  size_t tested_grasps, size_t current_grasp)
00159 {
00160   PickupFeedback feedback;
00161   feedback.total_grasps = grasp_container_.size();
00162   feedback.current_grasp = current_grasp + tested_grasps;
00163   action_server->publishFeedback(feedback);
00164 }
00165 
00166 void ObjectManipulator::graspPlanningFeedbackCallback(
00167                                              const object_manipulation_msgs::GraspPlanningFeedbackConstPtr &feedback)
00168 {
00169   ROS_DEBUG_STREAM_NAMED("manipulation", "Feedback from planning action, total grasps: " << feedback->grasps.size());
00170   grasp_container_.addGrasps(feedback->grasps);
00171 }
00172 
00173 void ObjectManipulator::graspPlanningDoneCallback(const actionlib::SimpleClientGoalState& state,
00174                                              const object_manipulation_msgs::GraspPlanningResultConstPtr &result)
00175 {
00176   if (state == actionlib::SimpleClientGoalState::SUCCEEDED)
00177   {
00178     ROS_DEBUG_STREAM_NAMED("manipulation", "Final result from planning action, total grasps: " << result->grasps.size());
00179     grasp_container_.addGrasps(result->grasps);
00180   }
00181   else
00182   {
00183     ROS_ERROR("Grasp planning action did not succeed");
00184   }
00185 }
00186 
00187 void ObjectManipulator::pickup(const PickupGoal::ConstPtr &pickup_goal,
00188                                actionlib::SimpleActionServer<object_manipulation_msgs::PickupAction> *action_server)
00189 {
00190   //the result that will be returned
00191   PickupResult result;
00192 
00193   //we are making some assumptions here. We are assuming that the frame of the cluster is the
00194   //cannonical frame of the system, so here we check that the frames of all recognitions
00195   //agree with that. 
00196   //TODO find a general solution for this problem
00197   if ( pickup_goal->target.cluster.points.size() > 0)
00198   {
00199     for (size_t i=0; i<pickup_goal->target.potential_models.size(); i++)
00200     {
00201       if ( pickup_goal->target.potential_models[i].pose.header.frame_id != pickup_goal->target.cluster.header.frame_id)
00202       {
00203         ROS_ERROR("Target object recognition result(s) not in the same frame as the cluster");
00204         result.manipulation_result.value = ManipulationResult::ERROR;
00205         action_server->setAborted(result);
00206         return;
00207       }
00208     }
00209   }
00210   
00211   //populate the grasp container
00212   grasp_container_.clear();
00213   bool using_planner_action;
00214   std::string planner_action;
00215   if (!pickup_goal->desired_grasps.empty())
00216   {
00217     //use the requested grasps, if any
00218     grasp_container_.addGrasps(pickup_goal->desired_grasps);
00219     using_planner_action = false;
00220   }
00221   else
00222   {
00223     if (use_probabilistic_planner_)
00224     {
00225         ROS_INFO("Using probabilistic planner");
00226         // use the default probabilistic planner
00227         planner_action = default_probabilistic_planner_;
00228     }
00229     else
00230     {
00231         // decide which grasp planner to call depending on the type of object
00232         if (!pickup_goal->target.potential_models.empty())
00233             planner_action = default_database_planner_;
00234         else
00235             planner_action = default_cluster_planner_;        
00236     }
00237 
00238     //call the planner action, which will populate the grasp container as feedback arrives
00239     object_manipulation_msgs::GraspPlanningGoal goal;
00240     goal.arm_name = pickup_goal->arm_name;
00241     goal.target = pickup_goal->target;
00242     goal.collision_object_name = pickup_goal->collision_object_name;
00243     goal.collision_support_surface_name = pickup_goal->collision_support_surface_name;
00244     goal.movable_obstacles = pickup_goal->movable_obstacles;
00245     try
00246     {
00247       grasp_planning_actions_.client(planner_action).sendGoal(goal, 
00248                                           boost::bind(&ObjectManipulator::graspPlanningDoneCallback, this, _1, _2),
00249                                           actionlib::SimpleActionClient<GraspPlanningAction>::SimpleActiveCallback(), 
00250                                           boost::bind(&ObjectManipulator::graspPlanningFeedbackCallback, this, _1));
00251     }
00252     catch (ServiceNotFoundException &ex)
00253     {
00254       ROS_ERROR("Planning action %s not found", planner_action.c_str());
00255       result.manipulation_result.value = ManipulationResult::ERROR;
00256       action_server->setAborted(result);
00257       return;
00258     }
00259     using_planner_action = true;
00260   }
00261   ScopedGoalCancel<GraspPlanningAction> goal_cancel(NULL);
00262   if (using_planner_action)
00263   {
00264     goal_cancel.setClient(&grasp_planning_actions_.client(planner_action));
00265   }
00266   //decide which grasp tester and performer will be used
00267   GraspTester *grasp_tester;
00268   GraspPerformer *grasp_performer;
00269   if (pickup_goal->ignore_collisions) 
00270   {
00271     grasp_tester = unsafe_grasp_tester_;
00272     grasp_performer = unsafe_grasp_performer_;
00273   }
00274   else 
00275   {
00276     grasp_tester = grasp_tester_with_approach_;
00277     if (pickup_goal->use_reactive_execution)
00278     {
00279       grasp_performer = reactive_grasp_performer_;
00280     }
00281     else
00282     {
00283       grasp_performer = standard_grasp_performer_;
00284     }
00285   }
00286 
00287   grasp_tester->setInterruptFunction(
00288                boost::bind(&actionlib::SimpleActionServer<object_manipulation_msgs::PickupAction>::isPreemptRequested,
00289                            action_server));
00290   grasp_performer->setInterruptFunction(
00291                boost::bind(&actionlib::SimpleActionServer<object_manipulation_msgs::PickupAction>::isPreemptRequested,
00292                            action_server));
00293 
00294   //PROF_RESET_ALL;
00295   //PROF_START_TIMER(TOTAL_PICKUP_TIMER);
00296 
00297   //try the grasps in the list until one succeeds
00298   result.manipulation_result.value = ManipulationResult::UNFEASIBLE;
00299   try
00300   {
00301     size_t tested_grasps = 0;
00302     while (1)
00303     {
00304       if (action_server->isPreemptRequested()) throw InterruptRequestedException();
00305 
00306       ROS_DEBUG_STREAM_NAMED("manipulation", "Object manipulator: getting grasps beyond " << tested_grasps);
00307       std::vector<object_manipulation_msgs::Grasp> new_grasps = grasp_container_.getGrasps(tested_grasps);
00308       if ( new_grasps.empty() )
00309       { 
00310         if ( using_planner_action && (grasp_planning_actions_.client(planner_action).getState() == 
00311                                       actionlib::SimpleClientGoalState::ACTIVE || 
00312                                       grasp_planning_actions_.client(planner_action).getState() == 
00313                                       actionlib::SimpleClientGoalState::PENDING) )
00314         {
00315           ROS_DEBUG_NAMED("manipulation", "Object manipulator: waiting for planner action to provide grasps");
00316           ros::Duration(0.25).sleep();
00317           continue;
00318         }
00319         else
00320         {
00321           ROS_INFO("Object manipulator: all grasps have been tested");
00322           break;
00323         }
00324       }
00325       grasp_tester->setFeedbackFunction(boost::bind(&ObjectManipulator::graspFeedback, 
00326                                                     this, action_server, tested_grasps,  _1));
00327       //test a batch of grasps
00328       std::vector<GraspExecutionInfo> execution_info;
00329       grasp_tester->testGrasps(*pickup_goal, new_grasps, execution_info, !pickup_goal->only_perform_feasibility_test);
00330       if (execution_info.empty()) throw GraspException("grasp tester provided empty ExecutionInfo");
00331       //try to perform them
00332       if (!pickup_goal->only_perform_feasibility_test)
00333       {
00334         grasp_performer->performGrasps(*pickup_goal, new_grasps, execution_info);
00335       }
00336       if (execution_info.empty()) throw GraspException("grasp performer provided empty ExecutionInfo");
00337       //copy information about tested grasps over in result
00338       for (size_t i=0; i<execution_info.size(); i++)
00339       {
00340         result.attempted_grasps.push_back(new_grasps[i]);
00341         result.attempted_grasp_results.push_back(execution_info[i].result_);
00342       }
00343       //see if we're done
00344       if (execution_info.back().result_.result_code == GraspResult::SUCCESS)
00345       {
00346         result.manipulation_result.value = ManipulationResult::SUCCESS;
00347         if (!pickup_goal->only_perform_feasibility_test)
00348         {
00349           result.grasp = new_grasps.at( execution_info.size() -1 );
00350           action_server->setSucceeded(result);
00351           return;
00352         }
00353       }
00354       //see if continuation is possible
00355       if (!execution_info.back().result_.continuation_possible)
00356       {
00357         if (pickup_goal->only_perform_feasibility_test)
00358         {
00359           ROS_ERROR("Continuation impossible when performing feasibility test");
00360         }
00361         result.grasp = new_grasps.at( execution_info.size() -1 );
00362         if (execution_info.back().result_.result_code == GraspResult::LIFT_FAILED)
00363           result.manipulation_result.value = ManipulationResult::LIFT_FAILED;
00364         else
00365           result.manipulation_result.value = ManipulationResult::FAILED;
00366         action_server->setAborted(result);
00367         return;
00368       }
00369       //remember how many grasps we've tried
00370       tested_grasps += execution_info.size();
00371     }
00372     //all the grasps have been tested
00373     if (pickup_goal->only_perform_feasibility_test && result.manipulation_result.value == ManipulationResult::SUCCESS)
00374     {
00375       action_server->setSucceeded(result);
00376     }
00377     else
00378     {
00379       action_server->setAborted(result);
00380     }
00381     return;
00382   }
00383   catch (InterruptRequestedException &ex)
00384   {
00385     ROS_DEBUG_NAMED("manipulation","Pickup goal preempted");
00386     action_server->setPreempted();
00387     return;
00388   }
00389   catch (MoveArmStuckException &ex)
00390   {
00391     ROS_ERROR("Grasp aborted because move_arm is stuck");
00392     result.manipulation_result.value = ManipulationResult::ARM_MOVEMENT_PREVENTED;
00393     action_server->setAborted(result);
00394     return;
00395   }
00396   catch (GraspException &ex)
00397   {
00398     ROS_ERROR("Grasp error; exception: %s", ex.what());
00399     result.manipulation_result.value = ManipulationResult::ERROR;
00400     action_server->setAborted(result);
00401     return;
00402   }
00403 }
00404 
00405 void ObjectManipulator::placeFeedback(
00406                                  actionlib::SimpleActionServer<object_manipulation_msgs::PlaceAction> *action_server,
00407                                  size_t tested_places, size_t total_places, size_t current_place)
00408 {
00409   PlaceFeedback feedback;
00410   feedback.total_locations = total_places;
00411   feedback.current_location = current_place + tested_places;
00412   action_server->publishFeedback(feedback);
00413 }
00414 
00415 void ObjectManipulator::place(const object_manipulation_msgs::PlaceGoal::ConstPtr &place_goal,
00416                               actionlib::SimpleActionServer<object_manipulation_msgs::PlaceAction> *action_server)
00417 {
00418   PlaceResult result;
00419   PlaceTester *place_tester = standard_place_tester_;
00420   PlacePerformer *place_performer = standard_place_performer_;
00421   if (place_goal->use_reactive_place) place_performer = reactive_place_performer_;
00422   
00423   place_tester->setInterruptFunction(
00424                boost::bind(&actionlib::SimpleActionServer<object_manipulation_msgs::PlaceAction>::isPreemptRequested,
00425                            action_server));
00426   place_performer->setInterruptFunction(
00427                boost::bind(&actionlib::SimpleActionServer<object_manipulation_msgs::PlaceAction>::isPreemptRequested,
00428                            action_server));
00429 
00430   std::vector<geometry_msgs::PoseStamped> place_locations = place_goal->place_locations;
00431   try
00432   {
00433     std::vector<PlaceExecutionInfo> execution_info;
00434     result.manipulation_result.value = ManipulationResult::UNFEASIBLE;
00435     size_t tested_places = 0;
00436     while (!place_locations.empty())
00437     {
00438       if (action_server->isPreemptRequested()) throw InterruptRequestedException();
00439       place_tester->setFeedbackFunction(boost::bind(&ObjectManipulator::placeFeedback, 
00440                                                     this, action_server, tested_places, place_locations.size(), _1));
00441       //test a batch of locations
00442       place_tester->testPlaces(*place_goal, place_locations, execution_info, 
00443                                !place_goal->only_perform_feasibility_test);
00444       if (execution_info.empty()) throw GraspException("place tester provided empty ExecutionInfo");
00445       //try to perform them
00446       if (!place_goal->only_perform_feasibility_test)
00447       {
00448         place_performer->performPlaces(*place_goal, place_locations, execution_info);
00449       }
00450       if (execution_info.empty()) throw GraspException("place performer provided empty ExecutionInfo");
00451       //copy information about tested places over in result
00452       std::vector<geometry_msgs::PoseStamped>::iterator it = place_locations.begin();
00453       for (size_t i=0; i<execution_info.size(); i++)
00454       {
00455         result.attempted_locations.push_back(place_locations[i]);
00456         result.attempted_location_results.push_back(execution_info[i].result_);
00457         it++;
00458       }
00459       //see if we're done
00460       if (execution_info.back().result_.result_code == PlaceLocationResult::SUCCESS)
00461       {
00462         result.manipulation_result.value = ManipulationResult::SUCCESS;
00463         if (!place_goal->only_perform_feasibility_test)
00464         {
00465           result.place_location = place_locations.at( execution_info.size() - 1 );
00466           action_server->setSucceeded(result);
00467           return;
00468         }
00469       }
00470       //see if continuation is possible
00471       if (!execution_info.back().result_.continuation_possible)
00472       {
00473         if (place_goal->only_perform_feasibility_test)
00474         {
00475           ROS_ERROR("Continuation impossible when performing feasibility test");
00476         }
00477         result.place_location = place_locations.at( execution_info.size() - 1 );
00478         if (execution_info.back().result_.result_code == PlaceLocationResult::RETREAT_FAILED)
00479           result.manipulation_result.value = ManipulationResult::RETREAT_FAILED;
00480         else
00481           result.manipulation_result.value = ManipulationResult::FAILED;
00482         action_server->setAborted(result);
00483         return;
00484       }
00485       //clear all the grasps we've tried
00486       tested_places += execution_info.size();
00487       execution_info.clear();      
00488       place_locations.erase(place_locations.begin(), it);
00489     }
00490     //all the grasps have been tested
00491     if (place_goal->only_perform_feasibility_test && result.manipulation_result.value == ManipulationResult::SUCCESS)
00492     {
00493       action_server->setSucceeded(result);
00494     }
00495     else
00496     {
00497       action_server->setAborted(result);
00498     }
00499     return;
00500   }
00501   catch (InterruptRequestedException &ex)
00502   {
00503     ROS_DEBUG_NAMED("manipulation","Place goal preempted");
00504     action_server->setPreempted();
00505     return;
00506   }
00507   catch (MoveArmStuckException &ex)
00508   {
00509     ROS_ERROR("Place aborted because move_arm is stuck");
00510     result.manipulation_result.value = ManipulationResult::ARM_MOVEMENT_PREVENTED;
00511     action_server->setAborted(result);
00512     return;
00513   }
00514   catch (GraspException &ex)
00515   {
00516     ROS_ERROR("Place error; exception: %s", ex.what());
00517     result.manipulation_result.value = ManipulationResult::ERROR;
00518     action_server->setAborted(result);
00519     return;
00520   } 
00521 }
00522 
00523 }
00524
object_manipulator.cpp
