custom_environment2.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, University of Colorado, Boulder
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Univ of CO, Boulder nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Dave Coleman
00036    Desc:   Custom environments for running MoveIt!
00037 */
00038 
00039 #include <moveit_visual_tools/visual_tools.h> // simple tool for showing grasps
00040 
00041 #ifndef BAXTER_PICK_PLACE__CUSTOM_ENVIRONMENT_
00042 #define BAXTER_PICK_PLACE__CUSTOM_ENVIRONMENT_
00043 
00044 namespace baxter_pick_place
00045 {
00046 
00047 // environment
00048 static const std::string SUPPORT_SURFACE1_NAME = "monitor";
00049 static const std::string SUPPORT_SURFACE2_NAME = "desk";
00050 static const std::string SUPPORT_SURFACE3_NAME = "table";
00051 static const std::string WALL1_NAME = "back_wall";
00052 static const std::string WALL2_NAME = "right_wall";
00053 static const std::string WALL3_NAME = "left_wall";
00054 
00055 // table dimensions
00056 static const double TABLE_HEIGHT = 0.818;
00057 
00058 static const double TABLE_WIDTH  = 0.87;
00059 static const double TABLE_DEPTH  = 0.44;
00060 static const double TABLE_X = 0.83;
00061 static const double TABLE_Y = 0.15;
00062 
00063 // object dimensions
00064 static const double OBJECT_SIZE = 0.04;
00065 
00066 void createEnvironment(moveit_visual_tools::VisualToolsPtr visual_tools_)
00067 {
00068   visual_tools_->cleanupCO(SUPPORT_SURFACE1_NAME);
00069   visual_tools_->cleanupCO(SUPPORT_SURFACE2_NAME);
00070   visual_tools_->cleanupCO(WALL1_NAME);
00071   visual_tools_->cleanupCO(WALL2_NAME);
00072   visual_tools_->cleanupCO(WALL3_NAME);
00073 
00074   // --------------------------------------------------------------------------------------------
00075   // Add objects to scene
00076 
00077   // Walls                          x,     y,     angle,  width, name
00078   visual_tools_->publishCollisionWall(-0.55, 0,     0,      2.2,   WALL1_NAME);  // back wall
00079   visual_tools_->publishCollisionWall(0.05,  -1.1,  M_PI/2, 2.0,   WALL2_NAME);  // baxter's right
00080   visual_tools_->publishCollisionWall(0.05,  1.1,   M_PI/2, 2.0,   WALL3_NAME);  // baxter's left
00081 
00082   // Tables                          x,       y,       angle, width,       height,       depth,       name
00083   visual_tools_->publishCollisionTable(0.78,    -0.8,    0,     0.4,         1.4,          0.47,        SUPPORT_SURFACE1_NAME); // computer monitor
00084   visual_tools_->publishCollisionTable(0.78,    -0.45,   0,     0.4,         0.7,          0.47,        SUPPORT_SURFACE2_NAME); // my desk
00085   visual_tools_->publishCollisionTable(TABLE_X, TABLE_Y, 0,     TABLE_WIDTH, TABLE_HEIGHT, TABLE_DEPTH, SUPPORT_SURFACE3_NAME); // andy table
00086 }
00087 
00088 double getTableHeight(double floor_offset)
00089 {
00090   return TABLE_HEIGHT + floor_offset + OBJECT_SIZE / 2;
00091 }
00092 
00093 void getTableWidthRange(double &y_min, double &y_max)
00094 {
00095   y_min = TABLE_Y - TABLE_WIDTH / 2;
00096   y_max = TABLE_Y + TABLE_WIDTH / 2;
00097 }
00098 
00099 void getTableDepthRange(double &x_min, double &x_max)
00100 {
00101   x_min = TABLE_X - TABLE_DEPTH / 2;
00102   x_max = TABLE_X + TABLE_DEPTH / 2;
00103 }
00104 
00105 } // namespace
00106 
00107 #endif


moveit_simple_grasps
Author(s): Dave Coleman
autogenerated on Fri Aug 28 2015 11:36:01