pr2_gripper_click: gripper_click

00001 /*
00002  * Copyright (c) 2009, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include "pr2_gripper_click/gripper_click_dot.h"
00031 
00032 #include <OGRE/OgreManualObject.h>
00033 
00034 #include <rviz/render_panel.h>
00035 
00036 #include <tf/transform_datatypes.h>
00037 
00038 #include <geometry_msgs/PoseStamped.h>
00039 
00040 #include <rviz_interaction_tools/unique_string_manager.h>
00041 #include <rviz_interaction_tools/image_tools.h>
00042 
00043 namespace pr2_gripper_click {
00044 
00045 GripperClickDot::GripperClickDot() :
00046     GripperClickPlugin<GripperClickPlaceAction>("Dot Indicator")
00047 {
00048   click_info_.valid_ = false;
00049 }
00050 
00051 void GripperClickDot::init( rviz::VisualizationManager *vis_manager, Ogre::SceneManager* scene_manager, Ogre::SceneNode* camera_node )
00052 {
00053   rviz_interaction_tools::UniqueStringManager usm;
00054 
00055   dot_object_ = scene_manager->createManualObject( usm.unique("GripperClickDotIndicator") );
00056   dot_object_->setUseIdentityProjection(true);
00057   dot_object_->setUseIdentityView(true);  
00058   dot_object_->setDynamic(true);
00059   dot_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_LIST);
00060   dot_object_->position(0.0, 0.0, 0.0);
00061   dot_object_->position(0.0, 0.0, 0.0);
00062   dot_object_->position(0.0, 0.0, 0.0);
00063   dot_object_->position(0.0, 0.0, 0.0);
00064   dot_object_->index(0);
00065   dot_object_->index(1);
00066   dot_object_->index(2);
00067   dot_object_->index(1);
00068   dot_object_->index(2);
00069   dot_object_->index(3);
00070   dot_object_->end();
00071   Ogre::AxisAlignedBox aabInf;
00072   aabInf.setInfinite();
00073   dot_object_->setBoundingBox(aabInf);
00074   //click indicator will be rendered last
00075   dot_object_->setRenderQueueGroup(Ogre::RENDER_QUEUE_OVERLAY);
00076   scene_manager->getRootSceneNode()->attachObject(dot_object_);
00077 }
00078 
00079 
00080 void GripperClickDot::show( )
00081 {
00082   dot_object_->setVisible( true );
00083 }
00084 
00085 void GripperClickDot::hide( )
00086 {
00087   dot_object_->setVisible( false );
00088 }
00089 
00090 
00091 bool GripperClickDot::setGoal( const GripperClickPlaceGoal::ConstPtr &goal )
00092 {
00093   camera_info_ = goal->sensor_data.camera_info;
00094   disparity_image_ = goal->sensor_data.disparity_image;
00095 
00096   click_info_.valid_ = false;
00097   click_info_.down_x_ = click_info_.down_y_ = 0;
00098 
00099   return true;
00100 }
00101 
00102 void GripperClickDot::update(float wall_dt, float ros_dt)
00103 {
00104   float c_x = click_info_.down_x_ / ((float)disparity_image_.image.width  / 2.0 ) - 1.0;
00105   float c_y = - (click_info_.down_y_ / ((float)disparity_image_.image.height / 2.0 ) - 1.0);
00106   float d = 0.02;
00107   dot_object_->beginUpdate(0);
00108   dot_object_->position(c_x + d, c_y + d, 0.0);
00109   dot_object_->position(c_x - d, c_y + d, 0.0);
00110   dot_object_->position(c_x - d, c_y - d, 0.0);
00111   dot_object_->position(c_x + d, c_y - d, 0.0);
00112   dot_object_->index(0);
00113   dot_object_->index(1);
00114   dot_object_->index(2);
00115   dot_object_->index(0);
00116   dot_object_->index(2);
00117   dot_object_->index(3);
00118   dot_object_->end();  
00119 }
00120 
00121 void GripperClickDot::onRenderWindowMouseEvents( wxMouseEvent& event, Ogre::Ray &mouse_ray )
00122 {
00123   int x = event.GetX();
00124   int y = event.GetY();
00125   
00126   if (event.ButtonDown(wxMOUSE_BTN_LEFT))
00127   {
00128     if (rviz_interaction_tools::hasDisparityValue(disparity_image_, y, x))
00129     {
00130       ROS_INFO("Good click");
00131       click_info_.down_x_ = x;
00132       click_info_.down_y_ = y;
00133       click_info_.valid_ = true;
00134     }
00135     else ROS_INFO("Bad click");
00136   }
00137 }
00138 
00139 bool GripperClickDot::hasValidResult()
00140 {
00141   return click_info_.valid_;
00142 }
00143 
00144 
00145 bool GripperClickDot::getResult(GripperClickPlaceResult &result)
00146 {
00147   if ( !hasValidResult() )
00148   {
00149     return false;
00150   }
00151 
00152   float px, py, pz;
00153   rviz_interaction_tools::getPoint(disparity_image_, camera_info_,
00154                                    click_info_.down_y_, click_info_.down_x_, 
00155                                    px, py, pz);
00156   
00157   result.place_pose.pose.position.x = px;
00158   result.place_pose.pose.position.y = py;
00159   result.place_pose.pose.position.z = pz;
00160   //for now, we return identity orientation
00161   result.place_pose.pose.orientation.x = 0;
00162   result.place_pose.pose.orientation.y = 0;
00163   result.place_pose.pose.orientation.z = 0;
00164   result.place_pose.pose.orientation.w = 1;
00165   result.place_pose.header.stamp = ros::Time::now();
00166   result.place_pose.header.frame_id = disparity_image_.header.frame_id;
00167   return true;
00168 }
00169 
00170 }
gripper_click_dot.cpp
