$search
00001 /* 00002 * Copyright (c) 2010, Thomas Ruehr <ruehr@cs.tum.edu> 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 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 #ifndef __OPERATEHANDLECONTROLLER_H__ 00031 #define __OPERATEHANDLECONTROLLER_H__ 00032 00033 #include <std_srvs/Empty.h> 00034 #include <tf/transform_listener.h> 00035 #include <geometry_msgs/PoseStamped.h> 00036 #include <sensor_msgs/PointCloud2.h> 00037 00038 00039 class RobotArm; 00040 class Pressure; 00041 00042 class OperateHandleController { 00043 00044 public: 00045 00046 static tf::Stamped<tf::Pose> getCopPose(const char name[], const char frame[]); 00047 static tf::Stamped<tf::Pose> getBowlPose(); 00048 static btVector3 getPlatePose(); 00049 static btVector3 getTabletPose(); 00050 00051 //returns a handle that can be used to close the thing again, 00052 static int operateHandle(int arm, tf::Stamped<tf::Pose> aM, int numretry = 0); 00053 00054 static int graspHandle(int arm_, tf::Stamped<tf::Pose> aM); 00055 00056 00057 static tf::Stamped<tf::Pose> getHandlePoseFromMarker(int arm_, int pos); 00058 00059 // starting from a nice initial pose 00060 static void close(int side_c, int handle_); 00061 00062 static void pickPlate(btVector3 plate, double width = 0.34); 00063 00064 static void getPlate(int object, double zHint = 0); 00065 00066 static void singleSidedPick(int side,tf::Stamped<tf::Pose> start, tf::Stamped<tf::Pose> end); 00067 00068 static int maxHandle; 00069 00070 static std::vector<std::vector<tf::Stamped<tf::Pose> *> > openingTraj; 00071 00072 //static void spinnerL(double l, double back); 00073 static void spinnerL(double x, double y, double z); 00074 00075 static void openGrippers(bool wait = true); 00076 static void closeGrippers(bool wait = true); 00077 00078 static void plateCarryPose(); 00079 00080 static void plateTuckPose(); 00081 static void plateTuckPoseLeft(); 00082 static void plateTuckPoseRight(); 00083 00084 static void plateAttackPose(); 00085 static void plateAttackPoseLeft(); 00086 static void plateAttackPoseRight(); 00087 00088 }; 00089 00090 00091 00092 00093 #endif