gripper_click_line.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_line.h"

#include <OGRE/OgreManualObject.h>

#include <rviz/render_panel.h>

#include <tf/transform_datatypes.h>

#include <geometry_msgs/PoseStamped.h>

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

namespace pr2_gripper_click {

GripperClickLine::GripperClickLine() :
    GripperClickPlugin<GripperClickPickupAction>("Line Drawing")
{
  click_info_.valid_ = false;
}


void GripperClickLine::init( rviz::VisualizationManager *vis_manager, Ogre::SceneManager* scene_manager, Ogre::SceneNode* camera_node )
{
  rviz_interaction_tools::UniqueStringManager usm;

  line_object_ = vis_manager->getSceneManager()->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 GripperClickLine::setGoal( const GripperClickPickupGoal::ConstPtr &goal )
{
  camera_info_ = goal->sensor_data.camera_info;
  disparity_image_ = goal->sensor_data.disparity_image;

  click_info_.valid_ = false;
  click_info_.down_x_ = click_info_.down_y_ = 0;
  click_info_.up_x_ = click_info_.up_y_ = 0;

  setDescription( "Click and drag the mouse to set the desired gripper position and rotation.\n\nPress OK when you are finished." );
  return true;
}


void GripperClickLine::show( )
{
  line_object_->setVisible( true );
}

void GripperClickLine::hide( )
{
  line_object_->setVisible( false );
}

void GripperClickLine::getGripperParams( bool &visible, Ogre::Vector3 &position, Ogre::Quaternion &orientation, float &angle )
{
  visible = hasValidResult();
  if ( visible )
  {
    computeTransform();

//    //move back the transform so we get the wrist roll link
//    btTransform back_trans(btQuaternion(0, 0, 0, 1), btVector3(-0.14, 0, 0));
//    trans = trans * back_trans;
    btQuaternion rot = gripper_transform_.getRotation();
    btVector3 trl = gripper_transform_.getOrigin();

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

    angle = gripper_angle_;
  }
}


void GripperClickLine::update(float wall_dt, float ros_dt)
{
  float c_x = click_info_.down_x_ / ((float)disparity_image_.image.width  / 2.0 ) - 1.0;
  float c_y = - (click_info_.down_y_ / ((float)disparity_image_.image.height / 2.0 ) - 1.0);
  float u_x = click_info_.up_x_ / ((float)disparity_image_.image.width  / 2.0 ) - 1.0;
  float u_y = - (click_info_.up_y_ / ((float)disparity_image_.image.height / 2.0 ) - 1.0);
  float d_x = u_x - c_x;
  float d_y = u_y - c_y;

  line_object_->beginUpdate(0);
  line_object_->position(c_x-d_x, c_y-d_y, 0.0);
  line_object_->position(c_x+d_x, c_y+d_y, 0.0);
  line_object_->index(0);
  line_object_->index(1);
  line_object_->index(0);
  line_object_->end();  
}

void GripperClickLine::onRenderWindowMouseEvents( wxMouseEvent& event, Ogre::Ray &mouse_ray )
{
  int x = event.GetX();
  int y = event.GetY();
  
  if (event.ButtonDown(wxMOUSE_BTN_LEFT))
  {
    if (rviz_interaction_tools::hasDisparityValue(disparity_image_, y, x))
    {
      ROS_INFO("Good click");
      click_info_.lbutton_down_ = true;
      click_info_.down_x_ = x;
      click_info_.down_y_ = y;
      click_info_.up_x_ = x;
      click_info_.up_y_ = y;
    }
    else ROS_INFO("Bad click");
  }
  if (event.ButtonUp(wxMOUSE_BTN_LEFT))
  {
    if (click_info_.lbutton_down_)
    {
      click_info_.lbutton_down_ = false;
    }
  }
  if (event.Dragging())
  {
    if (click_info_.lbutton_down_)
    {
      click_info_.up_x_ = x;
      click_info_.up_y_ = y;
      if (click_info_.up_x_ != click_info_.down_x_ ||
          click_info_.up_y_ != click_info_.down_y_ )
      {
        click_info_.valid_ = true;
      }
    }
  }
}

bool GripperClickLine::hasValidResult()
{
  if( !click_info_.valid_ || (click_info_.up_x_ == click_info_.down_x_ &&
                              click_info_.up_y_ == click_info_.down_y_ ) )
  {
    return false;
  }
  return true;
}

bool GripperClickLine::computeTransform()
{
  if ( !hasValidResult() )
  {
    return false;
  }

  float px, py, pz;
  rviz_interaction_tools::getPoint(disparity_image_, camera_info_,
           click_info_.down_y_, click_info_.down_x_,
           px, py, pz);
  btVector3 grasp_point(px, py, pz);
  float depth = grasp_point.z();
  btVector3 grasp_direction = grasp_point;
  grasp_direction.normalize();
  //wrist roll link is 13cm back from grasp point
  grasp_point = grasp_point - 0.13 * grasp_direction;

  //line up x with the grasp direction
  btVector3 x_dir = grasp_direction;

  //line up y with the line direction
  //image plane is x-y plane
  float delta_x = click_info_.up_x_ - click_info_.down_x_;
  float delta_y = click_info_.up_y_ - click_info_.down_y_;
  btVector3 y_dir(delta_x,
                  delta_y,
                  0);
  y_dir.normalize();

  //cross products give us the rest
  btVector3 z_dir = x_dir.cross(y_dir);
  z_dir.normalize();
  y_dir = z_dir.cross(x_dir);

  //build the rotation matrix and the transform
  btMatrix3x3 rot( x_dir.x(), x_dir.y(), x_dir.z(),
                   y_dir.x(), y_dir.y(), y_dir.z(),
                   z_dir.x(), z_dir.y(), z_dir.z() );
  rot = rot.transpose();
  btTransform grasp_tran(rot, grasp_point);

  gripper_transform_ = grasp_tran;

  float half_line_len = sqrt(delta_x*delta_x + delta_y*delta_y);
  float f = camera_info_.P[0]; //912.07 from narrow_stereo/left/camera_info/P[0]
  float gripper_opening = 2 * depth * half_line_len / f / 0.0857 * 0.5; //arbitrary? //i don't understand the conversion from "angle" to "opening"
  if(gripper_opening > .5) gripper_opening = .5;

  gripper_angle_ = gripper_opening;
  return true;
}

bool GripperClickLine::getResult(GripperClickPickupResult &result)
{
  if ( !hasValidResult() )
  {
    return false;
  }
  
  computeTransform();

  tf::poseTFToMsg(gripper_transform_, 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_;

  return true;
}
 
}