gripper_click_dot.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_dot.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 {

GripperClickDot::GripperClickDot() :
    GripperClickPlugin<GripperClickPlaceAction>("Dot Indicator")
{
  click_info_.valid_ = false;
}

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

  dot_object_ = scene_manager->createManualObject( usm.unique("GripperClickDotIndicator") );
  dot_object_->setUseIdentityProjection(true);
  dot_object_->setUseIdentityView(true);  
  dot_object_->setDynamic(true);
  dot_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_LIST);
  dot_object_->position(0.0, 0.0, 0.0);
  dot_object_->position(0.0, 0.0, 0.0);
  dot_object_->position(0.0, 0.0, 0.0);
  dot_object_->position(0.0, 0.0, 0.0);
  dot_object_->index(0);
  dot_object_->index(1);
  dot_object_->index(2);
  dot_object_->index(1);
  dot_object_->index(2);
  dot_object_->index(3);
  dot_object_->end();
  Ogre::AxisAlignedBox aabInf;
  aabInf.setInfinite();
  dot_object_->setBoundingBox(aabInf);
  //click indicator will be rendered last
  dot_object_->setRenderQueueGroup(Ogre::RENDER_QUEUE_OVERLAY);
  scene_manager->getRootSceneNode()->attachObject(dot_object_);
}


void GripperClickDot::show( )
{
  dot_object_->setVisible( true );
}

void GripperClickDot::hide( )
{
  dot_object_->setVisible( false );
}


bool GripperClickDot::setGoal( const GripperClickPlaceGoal::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;

  return true;
}

void GripperClickDot::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 d = 0.02;
  dot_object_->beginUpdate(0);
  dot_object_->position(c_x + d, c_y + d, 0.0);
  dot_object_->position(c_x - d, c_y + d, 0.0);
  dot_object_->position(c_x - d, c_y - d, 0.0);
  dot_object_->position(c_x + d, c_y - d, 0.0);
  dot_object_->index(0);
  dot_object_->index(1);
  dot_object_->index(2);
  dot_object_->index(0);
  dot_object_->index(2);
  dot_object_->index(3);
  dot_object_->end();  
}

void GripperClickDot::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_.down_x_ = x;
      click_info_.down_y_ = y;
      click_info_.valid_ = true;
    }
    else ROS_INFO("Bad click");
  }
}

bool GripperClickDot::hasValidResult()
{
  return click_info_.valid_;
}


bool GripperClickDot::getResult(GripperClickPlaceResult &result)
{
  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);
  
  result.place_pose.pose.position.x = px;
  result.place_pose.pose.position.y = py;
  result.place_pose.pose.position.z = pz;
  //for now, we return identity orientation
  result.place_pose.pose.orientation.x = 0;
  result.place_pose.pose.orientation.y = 0;
  result.place_pose.pose.orientation.z = 0;
  result.place_pose.pose.orientation.w = 1;
  result.place_pose.header.stamp = ros::Time::now();
  result.place_pose.header.frame_id = disparity_image_.header.frame_id;
  return true;
}

}