00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include "pr2_gripper_click/gripper_click_3d_gripper.h"
00031
00032
00033 #include <tf/transform_datatypes.h>
00034
00035 #include <geometry_msgs/PoseStamped.h>
00036
00037 #include <rviz/render_panel.h>
00038 #include <rviz/mesh_loader.h>
00039
00040 #include <OGRE/OgreEntity.h>
00041 #include <OGRE/OgreManualObject.h>
00042
00043 #include <rviz_interaction_tools/unique_string_manager.h>
00044 #include <rviz_interaction_tools/image_tools.h>
00045
00046 namespace pr2_gripper_click {
00047
00048 GripperClick3DGripper::GripperClick3DGripper() :
00049 GripperClickPlugin<GripperClickPickupAction>("3D Gripper"),
00050 gripper_angle_(0),
00051 gripper_roll_(0),
00052 valid_transform_(false)
00053 {
00054 }
00055
00056
00057 GripperClick3DGripper::~GripperClick3DGripper()
00058 {
00059 }
00060
00061
00062 void GripperClick3DGripper::init( rviz::VisualizationManager *vis_manager, Ogre::SceneManager* scene_manager, Ogre::SceneNode* camera_node )
00063 {
00064 rviz_interaction_tools::UniqueStringManager usm;
00065
00066 scene_node_ = camera_node->createChildSceneNode();
00067
00068 line_object_ = scene_manager->createManualObject( usm.unique("GripperClickLineIndicator") );
00069 line_object_->setDynamic(true);
00070 line_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP);
00071 line_object_->position(0.0, 0.0, 0.0);
00072 line_object_->position(0.0, 0.0, 0.0);
00073 line_object_->index(0);
00074 line_object_->index(1);
00075 line_object_->index(0);
00076 line_object_->end();
00077
00078 Ogre::AxisAlignedBox aabInf;
00079 aabInf.setInfinite();
00080 line_object_->setBoundingBox(aabInf);
00081
00082
00083 line_object_->setRenderQueueGroup(Ogre::RENDER_QUEUE_OVERLAY);
00084
00085 scene_node_->attachObject(line_object_);
00086 }
00087
00088 bool GripperClick3DGripper::setGoal( const GripperClickPickupGoal::ConstPtr &goal )
00089 {
00090 camera_info_ = goal->sensor_data.camera_info;
00091 disparity_image_ = goal->sensor_data.disparity_image;
00092 valid_transform_ = false;
00093
00094 setDescription("Please double-click on the desired object to set the initial gripper position.");
00095 return true;
00096 }
00097
00098
00099 void GripperClick3DGripper::show( )
00100 {
00101 scene_node_->setVisible(true);
00102 }
00103
00104 void GripperClick3DGripper::hide( )
00105 {
00106 scene_node_->setVisible(false);
00107 }
00108
00109 void GripperClick3DGripper::update(float wall_dt, float ros_dt)
00110 {
00111 }
00112
00113 void GripperClick3DGripper::getGripperParams( bool &visible, Ogre::Vector3 &position, Ogre::Quaternion &orientation, float &angle )
00114 {
00115 visible = valid_transform_;
00116 if ( visible )
00117 {
00118 btTransform trans = computeOrientation();
00119
00120
00121 btTransform back_trans(btQuaternion(0, 0, 0, 1), btVector3(-0.14, 0, 0));
00122 trans = trans * back_trans;
00123 btQuaternion rot = trans.getRotation();
00124 btVector3 trl = trans.getOrigin();
00125
00126 position = Ogre::Vector3(trl.x(), trl.y(), trl.z());
00127 orientation = Ogre::Quaternion(rot.w(), rot.x(), rot.y(), rot.z());
00128
00129 angle = gripper_angle_;
00130 }
00131 }
00132
00133
00134 btTransform GripperClick3DGripper::computeOrientation()
00135 {
00136 btTransform transform;
00137 float px, py, pz;
00138 rviz_interaction_tools::getPoint(disparity_image_, camera_info_, dlb_.y_, dlb_.x_, px, py, pz);
00139 transform.setOrigin(btVector3(px, py, pz));
00140
00141 float dy = lb_.y_;
00142 float dx = lb_.x_;
00143 float dz = std::min(sqrt(dx*dx + dy*dy), 200.0);
00144 dz = sqrt( 200 * 200 - dz * dz);
00145 btVector3 x_dir(dx, dy, dz);
00146 x_dir.normalize();
00147
00148 btVector3 y_dir(1, 0, 0);
00149 if (fabs(x_dir.dot(y_dir)) > 0.99) y_dir = btVector3(0, 1, 0);
00150
00151 btVector3 z_dir = x_dir.cross(y_dir);
00152 z_dir.normalize();
00153 y_dir = z_dir.cross(x_dir);
00154
00155 btMatrix3x3 rot( x_dir.x(), x_dir.y(), x_dir.z(),
00156 y_dir.x(), y_dir.y(), y_dir.z(),
00157 z_dir.x(), z_dir.y(), z_dir.z() );
00158 rot = rot.transpose();
00159
00160 transform = btTransform(rot, transform.getOrigin());
00161
00162 btTransform roll_trans( btQuaternion(btVector3(1,0,0), gripper_roll_) );
00163 transform = transform * roll_trans;
00164
00165 btTransform approach_trans(btQuaternion(0, 0, 0, 1), btVector3(gripper_approach_, 0, 0));
00166 transform = transform * approach_trans;
00167
00168 line_object_->beginUpdate(0);
00169 line_object_->position(px,py,pz);
00170 line_object_->position(transform.getOrigin().x(),transform.getOrigin().y(),transform.getOrigin().z());
00171 line_object_->index(0);
00172 line_object_->index(1);
00173 line_object_->index(0);
00174 line_object_->end();
00175
00176 return transform;
00177 }
00178
00179 void GripperClick3DGripper::onRenderWindowMouseEvents( wxMouseEvent& event, Ogre::Ray &mouse_ray )
00180 {
00181 int x = event.GetX();
00182 int y = event.GetY();
00183
00184 if (event.GetWheelRotation() >= event.GetWheelDelta())
00185 {
00186 gripper_roll_ += 0.2;
00187 if (gripper_roll_ > M_PI) gripper_roll_ -= M_PI;
00188 }
00189
00190 if (event.GetWheelRotation() <= -event.GetWheelDelta())
00191 {
00192 gripper_roll_ -= 0.2;
00193 if (gripper_roll_ < -M_PI) gripper_roll_ += M_PI;
00194 }
00195
00196 if (event.ButtonDClick(wxMOUSE_BTN_LEFT))
00197 {
00198 if (rviz_interaction_tools::hasDisparityValue(disparity_image_, y, x))
00199 {
00200 valid_transform_ = true;
00201 dlb_.x_ = x;
00202 dlb_.y_ = y;
00203 gripper_approach_ = 0;
00204 setDescription("Left-click and drag the mouse to change the approaching angle.\nRight-click and drag to move the gripper forward and backwards\nTurn the mouse wheel to change the gripper rotation.\nPress and turn the mouse wheel to adjust the gripper opening.\nYou can reset the position by double-clicking on the desired location.\n\nPress OK When you are finished.");
00205 }
00206 }
00207
00208 if (event.ButtonDown(wxMOUSE_BTN_LEFT))
00209 {
00210 lb_.down_ = true;
00211 lbdown_.x_ = lb_.x_ + x;
00212 lbdown_.y_ = lb_.y_ + y;
00213 }
00214 if (event.ButtonUp(wxMOUSE_BTN_LEFT)) lb_.down_ = false;
00215
00216 if (event.ButtonDown(wxMOUSE_BTN_RIGHT))
00217 {
00218 rb_.down_ = true;
00219 rb_.x_ = x;
00220 rb_.y_ = y;
00221 rb_.ref_ = gripper_approach_;
00222 }
00223 if (event.ButtonUp(wxMOUSE_BTN_RIGHT)) rb_.down_ = false;
00224
00225 if (event.ButtonDown(wxMOUSE_BTN_MIDDLE))
00226 {
00227 mb_.down_ = true;
00228 mb_.x_ = x;
00229 mb_.y_ = y;
00230 mb_.ref_ = gripper_angle_;
00231 }
00232 if (event.ButtonUp(wxMOUSE_BTN_MIDDLE)) mb_.down_ = false;
00233
00234 if (event.Dragging())
00235 {
00236 if (rb_.down_)
00237 {
00238 int dy = rb_.y_ - y;
00239 gripper_approach_ = rb_.ref_ + ((float)dy / 100.0) * 0.1;
00240 }
00241 if (mb_.down_)
00242 {
00243 int dy = y - mb_.y_;
00244 float da = ((float)dy/100.0) * 0.541;
00245 gripper_angle_ = mb_.ref_ + da;
00246 gripper_angle_ = std::min(gripper_angle_ , (float)0.541);
00247 gripper_angle_ = std::max(gripper_angle_ , (float)0.0);
00248 }
00249 if (lb_.down_)
00250 {
00251
00252
00253
00254
00255
00256 lb_.x_ = lbdown_.x_ - x;
00257 lb_.y_ = lbdown_.y_ - y;
00258 }
00259 }
00260
00261 }
00262
00263 bool GripperClick3DGripper::getResult(GripperClickPickupResult &result)
00264 {
00265
00266 btTransform transform = computeOrientation();
00267
00268 btTransform back_trans(btQuaternion(0, 0, 0, 1), btVector3(-0.14, 0, 0));
00269 transform = transform * back_trans;
00270 tf::poseTFToMsg(transform, result.grasp_pose.pose);
00271 result.grasp_pose.header.stamp = ros::Time::now();
00272 result.grasp_pose.header.frame_id = disparity_image_.header.frame_id;
00273 result.gripper_opening = gripper_angle_ / 0.5 * 0.0857;
00274 ROS_INFO("gripper_angle_: %.3f, using gripper opening of %.3f", gripper_angle_, result.gripper_opening);
00275 return true;
00276 }
00277
00278 }