object_manipulator: object_manipulator.cpp Source File

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 //#include <demo_synchronizer/synchronizer_client.h>
00041 
00042 #include "object_manipulator/grasp_execution/grasp_executor_with_approach.h"
00043 #include "object_manipulator/grasp_execution/reactive_grasp_executor.h"
00044 #include "object_manipulator/place_execution/place_executor.h"
00045 #include "object_manipulator/tools/grasp_marker_publisher.h"
00046 #include "object_manipulator/tools/exceptions.h"
00047 
00048 using object_manipulation_msgs::GraspableObject;
00049 using object_manipulation_msgs::PickupGoal;
00050 using object_manipulation_msgs::PickupResult;
00051 using object_manipulation_msgs::PickupFeedback;
00052 using object_manipulation_msgs::PlaceGoal;
00053 using object_manipulation_msgs::PlaceResult;
00054 using object_manipulation_msgs::PlaceFeedback;
00055 using object_manipulation_msgs::ManipulationResult;
00056 using object_manipulation_msgs::GraspResult;
00057 using object_manipulation_msgs::PlaceLocationResult;
00058 
00059 namespace object_manipulator {
00060 
00061 ObjectManipulator::ObjectManipulator() :
00062   priv_nh_("~"),
00063   root_nh_(""),
00064   grasp_planning_services_("", "", false),
00065   marker_pub_(NULL)
00066 {
00067   bool publish_markers = true;
00068   if (publish_markers)
00069   {
00070     marker_pub_ = new GraspMarkerPublisher();
00071   }
00072 
00073   grasp_executor_with_approach_ = new GraspExecutorWithApproach(marker_pub_);
00074   reactive_grasp_executor_ = new ReactiveGraspExecutor(marker_pub_);
00075   place_executor_ = new PlaceExecutor(marker_pub_);
00076   reactive_place_executor_ = new ReactivePlaceExecutor(marker_pub_);
00077 
00078   priv_nh_.param<std::string>("default_cluster_planner", default_cluster_planner_, "default_cluster_planner");
00079   priv_nh_.param<std::string>("default_database_planner", default_database_planner_, "default_database_planner");
00080   priv_nh_.param<std::string>("default_probabilistic_planner", default_probabilistic_planner_, 
00081                               "default_probabilistic_planner");
00082   priv_nh_.param<bool>("randomize_grasps", randomize_grasps_, false);
00083 
00084   ROS_INFO("Object manipulator ready. Default cluster planner: %s. Default database planner: %s.", 
00085            default_cluster_planner_.c_str(), default_database_planner_.c_str());
00086 }
00087 
00088 ObjectManipulator::~ObjectManipulator()
00089 {
00090   delete marker_pub_;
00091   delete grasp_executor_with_approach_;
00092   delete reactive_grasp_executor_;
00093   delete place_executor_;
00094   delete reactive_place_executor_;
00095 }
00096 
00097 void ObjectManipulator::pickup(const PickupGoal::ConstPtr &pickup_goal,
00098                                actionlib::SimpleActionServer<object_manipulation_msgs::PickupAction> *action_server)
00099 {
00100   //the result that will be returned
00101   PickupResult result;
00102   PickupFeedback feedback;
00103 
00104   //we are making some assumptions here. We are assuming that the frame of the cluster is the
00105   //cannonical frame of the system, so here we check that the frames of all recognitions
00106   //agree with that. 
00107   //TODO find a general solution for this problem
00108   if ( pickup_goal->target.cluster.points.size() > 0)
00109   {
00110     for (size_t i=0; i<pickup_goal->target.potential_models.size(); i++)
00111     {
00112       if ( pickup_goal->target.potential_models[i].pose.header.frame_id != pickup_goal->target.cluster.header.frame_id)
00113       {
00114         ROS_ERROR("Target object recognition result(s) not in the same frame as the cluster");
00115         result.manipulation_result.value = ManipulationResult::ERROR;
00116         action_server->setAborted(result);
00117         return;
00118       }
00119     }
00120   }
00121   
00122   //populate a list of grasps to be tried
00123   std::vector<object_manipulation_msgs::Grasp> grasps;
00124   if (!pickup_goal->desired_grasps.empty())
00125   {
00126     //use the requested grasps, if any
00127     grasps = pickup_goal->desired_grasps;
00128   }
00129   else
00130   {
00131     //decide which grasp planner to call depending on the type of object
00132     std::string planner_service;
00133     //option that directly calls low-level planners
00134     if (!pickup_goal->target.potential_models.empty()) planner_service = default_database_planner_;
00135     else planner_service = default_cluster_planner_;    
00136 
00137     //probabilistic planner
00138     //planner_service = default_probabilistic_planner_;
00139 
00140     //call the planner and save the list of grasps
00141     object_manipulation_msgs::GraspPlanning srv;
00142     srv.request.arm_name = pickup_goal->arm_name;
00143     srv.request.target = pickup_goal->target;
00144     srv.request.collision_object_name = pickup_goal->collision_object_name;
00145     srv.request.collision_support_surface_name = pickup_goal->collision_support_surface_name;
00146     try
00147     {
00148       if (!grasp_planning_services_.client(planner_service).call(srv))
00149       {
00150         ROS_ERROR("Object manipulator failed to call planner at %s", planner_service.c_str());
00151         result.manipulation_result.value = ManipulationResult::ERROR;
00152         action_server->setAborted(result);
00153         return;
00154       }
00155       if (srv.response.error_code.value != srv.response.error_code.SUCCESS)
00156       {
00157         ROS_ERROR("Object manipulator: grasp planner failed with error code %d", srv.response.error_code.value);
00158         result.manipulation_result.value = ManipulationResult::ERROR;
00159         action_server->setAborted(result);
00160         return;
00161       }
00162       grasps = srv.response.grasps;
00163     }
00164     catch (ServiceNotFoundException &ex)
00165     {
00166       ROS_ERROR("Planning service not found");
00167       result.manipulation_result.value = ManipulationResult::ERROR;
00168       action_server->setAborted(result);
00169       return;
00170     }
00171   }
00172   feedback.total_grasps = grasps.size();
00173   feedback.current_grasp = 0;
00174   action_server->publishFeedback(feedback);
00175 
00176   //decide which grasp executor will be used
00177   GraspExecutor *executor;
00178   if (pickup_goal->use_reactive_execution) executor = reactive_grasp_executor_;
00179   else executor = grasp_executor_with_approach_;
00180 
00181   //demo_synchronizer::getClient().sync(2, "Attempting to grasp an object.");
00182   //demo_synchronizer::getClient().rviz(1, "Collision models;Grasp execution;Interpolated IK");
00183 
00184   if (randomize_grasps_)
00185   {
00186     ROS_INFO("Randomizing grasps");
00187     std::random_shuffle(grasps.begin(), grasps.end());
00188   }
00189 
00190   //try the grasps in the list until one succeeds
00191   try
00192   {
00193     for (size_t i=0; i<grasps.size(); i++)
00194     {
00195       if (action_server->isPreemptRequested())
00196       {
00197         action_server->setPreempted();
00198         return;
00199       }
00200       feedback.current_grasp = i+1;
00201       action_server->publishFeedback(feedback);
00202       GraspResult grasp_result = executor->checkAndExecuteGrasp(*pickup_goal, grasps[i]);
00203       ROS_DEBUG("Grasp result code: %d; continuation: %d", grasp_result.result_code, grasp_result.continuation_possible);
00204       result.attempted_grasps.push_back(grasps[i]);
00205       result.attempted_grasp_results.push_back(grasp_result);
00206       if (grasp_result.result_code == GraspResult::SUCCESS)
00207       {
00208         //demo_synchronizer::getClient().sync(2, "Object grasped - ready to move.");
00209         //demo_synchronizer::getClient().rviz(1, "Collision models;Planning goal;Collision map;Environment contacts");
00210         result.manipulation_result.value = ManipulationResult::SUCCESS;
00211         result.grasp = grasps[i];
00212         action_server->setSucceeded(result);
00213         return;
00214       }
00215       if (!grasp_result.continuation_possible)
00216       {
00217         result.grasp = grasps[i];
00218         if (grasp_result.result_code == GraspResult::LIFT_FAILED)
00219           result.manipulation_result.value = ManipulationResult::LIFT_FAILED;
00220         else
00221           result.manipulation_result.value = ManipulationResult::FAILED;
00222         action_server->setAborted(result);
00223         return;
00224       }
00225     }
00226     //all the grasps have been deemed unfeasible
00227     result.manipulation_result.value = ManipulationResult::UNFEASIBLE;
00228     action_server->setAborted(result);
00229     return;
00230   }
00231   catch (MoveArmStuckException &ex)
00232   {
00233     ROS_ERROR("Grasp aborted because move_arm is stuck");
00234     result.manipulation_result.value = ManipulationResult::ARM_MOVEMENT_PREVENTED;
00235     action_server->setAborted(result);
00236     return;
00237   }
00238   catch (GraspException &ex)
00239   {
00240     ROS_ERROR("Grasp error; exception: %s", ex.what());
00241     result.manipulation_result.value = ManipulationResult::ERROR;
00242     action_server->setAborted(result);
00243     return;
00244   }
00245 }
00246 
00247 void ObjectManipulator::place(const object_manipulation_msgs::PlaceGoal::ConstPtr &place_goal,
00248                               actionlib::SimpleActionServer<object_manipulation_msgs::PlaceAction> *action_server)
00249 {
00250   PlaceResult result;
00251   PlaceFeedback feedback;
00252   PlaceExecutor *executor;
00253   if (place_goal->use_reactive_place)
00254   {
00255     executor = reactive_place_executor_;
00256   }
00257   else
00258   {
00259     executor = place_executor_;
00260   }
00261 
00262   feedback.total_locations = place_goal->place_locations.size();
00263   feedback.current_location = 0;
00264   action_server->publishFeedback(feedback);
00265 
00266   try
00267   {
00268     for (size_t i=0; i<place_goal->place_locations.size(); i++)
00269     {
00270       if (action_server->isPreemptRequested())
00271       {
00272         action_server->setPreempted();
00273         return;
00274       }
00275       feedback.current_location = i+1;
00276       action_server->publishFeedback(feedback);
00277       geometry_msgs::PoseStamped place_location = place_goal->place_locations[i];
00278       PlaceLocationResult location_result = executor->place(*place_goal, place_location);
00279       ROS_DEBUG("Place location result code: %d; continuation: %d", location_result.result_code, 
00280                location_result.continuation_possible);
00281       result.attempted_locations.push_back(place_goal->place_locations[i]);
00282       result.attempted_location_results.push_back(location_result);
00283       if (location_result.result_code == PlaceLocationResult::SUCCESS)
00284       {
00285         result.place_location = place_goal->place_locations[i];
00286         result.manipulation_result.value = ManipulationResult::SUCCESS;
00287         action_server->setSucceeded(result);
00288         return;
00289       }   
00290       if (!location_result.continuation_possible)
00291       {
00292         result.place_location = place_goal->place_locations[i];
00293         if (location_result.result_code == PlaceLocationResult::RETREAT_FAILED)
00294           result.manipulation_result.value = ManipulationResult::RETREAT_FAILED;
00295         else
00296           result.manipulation_result.value = ManipulationResult::FAILED;
00297         action_server->setAborted(result);
00298         return;
00299       }
00300     }
00301     //all the place locations have been deemed unfeasible
00302     result.manipulation_result.value = ManipulationResult::UNFEASIBLE;
00303     action_server->setAborted(result);
00304     return;
00305   }
00306   catch (MoveArmStuckException &ex)
00307   {
00308     ROS_ERROR("Place aborted because move_arm is stuck");
00309     result.manipulation_result.value = ManipulationResult::ARM_MOVEMENT_PREVENTED;
00310     action_server->setAborted(result);
00311     return;
00312   }
00313   catch (GraspException &ex)
00314   {
00315     ROS_ERROR("Place error; exception: %s", ex.what());
00316     result.manipulation_result.value = ManipulationResult::ERROR;
00317     action_server->setAborted(result);
00318     return;
00319   } 
00320 }
00321 
00322 }
00323