pr2_interactive_manipulation: interactive_manipulation

00001 /*
00002  * Copyright (c) 2009, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include "pr2_interactive_manipulation/interactive_manipulation_frame.h"
00031 #include "pr2_interactive_manipulation/object_selection_frame.h"
00032 
00033 #include <object_manipulation_msgs/Grasp.h>
00034 #include <object_manipulation_msgs/GraspPlanning.h>
00035 #include <object_manipulation_msgs/PlacePlanning.h>
00036 #include <object_manipulation_msgs/ClusterBoundingBox.h>
00037 
00038 #include <tf/transform_datatypes.h>
00039 
00040 #include <household_objects_database_msgs/GetModelDescription.h>
00041 
00042 #include <object_manipulator/tools/vector_tools.h>
00043 
00044 #include "pr2_interactive_manipulation/interactive_manipulation_display.h"
00045 #include "pr2_interactive_manipulation/arm_locations.h"
00046 
00047 namespace pr2_interactive_manipulation {
00048 
00049 InteractiveManipulationFrame::InteractiveManipulationFrame(InteractiveManipulationDisplay *display, 
00050                                                            wxWindow *parent, rviz::VisualizationManager* manager) :
00051   InteractiveManipulationFrameBase(parent),
00052   display_(display),
00053   root_nh_(""),
00054   priv_nh_("~"),
00055   executing_thread_(NULL),
00056   status_label_text_("idle"),
00057   l_gripper_controller_("/l_gripper_controller/gripper_action","l_gripper_joint"),
00058   r_gripper_controller_("/r_gripper_controller/gripper_action","r_gripper_joint"),
00059   new_object_list_(false),
00060   ago_(AdvancedOptionsDialog::getDefaults())
00061 {
00062   grasp_planning_service_name_ = "/grasp_planning_gripper_click";
00063   grasp_planning_srv_ = root_nh_.serviceClient<object_manipulation_msgs::GraspPlanning>(grasp_planning_service_name_, 
00064                                                                                         false);
00065   place_planning_service_name_ = "/place_planning_gripper_click";
00066   place_planning_srv_ = root_nh_.serviceClient<object_manipulation_msgs::PlacePlanning>(place_planning_service_name_, 
00067                                                                                         false);
00068   pickup_action_name_ = "/object_manipulator/object_manipulator_pickup";
00069   pickup_client_ = new actionlib::SimpleActionClient<object_manipulation_msgs::PickupAction>(pickup_action_name_, true);
00070 
00071   place_action_name_ = "/object_manipulator/object_manipulator_place";
00072   place_client_ = new actionlib::SimpleActionClient<object_manipulation_msgs::PlaceAction>(place_action_name_, true);
00073 
00074   create_model_action_name_ = "/create_object_model_server/model_object_in_hand_action";
00075   create_model_client_ = 
00076     new actionlib::SimpleActionClient<pr2_create_object_model::ModelObjectInHandAction>(create_model_action_name_, true);
00077 
00078   std::string graspable_objects_topic_name = "interactive_object_recognition_result";
00079   graspable_objects_sub_ = root_nh_.subscribe(graspable_objects_topic_name, 10, 
00080                                               &InteractiveManipulationFrame::graspableObjectsCallback, this);
00081 
00082   std::string get_model_description_name = "/objects_database_node/get_model_description";
00083   get_model_description_srv_ = root_nh_.serviceClient<household_objects_database_msgs::GetModelDescription>
00084     (get_model_description_name, true);
00085 
00086   std::string modeled_object_name = "/create_object_model_server/modeled_object_in_hand";
00087   modeled_object_sub_ = root_nh_.subscribe(modeled_object_name, 10, 
00088                                            &InteractiveManipulationFrame::modeledObjectsCallback, this);
00089 
00090   object_selection_frame_ = new ObjectSelectionFrame( manager, this );
00091 }
00092 
00093 InteractiveManipulationFrame::~InteractiveManipulationFrame()
00094 {
00095   if (executing_thread_)
00096   {
00097     if (!mutex_.try_lock())
00098     {
00099       ROS_INFO("Waiting for interactive manipulation execution thread to finish");
00100       interrupt_request_ = true;
00101       mutex_.lock();
00102       ROS_INFO("Interactive manipulation execution thread done");
00103     }
00104     mutex_.unlock();
00105     executing_thread_->join();
00106     delete executing_thread_;
00107     ROS_INFO("Interactive manipulation execution thread deleted");
00108   }
00109 
00110   delete object_selection_frame_;
00111 }
00112 
00113 void InteractiveManipulationFrame::graspableObjectsCallback(
00114                                           const pr2_interactive_object_detection::GraspableObjectListConstPtr &objects)
00115 {
00116   boost::mutex::scoped_lock lock(object_list_mutex_);
00117   received_objects_ = objects;
00118   new_object_list_ = true;
00119 }
00120 
00121 void InteractiveManipulationFrame::modeledObjectsCallback(const pr2_create_object_model::ObjectInHandConstPtr &object)
00122 {
00123   boost::mutex::scoped_lock(modeled_object_mutex_);
00124   if (object->arm_name=="right_arm")
00125   {
00126     grasp_info_right_.object_collision_name_ = object->collision_name;
00127     grasp_info_right_.object_cluster_ = object->cluster;
00128   }
00129   else
00130   {
00131     grasp_info_left_.object_collision_name_ = object->collision_name;
00132     grasp_info_left_.object_cluster_ = object->cluster;
00133   }
00134   setStatusLabel("received modeled object information");
00135 }
00136 
00137 void InteractiveManipulationFrame::setStatusLabel(std::string text)
00138 {
00139   boost::mutex::scoped_lock lock(status_label_mutex_);
00140   status_label_text_ = text;
00141 }
00142 
00143 std::string InteractiveManipulationFrame::getGraspableObjectName(const object_manipulation_msgs::GraspableObject &obj)
00144 {
00145   if (obj.potential_models.empty()) return "unknown object";
00146   household_objects_database_msgs::GetModelDescription desc;
00147   desc.request.model_id = obj.potential_models[0].model_id;
00148   if ( !get_model_description_srv_.call(desc)  || 
00149        desc.response.return_code.code != desc.response.return_code.SUCCESS ) return "database object";
00150   if (!desc.response.tags.empty()) return desc.response.tags[0];
00151   return desc.response.name;
00152 }
00153 
00154 std::string getGraspResultInfo(object_manipulation_msgs::GraspResult result)
00155 {
00156   switch (result.result_code)
00157   {
00158   case object_manipulation_msgs::GraspResult::SUCCESS: return ("grasp success");
00159   case object_manipulation_msgs::GraspResult::GRASP_OUT_OF_REACH: return ("grasp out of reach");
00160   case object_manipulation_msgs::GraspResult::GRASP_IN_COLLISION: return ("grasp in collision");
00161   case object_manipulation_msgs::GraspResult::GRASP_UNFEASIBLE: return ("grasp unfeasible");
00162   case object_manipulation_msgs::GraspResult::PREGRASP_OUT_OF_REACH: return ("pregrasp out of reach");
00163   case object_manipulation_msgs::GraspResult::PREGRASP_IN_COLLISION: return ("pregrasp in collision");
00164   case object_manipulation_msgs::GraspResult::PREGRASP_UNFEASIBLE: return ("pregrasp unfeasible");
00165   case object_manipulation_msgs::GraspResult::LIFT_OUT_OF_REACH: return ("lift position out of reach");
00166   case object_manipulation_msgs::GraspResult::LIFT_IN_COLLISION: return ("lift position in collision");
00167   case object_manipulation_msgs::GraspResult::LIFT_UNFEASIBLE: return ("lift position unfeasible");
00168   case object_manipulation_msgs::GraspResult::MOVE_ARM_FAILED: return ("arm movement failed");
00169   case object_manipulation_msgs::GraspResult::GRASP_FAILED: return ("grasp failed");
00170   case object_manipulation_msgs::GraspResult::LIFT_FAILED: return ("lift failed");
00171   case object_manipulation_msgs::GraspResult::RETREAT_FAILED: return ("retreat failed");
00172   default: return("unknown result code returned");
00173   }
00174 }
00175 
00176 std::string getPlaceLocationResultInfo(object_manipulation_msgs::PlaceLocationResult result)
00177 {
00178   switch (result.result_code)
00179   {
00180   case object_manipulation_msgs::PlaceLocationResult::SUCCESS: return ("place success");
00181   case object_manipulation_msgs::PlaceLocationResult::PLACE_OUT_OF_REACH: return ("place location out of reach");
00182   case object_manipulation_msgs::PlaceLocationResult::PLACE_IN_COLLISION: return ("place location in collision");
00183   case object_manipulation_msgs::PlaceLocationResult::PLACE_UNFEASIBLE: return ("place in location unfeasible");
00184   case object_manipulation_msgs::PlaceLocationResult::PREPLACE_OUT_OF_REACH: return ("preplace location out of reach");
00185   case object_manipulation_msgs::PlaceLocationResult::PREPLACE_IN_COLLISION: return ("preplace location in collision");
00186   case object_manipulation_msgs::PlaceLocationResult::PREPLACE_UNFEASIBLE: return ("preplace location unfeasible");
00187   case object_manipulation_msgs::PlaceLocationResult::RETREAT_OUT_OF_REACH: return ("retreat location out of reach");
00188   case object_manipulation_msgs::PlaceLocationResult::RETREAT_IN_COLLISION: return ("retreat location in collision");
00189   case object_manipulation_msgs::PlaceLocationResult::RETREAT_UNFEASIBLE: return ("retreat location unfeasible");
00190   case object_manipulation_msgs::PlaceLocationResult::MOVE_ARM_FAILED: return ("arm movement failed");
00191   case object_manipulation_msgs::PlaceLocationResult::PLACE_FAILED: return ("place failed");
00192   case object_manipulation_msgs::PlaceLocationResult::RETREAT_FAILED: return ("retreat failed");
00193   default: return("unknown result code returned");
00194   }
00195 }
00196 
00197 InteractiveManipulationFrame::DialogOptions InteractiveManipulationFrame::getDialogOptions()
00198 {
00199   DialogOptions go;
00200   go.collision_checked_ = collision_box_->IsChecked();
00201   go.arm_selection_ = arm_choice_->GetSelection();
00202   return go;
00203 }
00204 
00205 wxString getWxString(std::string str)
00206 {
00207   wxString mystring(str.c_str(), wxConvUTF8);
00208   return mystring;
00209 }
00210 
00211 void InteractiveManipulationFrame::update()
00212 {
00213   status_label_mutex_.lock();
00214   wxString mystring(status_label_text_.c_str(), wxConvUTF8);
00215   status_label_mutex_.unlock();
00216   status_label_->SetLabel(mystring);
00217 
00218   if (!mutex_.try_lock() )
00219   {
00220     //a thread is executing
00221     grasp_button_->Enable(false);
00222     place_button_->Enable(false);
00223     cancel_button_->Enable(true);
00224     arm_go_button_->Enable(false);
00225     look_button_->Enable(false);
00226 
00227     reset_button_->Enable(false);
00228     new_map_button_->Enable(false);
00229     gripper_slider_->Enable(false);
00230   }
00231   else
00232   {
00233     grasp_button_->Enable( object_selection_frame_->getState() == ObjectSelectionFrame::IDLE );
00234     place_button_->Enable(true);
00235     cancel_button_->Enable(false);
00236     arm_go_button_->Enable(true);
00237     look_button_->Enable(true);
00238     gripper_slider_->Enable(true);
00239     if (collision_box_->IsChecked())
00240     {
00241       reset_button_->Enable(true);
00242       new_map_button_->Enable(true);
00243     }
00244     mutex_.unlock();
00245   }
00246 
00247   if (!collision_box_->IsChecked())
00248   {
00249     new_map_button_->Enable(false);
00250     reset_button_->Enable(false);
00251     reset_choice_->Enable(false);
00252   }
00253   else
00254   {
00255     //buttons are enabled above, based on whether a thread is active
00256     reset_choice_->Enable(true);
00257   }
00258 
00259   //update list of graspable objects
00260   if (object_list_mutex_.try_lock())
00261   {
00262     if (new_object_list_)
00263     {
00264       new_object_list_ = false;
00265       graspable_objects_ = received_objects_->graspable_objects;
00266       grasp_object_choice_->Clear();
00267       grasp_object_choice_->Insert(getWxString("Gripper Click"),0);
00268       grasp_object_choice_->Insert(getWxString("Object Selection"),1);
00269       grasp_object_choice_->SetSelection(0);
00270     }
00271     object_list_mutex_.unlock();
00272   }
00273 
00274   object_selection_frame_->update();
00275 
00276   switch ( object_selection_frame_->getState() )
00277   {
00278     case ObjectSelectionFrame::ACCEPTED:
00279     {
00280       object_selection_frame_->setState( ObjectSelectionFrame::IDLE );
00281       unsigned int selected_object = object_selection_frame_->getSelectedObject();
00282 
00283       ROS_INFO( "Object selection was finished. Result: %d", selected_object );
00284 
00285       if ( selected_object < graspable_objects_.size() )
00286       {
00287         executing_thread_ = new boost::thread(boost::bind(&InteractiveManipulationFrame::executeRecognizedObjectGrasp,
00288                                                           this, getDialogOptions(), ago_,
00289                                                           graspable_objects_.at(selected_object) ));
00290       }
00291       else
00292       {
00293         ROS_ERROR("Invalid object selection");
00294       }
00295       break;
00296     }
00297     case ObjectSelectionFrame::CANCELED:
00298       ROS_INFO( "Object selection was canceled." );
00299       object_selection_frame_->setState( ObjectSelectionFrame::IDLE );
00300       break;
00301     case ObjectSelectionFrame::IDLE:
00302     case ObjectSelectionFrame::ACTIVE:
00303       break;
00304   }
00305 }
00306 
00307 geometry_msgs::Pose translatePose(geometry_msgs::Pose pose_in, btVector3 translation)
00308 {
00309   tf::Transform trans_in;
00310   tf::poseMsgToTF(pose_in, trans_in);
00311   tf::StampedTransform translate_trans;
00312   translate_trans.setIdentity();
00313   translate_trans.setOrigin(translation);
00314   tf::Transform trans_out = trans_in * translate_trans;
00315   geometry_msgs::Pose pose_out;
00316   tf::poseTFToMsg(trans_out, pose_out);
00317   return pose_out;
00318 }
00319 
00320 geometry_msgs::Pose preTranslatePose(geometry_msgs::Pose pose_in, btVector3 translation)
00321 {
00322   tf::Transform trans_in;
00323   tf::poseMsgToTF(pose_in, trans_in);
00324   tf::StampedTransform translate_trans;
00325   translate_trans.setIdentity();
00326   translate_trans.setOrigin(translation);
00327   tf::Transform trans_out = translate_trans * trans_in;
00328   geometry_msgs::Pose pose_out;
00329   tf::poseTFToMsg(trans_out, pose_out);
00330   return pose_out;
00331 }
00332 
00333 void InteractiveManipulationFrame::executeRecognizedObjectGrasp(DialogOptions options, 
00334                                                                 AdvancedGraspOptions ago,
00335                                                                 object_manipulation_msgs::GraspableObject object)
00336 {
00337   boost::mutex::scoped_lock(mutex_);
00338 
00339   std::string arm_name;
00340   if (options.arm_selection_ == 0) arm_name = "right_arm";
00341   else arm_name = "left_arm";
00342 
00343   setStatusLabel("waiting for collision map services...");
00344   ros::Time start_time = ros::Time::now();
00345   while (!collision_map_interface_.connectionsEstablished(ros::Duration(1.0))) 
00346   {
00347     if (!priv_nh_.ok() || interrupt_request_ || ros::Time::now() - start_time >= ros::Duration(5.0)) 
00348     {
00349       setStatusLabel("collision map services not found");
00350       return;
00351     }
00352   }
00353   
00354 
00355   if (options.collision_checked_)
00356   {
00357     setStatusLabel("clearing collision objects...");
00358     try
00359     {
00360       collision_map_interface_.resetCollisionModels();
00361       collision_map_interface_.resetAttachedModels();
00362     }
00363     catch (tabletop_collision_map_processing::CollisionMapException ex)
00364     {
00365       setStatusLabel("collision operation failed");
00366       return;
00367     }
00368   }
00369 
00370   //if collision check box checked, add object
00371   std::string collision_name;
00372   if (options.collision_checked_)
00373   {
00374     //compute the cluster bounding box
00375     object_manipulation_msgs::ClusterBoundingBox bbox;
00376     collision_map_interface_.getClusterBoundingBox(object.cluster, bbox.pose_stamped, bbox.dimensions);
00377     collision_map_interface_.processCollisionGeometryForBoundingBox(bbox, collision_name);
00378   }
00379 
00380   motion_planning_msgs::OrderedCollisionOperations ord;
00381   if (!options.collision_checked_)
00382   {
00383       motion_planning_msgs::CollisionOperation coll;
00384       coll.object1 = motion_planning_msgs::CollisionOperation::COLLISION_SET_ALL;
00385       coll.object2 = motion_planning_msgs::CollisionOperation::COLLISION_SET_ALL;
00386       coll.operation = motion_planning_msgs::CollisionOperation::DISABLE;
00387       ord.collision_operations.push_back(coll);
00388   }
00389   std::vector<motion_planning_msgs::LinkPadding> pad;
00390   if (options.collision_checked_)
00391   {
00392     pad = object_manipulator::concat( object_manipulator::MechanismInterface::gripperPadding("left_arm", 0.0),
00393                                       object_manipulator::MechanismInterface::gripperPadding("right_arm", 0.0) );
00394   }
00395   
00396   while ( !pickup_client_->waitForServer(ros::Duration(1.0)) && priv_nh_.ok() && !interrupt_request_) 
00397   {
00398     setStatusLabel("waiting for pickup action server...");
00399   }
00400   if (!priv_nh_.ok() || interrupt_request_) return;
00401   ROS_INFO("Picking up object %s", collision_name.c_str());
00402 
00403   //execute grasp
00404   object_manipulation_msgs::PickupGoal goal;
00405   goal.collision_object_name = collision_name;
00406   goal.target = object;
00407   goal.arm_name = arm_name;
00408   goal.additional_collision_operations = ord;
00409   goal.additional_link_padding = pad;
00410   goal.desired_approach_distance = 0.1;
00411   goal.min_approach_distance = 0.05;
00412   if (ago.lift_vertically_ == 0)
00413   {
00414     goal.lift.direction.header.frame_id = "base_link";
00415     goal.lift.direction.vector.x = 0;
00416     goal.lift.direction.vector.y = 0;
00417     goal.lift.direction.vector.z = 1; 
00418   }
00419   else
00420   {
00421     if (arm_name == "right_arm") goal.lift.direction.header.frame_id = "r_wrist_roll_link";
00422     else goal.lift.direction.header.frame_id = "l_wrist_roll_link";
00423     goal.lift.direction.vector.x = -1;
00424     goal.lift.direction.vector.y = 0;
00425     goal.lift.direction.vector.z = 0;
00426   }
00427   goal.lift.desired_distance = ago.lift_steps_ * 0.01;
00428   goal.lift.min_distance = goal.lift.desired_distance / 2.0;
00429   goal.use_reactive_lift = ago.reactive_force_;
00430   goal.use_reactive_execution = ago.reactive_grasping_;
00431   
00432   pickup_client_->sendGoal(goal);
00433   while (!pickup_client_->waitForResult(ros::Duration(1.0)) && priv_nh_.ok() && !interrupt_request_)
00434   {
00435     setStatusLabel("calling pickup action...");
00436   }
00437   if (!priv_nh_.ok() || interrupt_request_) return;
00438 
00439   object_manipulation_msgs::PickupResult result = *(pickup_client_->getResult());
00440   if (pickup_client_->getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
00441   {
00442     if (result.attempted_grasp_results.empty())
00443     {
00444       setStatusLabel("a serious error has occured, please call the WG helpdesk");
00445     }
00446     else
00447     {
00448       setStatusLabel(getGraspResultInfo(result.attempted_grasp_results[0]));
00449     }
00450     return;
00451   }
00452   
00453   //transform returned grasp into base link frame
00454   geometry_msgs::PoseStamped executed_grasp_stamped;
00455   executed_grasp_stamped.pose = result.grasp.grasp_pose;
00456   executed_grasp_stamped.header.frame_id = object.reference_frame_id;
00457   executed_grasp_stamped.header.stamp = ros::Time(0);
00458   try
00459   {
00460     executed_grasp_stamped = mech_interface_.transformPose("/base_link",executed_grasp_stamped);
00461   }
00462   catch(object_manipulator::GraspException &ex)
00463   {
00464     ROS_ERROR("Failed to convert returned grasp to base_link frame");
00465     executed_grasp_stamped.pose = GraspInfo::somePose();
00466   }
00467 
00468   //compute a gripper location for the grasp relative to a coordinate frame aligned with base_link
00469   //and with the origin in front of the fingers of the robot
00470   geometry_msgs::Pose grasp_pose;
00471   //keep the orientation of the original grasp pose, transformed to base_link
00472   grasp_pose.orientation = executed_grasp_stamped.pose.orientation;
00473   //move back to get a location for the wrist roll link
00474   grasp_pose = translatePose(grasp_pose, btVector3(-0.20, 0.0, 0.0));
00475   //move up so the gripper won't hit the table regardless of what orientation it's in
00476   grasp_pose = preTranslatePose(grasp_pose, btVector3(0.0, 0.0, 0.1));
00477   object_manipulation_msgs::Grasp grasp = result.grasp;
00478   grasp.grasp_pose = grasp_pose;
00479   ROS_INFO("Saving grasp: %f %f %f; %f %f %f %f",
00480            grasp_pose.position.x, grasp_pose.position.y, grasp_pose.position.z,
00481            grasp_pose.orientation.x, grasp_pose.orientation.y, grasp_pose.orientation.z, grasp_pose.orientation.w);
00482   
00483   modeled_object_mutex_.lock();
00484   if (arm_name == "right_arm") 
00485   {
00486     grasp_info_right_.object_collision_name_ = collision_name;
00487     grasp_info_right_.grasp_ = grasp;
00488   }
00489   else 
00490   {
00491     grasp_info_left_.object_collision_name_ = collision_name;
00492     grasp_info_left_.grasp_ = grasp;
00493   }
00494   modeled_object_mutex_.unlock();
00495 
00496   setStatusLabel("grasp completed");
00497 }
00498 
00499 void InteractiveManipulationFrame::executeGrasp(DialogOptions options,
00500                                                 AdvancedGraspOptions ago)
00501 {
00502   boost::mutex::scoped_lock lock(mutex_);
00503 
00504   std::string arm_name;
00505   if (options.arm_selection_ == 0) arm_name = "right_arm";
00506   else arm_name = "left_arm";
00507 
00508   setStatusLabel("waiting for collision map services...");
00509   ros::Time start_time = ros::Time::now();
00510   while (!collision_map_interface_.connectionsEstablished(ros::Duration(1.0))) 
00511   {
00512     if (!priv_nh_.ok() || interrupt_request_ || ros::Time::now() - start_time >= ros::Duration(5.0)) 
00513     {
00514       setStatusLabel("collision map services not found");
00515       return;
00516     }
00517   }
00518   
00519   //call grasp planning through gripper click service  
00520   setStatusLabel("waiting for grasp planning service...");
00521   while ( !ros::service::waitForService(grasp_planning_service_name_, ros::Duration(2.0)) && 
00522           priv_nh_.ok() && !interrupt_request_) 
00523   {
00524   }
00525   if (!priv_nh_.ok() || interrupt_request_) 
00526   {
00527     setStatusLabel("grasp planning service not found");
00528     return;
00529   }
00530 
00531   setStatusLabel("calling grasp planning service...");
00532   object_manipulation_msgs::GraspPlanning grasp_planning;
00533   grasp_planning.request.target.reference_frame_id = "base_link";
00534   grasp_planning.request.arm_name = arm_name;
00535   if (!grasp_planning_srv_.call(grasp_planning))
00536   {
00537     setStatusLabel("failed to call grasp planning service");
00538     return;
00539   }
00540   if (grasp_planning.response.error_code.value != grasp_planning.response.error_code.SUCCESS)
00541   {
00542     setStatusLabel("grasp planning returned an error code");
00543     return;
00544   }
00545   if (grasp_planning.response.grasps.size() != 1 )
00546   {
00547     setStatusLabel("grasp planning returned 0 or more than 1 grasps");
00548     return;
00549   }
00550   if (!priv_nh_.ok() || interrupt_request_) return;
00551 
00552   if (options.collision_checked_)
00553   {
00554     setStatusLabel("clearing collision objects...");
00555     try
00556     {
00557       collision_map_interface_.resetCollisionModels();
00558       collision_map_interface_.resetAttachedModels();
00559     }
00560     catch (tabletop_collision_map_processing::CollisionMapException ex)
00561     {
00562       setStatusLabel("collision operation failed");
00563       return;
00564     }
00565   }
00566 
00567   //if collision check box checked, add object, then take static map
00568   std::string collision_name;
00569   if (options.collision_checked_)
00570   {
00571     //create collision object at grasp location
00572     setStatusLabel("creating collision object...");
00573     object_manipulation_msgs::ClusterBoundingBox box;
00574     box.pose_stamped.header.frame_id = "base_link";
00575     box.pose_stamped.pose = translatePose(grasp_planning.response.grasps[0].grasp_pose, btVector3(0.19, 0, 0));
00576     box.dimensions.x = 0.14;
00577     box.dimensions.y = 0.18;
00578     box.dimensions.z = 0.06;
00579     collision_map_interface_.processCollisionGeometryForBoundingBox(box, collision_name);
00580   }
00581 
00582   while ( !pickup_client_->waitForServer(ros::Duration(1.0)) && priv_nh_.ok() && !interrupt_request_) 
00583   {
00584     setStatusLabel("waiting for pickup action server...");
00585   }
00586   if (!priv_nh_.ok() || interrupt_request_) return;
00587   ROS_INFO("Picking up object %s", collision_name.c_str());
00588 
00589   motion_planning_msgs::OrderedCollisionOperations ord;
00590   if (!options.collision_checked_)
00591   {
00592       motion_planning_msgs::CollisionOperation coll;
00593       coll.object1 = motion_planning_msgs::CollisionOperation::COLLISION_SET_ALL;
00594       coll.object2 = motion_planning_msgs::CollisionOperation::COLLISION_SET_ALL;
00595       coll.operation = motion_planning_msgs::CollisionOperation::DISABLE;
00596       ord.collision_operations.push_back(coll);
00597   }
00598   std::vector<motion_planning_msgs::LinkPadding> pad;
00599   if (options.collision_checked_)
00600   {
00601     pad = object_manipulator::concat( object_manipulator::MechanismInterface::gripperPadding("left_arm", 0.0),
00602                                       object_manipulator::MechanismInterface::gripperPadding("right_arm", 0.0) );
00603   }
00604 
00605   //execute grasp
00606   object_manipulation_msgs::PickupGoal goal;
00607   goal.collision_object_name = collision_name;
00608   goal.desired_grasps = grasp_planning.response.grasps;
00609   goal.additional_collision_operations = ord;
00610   goal.additional_link_padding = pad;
00611   goal.target.reference_frame_id = "base_link";
00612   goal.arm_name = arm_name;
00613   goal.desired_approach_distance = 0.1;
00614   goal.min_approach_distance = 0.05;
00615   if (ago.lift_vertically_ == 0)
00616   {
00617     goal.lift.direction.header.frame_id = "base_link";
00618     goal.lift.direction.vector.x = 0;
00619     goal.lift.direction.vector.y = 0;
00620     goal.lift.direction.vector.z = 1; 
00621   }
00622   else
00623   {
00624     if (arm_name == "right_arm") goal.lift.direction.header.frame_id = "r_wrist_roll_link";
00625     else goal.lift.direction.header.frame_id = "l_wrist_roll_link";
00626     goal.lift.direction.vector.x = -1;
00627     goal.lift.direction.vector.y = 0;
00628     goal.lift.direction.vector.z = 0;
00629   }
00630   goal.lift.desired_distance = ago.lift_steps_ * 0.01;
00631   goal.lift.min_distance = goal.lift.desired_distance / 2.0;
00632   goal.use_reactive_lift = ago.reactive_force_;
00633   goal.use_reactive_execution = ago.reactive_grasping_;
00634   
00635   pickup_client_->sendGoal(goal);
00636   while (!pickup_client_->waitForResult(ros::Duration(1.0)) && priv_nh_.ok() && !interrupt_request_)
00637   {
00638     setStatusLabel("calling pickup action...");
00639   }
00640   if (!priv_nh_.ok() || interrupt_request_) return;
00641 
00642   //kill the attached and collision objects as they get in the way later
00643   //the collision name when placing becomes useless, but I kept it in for now
00644   //if we reset collision models, their points re-appear in the map so we are
00645   //not doing that for now
00646   if (options.collision_checked_)
00647   {
00648     try 
00649     {
00650       //collision_map_interface_.resetCollisionModels();
00651       collision_map_interface_.resetAttachedModels();
00652     } 
00653     catch (tabletop_collision_map_processing::CollisionMapException &ex) 
00654     {
00655       //we don't actually return here, so this means we probably never get to see this message
00656       setStatusLabel("failed to reset collision and attached models");
00657     }
00658   }
00659 
00660   object_manipulation_msgs::PickupResult result = *(pickup_client_->getResult());
00661   if (pickup_client_->getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
00662   {
00663     if (result.attempted_grasp_results.empty())
00664     {
00665       setStatusLabel("a serious error has occured, please call the WG helpdesk");
00666     }
00667     else
00668     {
00669       setStatusLabel(getGraspResultInfo(result.attempted_grasp_results[0]));
00670     }
00671     return;
00672   }
00673   
00674   //compute a gripper location for the grasp relative to a coordinate frame aligned with base_link
00675   //and with the origin in front of the fingers of the robot
00676   geometry_msgs::Pose grasp_pose;
00677   //keep the orientation of the original grasp pose, since it was expressed in base_link
00678   grasp_pose.orientation = grasp_planning.response.grasps[0].grasp_pose.orientation;
00679   //move back to get a location for the wrist roll link
00680   grasp_pose = translatePose(grasp_pose, btVector3(-0.20, 0.0, 0.0));
00681   //move up so the gripper won't hit the table regardless of what orientation it's in
00682   grasp_pose = preTranslatePose(grasp_pose, btVector3(0.0, 0.0, 0.1));
00683   object_manipulation_msgs::Grasp grasp = grasp_planning.response.grasps[0];
00684   grasp.grasp_pose = grasp_pose;
00685   ROS_INFO("Saving grasp: %f %f %f; %f %f %f %f",
00686            grasp_pose.position.x, grasp_pose.position.y, grasp_pose.position.z,
00687            grasp_pose.orientation.x, grasp_pose.orientation.y, grasp_pose.orientation.z, grasp_pose.orientation.w);
00688   
00689   modeled_object_mutex_.lock();
00690   if (arm_name == "right_arm") 
00691   {
00692     grasp_info_right_.object_collision_name_ = collision_name;
00693     grasp_info_right_.grasp_ = grasp;
00694   }
00695   else 
00696   {
00697     grasp_info_left_.object_collision_name_ = collision_name;
00698     grasp_info_left_.grasp_ = grasp;
00699   }
00700   modeled_object_mutex_.unlock();
00701 
00702   setStatusLabel("grasp completed");
00703 }
00704 
00705 void InteractiveManipulationFrame::executePlace(DialogOptions options,
00706                                                 AdvancedGraspOptions ago)
00707 {
00708   boost::mutex::scoped_lock lock(mutex_);
00709 
00710   std::string arm_name;
00711   GraspInfo grasp_info;
00712 
00713   modeled_object_mutex_.lock();
00714   if (options.arm_selection_ == 0) 
00715   {
00716     arm_name = "right_arm";
00717     grasp_info = grasp_info_right_;
00718   }
00719   else 
00720   {
00721     arm_name = "left_arm";
00722     grasp_info = grasp_info_left_;
00723   }
00724   modeled_object_mutex_.unlock();
00725 
00726   setStatusLabel("waiting for collision map services...");
00727   ros::Time start_time = ros::Time::now();
00728   while (!collision_map_interface_.connectionsEstablished(ros::Duration(1.0))) 
00729   {
00730     if (!priv_nh_.ok() || interrupt_request_ || ros::Time::now() - start_time >= ros::Duration(5.0)) 
00731     {
00732       setStatusLabel("collision map services not found");
00733       return;
00734     }
00735   }
00736   
00737   //call place planning through gripper click service  
00738   while ( !ros::service::waitForService(place_planning_service_name_, ros::Duration(2.0)) && 
00739           priv_nh_.ok() && !interrupt_request_) 
00740   {
00741     setStatusLabel("waiting for place planning service...");
00742   }
00743   if (!priv_nh_.ok() || interrupt_request_) 
00744   {
00745     setStatusLabel("place planning service was not found");
00746     return;
00747   }
00748 
00749   setStatusLabel("calling place planning service...");
00750   object_manipulation_msgs::PlacePlanning place_planning;
00751   if (!place_planning_srv_.call(place_planning))
00752   {
00753     setStatusLabel("call to place planning service failed");
00754     return;
00755   }
00756   if (place_planning.response.error_code.value != place_planning.response.error_code.SUCCESS)
00757   {
00758     setStatusLabel("place planning returned an error code");
00759     return;
00760   }
00761   if (place_planning.response.place_locations.empty())
00762   {
00763     setStatusLabel("place planning returned no locations");
00764     return;
00765   }
00766     
00767   if (!priv_nh_.ok() || interrupt_request_) return;
00768 
00769   if (options.collision_checked_)
00770   {
00771     setStatusLabel("clearing collision objects...");
00772     try
00773     {
00774       collision_map_interface_.resetCollisionModels();
00775     }
00776     catch (tabletop_collision_map_processing::CollisionMapException ex)
00777     {
00778       setStatusLabel("collision operation failed");
00779     return;
00780     }
00781   }
00782 
00783   if (!priv_nh_.ok() || interrupt_request_) return;
00784 
00785   //if collision check box checked, add support object, then take static map
00786   std::string support_collision_name;
00787   if (options.collision_checked_)
00788   {
00789     //create collision support object at place location
00790     setStatusLabel("creating collision object...");
00791     object_manipulation_msgs::ClusterBoundingBox box;
00792     box.pose_stamped.header.frame_id = "base_link";
00793     box.pose_stamped.pose = translatePose(place_planning.response.place_locations[0].pose, btVector3(0, 0, -0.005));
00794     box.dimensions.x = 0.20;
00795     box.dimensions.y = 0.20;
00796     box.dimensions.z = 0.01;
00797     collision_map_interface_.processCollisionGeometryForBoundingBox(box, support_collision_name);
00798   }
00799 
00800   motion_planning_msgs::OrderedCollisionOperations ord;
00801   if (!options.collision_checked_)
00802   {
00803       motion_planning_msgs::CollisionOperation coll;
00804       coll.object1 = motion_planning_msgs::CollisionOperation::COLLISION_SET_ALL;
00805       coll.object2 = motion_planning_msgs::CollisionOperation::COLLISION_SET_ALL;
00806       coll.operation = motion_planning_msgs::CollisionOperation::DISABLE;
00807       ord.collision_operations.push_back(coll);
00808   }
00809   std::vector<motion_planning_msgs::LinkPadding> pad;
00810   if (options.collision_checked_)
00811   {
00812     pad = object_manipulator::concat( object_manipulator::MechanismInterface::gripperPadding("left_arm", 0.0),
00813                                       object_manipulator::MechanismInterface::gripperPadding("right_arm", 0.0) );
00814   }
00815 
00816   geometry_msgs::Pose grasp_pose = grasp_info.grasp_.grasp_pose;
00817   ROS_INFO("Place object %s using grasp: %f %f %f; %f %f %f %f,",
00818            grasp_info.object_collision_name_.c_str(),
00819            grasp_pose.position.x, grasp_pose.position.y, grasp_pose.position.z,
00820            grasp_pose.orientation.x, grasp_pose.orientation.y, grasp_pose.orientation.z, grasp_pose.orientation.w);
00821 
00822   object_manipulation_msgs::PlaceGoal goal;
00823   goal.arm_name = arm_name;
00824   goal.place_padding = 0.0;
00825   goal.collision_support_surface_name = support_collision_name;
00826   goal.collision_object_name = grasp_info.object_collision_name_;
00827   goal.additional_collision_operations = ord;
00828   goal.additional_link_padding = pad;
00829   ROS_INFO("Placing object %s on support %s", 
00830            grasp_info.object_collision_name_.c_str(), support_collision_name.c_str());
00831   goal.grasp = grasp_info.grasp_;
00832   goal.place_locations.push_back(place_planning.response.place_locations[0]);
00833   goal.desired_retreat_distance = 0.1;
00834   goal.min_retreat_distance = 0.05;
00835   if (ago.lift_vertically_ == 0)
00836   {
00837     goal.approach.direction.header.frame_id = "base_link";
00838     goal.approach.direction.vector.x = 0;
00839     goal.approach.direction.vector.y = 0;
00840     goal.approach.direction.vector.z = -1;
00841   }
00842   else
00843   {
00844     if (arm_name == "right_arm") goal.approach.direction.header.frame_id = "r_wrist_roll_link";
00845     else goal.approach.direction.header.frame_id = "l_wrist_roll_link";
00846     goal.approach.direction.vector.x = 1;
00847     goal.approach.direction.vector.y = 0;
00848     goal.approach.direction.vector.z = 0;
00849   }
00850   goal.approach.desired_distance = ago.lift_steps_ * 0.01;
00851   goal.approach.min_distance = goal.approach.desired_distance * 0.5;
00852   goal.use_reactive_place = ago.reactive_place_;
00853 
00854   place_client_->sendGoal(goal);
00855   while (!place_client_->waitForResult(ros::Duration(1.0)) && priv_nh_.ok() && !interrupt_request_)
00856   {
00857     setStatusLabel("calling place action...");
00858   }
00859   if (!priv_nh_.ok() || interrupt_request_) return;
00860     
00861   try 
00862   {
00863     collision_map_interface_.resetCollisionModels();
00864     setStatusLabel("collision models reset");
00865   } 
00866   catch (tabletop_collision_map_processing::CollisionMapException &ex) 
00867   {
00868     setStatusLabel("failed to reset collision models after placing");
00869   }    
00870   
00871   object_manipulation_msgs::PlaceResult result = *(place_client_->getResult());
00872   if (place_client_->getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
00873   {
00874     if (result.attempted_location_results.empty())
00875     {
00876       setStatusLabel("a serious error has occured, please call the WG helpdesk");
00877     }
00878     else
00879     {
00880       setStatusLabel(getPlaceLocationResultInfo(result.attempted_location_results[0]));
00881     }
00882     return;
00883   }
00884 
00885   setStatusLabel("place completed");
00886 }
00887 
00888 void InteractiveManipulationFrame::graspButtonClicked( wxCommandEvent& )
00889 {
00890   if (!mutex_.try_lock()) return;
00891   interrupt_request_ = false;
00892   
00893   int object_sel = grasp_object_choice_->GetSelection();
00894   if (object_sel == 0)
00895   {
00896     executing_thread_ = new boost::thread(boost::bind(&InteractiveManipulationFrame::executeGrasp, 
00897                                                       this, getDialogOptions(), ago_));
00898   }
00899   else
00900   {
00901     boost::mutex::scoped_lock lock(object_list_mutex_);
00902     object_selection_frame_->setData( received_objects_ );
00903   }
00904   mutex_.unlock();
00905 }
00906 
00907 void InteractiveManipulationFrame::placeButtonClicked( wxCommandEvent& )
00908 {
00909   if (!mutex_.try_lock()) return;
00910   interrupt_request_ = false;
00911   executing_thread_ = new boost::thread(boost::bind(&InteractiveManipulationFrame::executePlace, 
00912                                                     this, getDialogOptions(), ago_));
00913   mutex_.unlock();
00914 }
00915 
00916 void InteractiveManipulationFrame::takeMap()
00917 {
00918   boost::mutex::scoped_lock lock(mutex_);
00919   setStatusLabel("waiting for collision map services...");
00920   ros::Time start_time = ros::Time::now();
00921   while (!collision_map_interface_.connectionsEstablished(ros::Duration(1.0))) 
00922   {
00923     if (!priv_nh_.ok() || interrupt_request_ || ros::Time::now() - start_time >= ros::Duration(5.0)) 
00924     {
00925       setStatusLabel("collision map services not found");
00926       return;
00927     }
00928   }  
00929   setStatusLabel("acquiring new collision map");
00930   try
00931   {
00932     collision_map_interface_.takeStaticMap();
00933     setStatusLabel("new collision map acquired");
00934   }
00935   catch (tabletop_collision_map_processing::CollisionMapException ex)
00936   {
00937     setStatusLabel("collision map acquisition failed");
00938   }
00939   //we are also resetting the collision objects, which also triggers a rebroadcast
00940   //of the new map we've just takes
00941   try 
00942   {
00943     collision_map_interface_.resetCollisionModels();
00944   } 
00945   catch (tabletop_collision_map_processing::CollisionMapException &ex) 
00946   {
00947     setStatusLabel("failed to reset collision models after taking new map");
00948   }    
00949 
00950 }
00951 
00952 void InteractiveManipulationFrame::takeMapButtonClicked( wxCommandEvent& event )
00953 {
00954   if (!mutex_.try_lock()) return;
00955   interrupt_request_ = false;
00956   executing_thread_ = new boost::thread(boost::bind(&InteractiveManipulationFrame::takeMap, this));
00957   mutex_.unlock();
00958 }
00959 
00960 void InteractiveManipulationFrame::collisionReset(int reset_choice)
00961 {
00962   boost::mutex::scoped_lock lock(mutex_);
00963   switch (reset_choice)
00964   {
00965   case 0:
00966     //reset collision objects
00967     try 
00968     {
00969       collision_map_interface_.resetCollisionModels();
00970       setStatusLabel("collision models reset");
00971     } 
00972     catch (tabletop_collision_map_processing::CollisionMapException &ex) 
00973     {
00974       setStatusLabel("failed to reset collision models");
00975     }    
00976     break;
00977   case 1:
00978     //reset attached objects
00979     try 
00980     {
00981       collision_map_interface_.resetAttachedModels();
00982       setStatusLabel("attached models reset");
00983     } 
00984     catch (tabletop_collision_map_processing::CollisionMapException &ex) 
00985     {
00986       setStatusLabel("failed to reset attached models");
00987     }    
00988     break;
00989   case 2:
00990     //reset map
00991     try 
00992     {
00993       collision_map_interface_.resetStaticMap();
00994       setStatusLabel("collision map reset");
00995     } 
00996     catch (tabletop_collision_map_processing::CollisionMapException &ex) 
00997     {
00998       setStatusLabel("failed to reset collision map");
00999     }        
01000     break;
01001   }
01002 }
01003 
01004 void InteractiveManipulationFrame::resetButtonClicked( wxCommandEvent& event )
01005 {
01006   if (!mutex_.try_lock()) return;
01007   int reset_choice = reset_choice_->GetSelection();
01008   executing_thread_ = new boost::thread(boost::bind(&InteractiveManipulationFrame::collisionReset, this, reset_choice));
01009   mutex_.unlock();
01010 }
01011 
01012 void InteractiveManipulationFrame::armMotion(int arm_selection_choice, int arm_action_choice, 
01013                                              int arm_planner_choice, bool collision)
01014 {
01015   boost::mutex::scoped_lock lock(mutex_);
01016   std::string arm_name;
01017   if ( arm_selection_choice == 0) arm_name = "right_arm";
01018   else arm_name = "left_arm";
01019   motion_planning_msgs::OrderedCollisionOperations ord;
01020   if (!collision)
01021   {
01022       motion_planning_msgs::CollisionOperation coll;
01023       coll.object1 = motion_planning_msgs::CollisionOperation::COLLISION_SET_ALL;
01024       coll.object2 = motion_planning_msgs::CollisionOperation::COLLISION_SET_ALL;
01025       coll.operation = motion_planning_msgs::CollisionOperation::DISABLE;
01026       ord.collision_operations.push_back(coll);
01027   }
01028   std::vector<motion_planning_msgs::LinkPadding> pad;
01029   if (collision)
01030   {
01031     pad = object_manipulator::concat( object_manipulator::MechanismInterface::gripperPadding("left_arm", 0.0),
01032                                       object_manipulator::MechanismInterface::gripperPadding("right_arm", 0.0) );
01033   }
01034   switch(arm_action_choice)
01035   {
01036   case 0:
01037     {
01038       if (arm_planner_choice == 0)
01039       {
01040         setStatusLabel("moving arm to side using motion planner");
01041         try
01042         {
01043           if ( !mech_interface_.attemptMoveArmToGoal(arm_name, getSidePosition(arm_name), ord, pad) )
01044             setStatusLabel("failed to move arm to side (possible collisions)");
01045           else
01046             setStatusLabel("arm moved to side");
01047         }
01048         catch (object_manipulator::ServiceNotFoundException &ex)
01049         {
01050           setStatusLabel("a needed service or action server was not found");
01051         }
01052         catch (object_manipulator::MoveArmStuckException &ex)
01053         {
01054           setStatusLabel("arm is stuck in a colliding position");
01055         }
01056         catch (object_manipulator::MissingParamException &ex)
01057         {
01058           setStatusLabel("parameters missing; is manipulation pipeline running?");
01059         }
01060         catch (...)
01061         {
01062           setStatusLabel("an unknown error has occured, please call helpdesk");
01063         }
01064       }
01065       else
01066       {
01067         setStatusLabel("moving arm to side open-loop");
01068         try
01069         {
01070           mech_interface_.attemptTrajectory(arm_name, getSideTrajectory(arm_name), false, 3.0);
01071           setStatusLabel("arm moved to side");
01072         }
01073         catch (object_manipulator::ServiceNotFoundException &ex)
01074         {
01075           setStatusLabel("a needed service or action server was not found");
01076         }
01077         catch (object_manipulator::MissingParamException &ex)
01078         {
01079           setStatusLabel("parameters missing; is manipulation pipeline running?");
01080         }
01081         catch (object_manipulator::MechanismException &ex)
01082         {
01083           setStatusLabel("an error has occured, please call helpdesk");
01084         }
01085         catch (...)
01086         {
01087           setStatusLabel("an unknown error has occured, please call helpdesk");
01088         }
01089       }
01090     }
01091     break;
01092  case 1:
01093     {
01094       if (arm_planner_choice == 0)
01095       {
01096         setStatusLabel("moving arm to front using motion planner");
01097         try
01098         {
01099           if ( !mech_interface_.attemptMoveArmToGoal(arm_name, getFrontPosition(arm_name), ord, pad) )
01100             setStatusLabel("failed to move arm to front (possible collisions)");
01101           else
01102             setStatusLabel("arm moved to front");
01103         }
01104         catch (object_manipulator::ServiceNotFoundException &ex)
01105         {
01106           setStatusLabel("a needed service or action server was not found");
01107         }
01108         catch (object_manipulator::MoveArmStuckException &ex)
01109         {
01110           setStatusLabel("arm is stuck in a colliding position");
01111         }
01112         catch (object_manipulator::MissingParamException &ex)
01113         {
01114           setStatusLabel("parameters missing; is manipulation pipeline running?");
01115         }
01116         catch (...)
01117         {
01118           setStatusLabel("an unknown error has occured, please call helpdesk");
01119         }
01120       }
01121       else
01122       {
01123         setStatusLabel("moving arm to front open-loop");
01124         try
01125         {
01126           mech_interface_.attemptTrajectory(arm_name, getFrontTrajectory(arm_name), false, 3.0);
01127           setStatusLabel("arm moved to side");
01128         }
01129         catch (object_manipulator::ServiceNotFoundException &ex)
01130         {
01131           setStatusLabel("a needed service or action server was not found");
01132         }
01133         catch (object_manipulator::MissingParamException &ex)
01134         {
01135           setStatusLabel("parameters missing; is manipulation pipeline running?");
01136         }
01137         catch (object_manipulator::MechanismException &ex)
01138         {
01139           setStatusLabel("an error has occured, please call helpdesk");
01140         }
01141         catch (...)
01142         {
01143           setStatusLabel("an unknown error has occured, please call helpdesk");
01144         }
01145       }
01146     }
01147     break;
01148   case 2:
01149     //open gripper
01150     {
01151       setStatusLabel("opening gripper");
01152       object_manipulation_msgs::Grasp grasp;      
01153       try
01154       {
01155         mech_interface_.handPostureGraspAction(arm_name, grasp, 
01156                                                object_manipulation_msgs::GraspHandPostureExecutionGoal::RELEASE);
01157         setStatusLabel("gripper opened");
01158       }
01159       catch (object_manipulator::ServiceNotFoundException &ex)
01160       {
01161         setStatusLabel("a needed service or action server was not found");
01162       }
01163       catch (object_manipulator::GraspException &ex) 
01164       {
01165         setStatusLabel("gripper opening failed");
01166       } 
01167     }
01168     break;
01169   case 3:
01170     //close gripper
01171     {
01172       setStatusLabel("closing gripper");
01173       object_manipulation_msgs::Grasp grasp;
01174       
01175       grasp.grasp_posture.position.push_back(0.0);
01176       grasp.grasp_posture.effort.push_back(10000);
01177       try
01178       {
01179         mech_interface_.handPostureGraspAction(arm_name, grasp, 
01180                                                object_manipulation_msgs::GraspHandPostureExecutionGoal::GRASP);
01181         setStatusLabel("Gripper closed");
01182       }
01183       catch (object_manipulator::ServiceNotFoundException &ex)
01184       {
01185         setStatusLabel("a needed service or action server was not found");
01186       }
01187       catch (object_manipulator::MechanismException &ex) 
01188       {
01189         setStatusLabel("Gripper closing failed");
01190       }
01191     }
01192     break;
01193   }
01194 }
01195 
01196 void InteractiveManipulationFrame::lookAtTable()
01197 {
01198   boost::mutex::scoped_lock lock(mutex_);
01199   geometry_msgs::PointStamped target;
01200   target.point.x = 1;
01201   target.point.y = 0;
01202   target.point.z = 0;
01203   target.header.frame_id = "base_link";
01204   setStatusLabel( "moving head" );
01205   try
01206   {
01207     if ( !mech_interface_.pointHeadAction( target, "/narrow_stereo_optical_frame" ) ) 
01208     {
01209       setStatusLabel( "head movement failed");
01210     }
01211     else
01212     {
01213       setStatusLabel( "head movement completed");
01214     }
01215   }
01216   catch (object_manipulator::ServiceNotFoundException &ex)
01217   {
01218     setStatusLabel("a needed service or action server was not found");
01219   }
01220 }
01221 
01222 void InteractiveManipulationFrame::armGoButtonClicked( wxCommandEvent& event )
01223 {
01224   if (!mutex_.try_lock()) return;
01225   int arm_selection_choice = arm_choice_->GetSelection();
01226   int arm_action_choice = arm_action_choice_->GetSelection();
01227   int arm_planner_choice = arm_planner_choice_->GetSelection();
01228   bool collision = collision_box_->IsChecked();
01229   executing_thread_ = new boost::thread(boost::bind(&InteractiveManipulationFrame::armMotion, 
01230                                                     this, arm_selection_choice, arm_action_choice, 
01231                                                     arm_planner_choice, collision));
01232   mutex_.unlock();
01233 }
01234 
01235 void InteractiveManipulationFrame::lookButtonClicked( wxCommandEvent& event )
01236 {
01237   if (!mutex_.try_lock()) return;
01238   executing_thread_ = new boost::thread(boost::bind(&InteractiveManipulationFrame::lookAtTable, this));
01239   mutex_.unlock();
01240 }
01241 
01242 void InteractiveManipulationFrame::cancelButtonClicked( wxCommandEvent& )
01243 {
01244   interrupt_request_ = true;
01245 }
01246 
01247 void InteractiveManipulationFrame::collisionBoxClicked( wxCommandEvent& event )
01248 {
01249 }
01250 
01251 void InteractiveManipulationFrame::advancedOptionsClicked( wxCommandEvent& )
01252 {
01253   AdvancedOptionsDialog *dlg = new AdvancedOptionsDialog(this, NULL);
01254   dlg->setOptions(ago_);
01255   dlg->Show();
01256 }
01257 
01258 void InteractiveManipulationFrame::modelObject(DialogOptions options)
01259 {
01260   boost::mutex::scoped_lock lock(mutex_);
01261   std::string arm_name;
01262   if (options.arm_selection_ == 0) arm_name = "right_arm";
01263   else arm_name = "left_arm";
01264 
01265   //wait for collision map services
01266   setStatusLabel("waiting for collision map services...");
01267   ros::Time start_time = ros::Time::now();
01268   while (!collision_map_interface_.connectionsEstablished(ros::Duration(1.0))) 
01269   {
01270     if (!priv_nh_.ok() || interrupt_request_ || ros::Time::now() - start_time >= ros::Duration(5.0)) 
01271     {
01272       setStatusLabel("collision map services not found");
01273       return;
01274     }
01275   }
01276 
01277   //wait for action server
01278   while ( !create_model_client_->waitForServer(ros::Duration(1.0)) && priv_nh_.ok() && !interrupt_request_) 
01279   {
01280     setStatusLabel("waiting for model object in hand action server...");
01281   }
01282   if (!priv_nh_.ok() || interrupt_request_) return;
01283   ROS_INFO("Modeling object in %s", arm_name.c_str());
01284 
01285   //create and send the goal
01286   pr2_create_object_model::ModelObjectInHandGoal goal;
01287   goal.arm_name = arm_name;
01288   goal.keep_level = 0;
01289   goal.move_arm = 1;
01290   goal.add_to_collision_map = 1;
01291   create_model_client_->sendGoal(goal);
01292   
01293   while (!create_model_client_->waitForResult(ros::Duration(1.0)) && priv_nh_.ok() && !interrupt_request_)
01294   {
01295     setStatusLabel("calling model object in hand action...");
01296   }
01297   if (!priv_nh_.ok() || interrupt_request_) return;
01298 
01299   if (create_model_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
01300   {
01301     setStatusLabel("modeling object in hand completed");
01302   }
01303   else
01304   {
01305     setStatusLabel("modeling object in hand failed");
01306   }
01307 
01308 }
01309 
01310 void InteractiveManipulationFrame::modelObjectClicked( wxCommandEvent& )
01311 {
01312   if (!mutex_.try_lock()) return;
01313   interrupt_request_ = false;
01314   executing_thread_ = new boost::thread(boost::bind(&InteractiveManipulationFrame::modelObject, 
01315                                                     this, getDialogOptions()));
01316   mutex_.unlock();
01317 }
01318 
01319 void InteractiveManipulationFrame::moveGripper(int arm_selection_choice, double value)
01320 {
01321   boost::mutex::scoped_lock lock(mutex_);
01322   bool result;
01323   if (arm_selection_choice == 0) result = r_gripper_controller_.commandGripperValue(value);
01324   else result = l_gripper_controller_.commandGripperValue(value);
01325   if (!result)
01326   {
01327     setStatusLabel("failed to command gripper position");
01328   }
01329   else
01330   {
01331     setStatusLabel("sent gripper position command");
01332   }
01333 }
01334 
01335 void InteractiveManipulationFrame::gripperSliderScrollChanged( wxScrollEvent& )
01336 {
01337   if (!mutex_.try_lock()) return;
01338   int position = gripper_slider_->GetValue();
01339   double value = GripperController::GRIPPER_CLOSED + 
01340     (GripperController::GRIPPER_OPEN - GripperController::GRIPPER_CLOSED) * (double)position/100.0;
01341   int arm_selection_choice = arm_choice_->GetSelection();
01342   executing_thread_ = new boost::thread(boost::bind(&InteractiveManipulationFrame::moveGripper, 
01343                                                     this, arm_selection_choice, value));
01344   mutex_.unlock();
01345 
01346 }
01347 
01348 }
interactive_manipulation_frame.cpp
