$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2009, Willow Garage, Inc. 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 Willow Garage 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(s): Peter Brook 00036 00037 #ifndef _DATABASE_HELPER_CLASSES_H_ 00038 #define _DATABASE_HELPER_CLASSES_H_ 00039 00040 #include <database_interface/db_class.h> 00041 #include <geometry_msgs/Pose.h> 00042 00043 namespace database_interface { 00044 00046 template <> 00047 class DBField< std::vector<float> > : public DBFieldData< std::vector<float> > 00048 { 00049 public: 00050 DBField(Type type, DBClass *owner, std::string name, std::string table_name, bool write_permission) : 00051 DBFieldData< std::vector<float> >(type, owner, name, table_name, write_permission) {} 00052 00053 DBField(DBClass *owner, const DBField< std::vector<float> > *other) : 00054 DBFieldData< std::vector<float> >(owner, other) 00055 { 00056 this->copy(other); 00057 } 00058 00059 virtual bool fromBinary(const char* binary, size_t length) 00060 { 00061 //recall that the size given here is in bytes 00062 if (!length) 00063 { 00064 data_.clear(); 00065 return true; 00066 } 00067 //check if this is indeed a multiple of a number of floats 00068 if ( length % sizeof(float) != 0) 00069 { 00070 std::cerr << "Binary conversion of " << length << " bytes to vector<float> failed\n"; 00071 return false; 00072 } 00073 data_.resize(length / sizeof(float)); 00074 memcpy(&(data_[0]), binary, length); 00075 return true; 00076 } 00077 00078 virtual bool toBinary(const char* &binary, size_t &length) const 00079 { 00080 length = sizeof(float) * data_.size(); 00081 if (!data_.empty()) 00082 { 00083 binary = reinterpret_cast<const char*>(&(data_[0])); 00084 } 00085 return true; 00086 } 00087 }; 00088 00090 template <> 00091 class DBField< std::vector<uint8_t> > : public DBFieldData< std::vector<uint8_t> > 00092 { 00093 public: 00094 DBField(Type type, DBClass *owner, std::string name, std::string table_name, bool write_permission) : 00095 DBFieldData< std::vector<uint8_t> >(type, owner, name, table_name, write_permission) {} 00096 00097 DBField(DBClass *owner, const DBField< std::vector<uint8_t> > *other) : 00098 DBFieldData< std::vector<uint8_t> >(owner, other) 00099 { 00100 this->copy(other); 00101 } 00102 00103 virtual bool fromBinary(const char* binary, size_t length) 00104 { 00105 //recall that the size given here is in bytes 00106 if (!length) 00107 { 00108 data_.clear(); 00109 return true; 00110 } 00111 //check if this is indeed a multiple of a number of uint8's 00112 if ( length % sizeof(uint8_t) != 0) 00113 { 00114 std::cerr << "Binary conversion of " << length << " bytes to vector<uint8_t> failed\n"; 00115 return false; 00116 } 00117 data_.resize(length / sizeof(uint8_t)); 00118 memcpy(&(data_[0]), binary, length); 00119 return true; 00120 } 00121 00122 virtual bool toBinary(const char* &binary, size_t &length) const 00123 { 00124 length = sizeof(uint8_t) * data_.size(); 00125 if (!data_.empty()) 00126 { 00127 binary = reinterpret_cast<const char*>(&(data_[0])); 00128 } 00129 return true; 00130 } 00131 }; 00132 00133 00134 00135 00136 } //namespace database_interface 00137 00138 namespace household_objects_database { 00139 00140 /* Database convention is currently as follows: 00141 00142 - joints are represented in an array with the value of each DOF of the robot: {j1} 00143 (only one joint for the PR2) 00144 00145 - position and orientation are also in an array, with pose as a translation and quaternion: 00146 {tx, ty, tz, qw, qx, qy, qz} 00147 WARNING: in the database, the w component of the quaternion is first!!! 00148 */ 00149 00151 00154 class DatabasePose 00155 { 00156 public: 00157 geometry_msgs::Pose pose_; 00158 }; 00159 00160 std::istream& operator >> (std::istream &str, DatabasePose &dhp); 00161 std::ostream& operator << (std::ostream &str, const DatabasePose &dhp); 00162 00164 00167 class DatabaseHandPosture 00168 { 00169 public: 00170 std::vector<double> joint_angles_; 00171 }; 00172 00173 std::istream& operator >> (std::istream &str, DatabaseHandPosture &dhp); 00174 std::ostream& operator << (std::ostream &str, const DatabaseHandPosture &dhp); 00175 00176 } //namespace 00177 00178 #endif