gripper_click_grasp_adjust.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_gripper_click/gripper_click_grasp_adjust.h"


#include <tf/transform_datatypes.h>

#include <geometry_msgs/PoseStamped.h>

#include <rviz/render_panel.h>
#include <rviz/mesh_loader.h>

#include <OGRE/OgreEntity.h>
#include <OGRE/OgreLight.h>
#include <OGRE/OgreManualObject.h>

#include <rviz_interaction_tools/unique_string_manager.h>
#include <rviz_interaction_tools/image_tools.h>

#include <pcl/io/io.h>
#include <pcl/point_types.h>

#include <actionlib/client/simple_action_client.h>
#include <pr2_grasp_adjust/GraspAdjustAction.h>

namespace pr2_gripper_click {

GripperClickGraspAdjust::GripperClickGraspAdjust() :
  GripperClickPlugin<GripperClickPickupAction>("Grasp Adjust"),
  gripper_index_(0),
  valid_transform_(false),
  action_client_("grasp_adjust_action_server_node", false)  // this should maybe be false
{
  if(!action_client_.waitForServer(ros::Duration(2.0)))
    ROS_ERROR("Couldn't communicate with grasp adjust action server... continuing anyway.");
  ROS_INFO("Initialized grasp_adjust plugin!");
}


GripperClickGraspAdjust::~GripperClickGraspAdjust()
{
  scene_node_->getParent()->removeChild( scene_node_ );
  delete scene_node_;
}


void GripperClickGraspAdjust::init( rviz::VisualizationManager *vis_manager, Ogre::SceneManager* scene_manager, Ogre::SceneNode* camera_node )
{
  scene_node_ = scene_manager->getRootSceneNode()->createChildSceneNode();

  //gripper_ = new rviz_interaction_tools::Gripper( scene_manager, scene_node_ );

  rviz_interaction_tools::UniqueStringManager usm;

  line_object_ = scene_manager->createManualObject( usm.unique("GripperClickLineIndicator") );
//  line_object_->setUseIdentityProjection(true);
//  line_object_->setUseIdentityView(true);
  line_object_->setDynamic(true);
  line_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP);
  line_object_->position(0.0, 0.0, 0.0);
  line_object_->position(0.0, 0.0, 0.0);
  line_object_->index(0);
  line_object_->index(1);
  line_object_->index(0);
  line_object_->end();

  Ogre::AxisAlignedBox aabInf;
  aabInf.setInfinite();
  line_object_->setBoundingBox(aabInf);

  //click indicator will be rendered last
  line_object_->setRenderQueueGroup(Ogre::RENDER_QUEUE_OVERLAY);

  scene_manager->getRootSceneNode()->attachObject(line_object_);
}

bool GripperClickGraspAdjust::setGoal( const GripperClickPickupGoal::ConstPtr &goal )
{
  camera_info_ = goal->sensor_data.camera_info;
  disparity_image_ = goal->sensor_data.disparity_image;
  valid_transform_ = false;

  setDescription("Please double-click on the desired object to compute a grasp.");
  return true;
}


void GripperClickGraspAdjust::show( )
{
  scene_node_->setVisible(true);
}

void GripperClickGraspAdjust::hide( )
{
  scene_node_->setVisible(false);
}

void GripperClickGraspAdjust::getGripperParams( bool &visible, Ogre::Vector3 &position, Ogre::Quaternion &orientation, float &angle )
{
  visible = valid_transform_;
  if (visible)
  {
    // No shift correction, the grasp adjust already works in wrist_roll_link

    btQuaternion rot = gripper_transform_[gripper_index_].getRotation();
    btVector3 trl = gripper_transform_[gripper_index_].getOrigin();

    position = Ogre::Vector3(trl.x(), trl.y(), trl.z());
    orientation = Ogre::Quaternion(rot.w(), rot.x(), rot.y(), rot.z());

    angle = gripper_angle_[gripper_index_];
  }
}

void GripperClickGraspAdjust::update(float wall_dt, float ros_dt)
{
}

void GripperClickGraspAdjust::receiveGraspAdjustAction(const actionlib::SimpleClientGoalState &state,
                                                       const pr2_grasp_adjust::GraspAdjustResultConstPtr &result)
{
  gripper_index_ = 0;
  ROS_INFO("Received grasp adjust response with %d grasps", (int)result->grasp_poses.size());
  if(state.state_ == state.SUCCEEDED)
  {
    int num = result->grasp_poses.size();

    gripper_transform_.resize(num);
    gripper_score_.resize(num);
    gripper_angle_.resize(num);

    for(int i = 0; i < num; i++)
    {
      tf::poseMsgToTF(result->grasp_poses[i].pose, gripper_transform_[i]);
      gripper_angle_[i] = std::max( std::min(result->gripper_openings[i] , (float)0.541) , (float)0.0);
      gripper_score_[i] = result->grasp_scores[i];
    }

    valid_transform_ = true;
    setDescription("Use your mouse to adjust the computed grasp.\n\nClick OK when your're finished.");
  }
  else
  {
    setDescription("Could not compute a valid grasp.\n\nTry to double-click on a different location to compute a new grasp.");
  }
}

void GripperClickGraspAdjust::requestGraspAdjustAction()
{
  valid_transform_ = false;
  gripper_index_ = 0;
  pcl::PointCloud<pcl::PointXYZ> cloud;
  cloud.header = disparity_image_.header;
  cloud.is_dense = false;

  int seed_index = 0;
  int height = disparity_image_.image.height;
  int width = disparity_image_.image.width;

  for(int x = 0; x < width; x++ )
    for(int y = 0; y < height; y++)
      if(rviz_interaction_tools::hasDisparityValue(disparity_image_, y, x))
      {
        if(abs(x - dlb_.x_) > width / 5 ) continue;
        if(abs(y - dlb_.y_) > height / 5 ) continue;

        pcl::PointXYZ pt;
        rviz_interaction_tools::getPoint(disparity_image_, camera_info_, y, x, pt.x, pt.y, pt.z);
        cloud.push_back(pt);
        if( dlb_.x_ == x && dlb_.y_ == y)
          seed_index = cloud.size() - 1;
      }

  pr2_grasp_adjust::GraspAdjustGoal goal;
  pcl::toROSMsg(cloud, goal.cloud);

  goal.pose_stamped.header = disparity_image_.header;
  float px, py, pz;
  rviz_interaction_tools::getPoint(disparity_image_, camera_info_, dlb_.y_, dlb_.x_, px, py, pz);
  tf::pointTFToMsg(tf::Point(px, py, pz), goal.pose_stamped.pose.position);
  tf::quaternionTFToMsg(tf::Quaternion(0,0,0,1), goal.pose_stamped.pose.orientation);
  goal.use_orientation = false;
  goal.seed_index = seed_index;
  goal.cloud.header.stamp = ros::Time::now();
  goal.pose_stamped.header.stamp = goal.cloud.header.stamp;

  action_client_.sendGoal(goal,
              boost::bind(&GripperClickGraspAdjust::receiveGraspAdjustAction, this, _1, _2),
              ActionClient::SimpleActiveCallback(),  // null
              ActionClient::SimpleFeedbackCallback());  // null

  setDescription("Computing grasp..");
}

void GripperClickGraspAdjust::onRenderWindowMouseEvents( wxMouseEvent& event, Ogre::Ray &mouse_ray )
{
  int x = event.GetX();
  int y = event.GetY();

  if (event.GetWheelRotation() >= event.GetWheelDelta())
  {
    if(valid_transform_)
    {
      //printf("Rotating up\n");
      tf::Transform rotate = tf::Transform(tf::Quaternion(tf::Vector3(1, 0, 0), 0.1), tf::Vector3(0,0,0));
      gripper_transform_[gripper_index_] = gripper_transform_[gripper_index_] * rotate;
    }

//    gripper_roll_[i] += 0.2;
//    if (gripper_roll_[i] > M_PI) gripper_roll_[i] -= M_PI;
  }

  if (event.GetWheelRotation() <= -event.GetWheelDelta())
  {
    if(valid_transform_)
    {
      //printf("Rotating down\n");
      tf::Transform rotate = tf::Transform(tf::Quaternion(tf::Vector3(1, 0, 0), -0.1), tf::Vector3(0,0,0));
      gripper_transform_[gripper_index_] = gripper_transform_[gripper_index_] * rotate;
    }
  }

  if (event.ButtonDClick(wxMOUSE_BTN_LEFT))
  {
    if (rviz_interaction_tools::hasDisparityValue(disparity_image_, y, x))
    {
      dlb_.x_ = x;
      dlb_.y_ = y;
//      gripper_approach_ = 0;

      requestGraspAdjustAction();
    }
  }

  if (event.ButtonDown(wxMOUSE_BTN_LEFT))
  {
    lb_.down_ = true;
    if(valid_transform_)
    {
      gripper_index_++;
      if(gripper_index_ >= gripper_transform_.size())
        gripper_index_ = 0;
    }
    //lb_.x_ = dlb_.x_ - x;
    //lb_.y_ = dlb_.y_ - y;
  }
  if (event.ButtonUp(wxMOUSE_BTN_LEFT)) lb_.down_ = false;

  if (event.ButtonDown(wxMOUSE_BTN_RIGHT))
  {
    rb_.down_ = true;
    rb_.x_ = x;
    rb_.y_ = y;

    //rb_.ref_ = gripper_approach_[i];
//    if(valid_transform_)
//    {
//      gripper_index_++;
//      if(gripper_index_ == gripper_transform_.size())
//        gripper_index_ = 0;
//    }
  }
  if (event.ButtonUp(wxMOUSE_BTN_RIGHT)) rb_.down_ = false;

  if (event.ButtonDown(wxMOUSE_BTN_MIDDLE)) 
  {
    mb_.down_ = true;
    mb_.x_ = x;
    mb_.y_ = y;
    if(valid_transform_)
      mb_.ref_ = gripper_angle_ [gripper_index_];
  }
  if (event.ButtonUp(wxMOUSE_BTN_MIDDLE)) mb_.down_ = false;

  if (event.Dragging())
  {
    if (rb_.down_) 
    {
      if(valid_transform_)
      {
        //printf("Translating...\n");
        int dy = rb_.y_ - y;
        rb_.y_ = y;
        float dt = rb_.ref_ + ((float)dy / 100.0) * 0.1;
        tf::Transform translate = tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(dt,0,0));
        gripper_transform_[gripper_index_] = gripper_transform_[gripper_index_] * translate ;
      }
//
    }
    if (mb_.down_)
    {
      if(valid_transform_)
      {
        //printf("Changing angle...\n");
        int dy = y - mb_.y_;
        float da = ((float)dy/100.0) * 0.541;
        gripper_angle_[gripper_index_] = mb_.ref_ + da;
        gripper_angle_[gripper_index_] = std::min(gripper_angle_[gripper_index_] , (float)0.541);
        gripper_angle_[gripper_index_] = std::max(gripper_angle_[gripper_index_] , (float)0.0);
      }
    }
    if (lb_.down_)
    {
      lb_.x_ = dlb_.x_ - x;
      lb_.y_ = dlb_.y_ - y;
    }
  }

}

bool GripperClickGraspAdjust::getResult(GripperClickPickupResult &result)
{

  tf::poseTFToMsg(gripper_transform_[gripper_index_], result.grasp_pose.pose);
  result.grasp_pose.header.stamp = ros::Time::now();
  result.grasp_pose.header.frame_id = disparity_image_.header.frame_id;
  result.gripper_opening = gripper_angle_[gripper_index_] / 0.5 * 0.0857;
  ROS_INFO("gripper_angle_: %.3f, using gripper opening of %.3f", gripper_angle_[gripper_index_], result.gripper_opening);
  return true;
}

} //namespace pr2_gripper_click