interactive_manipulation_backend.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2009, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include "pr2_interactive_manipulation/interactive_manipulation_backend.h"

#include <object_manipulation_msgs/Grasp.h>
#include <object_manipulation_msgs/GraspPlanning.h>
#include <object_manipulation_msgs/PlacePlanning.h>
#include <object_manipulation_msgs/ClusterBoundingBox.h>

#include <object_manipulator/tools/vector_tools.h>

#include <tf/transform_datatypes.h>

#include <household_objects_database_msgs/GetModelDescription.h>

#include <sensor_msgs/point_cloud_conversion.h>

#include "pr2_interactive_manipulation/arm_locations.h"

using object_manipulation_msgs::ManipulationResult;

namespace pr2_interactive_manipulation {

geometry_msgs::Pose translatePose(geometry_msgs::Pose pose_in, btVector3 translation)
{
  tf::Transform trans_in;
  tf::poseMsgToTF(pose_in, trans_in);
  tf::StampedTransform translate_trans;
  translate_trans.setIdentity();
  translate_trans.setOrigin(translation);
  tf::Transform trans_out = trans_in * translate_trans;
  geometry_msgs::Pose pose_out;
  tf::poseTFToMsg(trans_out, pose_out);
  return pose_out;
}

geometry_msgs::Pose preTranslatePose(geometry_msgs::Pose pose_in, btVector3 translation)
{
  tf::Transform trans_in;
  tf::poseMsgToTF(pose_in, trans_in);
  tf::StampedTransform translate_trans;
  translate_trans.setIdentity();
  translate_trans.setOrigin(translation);
  tf::Transform trans_out = translate_trans * trans_in;
  geometry_msgs::Pose pose_out;
  tf::poseTFToMsg(trans_out, pose_out);
  return pose_out;
}

std::string getGraspResultInfo(object_manipulation_msgs::GraspResult result)
{
  switch (result.result_code)
  {
  case object_manipulation_msgs::GraspResult::SUCCESS: return ("grasp success");
  case object_manipulation_msgs::GraspResult::GRASP_OUT_OF_REACH: return ("grasp out of reach");
  case object_manipulation_msgs::GraspResult::GRASP_IN_COLLISION: return ("grasp in collision");
  case object_manipulation_msgs::GraspResult::GRASP_UNFEASIBLE: return ("grasp unfeasible");
  case object_manipulation_msgs::GraspResult::PREGRASP_OUT_OF_REACH: return ("pregrasp out of reach");
  case object_manipulation_msgs::GraspResult::PREGRASP_IN_COLLISION: return ("pregrasp in collision");
  case object_manipulation_msgs::GraspResult::PREGRASP_UNFEASIBLE: return ("pregrasp unfeasible");
  case object_manipulation_msgs::GraspResult::LIFT_OUT_OF_REACH: return ("lift position out of reach");
  case object_manipulation_msgs::GraspResult::LIFT_IN_COLLISION: return ("lift position in collision");
  case object_manipulation_msgs::GraspResult::LIFT_UNFEASIBLE: return ("lift position unfeasible");
  case object_manipulation_msgs::GraspResult::MOVE_ARM_FAILED: return ("arm movement failed");
  case object_manipulation_msgs::GraspResult::GRASP_FAILED: return ("grasp failed");
  case object_manipulation_msgs::GraspResult::LIFT_FAILED: return ("lift failed");
  case object_manipulation_msgs::GraspResult::RETREAT_FAILED: return ("retreat failed");
  default: return("unknown result code returned");
  }
}

std::string getPlaceLocationResultInfo(object_manipulation_msgs::PlaceLocationResult result)
{
  switch (result.result_code)
  {
  case object_manipulation_msgs::PlaceLocationResult::SUCCESS: return ("place success");
  case object_manipulation_msgs::PlaceLocationResult::PLACE_OUT_OF_REACH: return ("place location out of reach");
  case object_manipulation_msgs::PlaceLocationResult::PLACE_IN_COLLISION: return ("place location in collision");
  case object_manipulation_msgs::PlaceLocationResult::PLACE_UNFEASIBLE: return ("place in location unfeasible");
  case object_manipulation_msgs::PlaceLocationResult::PREPLACE_OUT_OF_REACH: return ("preplace location out of reach");
  case object_manipulation_msgs::PlaceLocationResult::PREPLACE_IN_COLLISION: return ("preplace location in collision");
  case object_manipulation_msgs::PlaceLocationResult::PREPLACE_UNFEASIBLE: return ("preplace location unfeasible");
  case object_manipulation_msgs::PlaceLocationResult::RETREAT_OUT_OF_REACH: return ("retreat location out of reach");
  case object_manipulation_msgs::PlaceLocationResult::RETREAT_IN_COLLISION: return ("retreat location in collision");
  case object_manipulation_msgs::PlaceLocationResult::RETREAT_UNFEASIBLE: return ("retreat location unfeasible");
  case object_manipulation_msgs::PlaceLocationResult::MOVE_ARM_FAILED: return ("arm movement failed");
  case object_manipulation_msgs::PlaceLocationResult::PLACE_FAILED: return ("place failed");
  case object_manipulation_msgs::PlaceLocationResult::RETREAT_FAILED: return ("retreat failed");
  default: return("unknown result code returned");
  }
}

InteractiveManipulationBackend::InteractiveManipulationBackend() : 
  root_nh_(""),
  priv_nh_("~")
{
  grasp_planning_service_name_ = "/grasp_planning_gripper_click";
  grasp_planning_srv_ = root_nh_.serviceClient<object_manipulation_msgs::GraspPlanning>(grasp_planning_service_name_, 
                                                                                        false);
  place_planning_service_name_ = "/place_planning_gripper_click";
  place_planning_srv_ = root_nh_.serviceClient<object_manipulation_msgs::PlacePlanning>(place_planning_service_name_, 
                                                                                        false);
  pickup_action_name_ = "/object_manipulator/object_manipulator_pickup";
  pickup_client_ = new actionlib::SimpleActionClient<object_manipulation_msgs::PickupAction>(pickup_action_name_, true);
  
  place_action_name_ = "/object_manipulator/object_manipulator_place";
  place_client_ = new actionlib::SimpleActionClient<object_manipulation_msgs::PlaceAction>(place_action_name_, true);
  
  std::string graspable_objects_topic_name = "interactive_object_recognition_result";
  graspable_objects_sub_ = root_nh_.subscribe(graspable_objects_topic_name, 10, 
                                              &InteractiveManipulationBackend::graspableObjectsCallback, this);

  std::string get_model_description_name = "/objects_database_node/get_model_description";
  get_model_description_srv_ = root_nh_.serviceClient<household_objects_database_msgs::GetModelDescription>
    (get_model_description_name, true);

  create_model_action_name_ = "/create_object_model_server/model_object_in_hand_action";
  create_model_client_ = 
    new actionlib::SimpleActionClient<pr2_create_object_model::ModelObjectInHandAction>(create_model_action_name_, true);

  action_name_ = "imgui_action";  
  action_server_ = new actionlib::SimpleActionServer<IMGUIAction>(root_nh_, action_name_, 
                                               boost::bind(&InteractiveManipulationBackend::actionCallback, this, _1),
                                                                 false);
  action_server_->start();
  
  ROS_INFO("IM Backend ready");
}

InteractiveManipulationBackend::~InteractiveManipulationBackend()
{
  delete action_server_;
}

void InteractiveManipulationBackend::actionCallback(const IMGUIGoalConstPtr &goal)
{
  ROS_DEBUG("IM Backend received goal with command %d", goal->command.command);
  IMGUIResult im_result;
  switch (goal->command.command)
  {
  case IMGUICommand::PICKUP:
    if (goal->options.grasp_selection == 0)
    {
      im_result.result.value = pickupObject(goal->options);
    }
    else
    {
      im_result.result.value = pickupObject(goal->options, goal->options.selected_object);
    }
    break;
  case IMGUICommand::PLACE:
    im_result.result.value = placeObject(goal->options);
    break;
  case IMGUICommand::TAKE_MAP:
    takeMap();
    im_result.result.value = object_manipulation_msgs::ManipulationResult::SUCCESS;
    break;
  case IMGUICommand::RESET:
    collisionReset(goal->options.reset_choice);
    im_result.result.value = object_manipulation_msgs::ManipulationResult::SUCCESS;
    break;
  case IMGUICommand::MOVE_ARM:
    armMotion(goal->options.arm_selection, goal->options.arm_action_choice, 
              goal->options.arm_planner_choice, goal->options.collision_checked, im_result.result);
    break;
  case IMGUICommand::LOOK_AT_TABLE:
    lookAtTable();
    im_result.result.value = object_manipulation_msgs::ManipulationResult::SUCCESS;
    break;
  case IMGUICommand::MOVE_GRIPPER:
    moveGripper(goal->options);
    im_result.result.value = object_manipulation_msgs::ManipulationResult::SUCCESS;
    break;
  case IMGUICommand::MODEL_OBJECT:
    im_result.result.value = modelObject(goal->options);
    break;
  default:
    ROS_ERROR("IM Backend could not understand command: %d", goal->command.command);
    setStatusLabel("Command not yet implemented");
    im_result.result.value = object_manipulation_msgs::ManipulationResult::ERROR;
  }
  action_server_->setSucceeded(im_result);
  ROS_DEBUG("IM Backend: goal finished");
}

void InteractiveManipulationBackend::graspableObjectsCallback(
                                          const pr2_interactive_object_detection::GraspableObjectListConstPtr &objects)
{
  boost::mutex::scoped_lock lock(received_objects_mutex_);
  received_objects_ = objects;
}

void InteractiveManipulationBackend::setStatusLabel(std::string text)
{
  if (!action_server_->isActive()) ROS_WARN("IM Backend trying to give feedback, but no goal is active. Text: %s", 
                                            text.c_str());
  IMGUIFeedback feedback;
  feedback.status = text;
  action_server_->publishFeedback(feedback);
  ROS_DEBUG_STREAM("IM backend feedback: " << text);
  ROS_INFO_STREAM( text );
}

bool InteractiveManipulationBackend::interruptRequested()
{
  return ( !priv_nh_.ok() || action_server_->isPreemptRequested() );
}

void populateGraspOptions(object_manipulation_msgs::PickupGoal &pickup_goal, 
                          std::string arm_name, const IMGUIOptions &options)
{
  pickup_goal.arm_name = arm_name;
  pickup_goal.desired_approach_distance = 0.1;
  pickup_goal.min_approach_distance = 0.05;
  if ( options.adv_options.lift_vertically )
  {
    if (arm_name == "right_arm") pickup_goal.lift.direction.header.frame_id = "r_wrist_roll_link";
    else pickup_goal.lift.direction.header.frame_id = "l_wrist_roll_link";
    pickup_goal.lift.direction.vector.x = -1;
    pickup_goal.lift.direction.vector.y = 0;
    pickup_goal.lift.direction.vector.z = 0;
  }
  else
  {
    pickup_goal.lift.direction.header.frame_id = "base_link";
    pickup_goal.lift.direction.vector.x = 0;
    pickup_goal.lift.direction.vector.y = 0;
    pickup_goal.lift.direction.vector.z = 1;
  }
  pickup_goal.lift.desired_distance = options.adv_options.lift_steps * 0.01;
  pickup_goal.lift.min_distance = pickup_goal.lift.desired_distance / 2.0;
  pickup_goal.use_reactive_lift = options.adv_options.reactive_force;
  pickup_goal.use_reactive_execution = options.adv_options.reactive_grasping;
}

bool InteractiveManipulationBackend::processCollisionMapForPickup(const IMGUIOptions &options,
                                                                  object_manipulation_msgs::PickupGoal &goal)
{
  if (options.collision_checked)
  {
    // make sure collision services are available
    setStatusLabel("waiting for collision map services...");
    ros::Time start_time = ros::Time::now();
    while (!collision_map_interface_.connectionsEstablished(ros::Duration(1.0))) 
    {
      if (interruptRequested() || ros::Time::now() - start_time >= ros::Duration(5.0))
      {
        setStatusLabel("collision map services not found");
        return false;
      }
    }
    if ( !goal.target.cluster.points.empty() )
    {
      //if we have a point cloud, we will use its bounding box as the collision object
      setStatusLabel("creating collision object from point cloud...");
      object_manipulation_msgs::ClusterBoundingBox bbox;
      collision_map_interface_.getClusterBoundingBox(goal.target.cluster, bbox.pose_stamped, bbox.dimensions);
      collision_map_interface_.processCollisionGeometryForBoundingBox(bbox, goal.collision_object_name);
    }
    else if (!goal.target.potential_models.empty())
    {
      //if we have a potential recognized model, we will use its mesh
      setStatusLabel("creating collision object from mesh...");
      try
      {
        collision_map_interface_.processCollisionGeometryForObject(goal.target.potential_models[0],
                                                                   goal.collision_object_name);
      }
      catch (tabletop_collision_map_processing::CollisionMapException &ex)
      {
        setStatusLabel("failed to add mesh to collision map");
        return false;
      }
    }
    else
    {
      //if not, then the gripper can hit anything during approach and lift
      goal.collision_support_surface_name = motion_planning_msgs::CollisionOperation::COLLISION_SET_ALL;
      goal.allow_gripper_support_collision = true;
    }
    //also set 0 padding on fingertip links
    goal.additional_link_padding = object_manipulator::concat( 
                                            object_manipulator::MechanismInterface::gripperPadding("left_arm", 0.0),
                                            object_manipulator::MechanismInterface::gripperPadding("right_arm", 0.0) );
  }
  else
  {
    //if collisions are disabled, anything can hit anything
    motion_planning_msgs::CollisionOperation coll;
    coll.object1 = motion_planning_msgs::CollisionOperation::COLLISION_SET_ALL;
    coll.object2 = motion_planning_msgs::CollisionOperation::COLLISION_SET_ALL;
    coll.operation = motion_planning_msgs::CollisionOperation::DISABLE;
    goal.additional_collision_operations.collision_operations.push_back(coll);
  }
  return true;
}

int InteractiveManipulationBackend::pickupObject(const IMGUIOptions &options, 
                                                 object_manipulation_msgs::GraspableObject object)
{
  std::string arm_name;
  if (options.arm_selection == 0) arm_name = "right_arm";
  else arm_name = "left_arm";

  ROS_INFO("Graspable object has %d points and %d database models",
           (int)object.cluster.points.size(), (int)object.potential_models.size());

  //convert the graspable object to base link frame. this also sets its reference_frame_id to that
  //so any grasps returned will be in that frame
  try
  {
    mech_interface_.convertGraspableObjectComponentsToFrame(object, "base_link");
  }
  catch(object_manipulator::MechanismException &ex)
  {
    ROS_ERROR_STREAM("Conversion error: " << ex.what());
    setStatusLabel("failed to convert object to desired frame");
    return object_manipulation_msgs::ManipulationResult::ERROR;
  }

  //prepare pickup goal
  object_manipulation_msgs::PickupGoal pickup_goal;

  //if the object is empty, we'll have to ask the user for help on desired grasps
  if ( object.cluster.points.empty() && object.potential_models.empty() )
  {
    int success = callGripperClickPickup( arm_name, pickup_goal.desired_grasps );
    if ( success != object_manipulation_msgs::ManipulationResult::SUCCESS ) return success;
    pickup_goal.target.reference_frame_id = "base_link";
  }
  else
  {
    pickup_goal.target = object;
  }

  //take care of collision names and operations
  if (!processCollisionMapForPickup(options, pickup_goal)) return object_manipulation_msgs::ManipulationResult::ERROR;

  //populate the rest of the grasp options
  populateGraspOptions(pickup_goal, arm_name, options);

  //wait for pickup server to come up
  setStatusLabel("waiting for pickup action server...");
  while ( !pickup_client_->waitForServer(ros::Duration(1.0)) && !interruptRequested() ) {}
  if (interruptRequested())
  {
    setStatusLabel("Pickup canceled");
    pickup_client_->cancelGoal();
    return object_manipulation_msgs::ManipulationResult::CANCELLED;
  }

  //call pickup and wait for operation to finish
  pickup_client_->sendGoal(pickup_goal);
  while (!pickup_client_->waitForResult(ros::Duration(1.0)) && 
         pickup_client_->isServerConnected() && !interruptRequested())
  {
    ROS_DEBUG("Waiting for pickup result..");
  }

  //check for cancel or shutdown
  if (interruptRequested())
  {
    setStatusLabel("Pickup canceled.");
    pickup_client_->cancelGoal();
    return object_manipulation_msgs::ManipulationResult::CANCELLED;
  }

  //get back pickup result
  object_manipulation_msgs::PickupResult pickup_result = *(pickup_client_->getResult());
  if (pickup_client_->getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
  {
    if (pickup_result.attempted_grasp_results.empty())
    {
      setStatusLabel("Pickup failed.");
      return object_manipulation_msgs::ManipulationResult::ERROR;
    }
    else
    {
      setStatusLabel(getGraspResultInfo(pickup_result.attempted_grasp_results[0]));
    }
    return object_manipulation_msgs::ManipulationResult::UNFEASIBLE;
  }
  
  //compute a gripper location for the grasp relative to a coordinate frame aligned with base_link
  //and with the origin in front of the fingers of the robot
  geometry_msgs::Pose grasp_pose;
  //keep the orientation of the original grasp pose, transformed to base_link
  grasp_pose.orientation = pickup_result.grasp.grasp_pose.orientation;
  //move back to get a location for the wrist roll link
  grasp_pose = translatePose(grasp_pose, btVector3(-0.20, 0.0, 0.0));

  //save object and grasp info for later
  getGraspInfo(arm_name)->object_ = object;
  getGraspInfo(arm_name)->object_collision_name_ = pickup_goal.collision_object_name;
  getGraspInfo(arm_name)->grasp_.grasp_pose = grasp_pose;
  getGraspInfo(arm_name)->object_orientation_ = GraspInfo::identityQuaternion();

  setStatusLabel("grasp completed");
  return object_manipulation_msgs::ManipulationResult::SUCCESS;
}

void populatePlaceOptions(object_manipulation_msgs::PlaceGoal &place_goal, 
                          std::string arm_name, const IMGUIOptions &options)
{
  place_goal.arm_name = arm_name;
  place_goal.place_padding = 0.0;
  place_goal.desired_retreat_distance = 0.1;
  place_goal.min_retreat_distance = 0.05;
  if (options.adv_options.lift_vertically == 0)
  {
    place_goal.approach.direction.header.frame_id = "base_link";
    place_goal.approach.direction.vector.x = 0;
    place_goal.approach.direction.vector.y = 0;
    place_goal.approach.direction.vector.z = -1;
  }
  else
  {
    if (arm_name == "right_arm") place_goal.approach.direction.header.frame_id = "r_wrist_roll_link";
    else place_goal.approach.direction.header.frame_id = "l_wrist_roll_link";
    place_goal.approach.direction.vector.x = 1;
    place_goal.approach.direction.vector.y = 0;
    place_goal.approach.direction.vector.z = 0;
  }
  place_goal.approach.desired_distance = options.adv_options.lift_steps * 0.01;
  place_goal.approach.min_distance = place_goal.approach.desired_distance * 0.5;
  place_goal.use_reactive_place = options.adv_options.reactive_place;
}

int InteractiveManipulationBackend::placeObject(const IMGUIOptions &options)
{
  std::string arm_name;
  if (options.arm_selection == 0) arm_name = "right_arm";
  else arm_name = "left_arm";
  
  //call place planning through gripper click service  
  while ( !ros::service::waitForService(place_planning_service_name_, ros::Duration(2.0)) && 
          !interruptRequested())
  {
    setStatusLabel("waiting for place planning service...");
  }
  if (interruptRequested())
  {
    setStatusLabel("place planning service was not found");
    return object_manipulation_msgs::ManipulationResult::ERROR;
  }
  setStatusLabel("calling place planning service...");
  object_manipulation_msgs::PlacePlanning place_planning;
  //fill in info from the grasp
  place_planning.request.grasp_pose = getGraspInfo(arm_name)->grasp_.grasp_pose;
  place_planning.request.target = getGraspInfo(arm_name)->object_;
  place_planning.request.default_orientation = getGraspInfo(arm_name)->object_orientation_;
  if (!place_planning_srv_.call(place_planning))
  {
    setStatusLabel("call to place planning service failed");
    return object_manipulation_msgs::ManipulationResult::ERROR;
  }
  if (place_planning.response.error_code.value != place_planning.response.error_code.SUCCESS)
  {
    setStatusLabel("place planning returned an error code");
    return object_manipulation_msgs::ManipulationResult::CANCELLED;
  }
  if (place_planning.response.place_locations.empty())
  {
    setStatusLabel("place planning returned no locations");
    return object_manipulation_msgs::ManipulationResult::ERROR;
  }
    
  if (interruptRequested()) return object_manipulation_msgs::ManipulationResult::CANCELLED;

  //prepare place goal
  object_manipulation_msgs::PlaceGoal place_goal;

  //take care of collision map
  if (!options.collision_checked)
  {
    //collisions off, disable anything against anything
    motion_planning_msgs::CollisionOperation coll;
    coll.object1 = motion_planning_msgs::CollisionOperation::COLLISION_SET_ALL;
    coll.object2 = motion_planning_msgs::CollisionOperation::COLLISION_SET_ALL;
    coll.operation = motion_planning_msgs::CollisionOperation::DISABLE;
    place_goal.additional_collision_operations.collision_operations.push_back(coll);
  }
  else
  {
    //collisions on, 0 link padding on fingertips
    place_goal.additional_link_padding = 
      object_manipulator::concat( object_manipulator::MechanismInterface::gripperPadding("left_arm", 0.0),
                                  object_manipulator::MechanismInterface::gripperPadding("right_arm", 0.0) );
    //anything can be hit during move from pre-place to place
    place_goal.collision_support_surface_name = motion_planning_msgs::CollisionOperation::COLLISION_SET_ALL;
    place_goal.allow_gripper_support_collision = true;
    //collision name of the currently grasped object
    place_goal.collision_object_name = getGraspInfo(arm_name)->object_collision_name_;
  }

  //set all the other place options
  populatePlaceOptions(place_goal, arm_name, options);
  place_goal.grasp.grasp_pose.orientation.w = 1;
  place_goal.place_locations.push_back(place_planning.response.place_locations[0]);

  //debug output
  geometry_msgs::Pose grasp_pose = place_goal.grasp.grasp_pose;
  ROS_INFO("Placing object %s on support %s using grasp: %f %f %f; %f %f %f %f", 
           place_goal.collision_object_name.c_str(), place_goal.collision_support_surface_name.c_str(),
           grasp_pose.position.x, grasp_pose.position.y, grasp_pose.position.z,
           grasp_pose.orientation.x, grasp_pose.orientation.y, grasp_pose.orientation.z, grasp_pose.orientation.w);

  //make sure place action server is there
  setStatusLabel("waiting for place action server...");
  while ( !place_client_->waitForServer(ros::Duration(1.0)) && !interruptRequested())
  {
    setStatusLabel("waiting for place action server...");
  }
  if (interruptRequested())
  {
    setStatusLabel("place canceled");
    place_client_->cancelGoal();
    return object_manipulation_msgs::ManipulationResult::CANCELLED;
  }

  //send the goal
  place_client_->sendGoal(place_goal);
  while (!place_client_->waitForResult(ros::Duration(1.0)) && !interruptRequested())
  {
    setStatusLabel("calling place action...");
  }
  if (interruptRequested())
  {
    setStatusLabel("place canceled");
    place_client_->cancelGoal();
    return object_manipulation_msgs::ManipulationResult::CANCELLED;
  }
      
  //get the result
  object_manipulation_msgs::PlaceResult result = *(place_client_->getResult());
  if (place_client_->getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
  {
    if (result.attempted_location_results.empty())
    {
      setStatusLabel("a serious error has occured, please call the WG helpdesk");
      return object_manipulation_msgs::ManipulationResult::ERROR;
    }
    else
    {
      setStatusLabel(getPlaceLocationResultInfo(result.attempted_location_results[0]));
    }
    return object_manipulation_msgs::ManipulationResult::UNFEASIBLE;
  }

  setStatusLabel("place completed");
  return object_manipulation_msgs::ManipulationResult::SUCCESS;
}

void InteractiveManipulationBackend::takeMap()
{
  setStatusLabel("waiting for collision map services...");
  ros::Time start_time = ros::Time::now();
  while (!collision_map_interface_.connectionsEstablished(ros::Duration(1.0))) 
  {
    if (interruptRequested() || ros::Time::now() - start_time >= ros::Duration(5.0))
    {
      setStatusLabel("collision map services not found");
      return;
    }
  }  
  setStatusLabel("acquiring new collision map");
  try
  {
    collision_map_interface_.takeStaticMap();
    setStatusLabel("new collision map acquired");
  }
  catch (tabletop_collision_map_processing::CollisionMapException ex)
  {
    setStatusLabel("collision map acquisition failed");
  }
  //we are also resetting the collision objects, which also triggers a rebroadcast
  //of the new map we've just taken
  try 
  {
    collision_map_interface_.resetCollisionModels();
  } 
  catch (tabletop_collision_map_processing::CollisionMapException &ex) 
  {
    setStatusLabel("failed to reset collision models after taking new map");
  }    

}

void InteractiveManipulationBackend::collisionReset(int reset_choice)
{
  try
  {
    switch (reset_choice)
    {
    case 0:
      //reset collision objects
      collision_map_interface_.resetCollisionModels();
      setStatusLabel("collision models reset");
      break;
    case 1:
      //reset attached objects
      collision_map_interface_.resetAttachedModels();
      setStatusLabel("attached models reset");
      break;
    case 2:
      //reset map
      collision_map_interface_.resetStaticMap();
      setStatusLabel("collision map reset");
      break;
    }
  }
  catch (tabletop_collision_map_processing::CollisionMapException &ex) 
  {
    setStatusLabel("failed to perform requested collision operation");
  }    
}

void InteractiveManipulationBackend::armMotion(int arm_selection_choice, int arm_action_choice, 
                                               int arm_planner_choice, bool collision,
                                               ManipulationResult &result)
{
  std::string arm_name;
  if ( arm_selection_choice == 0) arm_name = "right_arm";
  else arm_name = "left_arm";

  try
  {
    ROS_DEBUG("Attempting arm motion");
    motion_planning_msgs::OrderedCollisionOperations ord;
    if (!collision)
    {
      motion_planning_msgs::CollisionOperation coll;
      coll.object1 = motion_planning_msgs::CollisionOperation::COLLISION_SET_ALL;
      coll.object2 = motion_planning_msgs::CollisionOperation::COLLISION_SET_ALL;
      coll.operation = motion_planning_msgs::CollisionOperation::DISABLE;
      ord.collision_operations.push_back(coll);
    }
    std::vector<motion_planning_msgs::LinkPadding> pad;
    if (collision)
    {
      pad = object_manipulator::concat( object_manipulator::MechanismInterface::gripperPadding("left_arm", 0.0),
                                        object_manipulator::MechanismInterface::gripperPadding("right_arm", 0.0) );
    }

    switch(arm_action_choice)
    {
    case 0: //side
      {
        if (arm_planner_choice == 0)
        {
          setStatusLabel("moving arm to side using motion planner");
          if ( !mech_interface_.attemptMoveArmToGoal(arm_name, getSidePosition(arm_name), ord, pad) )
          {
            result.value = result.FAILED;
            setStatusLabel("failed to move arm to side (possible collisions)");
          }
          else
          {
            setStatusLabel("arm moved to side");
            result.value = result.SUCCESS;
          }
        }
        else
        {
          setStatusLabel("moving arm to side open-loop");
          mech_interface_.attemptTrajectory(arm_name, getSideTrajectory(arm_name), false, 3.0);
          setStatusLabel("arm moved to side");
          result.value = result.SUCCESS;
        }
      }
      break;
    case 1: //front
      {
        if (arm_planner_choice == 0)
        {
          setStatusLabel("moving arm to front using motion planner");
          if ( !mech_interface_.attemptMoveArmToGoal(arm_name, getFrontPosition(arm_name), ord, pad) )
          {
            setStatusLabel("failed to move arm to front (possible collisions)");
            result.value = result.FAILED;
          }
          else
          {
            setStatusLabel("arm moved to front");
            result.value = result.SUCCESS;
          }
        }
        else
        {
          setStatusLabel("moving arm to front open-loop");
          mech_interface_.attemptTrajectory(arm_name, getFrontTrajectory(arm_name), false, 3.0);
          setStatusLabel("arm moved to front");
          result.value = result.SUCCESS;
        }
      }
      break;
    case 2: //side handoff
      {
        if (arm_planner_choice == 0)
        {
          setStatusLabel("moving arm to handoff using motion planner");
          if ( !mech_interface_.attemptMoveArmToGoal(arm_name, getSideHandoffPosition(arm_name), ord, pad) )
          {
            setStatusLabel("failed to move arm to handoff (possible collisions)");
            result.value = result.FAILED;
          }
          else
          {
            setStatusLabel("arm moved to handoff");
            result.value = result.SUCCESS;
          }
        }
        else
        {
          setStatusLabel("moving arm to handoff open-loop");
          mech_interface_.attemptTrajectory(arm_name, getSideHandoffTrajectory(arm_name), false, 3.0);
          setStatusLabel("arm moved to handoff");
          result.value = result.SUCCESS;
        }
      }
    default:
      setStatusLabel("unknown operation requested");
    }
  }
  catch (object_manipulator::ServiceNotFoundException &ex)
  {
    setStatusLabel("a needed service or action server was not found");
    result.value = result.ERROR;
  }
  catch (object_manipulator::MoveArmStuckException &ex)
  {
    setStatusLabel("arm is stuck in a colliding position");
    result.value = result.ERROR;
  }
  catch (object_manipulator::MissingParamException &ex)
  {
    setStatusLabel("parameters missing; is manipulation pipeline running?");
    result.value = result.ERROR;
  }
  catch (object_manipulator::MechanismException &ex)
  {
    setStatusLabel("an error has occured, please call helpdesk");
    result.value = result.ERROR;
  }
  catch (...)
  {
    setStatusLabel("an unknown error has occured, please call helpdesk");
    result.value = result.ERROR;
  }
}

void InteractiveManipulationBackend::lookAtTable()
{
  geometry_msgs::PointStamped target;
  target.point.x = 1;
  target.point.y = 0;
  target.point.z = 0;
  target.header.frame_id = "base_link";
  setStatusLabel( "moving head" );
  try
  {
    if ( !mech_interface_.pointHeadAction( target, "/narrow_stereo_optical_frame" ) ) 
    {
      setStatusLabel( "head movement failed");
    }
    else
    {
      setStatusLabel( "head movement completed");
    }
  }
  catch (object_manipulator::ServiceNotFoundException &ex)
  {
    setStatusLabel("a needed service or action server was not found");
  }
}

void InteractiveManipulationBackend::moveGripper(IMGUIOptions options)
{
  std::string arm_name;
  if (options.arm_selection == 0) arm_name = "right_arm";
  else arm_name = "left_arm";
  double value = GripperController::GRIPPER_CLOSED + 
    (GripperController::GRIPPER_OPEN - GripperController::GRIPPER_CLOSED) * 
    (double)(options.gripper_slider_position)/100.0;
  bool result = gripper_controller_.commandGripperValue(arm_name, value);
  if (!result) setStatusLabel("failed to command gripper position");
  else setStatusLabel("sent gripper position command");
}

int InteractiveManipulationBackend::modelObject(IMGUIOptions options)
{
  std::string arm_name;
  if (options.arm_selection == 0) arm_name = "right_arm";
  else arm_name = "left_arm";

  //wait for collision map services
  setStatusLabel("waiting for collision map services...");
  ros::Time start_time = ros::Time::now();
  while (!collision_map_interface_.connectionsEstablished(ros::Duration(1.0))) 
  {
    if (interruptRequested() || ros::Time::now() - start_time >= ros::Duration(5.0))
    {
      setStatusLabel("collision map services not found");
      return object_manipulation_msgs::ManipulationResult::ERROR;
    }
  }

  //wait for action server
  while ( !create_model_client_->waitForServer(ros::Duration(1.0)) && !interruptRequested())
  {
    setStatusLabel("waiting for model object in hand action server...");
  }
  if (interruptRequested()) return object_manipulation_msgs::ManipulationResult::CANCELLED;
  ROS_INFO("Modeling object in %s", arm_name.c_str());

  //create and send the goal
  pr2_create_object_model::ModelObjectInHandGoal goal;
  goal.arm_name = arm_name;
  goal.keep_level = 0;
  goal.move_arm = 1;
  goal.add_to_collision_map = 1;
  create_model_client_->sendGoal(goal);
  
  while (!create_model_client_->waitForResult(ros::Duration(1.0)) && !interruptRequested())
  {
    setStatusLabel("calling model object in hand action...");
  }
  if (interruptRequested())
  {
    setStatusLabel("object modeling canceled");
    create_model_client_->cancelGoal();
    return object_manipulation_msgs::ManipulationResult::CANCELLED;
  }

  if (create_model_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED && 
      !create_model_client_->getResult()->cluster.data.empty())
  {
    setStatusLabel("modeling object in hand completed");
    ROS_INFO("modeled object with collision name %s",
             create_model_client_->getResult()->collision_name.c_str());
    object_manipulation_msgs::GraspableObject object;
    sensor_msgs::convertPointCloud2ToPointCloud( create_model_client_->getResult()->cluster, object.cluster );
    object.reference_frame_id = create_model_client_->getResult()->cluster.header.frame_id;
    if (object.reference_frame_id != "r_wrist_roll_link" && 
        object.reference_frame_id != "l_wrist_roll_link" )
    {
      ROS_ERROR_STREAM("Object model expected in gripper frame and received in frame " << object.reference_frame_id);
      setStatusLabel("unexpected frame for modeled object");
      return object_manipulation_msgs::ManipulationResult::ERROR;
    }
    
    std::string arm_name;
    if ( options.arm_selection == 0) arm_name = "right_arm";
    else arm_name = "left_arm";
    getGraspInfo(arm_name)->object_ = object;
    //grasp is identity since object is in gripper frame
    getGraspInfo(arm_name)->grasp_.grasp_pose = GraspInfo::identityPose();
    getGraspInfo(arm_name)->object_collision_name_ = create_model_client_->getResult()->collision_name;

    return object_manipulation_msgs::ManipulationResult::SUCCESS;
  }
  else
  {
    setStatusLabel("modeling object in hand failed");
    return object_manipulation_msgs::ManipulationResult::ERROR;
  }
}


int InteractiveManipulationBackend::callGripperClickPickup( std::string arm_name, 
                                                            std::vector<object_manipulation_msgs::Grasp> &grasps )
{
  //call grasp planning through gripper click service
  setStatusLabel("waiting for grasp planning service...");
  while ( !ros::service::waitForService(grasp_planning_service_name_, ros::Duration(2.0)) && !interruptRequested()) {}
  if (interruptRequested())
  {
    setStatusLabel("grasp planning service not found");
    return object_manipulation_msgs::ManipulationResult::ERROR;
  }

  setStatusLabel("calling grasp planning service...");
  object_manipulation_msgs::GraspPlanning grasp_planning;
  grasp_planning.request.target.reference_frame_id = "base_link";
  grasp_planning.request.arm_name = arm_name;
  if (!grasp_planning_srv_.call(grasp_planning))
  {
    setStatusLabel("failed to call grasp planning service");
    return object_manipulation_msgs::ManipulationResult::ERROR;
  }
  if (grasp_planning.response.error_code.value != grasp_planning.response.error_code.SUCCESS)
  {
    setStatusLabel("grasp planning returned an error code");
    return object_manipulation_msgs::ManipulationResult::CANCELLED;
  }
  if (grasp_planning.response.grasps.size() != 1 )
  {
    setStatusLabel("grasp planning returned 0 or more than 1 grasps");
    return object_manipulation_msgs::ManipulationResult::ERROR;
  }
  if (interruptRequested()) return object_manipulation_msgs::ManipulationResult::CANCELLED;

  grasps = grasp_planning.response.grasps;
  return object_manipulation_msgs::ManipulationResult::SUCCESS;
}



}

---

pr2_interactive_manipulation
Author(s): Matei Ciocarlie
autogenerated on Fri Jan 11 09:11:03 2013