object_manipulator: object_manipulator: descend_retreat

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 #include <object_manipulator/place_execution/descend_retreat_place.h>
00035 
00036 #include "object_manipulator/tools/mechanism_interface.h"
00037 #include "object_manipulator/tools/exceptions.h"
00038 #include "object_manipulator/tools/hand_description.h"
00039 #include "object_manipulator/tools/vector_tools.h"
00040 
00041 using arm_navigation_msgs::ArmNavigationErrorCodes;
00042 using object_manipulation_msgs::PlaceLocationResult;
00043 
00044 namespace object_manipulator {
00045 
00047 float StandardPlaceTester::EPS = 1.0e-6;
00048 
00049 // ---------------------------- Definitions ---------------------------------
00050 
00051 void PlaceTester::testPlaces(const object_manipulation_msgs::PlaceGoal &goal,
00052                              const std::vector<geometry_msgs::PoseStamped> &place_locations,
00053                              std::vector<PlaceExecutionInfo> &execution_info,
00054                              bool return_on_first_hit)
00055 {
00056   execution_info.clear();
00057   for (size_t i=0; i<place_locations.size(); i++)
00058   {
00059     ROS_DEBUG_NAMED("manipulation","Place tester: testing place %zd out of batch of %zd", i, place_locations.size());
00060     if (feedback_function_) feedback_function_(i);
00061     if (interrupt_function_ && interrupt_function_()) throw InterruptRequestedException();
00062     PlaceExecutionInfo info;
00063 
00064     //compute gripper location for final place
00065     info.gripper_place_pose_ = computeGripperPose(place_locations[i], goal.grasp.grasp_pose, 
00066                                                   handDescription().robotFrame(goal.arm_name));
00067     if (marker_publisher_)
00068     {
00069       info.marker_id_ = marker_publisher_->addGraspMarker(info.gripper_place_pose_);
00070       marker_publisher_->colorGraspMarker(info.marker_id_, 1.0, 0.0, 1.0); //magenta
00071     }
00072     testPlace(goal, place_locations[i], info);
00073     execution_info.push_back(info);
00074     if (info.result_.result_code == info.result_.SUCCESS && return_on_first_hit) return;
00075   }
00076 }
00077 
00078 geometry_msgs::PoseStamped 
00079 PlaceTester::computeGripperPose(geometry_msgs::PoseStamped place_location, 
00080                                         geometry_msgs::Pose grasp_pose, 
00081                                         std::string frame_id)
00082 {
00083   //get the gripper pose relative to place location
00084   tf::Transform place_trans;
00085   tf::poseMsgToTF(place_location.pose, place_trans);
00086   tf::Transform grasp_trans;
00087   tf::poseMsgToTF(grasp_pose, grasp_trans);
00088   grasp_trans = place_trans * grasp_trans;
00089 
00090   //get it in the requested frame
00091   tf::Stamped<tf::Pose> grasp_trans_stamped;
00092   grasp_trans_stamped.setData(grasp_trans);
00093   grasp_trans_stamped.frame_id_ = place_location.header.frame_id;
00094   grasp_trans_stamped.stamp_ = ros::Time::now();
00095   if (!listener_.waitForTransform(frame_id, place_location.header.frame_id, ros::Time::now(), ros::Duration(1.0)))
00096   {
00097     ROS_ERROR("Object place: tf does not have transform from %s to %s", 
00098               place_location.header.frame_id.c_str(),
00099               frame_id.c_str());
00100     throw MechanismException( std::string("Object place: tf does not have transform from ") + 
00101                               place_location.header.frame_id.c_str() + std::string(" to ") + 
00102                               frame_id);
00103   }
00104   tf::Stamped<tf::Pose> grasp_trans_base;
00105   try
00106   {
00107     listener_.transformPose( frame_id, grasp_trans_stamped, grasp_trans_base);
00108   }
00109   catch (tf::TransformException ex)
00110   {
00111     ROS_ERROR("Object place: tf failed to transform gripper pose into frame %s; exception: %s", 
00112               frame_id.c_str(), ex.what());
00113     throw MechanismException( std::string("tf failed to transform gripper pose into frame ") + 
00114                               frame_id + std::string("; tf exception: ") +  
00115                               std::string(ex.what()) );
00116   }
00117   
00118   geometry_msgs::PoseStamped gripper_pose;
00119   tf::poseTFToMsg(grasp_trans_base, gripper_pose.pose);
00120   gripper_pose.header.frame_id = frame_id;
00121   gripper_pose.header.stamp = ros::Time::now();
00122   return gripper_pose;
00123 }
00124 
00125 
00126 void PlacePerformer::performPlaces(const object_manipulation_msgs::PlaceGoal &goal,
00127                                    const std::vector<geometry_msgs::PoseStamped> &place_locations,
00128                                    std::vector<PlaceExecutionInfo> &execution_info)
00129 {
00130   for (size_t i=0; i<place_locations.size(); i++)
00131   {
00132     if (feedback_function_) feedback_function_(i);
00133     if (interrupt_function_ && interrupt_function_()) throw InterruptRequestedException();
00134     if (i>= execution_info.size()) throw GraspException("Place Performer: not enough execution info provided");
00135     if (execution_info[i].result_.result_code != PlaceLocationResult::SUCCESS) continue;
00136     ROS_DEBUG_NAMED("manipulation","Place performer: trying place %zd out of batch of %zd", i, place_locations.size());
00137     performPlace(goal, place_locations[i], execution_info[i]);
00138     if (execution_info[i].result_.result_code == execution_info[i].result_.SUCCESS || 
00139         !execution_info[i].result_.continuation_possible) return;
00140   }
00141 }
00142 
00143 // ---------------------------- Testers ---------------------------------
00144 
00145 void StandardPlaceTester::testPlace(const object_manipulation_msgs::PlaceGoal &place_goal,
00146                                     const geometry_msgs::PoseStamped &place_location,
00147                                     PlaceExecutionInfo &execution_info)
00148 {
00149   //disable collisions between grasped object and table
00150   arm_navigation_msgs::OrderedCollisionOperations ord;
00151   arm_navigation_msgs::CollisionOperation coll;
00152   coll.operation = arm_navigation_msgs::CollisionOperation::DISABLE;
00153   if (!place_goal.collision_object_name.empty() && !place_goal.collision_support_surface_name.empty())
00154   {
00155     coll.object1 = place_goal.collision_object_name;
00156     coll.object2 = place_goal.collision_support_surface_name;
00157     ord.collision_operations.push_back(coll);
00158   }
00159   if (place_goal.allow_gripper_support_collision)
00160   {
00161     coll.object1 = handDescription().gripperCollisionName(place_goal.arm_name);
00162     coll.object2 = place_goal.collision_support_surface_name;
00163     ord.collision_operations.push_back(coll);
00164   }
00165   ord.collision_operations = concat(ord.collision_operations, 
00166                                     place_goal.additional_collision_operations.collision_operations);
00167 
00168   //zero padding on fingertip links
00169   //std::vector<arm_navigation_msgs::LinkPadding> link_padding = 
00170   //  MechanismInterface::fingertipPadding(place_goal.arm_name, 0.0);
00171   std::vector<arm_navigation_msgs::LinkPadding> link_padding = 
00172     MechanismInterface::gripperPadding(place_goal.arm_name, 0.0);
00173 
00174   // padding on grasped object, which is still attached to the gripper
00175   arm_navigation_msgs::LinkPadding padding;
00176   padding.link_name = handDescription().attachedName(place_goal.arm_name);
00177   padding.padding = place_goal.place_padding;
00178   link_padding.push_back(padding);
00179   link_padding = concat(link_padding, place_goal.additional_link_padding);
00180 
00181   geometry_msgs::Vector3Stamped place_direction;
00182   place_direction.header.frame_id = place_goal.approach.direction.header.frame_id;
00183   place_direction.header.stamp = ros::Time::now();
00184   place_direction.vector = mechInterface().negate(place_goal.approach.direction.vector);
00185 
00186   //search backwards from place to pre-place
00187   std::vector<double> empty;
00188   float actual_distance;
00189   int error_code = mechInterface().getInterpolatedIK(place_goal.arm_name, 
00190                                                      execution_info.gripper_place_pose_, place_direction,
00191                                                      place_goal.approach.desired_distance,
00192                                                      empty, 
00193                                                      place_goal.grasp.grasp_posture, 
00194                                                      ord, link_padding, 
00195                                                      true, execution_info.descend_trajectory_, actual_distance);
00196   ROS_DEBUG_NAMED("manipulation"," Place trajectory: actual(%f), min(%f), desired (%f)", 
00197             actual_distance, place_goal.approach.min_distance, place_goal.approach.desired_distance);
00198 
00199   if (actual_distance < place_goal.approach.min_distance - EPS)
00200   {
00201     ROS_DEBUG_NAMED("manipulation","Place trajectory below min. threshold");
00202     if (execution_info.descend_trajectory_.points.empty())
00203     {
00204       ROS_DEBUG_NAMED("manipulation","Place trajectory empty; problem is with place location");
00205       if (error_code == ArmNavigationErrorCodes::COLLISION_CONSTRAINTS_VIOLATED) {
00206         execution_info.result_ = Result(PlaceLocationResult::PLACE_IN_COLLISION, true);
00207         return;
00208       }
00209       else if (error_code == ArmNavigationErrorCodes::JOINT_LIMITS_VIOLATED) {
00210         execution_info.result_ = Result(PlaceLocationResult::PLACE_OUT_OF_REACH, true);
00211         return;
00212       }
00213       else {
00214         execution_info.result_ = Result(PlaceLocationResult::PLACE_UNFEASIBLE, true);      
00215         return;
00216       }
00217     }
00218     if (error_code == ArmNavigationErrorCodes::COLLISION_CONSTRAINTS_VIOLATED) {
00219       execution_info.result_ = Result(PlaceLocationResult::PREPLACE_IN_COLLISION, true);
00220       return;
00221     }
00222     else if (error_code == ArmNavigationErrorCodes::JOINT_LIMITS_VIOLATED) {
00223       execution_info.result_ = Result(PlaceLocationResult::PREPLACE_OUT_OF_REACH, true);
00224       return;
00225     }
00226     else {
00227       execution_info.result_ = Result(PlaceLocationResult::PREPLACE_UNFEASIBLE, true);      
00228       return;
00229     }
00230   }
00231 
00232 
00233   //make sure first position is feasible with default padding
00234   if ( !mechInterface().checkStateValidity(place_goal.arm_name, 
00235                                            execution_info.descend_trajectory_.points.front().positions,
00236                                            place_goal.additional_collision_operations,
00237                                            place_goal.additional_link_padding) )
00238   {
00239     ROS_DEBUG_NAMED("manipulation","First pose in place trajectory is unfeasible with default padding");
00240     execution_info.result_ = Result(PlaceLocationResult::PREPLACE_UNFEASIBLE, true);
00241     return;
00242   }
00243   
00244 
00245   ord.collision_operations.clear();
00246   coll.operation = arm_navigation_msgs::CollisionOperation::DISABLE;
00247   //disable all collisions on grasped object, since we are no longer holding it during the retreat
00248   if (!place_goal.collision_object_name.empty())
00249   {
00250     coll.object1 = place_goal.collision_object_name;
00251     coll.object2 = coll.COLLISION_SET_ALL;
00252     ord.collision_operations.push_back(coll);
00253   }
00254   if (place_goal.allow_gripper_support_collision)
00255   {
00256     coll.object1 = handDescription().gripperCollisionName(place_goal.arm_name);
00257     coll.object2 = place_goal.collision_support_surface_name;
00258     ord.collision_operations.push_back(coll);
00259   }
00260   ord.collision_operations = concat(ord.collision_operations, 
00261                                     place_goal.additional_collision_operations.collision_operations);
00262 
00263   geometry_msgs::Vector3Stamped retreat_direction;
00264   retreat_direction.header.stamp = ros::Time::now();
00265   retreat_direction.header.frame_id = handDescription().gripperFrame(place_goal.arm_name);
00266   retreat_direction.vector = mechInterface().negate( handDescription().approachDirection(place_goal.arm_name) );
00267 
00268   //search from place to retreat, using solution from place as seed
00269   std::vector<double> place_joint_angles = execution_info.descend_trajectory_.points.back().positions;  
00270   mechInterface().getInterpolatedIK(place_goal.arm_name, 
00271                                     execution_info.gripper_place_pose_, retreat_direction,
00272                                     place_goal.desired_retreat_distance,
00273                                     place_joint_angles, 
00274                                     place_goal.grasp.pre_grasp_posture, 
00275                                     ord, link_padding, 
00276                                     false, execution_info.retreat_trajectory_, actual_distance);
00277   ROS_DEBUG_NAMED("manipulation","Retreat trajectory: actual (%f), min (%f) and desired (%f)", actual_distance,
00278            place_goal.min_retreat_distance, place_goal.desired_retreat_distance);
00279 
00280   if (actual_distance < place_goal.min_retreat_distance - EPS)
00281   {
00282    ROS_DEBUG_NAMED("manipulation","Retreat trajectory below min. threshold");
00283     if (execution_info.retreat_trajectory_.points.empty())
00284     {
00285       ROS_DEBUG_NAMED("manipulation","Retreat trajectory empty; problem is with place location");
00286       if (error_code == ArmNavigationErrorCodes::COLLISION_CONSTRAINTS_VIOLATED) {
00287         execution_info.result_ = Result(PlaceLocationResult::PLACE_IN_COLLISION, true);
00288         return;
00289       }
00290       else if (error_code == ArmNavigationErrorCodes::JOINT_LIMITS_VIOLATED) {
00291         execution_info.result_ = Result(PlaceLocationResult::PLACE_OUT_OF_REACH, true);
00292         return;
00293       }
00294       else {
00295         execution_info.result_ = Result(PlaceLocationResult::PLACE_UNFEASIBLE, true);      
00296         return;
00297       }
00298     }
00299     if (error_code == ArmNavigationErrorCodes::COLLISION_CONSTRAINTS_VIOLATED) {
00300       execution_info.result_ = Result(PlaceLocationResult::RETREAT_IN_COLLISION, true);
00301       return;
00302     }
00303     else if (error_code == ArmNavigationErrorCodes::JOINT_LIMITS_VIOLATED) {
00304       execution_info.result_ = Result(PlaceLocationResult::RETREAT_OUT_OF_REACH, true);
00305       return;
00306     }
00307     else {
00308       execution_info.result_ = Result(PlaceLocationResult::RETREAT_UNFEASIBLE, true);  
00309       return;
00310     }
00311   }
00312   execution_info.result_ = Result(PlaceLocationResult::SUCCESS, true);
00313 }
00314 
00315 // ---------------------------- Performers ---------------------------------
00316 
00331 PlaceLocationResult StandardPlacePerformer::retreat(const object_manipulation_msgs::PlaceGoal &place_goal)
00332 {
00333   arm_navigation_msgs::OrderedCollisionOperations ord;
00334   arm_navigation_msgs::CollisionOperation coll;
00335   coll.object1 = handDescription().gripperCollisionName(place_goal.arm_name);
00336   coll.operation = arm_navigation_msgs::CollisionOperation::DISABLE;
00337   //disable collision between gripper and object
00338   if (!place_goal.collision_object_name.empty())
00339   {
00340     coll.object2 = place_goal.collision_object_name;
00341     ord.collision_operations.push_back(coll);
00342   }
00343   //disable collision between gripper and table
00344   if (!place_goal.collision_support_surface_name.empty()) 
00345   {
00346     coll.object2 = place_goal.collision_support_surface_name;
00347     ord.collision_operations.push_back(coll);
00348   }
00349   ord.collision_operations = concat(ord.collision_operations, 
00350                                     place_goal.additional_collision_operations.collision_operations);
00351 
00352   //zero padding on gripper; shouldn't matter much due to collisions disabled above
00353   std::vector<arm_navigation_msgs::LinkPadding> link_padding 
00354     = concat(MechanismInterface::gripperPadding(place_goal.arm_name, 0.0),
00355              place_goal.additional_link_padding);
00356 
00357   geometry_msgs::Vector3Stamped direction;
00358   direction.header.stamp = ros::Time::now();
00359   direction.header.frame_id = handDescription().gripperFrame(place_goal.arm_name);
00360   direction.vector = mechInterface().negate( handDescription().approachDirection(place_goal.arm_name) );
00361 
00362   float actual_distance;
00363   mechInterface().translateGripper(place_goal.arm_name, 
00364                                    direction, 
00365                                    ord, link_padding, 
00366                                    place_goal.desired_retreat_distance, 0, actual_distance);
00367   if (actual_distance < place_goal.min_retreat_distance)
00368   {
00369     ROS_DEBUG_NAMED("manipulation","Object place: retreat incomplete (%f executed and %f desired)", 
00370              actual_distance, place_goal.min_retreat_distance);
00371     return Result(PlaceLocationResult::RETREAT_FAILED, false);
00372   }
00373 
00374   return Result(PlaceLocationResult::SUCCESS, true);
00375 }
00376 
00380 PlaceLocationResult 
00381 StandardPlacePerformer::placeApproach(const object_manipulation_msgs::PlaceGoal &place_goal,
00382                                       const PlaceExecutionInfo &execution_info)
00383 {
00384   mechInterface().attemptTrajectory(place_goal.arm_name, execution_info.descend_trajectory_, true);
00385   return Result(PlaceLocationResult::SUCCESS, true);
00386 }
00387 
00388 
00389 void StandardPlacePerformer::performPlace(const object_manipulation_msgs::PlaceGoal &place_goal,
00390                                           const geometry_msgs::PoseStamped &place_location,
00391                                           PlaceExecutionInfo &execution_info)
00392 {
00393   //whether we are using the constraints or not
00394   bool use_constraints = true;
00395   //check if we can actually understand the constraints
00396   if(!constraintsUnderstandable(place_goal.path_constraints))
00397   {
00398     ROS_WARN("Constraints passed to place executor are of types not yet handled. Ignoring them.");
00399     use_constraints = false;
00400   }
00401   if (place_goal.path_constraints.orientation_constraints.empty())
00402   {
00403     use_constraints = false;
00404   }  
00405   if(use_constraints)
00406   {
00407     //recompute the pre-place pose from the already computed trajectory
00408     geometry_msgs::PoseStamped place_pose;
00409     place_pose.header.frame_id = place_location.header.frame_id;
00410     place_pose.header.stamp = ros::Time(0.0);
00411     //ROS_DEBUG_NAMED("manipulation","Place pose in frame: %s",place_pose.header.frame_id.c_str());
00412     if(!mechInterface().getFK(place_goal.arm_name, execution_info.descend_trajectory_.points.front().positions, 
00413                               place_pose))
00414     {
00415       ROS_ERROR("Could not re-compute pre-place pose based on trajectory");
00416       throw MechanismException("Could not re-compute pre-place pose based on trajectory");       
00417     }
00418     ROS_DEBUG_NAMED("manipulation","Attempting move arm to pre-place with constraints");
00419     if (!mechInterface().moveArmConstrained(place_goal.arm_name, place_pose, 
00420                                             place_goal.additional_collision_operations,
00421                                             place_goal.additional_link_padding,
00422                                             place_goal.path_constraints, 
00423                                             execution_info.descend_trajectory_.points.front().positions[2], 
00424                                             false)) 
00425     {
00426       //TODO: in the future, this should be hard stop, with an informative message back 
00427       //to the caller saying the constraints have failed
00428       ROS_WARN("Object place: move_arm to pre-place with constraints failed. Trying again without constraints.");
00429       use_constraints = false;
00430     }
00431   }
00432   //try to go to the pre-place pose without constraints
00433   if(!use_constraints)
00434   {
00435     ROS_DEBUG_NAMED("manipulation","Attempting move arm to pre-place without constraints");
00436     if ( !mechInterface().attemptMoveArmToGoal(place_goal.arm_name, 
00437                                                execution_info.descend_trajectory_.points.front().positions,
00438                                                place_goal.additional_collision_operations,
00439                                                place_goal.additional_link_padding) ) 
00440     {
00441       ROS_DEBUG_NAMED("manipulation","Object place: move_arm (without constraints) to pre-place reports failure");
00442       execution_info.result_ = Result(PlaceLocationResult::MOVE_ARM_FAILED, true);
00443       return;
00444     }
00445   }
00446   ROS_DEBUG_NAMED("manipulation"," Arm moved to pre-place");
00447 
00448   execution_info.result_ = placeApproach(place_goal, execution_info);
00449   if ( execution_info.result_.result_code != PlaceLocationResult::SUCCESS)
00450   {
00451     ROS_DEBUG_NAMED("manipulation"," Pre-place to place approach failed");
00452     execution_info.result_ = Result(PlaceLocationResult::PLACE_FAILED, false);
00453     return;
00454   }
00455   ROS_DEBUG_NAMED("manipulation"," Place trajectory done");
00456 
00457   mechInterface().detachAndAddBackObjectsAttachedToGripper(place_goal.arm_name, place_goal.collision_object_name);
00458   ROS_DEBUG_NAMED("manipulation"," Object detached");
00459 
00460   mechInterface().handPostureGraspAction(place_goal.arm_name, place_goal.grasp,
00461                                          object_manipulation_msgs::GraspHandPostureExecutionGoal::RELEASE, -1);
00462   ROS_DEBUG_NAMED("manipulation"," Object released");
00463 
00464   execution_info.result_ = retreat(place_goal);
00465   if (execution_info.result_.result_code != PlaceLocationResult::SUCCESS)
00466   {
00467     execution_info.result_ = Result(PlaceLocationResult::RETREAT_FAILED, false);
00468     return;
00469   }
00470   execution_info.result_ = Result(PlaceLocationResult::SUCCESS, true);
00471 }
00472 
00476 bool StandardPlacePerformer::constraintsUnderstandable(const arm_navigation_msgs::Constraints &constraints)
00477 {
00478   if(constraints.position_constraints.empty() && constraints.orientation_constraints.empty() && 
00479      constraints.joint_constraints.empty() && constraints.visibility_constraints.empty())
00480     return true;
00481   if(constraints.orientation_constraints.size() == 1 && constraints.position_constraints.empty() 
00482      && constraints.joint_constraints.empty() && constraints.visibility_constraints.empty())
00483     return true;
00484   return false;
00485 }
00486 
00487 PlaceLocationResult 
00488 ReactivePlacePerformer::placeApproach(const object_manipulation_msgs::PlaceGoal &place_goal,
00489                                       const PlaceExecutionInfo &execution_info)
00490 {
00491   //prepare the goal for reactive placing
00492   object_manipulation_msgs::ReactivePlaceGoal reactive_place_goal;  
00493   reactive_place_goal.arm_name = place_goal.arm_name;
00494   reactive_place_goal.collision_object_name = place_goal.collision_object_name;
00495   reactive_place_goal.collision_support_surface_name = place_goal.collision_support_surface_name;
00496   reactive_place_goal.trajectory = execution_info.descend_trajectory_;
00497   reactive_place_goal.final_place_pose = execution_info.gripper_place_pose_;
00498 
00499   //give the reactive place 1 minute to do its thing
00500   ros::Duration timeout = ros::Duration(60.0);
00501   ROS_DEBUG_NAMED("manipulation"," Calling the reactive place action");
00502   mechInterface().reactive_place_action_client_.client(place_goal.arm_name).sendGoal(reactive_place_goal);
00503   if ( !mechInterface().reactive_place_action_client_.client(place_goal.arm_name).waitForResult(timeout) )
00504   {
00505     ROS_ERROR("  Reactive place timed out");
00506     return Result(PlaceLocationResult::PLACE_FAILED, false);
00507   }
00508   object_manipulation_msgs::ReactivePlaceResult reactive_place_result = 
00509     *mechInterface().reactive_place_action_client_.client(place_goal.arm_name).getResult();
00510   if (reactive_place_result.manipulation_result.value != reactive_place_result.manipulation_result.SUCCESS)
00511   {
00512     ROS_ERROR("  Reactive place failed with error code %d", reactive_place_result.manipulation_result.value);
00513     return Result(PlaceLocationResult::PLACE_FAILED, false);
00514   }
00515   ROS_DEBUG_NAMED("manipulation","  Reactive place action succeeded");
00516   return Result(PlaceLocationResult::SUCCESS, true);
00517 }
00518 
00519 }
descend_retreat_place.cpp
