$search

pr2_interactive_gripper_pose_action.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2011, 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.
 */

// author: Adam Leeper

// **** 10 ***** 20 ****** 30 ****** 40 ****** 50 ****** 60 ****** 70 ****** 80 ****** 90 ***** 100 ***** 110 ***** 120

#include <ros/ros.h>
#include <math.h>

#include <interactive_markers/interactive_marker_server.h>
#include <interactive_markers/menu_handler.h>

#include <object_manipulator/tools/mechanism_interface.h>
#include <object_manipulator/tools/msg_helpers.h>

#include <boost/thread.hpp>
#include <tf/transform_listener.h>
#include <boost/thread/recursive_mutex.hpp>

#include <actionlib/client/simple_action_client.h>
#include <actionlib/server/simple_action_server.h>
#include <point_cloud_server/StoreCloudAction.h>
#include <object_manipulation_msgs/GraspPlanningAction.h>

#include <pr2_object_manipulation_msgs/TestGripperPoseAction.h>
#include <pr2_object_manipulation_msgs/GetGripperPoseAction.h>
#include <pr2_object_manipulation_msgs/CameraFocus.h>

#include <sensor_msgs/point_cloud_conversion.h>

#include "eigen_conversions/eigen_msg.h"

#include "eigen3/Eigen/Geometry"
#include "pcl/io/pcd_io.h"
#include "pcl/point_types.h"
#include "pcl_ros/transforms.h"

#include <household_objects_database_msgs/GetModelDescription.h>
#include <household_objects_database_msgs/GetModelMesh.h>

#include <interactive_marker_helpers/interactive_marker_helpers.h>

using namespace object_manipulator;
using namespace visualization_msgs;
using namespace interactive_markers;
using namespace pr2_object_manipulation_msgs;
using namespace im_helpers;

typedef pcl::PointXYZ PointT;



class GripperPoseAction
{
protected:

  // ****************** class members *********************

  std::vector<geometry_msgs::PoseStamped> planner_poses_;
  std::vector<PoseState> planner_states_;
  int planner_index_;
  geometry_msgs::PoseStamped button_marker_pose_;

  geometry_msgs::PoseStamped gripper_pose_;
  geometry_msgs::PoseStamped control_offset_;
  float gripper_opening_;
  float gripper_angle_;
  pcl::PointCloud<PointT>::Ptr object_cloud_;
  bool active_;
  bool object_model_;
  bool always_call_planner_;

  int interface_number_;
  int task_number_;
  bool double_menu_;
  bool testing_planned_grasp_;
  int tested_grasp_index_;
  bool testing_current_grasp_;

  PoseState pose_state_;
  ros::NodeHandle nh_;
  ros::NodeHandle pnh_;
  ros::Subscriber sub_seed_;
  ros::ServiceClient get_model_mesh_client_;
  ros::Timer spin_timer_;
  ros::Timer slow_sync_timer_;
  InteractiveMarkerServer server_;
  
  ros::Publisher pub_cloud_;
  ros::Publisher pub_focus_;

  MenuHandler menu_gripper_;
  MenuHandler::EntryHandle accept_handle_;
  MenuHandler::EntryHandle cancel_handle_;
  MenuHandler::EntryHandle focus_handle_;

  tf::TransformListener tfl_;

  object_manipulator::MechanismInterface mechanism_;

  //actionlib::SimpleActionClient<pr2_object_manipulation_msgs::TestGripperPoseAction> test_pose_client_;
  //actionlib::SimpleActionClient<object_manipulation_msgs::GraspPlanningAction> grasp_plan_client_;
  //actionlib::SimpleActionClient<point_cloud_server::StoreCloudAction> cloud_server_client_;

  object_manipulator::ActionWrapper<pr2_object_manipulation_msgs::TestGripperPoseAction> test_pose_client_;
  object_manipulator::ActionWrapper<object_manipulation_msgs::GraspPlanningAction> grasp_plan_client_;
  object_manipulator::ActionWrapper<point_cloud_server::StoreCloudAction> cloud_server_client_;

  std::string get_pose_name_;
  actionlib::SimpleActionServer<pr2_object_manipulation_msgs::GetGripperPoseAction> get_pose_server_;

public:

  GripperPoseAction() :
      planner_index_(0),
      gripper_angle_(0.541),
      object_cloud_(new pcl::PointCloud<PointT>()),
      active_(false),
      object_model_(false),
      pose_state_(UNTESTED),
      nh_("/"),
      pnh_("~"),
      server_("pr2_marker_control", "server 2", false),
      tfl_(nh_),
      test_pose_client_("test_gripper_pose", true),
      grasp_plan_client_("grasp_plan_action", true),
      cloud_server_client_("point_cloud_server_action", true),
      get_pose_name_(ros::this_node::getName()),
      get_pose_server_(nh_, get_pose_name_, false)
  {
    ROS_INFO( "pr2_interactive_gripper_pose_action is starting up." );

    ros::Duration(1.0).sleep();

    pnh_.param<bool>("always_call_planner", always_call_planner_, false);

    nh_.param<int>("interactive_grasping/interface_number", interface_number_, 0);
    if (!interface_number_) ROS_WARN("No interface number specified for grasping study; using default configuration");
    else ROS_INFO("Using interface number %d for grasping study", interface_number_);
    nh_.param<int>("interactive_grasping/task_number", task_number_, 0);
    if (!task_number_) ROS_WARN("No task number specified for grasping study; using default configuration");
    else ROS_INFO("Using task number %d for grasping study", task_number_);
    pnh_.param<bool>("double_menu", double_menu_, false);

    if(interface_number_ == 4) always_call_planner_ = true;
    else if(interface_number_ >= 1) always_call_planner_ = false;

    // Dummy box to avoid empty_server bug...
    geometry_msgs::PoseStamped ps;
    ps.header.frame_id = "/base_link";
    server_.insert(makeButtonBox( "test_box", ps, 0.01, false, false));

    pub_cloud_ = nh_.advertise<sensor_msgs::PointCloud2>("cloud_debug", 1);
    pub_focus_ = nh_.advertise<pr2_object_manipulation_msgs::CameraFocus>("camera_focus", 1);

    get_model_mesh_client_ = nh_.serviceClient<household_objects_database_msgs::GetModelMesh>("objects_database_node/get_model_mesh", false);

    spin_timer_ =  nh_.createTimer(ros::Duration(0.05), boost::bind( &GripperPoseAction::spinOnce, this ) );

    sub_seed_ = nh_.subscribe<geometry_msgs::PoseStamped>("cloud_click_point", 1, boost::bind(&GripperPoseAction::setSeed, this, _1));
    
    // Initialization must happen at the end!
    initMenus();
    //initMarkers();
    
    //register the goal and feeback callbacks
    get_pose_server_.registerGoalCallback(    boost::bind(&GripperPoseAction::goalCB, this));
    get_pose_server_.registerPreemptCallback( boost::bind(&GripperPoseAction::preemptCB, this));

    get_pose_server_.start();
  }

  ~GripperPoseAction()
  {
  }

  bool transformGripperPose(const std::string frame_id = "/base_link")
  {
  /*  try{
      tfl_.waitForTransform(frame_id, gripper_pose_.header.frame_id, gripper_pose_.header.stamp, ros::Duration(3.0));
      tfl_.transformPose(frame_id, gripper_pose_, gripper_pose_);
      return true;
    }
    catch(...){
      ROS_ERROR("TF can't transform!");
      return false;
    }
*/
    return true;
  }

  void updateGripperOpening()
  {
    gripper_opening_ = gripper_angle_ * 0.1714;
  }

  void updateGripperAngle()
  {
    gripper_angle_ = gripper_opening_ * 5.834;
  }

  void setSeed(const geometry_msgs::PoseStampedConstPtr &seed)
  {
    if(!active_) return;
    ROS_DEBUG("Setting seed.");
    geometry_msgs::PoseStamped ps = *seed;
    ROS_DEBUG_STREAM("Input seed was \n" << ps);
    tf::Pose pose;
    tf::poseMsgToTF(ps.pose, pose);
    tf::Quaternion q = pose.getRotation();
    btMatrix3x3 rot(q);
    btMatrix3x3 perm(  0, 0, 1,
                       0, 1, 0,
                      -1, 0, 0);
    (rot*perm).getRotation(q);
    //tf::quaternionTFToMsg(q, ps.pose.orientation);
    pose.setRotation(q);

    if(object_model_) pose = pose*tf::Transform(tf::Quaternion(tf::Vector3(0,1,0), M_PI/2.0), tf::Vector3(0,0,0)).inverse();

    tf::poseTFToMsg(pose, ps.pose);
    //ps.header = seed->header;
    //ROS_INFO_STREAM("Processed seed before wrist offset was \n" << ps);

    gripper_pose_ = toWrist(ps);
    //ROS_INFO_STREAM("Processed seed was \n" << gripper_pose_);
    //transformGripperPose();
    //ROS_DEBUG_STREAM("But after transforming it is \n" << gripper_pose_);
    //ROS_INFO_STREAM("ansforming it is \n" << gripper_pose_);
    //transformGripperPose();

    pose_state_ = UNTESTED;

    testing_planned_grasp_ = false;
    testing_current_grasp_ = true;
    if(always_call_planner_)
    {
      ROS_ERROR("Always call planner is on");
      graspPlanCB();
    }
    else
    {
      initMarkers();
      testPose(gripper_pose_, gripper_opening_);
    }
    //focus the camera on the new pose
    focusCB();
  }

  void setIdle(){
    active_ = false;
    server_.erase("ghosted_gripper");
    server_.erase("gripper_controls");
    server_.erase("object_cloud");
    //server_.erase("grasp_toggle");
    eraseAllGraspMarkers();
    planner_states_.clear();
    planner_poses_.clear();  
  }


  void goalCB()
  {
    active_ = true;
    object_model_ = false;
    planner_index_ = 0;
    ROS_INFO("Ghosted gripper called");
    pr2_object_manipulation_msgs::GetGripperPoseGoal goal = *get_pose_server_.acceptNewGoal();
    object_cloud_.reset( new pcl::PointCloud<PointT>());
    pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
    control_offset_.pose = object_manipulator::msg::createPoseMsg(tf::Pose(tf::Quaternion::getIdentity(), tf::Vector3(0.15,0,0)));

    if( !goal.object.cluster.points.empty() ||
        !goal.object.potential_models.empty() )
    {
      ROS_INFO("Goal object contains %d cluster and %d models.",
               !goal.object.cluster.points.empty(), (int)goal.object.potential_models.size() );
      // Something to draw...
      try
      {
        ROS_INFO("Converting to reference_frame...");
        mechanism_.convertGraspableObjectComponentsToFrame(goal.object, goal.object.reference_frame_id);

        bool use_cloud = goal.object.potential_models.empty();

        // Try to use object model, if there is one
        if(!use_cloud)
        {
          ROS_INFO("Goal contains object model; looking for model mesh...");
          // Connect to databse, get object mesh.
          arm_navigation_msgs::Shape mesh;
          if(!getModelMesh(goal.object.potential_models[0].model_id, mesh))
          {
            ROS_INFO("Unable to get database model, continuing with cluster.");
            use_cloud = true;
          }

          if(!use_cloud)
          {
            object_model_ = true;
            for(unsigned int i = 0; i < mesh.vertices.size(); i++)
            {
              PointT pt;
              pt.x = mesh.vertices[i].x;
              pt.y = mesh.vertices[i].y;
              pt.z = mesh.vertices[i].z;
              cloud->points.push_back(pt);
            }
            cloud->width = cloud->points.size();
            cloud->height = 1;
            cloud->is_dense = false;
            cloud->header.frame_id = goal.object.reference_frame_id;

            geometry_msgs::Pose &m = goal.object.potential_models[0].pose.pose;
            Eigen::Affine3f affine = Eigen::Translation3f(m.position.x,
                                                                m.position.y,
                                                                m.position.z) *
                                         Eigen::Quaternionf(m.orientation.w,
                                                            m.orientation.x,
                                                            m.orientation.y,
                                                            m.orientation.z);
            pcl::transformPointCloud(*cloud, *cloud, affine);

            tf::Transform T_o, T_g;
            tf::poseMsgToTF(goal.object.potential_models[0].pose.pose, T_o);
            tf::poseMsgToTF(goal.grasp.grasp_pose, T_g);
            tf::Transform T = T_g.inverse()*T_o;
            tf::poseTFToMsg(T, control_offset_.pose);

          }
        }

        if(use_cloud)
        {
          // Store point cloud

          sensor_msgs::PointCloud2 converted_cloud;
          sensor_msgs::convertPointCloudToPointCloud2 (goal.object.cluster, converted_cloud);

          pcl::fromROSMsg(converted_cloud, *cloud);
        }

        geometry_msgs::Pose &m = goal.grasp.grasp_pose;
        Eigen::Affine3f affine = Eigen::Translation3f(m.position.x,
                                                            m.position.y,
                                                            m.position.z) *
                                     Eigen::Quaternionf(m.orientation.w,
                                                        m.orientation.x,
                                                        m.orientation.y,
                                                        m.orientation.z);
        affine = affine.inverse();
        pcl::transformPointCloud(*cloud, *object_cloud_, affine);
      }
      catch(...){
        ROS_ERROR("%s: Error converting graspable object to reference frame id [%s]!",
                  get_pose_name_.c_str(), goal.object.reference_frame_id.c_str());
      }
    }
    if (goal.gripper_pose.pose.orientation.x == 0 &&
        goal.gripper_pose.pose.orientation.y == 0 &&
        goal.gripper_pose.pose.orientation.z == 0 &&
        goal.gripper_pose.pose.orientation.w == 0 )
    {
      ROS_INFO("Empty pose passed in; using default");
      gripper_pose_ = getDefaultPose(goal.arm_name);
      //ROS_DEBUG_STREAM("Default was \n" << gripper_pose_);
      //transformGripperPose();
      //ROS_DEBUG_STREAM("Default after transform is\n" << gripper_pose_);
      gripper_opening_ = 0.086;
      updateGripperAngle();
    }
    else
    {
      gripper_pose_ = goal.gripper_pose;
      //transformGripperPose();
      gripper_opening_ = goal.gripper_opening;
      updateGripperAngle();
    }
    if (get_pose_server_.isPreemptRequested())
    {
      if(test_pose_client_.client().getState() == actionlib::SimpleClientGoalState::ACTIVE ||
         test_pose_client_.client().getState() == actionlib::SimpleClientGoalState::PENDING )
      {
        test_pose_client_.client().cancelAllGoals();
      }
      get_pose_server_.setPreempted();
      setIdle();
      return;
    }

    pose_state_ = UNTESTED;
    initMarkers();
    pr2_object_manipulation_msgs::TestGripperPoseGoal test_goal;
    test_goal.gripper_poses.push_back(gripper_pose_);
    test_goal.gripper_openings.push_back(gripper_opening_);
    test_pose_client_.client().sendGoal( test_goal, boost::bind(&GripperPoseAction::testGripperResultCallback, this, _1, _2));
  }


  void preemptCB()
  {
    ROS_INFO("%s: Preempted", get_pose_name_.c_str());
    test_pose_client_.client().cancelAllGoals();
    grasp_plan_client_.client().cancelGoal();
    get_pose_server_.setPreempted();
    setIdle();
  }

  geometry_msgs::PoseStamped toWrist(const geometry_msgs::PoseStamped &ps)
  {
    geometry_msgs::PoseStamped out;
    out.header = ps.header;
    tf::Transform T, P;
    tf::poseMsgToTF(ps.pose, P);
    tf::poseMsgToTF(control_offset_.pose, T);
    tf::poseTFToMsg( P*T.inverse(), out.pose);
    //out.pose = object_manipulator::msg::applyShift(ps.pose, tf::Vector3(-0.15,0,0));
    return out;
  }

  geometry_msgs::PoseStamped fromWrist(const geometry_msgs::PoseStamped &ps)
  {
    geometry_msgs::PoseStamped out;
    out.header = ps.header;
    tf::Transform T, P;
    tf::poseMsgToTF(ps.pose, P);
    tf::poseMsgToTF(control_offset_.pose, T);
    tf::poseTFToMsg( P*T, out.pose);
    //out.pose = object_manipulator::msg::applyShift(ps.pose, tf::Vector3(-0.15,0,0));
    return out;
  }


  void updatePoses()
  {
    server_.setPose("ghosted_gripper", gripper_pose_.pose, gripper_pose_.header);
    server_.setPose("object_cloud", gripper_pose_.pose, gripper_pose_.header);
  }

  geometry_msgs::PoseStamped getDefaultPose(std::string arm_name)
  {
    ros::Time now = ros::Time(0);
    tfl_.waitForTransform("r_gripper_tool_frame","/base_link", now, ros::Duration(2.0));
    tf::StampedTransform stamped;
    geometry_msgs::TransformStamped ts;
    geometry_msgs::PoseStamped ps;
    std::string arm_frame;
    if (arm_name == "right_arm") arm_frame="r_wrist_roll_link";
    else if (arm_name == "left_arm") arm_frame="l_wrist_roll_link";
    else 
    {
      arm_frame="r_wrist_roll_link";
      ROS_ERROR("Unknown arm name passed to ghosted gripper");
    }
    tfl_.lookupTransform("/base_link", arm_frame, now, stamped);
    tf::transformStampedTFToMsg(stamped, ts);
    ps = msg::createPoseStampedMsg(ts);
    return ps;
  }


// **** 10 ***** 20 ****** 30 ****** 40 ****** 50 ****** 60 ****** 70 ****** 80 ****** 90 ***** 100 ***** 110 ***** 120

  void initMarkers()
  { 
    initGripperMarker();
    initObjectMarker();
    initGripperControl();
  }

  void initGraspMarkers()
  {
    for(unsigned int i = 0; i < planner_poses_.size(); i++)
    {
      // create arrow marker
      std::stringstream ss;
      ss << "grasp_" << i;
      server_.insert(makeGraspMarker(ss.str().c_str(), planner_poses_[i], 1.0, planner_states_[i]));
      server_.setCallback(ss.str(), boost::bind( &GripperPoseAction::selectGrasp, this, _1),
                          visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN );

      // TODO figure out how to delete stuff gracefully
    }
  }

  void eraseAllGraspMarkers()
  {
    for(unsigned int i = 0; i < planner_poses_.size(); i++)
    {
      // delete
      std::stringstream ss;
      ss << "grasp_" << i;
      server_.erase(ss.str().c_str());
    }
  }

  void selectGrasp( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
  {
    int index = atoi(feedback->marker_name.substr(6).c_str());
    if( index >= (int)planner_poses_.size() )  return;
    ROS_ERROR("Selecting grasp %d", index);

    planner_index_ = index;

    gripper_pose_ = planner_poses_[ planner_index_ ];
    pose_state_ = planner_states_[ planner_index_ ];
    initMarkers();
    //initButtonMarker();
    {
      if(pose_state_ == UNTESTED) 
      {
        ROS_ERROR("Here 5");
        testing_planned_grasp_ = true;
        tested_grasp_index_ = planner_index_;
        testing_current_grasp_ = true;
        testPose(gripper_pose_, gripper_opening_);
      }
    }

  }

  void initButtonMarker()
  {
    button_marker_pose_.header.stamp = ros::Time(0);
    int num = 0;
    if(planner_poses_.size()) num = planner_index_ + 1;
    server_.insert(makeListControl("grasp_toggle", button_marker_pose_, num, planner_poses_.size(), 0.3));
    //               , boost::bind( &GripperPoseAction::cycleGrasps, this));
    server_.setCallback("grasp_toggle", boost::bind( &GripperPoseAction::cycleGrasps, this, _1),
                        visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN );
  }

  void initObjectMarker()
  {
    if(object_cloud_->points.size())
    {
      server_.insert(makeCloudMarker( "object_cloud", gripper_pose_, 0.004, 
                                      object_manipulator::msg::createColorMsg(0.2, 0.8, 0.2,1.0) ));
    }
  }

  void initGripperMarker()
  {
    float r,g,b;
    switch(pose_state_)
    {
    case INVALID:
      r = 1.0; g = 0.2; b = 0.2;
      break;
    case VALID:
      r = 0.2; g = 1.0; b = 0.2;
      break;
    default:
      r = 0.5; g = 0.5; b = 0.5;
    }
    std_msgs::ColorRGBA color = object_manipulator::msg::createColorMsg(r,g,b,1.0);

    server_.insert(makeGripperMarker( "ghosted_gripper", gripper_pose_, 1.01, gripper_angle_, false, color),
                   boost::bind( &GripperPoseAction::gripperClickCB, this, _1));
    menu_gripper_.apply(server_, "ghosted_gripper");
  }

  void initGripperControl()
  {
    geometry_msgs::PoseStamped ps = fromWrist(gripper_pose_);
    ps.header.stamp = ros::Time(0);
    server_.insert(make6DofMarker( "gripper_controls", ps, 0.2, false, false),
                   boost::bind( &GripperPoseAction::updateGripper, this, _1));
    menu_gripper_.apply(server_, "gripper_controls");
  }


protected:


  void spinOnce()
  {
    //updatePoses();
    server_.applyChanges();
  }

  void slowSync()
  {
  }

  void testGripperResultCallback(const actionlib::SimpleClientGoalState& state,
                                 const pr2_object_manipulation_msgs::TestGripperPoseResultConstPtr &result)
  {
    if (result->valid.empty())
    {
      ROS_ERROR("Test gripper pose returned with empty result list");
      return;
    }
    if(state.state_ == state.SUCCEEDED)
    {
      ROS_DEBUG("Test pose action returned with result %d", (int)result->valid[0]);
      PoseState state = INVALID;
      if (result->valid[0]) state = VALID;
      if (testing_planned_grasp_)
      {
        if (planner_states_.empty() || tested_grasp_index_ < 0 || tested_grasp_index_ >= (int)planner_states_.size() )
        {
          ROS_ERROR("Current grasp tested, planner states has %zd entries and tested_grasp_index is %d",
                    planner_states_.size(), tested_grasp_index_);
        }
          else
        {
          planner_states_[tested_grasp_index_] = state;
          initGraspMarkers();
        }
      }
      if (testing_current_grasp_)
      {
        pose_state_ = state;
        initGripperMarker();
      } 
      //fire up a new test if we have one to do
      if( active_ )
      {
        for (size_t i=0; i<planner_states_.size(); i++)
        {
          if (planner_states_[i] == UNTESTED)
          {
            ROS_ERROR("Here 6");
            testing_planned_grasp_ = true;
            tested_grasp_index_ = i;
            testing_current_grasp_ = false;
            testPose(planner_poses_[ i ], gripper_opening_);
          }
        }
      }
    }
    else
    {
      ROS_WARN("Test pose action did not succeed; state = %d", (int)state.state_);
    }
  }

  void graspPlanCB()
  {
    eraseAllGraspMarkers();
    planner_index_ = 0;
    planner_poses_.resize(0);
    planner_states_.resize(0);

    point_cloud_server::StoreCloudGoal cloud_goal;
    cloud_goal.action = cloud_goal.GET;
    cloud_goal.name = "interactive_manipulation_snapshot";
    cloud_server_client_.client().sendGoal(cloud_goal);
    if(!cloud_server_client_.client().waitForResult(ros::Duration(3.0)))
    {
      ROS_WARN("Timed-out while waiting for cloud from server!");
      return;
    }

    object_manipulation_msgs::GraspPlanningGoal plan_goal;
    plan_goal.target.region.cloud = cloud_server_client_.client().getResult()->cloud;
    plan_goal.target.region.roi_box_pose = fromWrist(gripper_pose_);
    //plan_goal.target.region.roi_box_pose.header.frame_id = "/base_link";
    plan_goal.target.region.roi_box_dims = object_manipulator::msg::createVector3Msg(0.3, 0.3, 0.3);
    int cloud_size = plan_goal.target.region.cloud.width * plan_goal.target.region.cloud.height;
    plan_goal.target.reference_frame_id = gripper_pose_.header.frame_id;
    object_manipulation_msgs::Grasp seed;
    seed.grasp_pose = gripper_pose_.pose;
    plan_goal.grasps_to_evaluate.push_back(seed);
    ROS_DEBUG_STREAM("Requesting adjustment on cloud with " << cloud_size << " points, pose \n" << seed);
    //pub_cloud_.publish(plan_goal.target.region.cloud);
    grasp_plan_client_.client().sendGoal( plan_goal, boost::bind(&GripperPoseAction::graspPlanResultCB, this, _1, _2));

    button_marker_pose_ = gripper_pose_;
    button_marker_pose_.header.stamp = ros::Time(0);
    button_marker_pose_.pose.orientation = geometry_msgs::Quaternion();
    button_marker_pose_.pose.orientation.w = 1;
    button_marker_pose_.pose.position.z -= 0.2;
    //initButtonMarker();
    //if(always_call_planner)
    //{
    server_.erase("gripper_controls");
    server_.erase("ghosted_gripper");
    //}
  }

  void graspPlanResultCB(const actionlib::SimpleClientGoalState& state,
                         const object_manipulation_msgs::GraspPlanningResultConstPtr &result)
  {
    planner_index_ = 0;
    if(state.state_ == state.SUCCEEDED)
    {
      ROS_INFO("Grasp plan action succeeded.");
      
      int num = result->grasps.size();

      planner_poses_.resize(num + 1);
      planner_states_.resize(num + 1);
      //gripper_score_.resize(num);
      //gripper_angle_.resize(num);

      for(int i = 0; i < num; i++)
      {
        planner_poses_[i].pose = result->grasps[i].grasp_pose;
        planner_poses_[i].header = gripper_pose_.header;
        planner_poses_[i].header.stamp = ros::Time(0);
        planner_states_[i] = UNTESTED;
        //object_manipulation_msgs::Grasp grasp = result->grasps[i];
        //gripper_angle_[i] = std::max( std::min(1.f , (float)0.541) , (float)0.0);
        //gripper_score_[i] = grasp.success_probability;
      }
      planner_poses_[num] = gripper_pose_;
      planner_poses_[num].header.stamp = ros::Time(0);
      planner_states_[num] = UNTESTED;
      initGraspMarkers();
      ROS_ERROR("Here 1");
      testing_planned_grasp_ = true;
      tested_grasp_index_ = 0;
      testing_current_grasp_ = false;
      testPose(planner_poses_[ 0 ], gripper_opening_);
    }
    else
    {
      ROS_WARN("Grasp plan action did not succeed; state = %d", (int)state.state_);
      planner_poses_.resize(1);
      planner_states_.resize(1);
      planner_poses_[0] = gripper_pose_;
      planner_poses_[0].header.stamp = ros::Time(0);
      planner_states_[0] = UNTESTED;
      gripper_pose_ = planner_poses_[0];
      initMarkers();
      //initButtonMarker();
      pose_state_ = UNTESTED;
      testing_current_grasp_ = true;
      ROS_ERROR("Here 2");
      testing_planned_grasp_ = true;
      tested_grasp_index_ = 0;
      testPose(gripper_pose_, gripper_opening_);
    }
  }

  void cycleGrasps(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
  {
    ROS_ERROR("Cycle grasps");
    //ROS_WARN("Grasp cycling not yet implemented!");
    // Need to:
    // Store the returned grasp list
    // Store a bool list to remeber if a grasp pose is valid
    // Set the gripper opening to max (?)
    // Cycle through the grasps, calling the test_pose action if needed
    //ROS_INFO_STREAM("Got feedback! \n" << *feedback);

    //if(!feedback->control_name.compare("left")) planner_index_ --;
    //if(!feedback->control_name.compare("right")) planner_index_++;

    planner_index_++;
    if( planner_index_ == (int)planner_poses_.size() )  planner_index_ = 0;
    //if( planner_index_ < 0 )  planner_index_ = 0;
    
    gripper_pose_ = planner_poses_[ planner_index_ ];
    pose_state_ = planner_states_[ planner_index_ ];
    initMarkers();
    initButtonMarker();
    if(pose_state_ == UNTESTED) 
    {
      testing_current_grasp_ = true;
      ROS_ERROR("Here 3");
      testing_planned_grasp_ = true;
      tested_grasp_index_ = planner_index_;
      testPose(gripper_pose_, gripper_opening_);
    }
  }

  void acceptCB()
  {
    if( pose_state_ == VALID )
    {
      if(test_pose_client_.client().getState() == actionlib::SimpleClientGoalState::ACTIVE ||
         test_pose_client_.client().getState() == actionlib::SimpleClientGoalState::PENDING )
      {
        test_pose_client_.client().cancelGoal();
      }
      setIdle();
      pr2_object_manipulation_msgs::GetGripperPoseResult result;
      result.gripper_pose = gripper_pose_;
      result.gripper_opening = gripper_opening_;
      get_pose_server_.setSucceeded(result);
    }
  }

  void cancelCB()
  {
    if(test_pose_client_.client().getState() == actionlib::SimpleClientGoalState::ACTIVE ||
       test_pose_client_.client().getState() == actionlib::SimpleClientGoalState::PENDING )
    {
      test_pose_client_.client().cancelGoal();
    }
    get_pose_server_.setAborted();
    setIdle();
  }

  void focusCB()
  {
    pr2_object_manipulation_msgs::CameraFocus msg;
    msg.focal_point.point = gripper_pose_.pose.position;
    msg.focal_point.header = gripper_pose_.header;
    pub_focus_.publish(msg);
  }

  void gripperClickCB( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
  {
    if(interface_number_ == 2) return;

    ros::Time now = ros::Time(0);

    switch ( feedback->event_type )
    {
    case visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK:
        //ROS_DEBUG_STREAM( "Marker is being moved, stored pose is invalidated." );
        float max_gripper_angle = 0.541;
        gripper_angle_ -= 0.12;
        if(gripper_angle_ < 0.04)
          gripper_angle_ = max_gripper_angle;
        updateGripperOpening();
        initGripperMarker();
        ROS_DEBUG( "Gripper opening = %.2f, angle = %.2f", gripper_opening_, gripper_angle_);
        break;
    }
  }

  void testPose(geometry_msgs::PoseStamped pose, float opening)
  {
    pr2_object_manipulation_msgs::TestGripperPoseGoal goal;
    goal.gripper_poses.push_back(pose);
    goal.gripper_openings.push_back(opening);
    test_pose_client_.client().sendGoal( goal, boost::bind(&GripperPoseAction::testGripperResultCallback, this, _1, _2));
  }

  void updateGripper( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
  {
    ros::Time now = ros::Time(0);

    switch ( feedback->event_type )
    {
    case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN:
        ROS_DEBUG_STREAM( "Marker is being moved, stored pose is invalidated." );
        test_pose_client_.client().cancelAllGoals();
        pose_state_ = UNTESTED;
        break;
    case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP:
        //ROS_INFO_STREAM("MOUSE_UP \n" << feedback->pose);
        ROS_DEBUG_STREAM( "Marker was released, storing pose and checking." );
//        if(gripper_pose_.header.frame_id.compare(feedback->header.frame_id))  fix = true;
        gripper_pose_.pose = feedback->pose;
        gripper_pose_.header = feedback->header;
        gripper_pose_ = toWrist(gripper_pose_);
        //transformGripperPose();
        //ROS_INFO_STREAM("MOUSE_UP \n" << gripper_pose_);
        initMarkers();
        testing_planned_grasp_ = false;
        testing_current_grasp_ = true;
        testPose(gripper_pose_, gripper_opening_);
        break;
    case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
        ROS_DEBUG_STREAM("POSE_UPDATE in frame " << feedback->header.frame_id << std::endl << feedback->pose);
        //if(gripper_pose_.header.frame_id.compare(feedback->header.frame_id))  fix = true;
        gripper_pose_.pose = feedback->pose;
        gripper_pose_.header = feedback->header;
        gripper_pose_ = toWrist(gripper_pose_);
        updatePoses();
        break;
    }
  }

  void initMenus()
  {
    accept_handle_ = menu_gripper_.insert("Accept", boost::bind( &GripperPoseAction::acceptCB, this ) );
    if(double_menu_) menu_gripper_.insert("------", boost::bind( &GripperPoseAction::acceptCB, this ) );
    cancel_handle_ = menu_gripper_.insert("Cancel", boost::bind( &GripperPoseAction::cancelCB, this ) );
    if(double_menu_) menu_gripper_.insert("------", boost::bind( &GripperPoseAction::cancelCB, this ) );
    focus_handle_ = menu_gripper_.insert("Focus camera", boost::bind( &GripperPoseAction::focusCB, this ) );
    if(double_menu_) menu_gripper_.insert("------", boost::bind( &GripperPoseAction::focusCB, this ) );
    if(interface_number_ == 0  || interface_number_ == 4)
    {
      menu_gripper_.insert("Auto-Adjust", boost::bind( &GripperPoseAction::graspPlanCB, this ) );
      if(double_menu_) menu_gripper_.insert("-----------", boost::bind( &GripperPoseAction::graspPlanCB, this ) );
    }
  }

  bool getModelMesh( int model_id, arm_navigation_msgs::Shape& mesh )
  {
    household_objects_database_msgs::GetModelMesh mesh_srv;

    mesh_srv.request.model_id = model_id;
    if ( !get_model_mesh_client_.call(mesh_srv) )
    {
      ROS_ERROR("Failed to call get model mesh service");
      return false;
    }

    if (mesh_srv.response.return_code.code != household_objects_database_msgs::DatabaseReturnCode::SUCCESS)
    {
      ROS_ERROR("Model mesh service reports an error (code %d)", mesh_srv.response.return_code.code);
      return false;
    }

    mesh = mesh_srv.response.mesh;
    return true;
  }



  visualization_msgs::InteractiveMarker makeCloudMarker( const char *name, const geometry_msgs::PoseStamped &stamped,
                                                         float point_size, std_msgs::ColorRGBA color)
  {
    InteractiveMarker int_marker;
    int_marker.name = name;
    int_marker.pose = stamped.pose;
    int_marker.header = stamped.header;

    Marker marker;
    marker.color = color;
    marker.frame_locked = false;

    if(object_cloud_->points.size())
    {
      //int_marker.header = object_cloud_->header;
      marker.scale.x = point_size;
      marker.scale.y = point_size;
      marker.scale.z = point_size;
      marker.type = visualization_msgs::Marker::SPHERE_LIST;

      int num_points = object_cloud_->points.size();
      marker.points.resize( num_points );
//      marker.colors.resize( num_points );

      //ROS_INFO_STREAM( "Adding point cluster. #points=" << object_.cluster.points.size() );

      for ( int i=0; i<num_points; i++)
      {
        marker.points[i].x = object_cloud_->points[i].x;
        marker.points[i].y = object_cloud_->points[i].y;
        marker.points[i].z = object_cloud_->points[i].z;
//        marker.colors[i].r = object_cloud_->points[i].r/255.;
//        marker.colors[i].g = object_cloud_->points[i].g/255.;
//        marker.colors[i].b = object_cloud_->points[i].b/255.;
//        marker.colors[i].a = 1.0;
      }
    }
    else
    {

    }

    InteractiveMarkerControl control;
    control.always_visible = true;
    control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
    control.orientation_mode = visualization_msgs::InteractiveMarkerControl::INHERIT;

    control.markers.push_back( marker );

    int_marker.controls.push_back( control );

    return int_marker;
  }

};


int main(int argc, char** argv)
{
  ros::init(argc, argv, "pr2_interactive_pose_select_action");
  GripperPoseAction gripper_pose_action;

  ros::Duration(1.0).sleep();

  ros::spin();
}

---

pr2_interactive_gripper_pose_action
Author(s): Adam Leeper
autogenerated on Tue Mar 5 14:21:19 2013