gripper_click_place_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_place_cartesian.h"

#include <OGRE/OgreManualObject.h>

#include <rviz/render_panel.h>
#include <rviz/common.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 {

GripperClickPlaceCartesian::GripperClickPlaceCartesian() :
    GripperClickPlugin<GripperClickPlaceAction>("Cartesian Control"),
    initialized_(false)
{
}

void GripperClickPlaceCartesian::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(true);
  cartesian_control_->hide();

  point_cloud_ = new ogre_tools::PointCloud();

  gripper_scene_node_ = scene_manager->getRootSceneNode()->createChildSceneNode();
  gripper_scene_node_->attachObject( point_cloud_ );

  main_win_point_cloud_ = new ogre_tools::PointCloud();

  main_win_object_scene_node_ = vis_manager->getSceneManager()->getRootSceneNode()->createChildSceneNode();
  main_win_object_scene_node_->attachObject( main_win_point_cloud_ );
}

//TODO; destructor

void GripperClickPlaceCartesian::show( )
{
  main_win_object_scene_node_->setVisible(initialized_);
  main_win_point_cloud_->setVisible(initialized_);
  gripper_scene_node_->setVisible(initialized_);
  if ( initialized_ )
  {
    cartesian_control_->show();
    if ( !main_win_point_cloud_->isAttached() )
    {
      main_win_object_scene_node_->attachObject( main_win_point_cloud_ );
    }
    if ( !point_cloud_->isAttached() )
    {
      gripper_scene_node_->attachObject( point_cloud_ );
    }
  }
}

void GripperClickPlaceCartesian::hide( )
{
  main_win_object_scene_node_->setVisible(false);
  main_win_point_cloud_->detachFromParent();
  point_cloud_->detachFromParent();
  gripper_scene_node_->setVisible(false);
  cartesian_control_->hide();
}

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

  point_cloud_->clear();
  main_win_point_cloud_->clear();

  ROS_INFO("Place click received grasp orientation: %f %f %f %f, default object orientation %f %f %f %f",
           goal->grasp_pose.orientation.w, goal->grasp_pose.orientation.x,
           goal->grasp_pose.orientation.y, goal->grasp_pose.orientation.z,
           goal->default_orientation.w, goal->default_orientation.x,
           goal->default_orientation.y, goal->default_orientation.z);

  //WARNING: this essentially expects the default object orientation to be relative to base_link
  object_orientation_ = Ogre::Quaternion( goal->default_orientation.w, goal->default_orientation.x,
                                          goal->default_orientation.y, goal->default_orientation.z );
  if (object_orientation_.Norm() < 1.0e-5)
  {
    ROS_WARN("Gripper click place received 0-length quaternion for default object orientation. Using identity.");
    object_orientation_ = Ogre::Quaternion(1, 0, 0, 0);
  }

  //default gripper position relative to object is so that we are controlling a point between the fingers
  //(20 cm in front of wrist), and we use the grasp's orientation
  gripper_orientation_ = Ogre::Quaternion( goal->grasp_pose.orientation.w, goal->grasp_pose.orientation.x,
                                           goal->grasp_pose.orientation.y, goal->grasp_pose.orientation.z );
  if (gripper_orientation_.Norm() < 1.0e-5)
  {
    ROS_WARN("Gripper click place received 0-length quaternion for grasp orientation. Using identity.");
    gripper_orientation_ = Ogre::Quaternion(1, 0, 0, 0);
  }
  gripper_position_ = Ogre::Vector3(goal->grasp_pose.position.x, goal->grasp_pose.position.y, 
                                    goal->grasp_pose.position.z); 

  rviz::robotToOgre( gripper_orientation_ );
  rviz::robotToOgre( gripper_position_ );

  //copy point cloud
  if ( goal->object.cluster.points.size() > 0 && (goal->object.cluster.header.frame_id == "r_wrist_roll_link" || 
                                                  goal->object.cluster.header.frame_id == "l_wrist_roll_link") )
  {
    ROS_DEBUG_STREAM( "Received " << goal->object.cluster.points.size() << 
                      " points in frame " << goal->object.cluster.header.frame_id );

    //TODO: fix this, hack for HE visit!
    if (goal->object.cluster.header.frame_id !="r_wrist_roll_link" && 
        goal->object.cluster.header.frame_id !="l_wrist_roll_link")
    {
      ROS_ERROR("Gripper place with object only works if cloud is in {l,r}_wrist_roll_link");
      return false;
    }
    gripper_orientation_ = Ogre::Quaternion(1, 0, 0, 0);
    object_orientation_ = Ogre::Quaternion(1, 0, 0, 0);

    /*
    //convert point cloud to the reference_frame_id of the object
    //which by definition is the frame that the grasp is expressed in
    if (goal->object.cluster.header.frame_id != goal->object.reference_frame_id)
    {
      //for now, we just complain if it's not
      ROS_ERROR("Gripper click for place: object point cloud is not in reference_frame_id. "
                "Sorry, this case is not handled yet.");
      return false;
    }
    //at this point, the grasp pose will give us the transform from point cloud to gripper
    */

    std::vector<ogre_tools::PointCloud::Point> raw_points;
    raw_points.resize( goal->object.cluster.points.size() );

    Ogre::Vector3 mean(0,0,0);

    for ( unsigned i=0; i<goal->object.cluster.points.size(); i++ )
    {
      raw_points[i].x = goal->object.cluster.points[i].x;
      raw_points[i].y = goal->object.cluster.points[i].y;
      raw_points[i].z = goal->object.cluster.points[i].z;
      raw_points[i].setColor( 1, 0, 0 );

      mean += Ogre::Vector3( goal->object.cluster.points[i].x, 
                             goal->object.cluster.points[i].y, 
                             goal->object.cluster.points[i].z );
    }

    mean /= raw_points.size();
    gripper_position_ = -1.0 * mean;

    ROS_INFO( "point cloud has %d extra channels", (int)goal->object.cluster.channels.size() );

    //apply color information if there's any
    for( unsigned c=0; c<goal->object.cluster.channels.size(); c++)
    {
      if ( goal->object.cluster.channels[c].values.size() != goal->object.cluster.points.size() )
      {
        continue;
      }

      ROS_INFO( "channel %d: %s", c, goal->object.cluster.channels[c].name.c_str() );

      if ( goal->object.cluster.channels[c].name == "intensity" )
      {
        for ( unsigned i=0; i<goal->object.cluster.points.size(); i++ )
        {
          raw_points[i].setColor( goal->object.cluster.channels[c].values[i], 
                                  goal->object.cluster.channels[c].values[i], 
                                  goal->object.cluster.channels[c].values[i] );
        }
      }
      if ( goal->object.cluster.channels[c].name == "rgb" )
      {
        for ( unsigned i=0; i<goal->object.cluster.points.size(); i++ )
        {
          raw_points[i].color = *( (uint32_t*) & (goal->object.cluster.channels[c].values[i]) );
        }
      }
    }

    point_cloud_->addPoints( &raw_points[0], raw_points.size() );
    main_win_point_cloud_->addPoints( &raw_points[0], raw_points.size() );
  }

  hide();
  initialized_ = false;

  setDescription("Please double-click at the spot where you want to place the object.");
  return true;
}

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

  gripper_scene_node_->setPosition( cartesian_control_->getPosition() );
  gripper_scene_node_->setOrientation( cartesian_control_->getOrientation() );
  main_win_object_scene_node_->setPosition( cartesian_control_->getPosition() );
  main_win_object_scene_node_->setOrientation( cartesian_control_->getOrientation() );
}

void GripperClickPlaceCartesian::onRenderWindowMouseEvents( wxMouseEvent& event, Ogre::Ray &mouse_ray )
{
  int x = event.GetX();
  int y = event.GetY();
  
  if ( !initialized_ )
  {
    if ( event.ButtonDown(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;
      // rotate the controls 45 degrees relative to the reference frame (should be base_link), 
      //so all axes are more or less visible
      Ogre::Quaternion controls_orientation( Ogre::Degree( 45 ), Ogre::Vector3(0,1,0) );

      // gripper pose relative to the fixed frame
      Ogre::Vector3 gripper_position = controls_position + object_orientation_ * gripper_position_;
      Ogre::Quaternion gripper_orientation = object_orientation_ * gripper_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 position by double-clicking on the desired location.\n\nPress OK When you are finished.");
      initialized_ = true;
      show();
    }
  }
  else
  {
    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 GripperClickPlaceCartesian::hasValidResult()
{
  return initialized_;
}


bool GripperClickPlaceCartesian::getResult(GripperClickPlaceResult &result)
{
  if ( !hasValidResult() )
  {
    return false;
  }

  result.place_pose = cartesian_control_->getPose();
  return true;
}

void GripperClickPlaceCartesian::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 = 0;
}

}