$search
00001 /****************************************************************************** 00002 * \file 00003 * 00004 * $Id: Object.h 676 2012-04-19 18:32:07Z xlokaj03 $ 00005 * 00006 * Copyright (C) Brno University of Technology 00007 * 00008 * This file is part of software developed by Robo@FIT group. 00009 * 00010 * Author: Tomas Lokaj (xlokaj03@stud.fit.vutbr.cz) 00011 * Supervised by: Michal Spanel (spanel@fit.vutbr.cz) 00012 * Date: dd.mm.2011 00013 * 00014 * This file is free software: you can redistribute it and/or modify 00015 * it under the terms of the GNU Lesser General Public License as published by 00016 * the Free Software Foundation, either version 3 of the License, or 00017 * (at your option) any later version. 00018 * 00019 * This file is distributed in the hope that it will be useful, 00020 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00021 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00022 * GNU Lesser General Public License for more details. 00023 * 00024 * You should have received a copy of the GNU Lesser General Public License 00025 * along with this file. If not, see <http://www.gnu.org/licenses/>. 00026 */ 00027 00028 #pragma once 00029 #ifndef OBJECT_H_ 00030 #define OBJECT_H_ 00031 00032 #include "bounding_box.h" 00033 00034 namespace srs_interaction_primitives 00035 { 00036 00040 enum 00041 { 00042 _, GRASP_1, GRASP_2, GRASP_3, GRASP_4, GRASP_5, GRASP_6 00043 }; 00044 00048 enum ObjectResource 00049 { 00050 SHAPE, RESOURCE_FILE 00051 }; 00052 00056 struct PreGraspPosition 00057 { 00058 std::string name; 00059 bool enabled; 00060 geometry_msgs::Pose pose; 00061 00062 PreGraspPosition() 00063 { 00064 enabled = false; 00065 } 00066 }; 00067 00078 class Object : public BoundingBox 00079 { 00080 public: 00087 Object(InteractiveMarkerServerPtr server, std::string frame_id, std::string name); 00088 00092 void insert(); 00093 00098 void setBoundingBoxLWH(geometry_msgs::Point bounding_box_lwh) 00099 { 00100 bounding_box_lwh_ = bounding_box_lwh; 00101 } 00102 00107 geometry_msgs::Point getBoundingBoxLWH() 00108 { 00109 return bounding_box_lwh_; 00110 } 00111 00117 void setPoseLWH(geometry_msgs::Pose pose, geometry_msgs::Point bounding_box_lwh); 00118 00123 void setAllowPregrasp(bool allow_pregrasp) 00124 { 00125 allow_pregrasp_ = allow_pregrasp; 00126 } 00127 00132 void setResource(std::string resource) 00133 { 00134 object_resource_ = RESOURCE_FILE; 00135 resource_ = resource; 00136 } 00137 00142 std::string getResource() 00143 { 00144 return resource_; 00145 } 00146 00151 void setUseMaterial(bool use_material) 00152 { 00153 use_material_ = use_material; 00154 } 00155 00160 bool getUseMaterial() 00161 { 00162 return use_material_; 00163 } 00164 00169 void setShape(arm_navigation_msgs::Shape shape) 00170 { 00171 object_resource_ = SHAPE; 00172 shape_ = shape; 00173 } 00174 00179 arm_navigation_msgs::Shape getShape() 00180 { 00181 return shape_; 00182 } 00183 00188 virtual void setAllowObjectInteraction(bool allow); 00189 00195 void addPreGraspPosition(int pos_id, geometry_msgs::Pose pose); 00196 00201 void removePreGraspPosition(int pos_id); 00202 00206 void addPregraspPositions(); 00207 00211 void removePregraspPositions(); 00212 00216 void updateControls(); 00217 00221 void objectCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); 00222 00226 void menuCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); 00227 00228 private: 00229 void create(); 00230 void createMenu(); 00231 void createMesh(); 00232 void createBoundingBoxControl(); 00233 00234 // Object's attributes 00235 ObjectResource object_resource_; 00236 visualization_msgs::Marker mesh_; 00237 arm_navigation_msgs::Shape shape_; 00238 std::string resource_; 00239 bool use_material_; 00240 bool move_arm_to_pregrasp_onclick_; 00241 bool allow_object_interaction_, allow_pregrasp_; 00242 00243 geometry_msgs::Point bounding_box_lwh_; 00244 bool translated_; 00245 00246 bool show_pregrasp_control_; 00247 00248 PreGraspPosition pregrasp1_, pregrasp2_, pregrasp3_, pregrasp4_, pregrasp5_, pregrasp6_; 00249 00250 visualization_msgs::InteractiveMarkerControl pregrasp1Control_, pregrasp2Control_, pregrasp3Control_, 00251 pregrasp4Control_, pregrasp5Control_, pregrasp6Control_; 00252 00253 // Menu handlers 00254 interactive_markers::MenuHandler::EntryHandle menu_handler_interaction_, menu_handler_interaction_movement_, 00255 menu_handler_interaction_rotation_, menu_handler_show_pregrasp_, 00256 menu_handler_move_to_pregrasp_; 00257 }; 00258 00259 } 00260 00261 #endif /* OBJECT_H_ */ 00262