gripper_click_pickup_cartesian.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_pickup_cartesian.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>

#include <algorithm>

namespace pr2_gripper_click {

GripperClickPickupCartesian::GripperClickPickupCartesian() :
    GripperClickPlugin<GripperClickPickupAction>("Cartesian Control"),
    initialized_(false),
    gripper_angle_(0)
{
}

void GripperClickPickupCartesian::init( rviz::VisualizationManager *vis_manager, Ogre::SceneManager* scene_manager, Ogre::SceneNode* camera_node )
{
  camera_node_ = camera_node;

  rviz_interaction_tools::UniqueStringManager usm;

  cartesian_control_ = new rviz_interaction_tools::CartesianControl( scene_manager->getRootSceneNode(), vis_manager );
  cartesian_control_->setFixedControlsOrientation(false);
  cartesian_control_->hide();
}

//TODO; destructor

void GripperClickPickupCartesian::show( )
{
  if ( initialized_ )
  {
    cartesian_control_->show();
  }
}

void GripperClickPickupCartesian::hide( )
{
  cartesian_control_->hide();
}

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

  hide();
  initialized_ = false;

  gripper_angle_ = 0.541;

  setDescription("Please double-click on the desired object to set the initial gripper position.");

  return true;
}

void GripperClickPickupCartesian::update(float wall_dt, float ros_dt)
{
  cartesian_control_->update();
}

void GripperClickPickupCartesian::onRenderWindowMouseEvents( wxMouseEvent& event, Ogre::Ray &mouse_ray )
{
  int x = event.GetX();
  int y = event.GetY();
  
  if (event.ButtonDClick(wxMOUSE_BTN_LEFT) && rviz_interaction_tools::hasDisparityValue(disparity_image_, y, x))
  {
    float px, py, pz;
    rviz_interaction_tools::getPoint(disparity_image_, camera_info_, y, x, px, py, pz);

    Ogre::Vector3 controls_position(px,py,pz);
    controls_position = camera_node_->getPosition() + camera_node_->getOrientation()*controls_position;

    Ogre::Quaternion controls_orientation( Ogre::Degree( 225 ), Ogre::Vector3(0,1,0) );
    controls_orientation= controls_orientation * Ogre::Quaternion( Ogre::Degree( -90 ), Ogre::Vector3(0,0,1) );

    // Ogre::Quaternion( Ogre::Degree( 45 ), Ogre::Vector3(0,1,0) ) * Ogre::Quaternion( Ogre::Degree( -90 ), Ogre::Vector3(0,0,1) )

    Ogre::Vector3 gripper_position = controls_position - controls_orientation * Ogre::Vector3(0.20,0,0);
    Ogre::Quaternion gripper_orientation = controls_orientation;

    cartesian_control_->setPosition( gripper_position );
    cartesian_control_->setOrientation( gripper_orientation );

    cartesian_control_->setControlsPosition( controls_position );
    cartesian_control_->setControlsOrientation( controls_orientation );

    setDescription("Use the box handles to drag the gripper and the wheels to turn it. You can reset the gripper position\nby double-clicking on the desired location.\n\nUse the mouse wheel to adjust the gripper opening.\n\nPress OK When you are finished.");

    initialized_ = true;
    show();
  }
  else
  {
    if( event.GetWheelRotation() != 0 )
    {
      gripper_angle_ -= 0.1 * float(event.GetWheelRotation()) / float(event.GetWheelDelta());
      if ( gripper_angle_ > 0.541 ) gripper_angle_ = 0.541;
      if ( gripper_angle_ < 0 ) gripper_angle_ = 0;
    }

    if ( event.ButtonDown(wxMOUSE_BTN_LEFT) )
    {
      cartesian_control_->mouseDown( mouse_ray );
    }
    if ( event.ButtonUp(wxMOUSE_BTN_LEFT) )
    {
      cartesian_control_->mouseUp( mouse_ray );
    }
    else
    {
      cartesian_control_->mouseMove( mouse_ray );
    }
  }
}

bool GripperClickPickupCartesian::hasValidResult()
{
  return initialized_;
}


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

  result.grasp_pose = cartesian_control_->getPose();
  result.gripper_opening = gripper_angle_;
  return true;
}

void GripperClickPickupCartesian::getGripperParams( bool &visible, Ogre::Vector3 &position, Ogre::Quaternion &orientation, float &angle )
{
  visible = initialized_;
  position = camera_node_->convertWorldToLocalPosition( cartesian_control_->getPosition() );
  orientation = camera_node_->convertWorldToLocalOrientation( cartesian_control_->getOrientation() );
  angle = gripper_angle_;
}

}