RelPoseFactory.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2009 by Ulrich Friedrich Klank <klank@in.tum.de>
00003  *
00004  * This program is free software; you can redistribute it and/or modify
00005  * it under the terms of the GNU General Public License as published by
00006  * the Free Software Foundation; either version 3 of the License, or
00007  * (at your option) any later version.
00008  *
00009  * This program is distributed in the hope that it will be useful,
00010  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00011  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012  * GNU General Public License for more details.
00013  *
00014  * You should have received a copy of the GNU General Public License
00015  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
00016  */
00017 
00018 
00019 #include "RelPoseFactory.h"
00020 #include "XMLTag.h"
00021 #define XML_PROPERTY_NUM "Num"
00022 
00023 using namespace cop;
00024 
00025 #ifdef NO_LO_SERVICE_AVAILABLE
00026 std::vector<RelPose*> RelPoseFactory::s_relPoses;
00027 #else  /*NO_LO_SERVICE_AVAILABLE*/
00028 Comm* RelPoseFactory::s_loService = NULL;
00029 #endif /*NO_LO_SERVICE_AVAILABLE*/
00030 
00031 RelPoseFactory::RelPoseFactory(void)
00032 {
00033 }
00034 
00035 RelPoseFactory::~RelPoseFactory(void)
00036 {
00037 }
00038 
00039 void RelPoseFactory::DisposeList()
00040 {
00041 #ifdef NO_LO_SERVICE_AVAILABLE
00042   for(std::vector<RelPose*>::iterator iter = s_relPoses.begin();
00043     iter != s_relPoses.end(); iter++)
00044   {
00045     delete (*iter);
00046   }
00047   s_relPoses.clear();
00048 #endif /*NO_LO_SERVICE_AVAILABLE*/
00049 }
00050 
00051 #ifdef NO_LO_SERVICE_AVAILABLE
00052 XMLTag* RelPoseFactory::SaveList()
00053 {
00054   XMLTag* tag = new XMLTag(XML_NODE_RELPOSELIST);
00055   tag->AddProperty(XML_PROPERTY_NUM, (int)s_relPoses.size());
00056   for(std::vector<RelPose*>::iterator iter = s_relPoses.begin();
00057     iter != s_relPoses.end(); iter++)
00058   {
00059     tag->AddChild((*iter)->Save());
00060   }
00061   return tag;
00062 }
00063 
00064 void RelPoseFactory::LoadList(XMLTag* tag)
00065 {
00066   if(tag != NULL)
00067   {
00068     int num = tag->GetPropertyInt(XML_PROPERTY_NUM);
00069     for(int i = 0; i< num; i++)
00070     {
00071       FRelPose(tag->GetChild(i));
00072     }
00073   }
00074 }
00075 #else
00076 #endif /*NO_LO_SERVICE_AVAILABLE*/
00077 
00078 
00079 RelPose* RelPoseFactory::FRelPoseWorld()
00080 {
00081   RelPose* pose = GetRelPose(ID_WORLD);
00082 #ifdef NO_LO_SERVICE_AVAILABLE
00083   if(pose == NULL)
00084     return GetRelPoseIndex(SetRelPose(new RelPose()));
00085   else
00086     return pose;
00087 #else
00088   return pose;
00089 #endif /*NO_LO_SERVICE_AVAILABLE*/
00090 }
00091 
00092 RelPose* RelPoseFactory::FRelPose(XMLTag* tag)
00093 {
00094   if(tag != NULL && tag->GetName().compare(XML_NODE_RELPOSE) == 0)
00095   {
00096     int id = tag->GetPropertyInt(XML_ATTRIBUTE_LOID);
00097     if(id == 0)
00098     {
00099       std::string name = tag->GetProperty(XML_ATTRIBUTE_LOID);
00100       return GetRelPose(name);
00101     }
00102     if(id == ID_WORLD)
00103     {
00104       RelPose* world = GetRelPose(id);
00105 
00106       return world == NULL ? FRelPoseWorld() : world;
00107     }
00108     RelPose* dingens = GetRelPose(id) ;
00109     if(dingens != NULL)
00110       return dingens;
00111     printf("Trying to load non existing LO!\n");
00112     return NULL;
00113   }
00114   else
00115    return NULL;
00116 }
00117 
00118 inline RelPose* RelPoseFactory::GetRelPose(LocatedObjectID_t id)
00119 {
00120 #ifdef NO_LO_SERVICE_AVAILABLE
00121   for(std::vector<RelPose*>::iterator iter = s_relPoses.begin();
00122     iter != s_relPoses.end(); iter++)
00123   {
00124     if((*iter)->m_uniqueID == id)
00125       return (*iter);
00126   }
00127   return NULL;
00128 #else /*NO_LO_SERVICE_AVAILABLE*/
00129   return s_loService->GetPose(id);
00130 #endif /*NO_LO_SERVICE_AVAILABLE*/
00131 }
00132 
00133 RelPose* RelPoseFactory::GetRelPose(LocatedObjectID_t poseId, LocatedObjectID_t parentPoseId)
00134 {
00135 #ifdef NO_LO_SERVICE_AVAILABLE
00136   throw "Not yet implemented";
00137 #else /*NO_LO_SERVICE_AVAILABLE*/
00138   if(poseId != parentPoseId)
00139     return s_loService->GetPoseRelative(poseId, parentPoseId);
00140   else
00141     return s_loService->GetPose(poseId);
00142 #endif /*NO_LO_SERVICE_AVAILABLE*/
00143 }
00144 
00145 
00146 RelPose* RelPoseFactory::GetRelPose(std::string name)
00147 {
00148 #ifdef NO_LO_SERVICE_AVAILABLE
00149   throw "Not yet implemented";
00150 #else /*NO_LO_SERVICE_AVAILABLE*/
00151     return s_loService->GetPose(name);
00152 #endif /*NO_LO_SERVICE_AVAILABLE*/
00153 }
00154 
00155 #ifdef NO_LO_SERVICE_AVAILABLE
00156 inline RelPose* RelPoseFactory::GetRelPoseIndex(LocatedObjectID_t index)
00157 {
00158   return s_relPoses[index];
00159 }
00160 #endif /*NO_LO_SERVICE_AVAILABLE*/
00161 
00162 
00163 #ifdef NO_LO_SERVICE_AVAILABLE
00164 inline int RelPoseFactory::SetRelPose(RelPose* pose)
00165 {
00166   s_relPoses.push_back(pose);
00167   jlo::LocatedObject::SetLastID(pose->m_uniqueID);
00168   return (int)s_relPoses.size() - 1;
00169 }
00170 #endif /*NO_LO_SERVICE_AVAILABLE*/
00171 
00172 RelPose* RelPoseFactory::FRelPose(jlo::LocatedObject& pose)
00173 {
00174 #ifdef NO_LO_SERVICE_AVAILABLE
00175   RelPose* relpose = GetRelPose(pose.m_uniqueID);
00176   if(relpose == NULL)
00177   {
00178     relpose = GetRelPoseIndex(SetRelPose(new RelPose(pose)));
00179   }
00180   return relpose;
00181 #else /*NO_LO_SERVICE_AVAILABLE*/
00182   return new RelPose(pose);
00183 #endif /*NO_LO_SERVICE_AVAILABLE*/
00184 }
00185 
00186 RelPose* RelPoseFactory::FRelPoseIdentityChild(RelPose* parent)
00187 {
00188 #ifdef NO_LO_SERVICE_AVAILABLE
00189   throw "Not yet implemented: FRelPoseIdentityChild";
00190 #else /*NO_LO_SERVICE_AVAILABLE*/
00191   IdentityMatrix m(4);
00192   Matrix cov(6,6);
00193   cov << 0.0<< 0.0<< 0.0<< 0.0<< 0.0<< 0.0<<
00194          0.0<< 0.0<< 0.0<< 0.0<< 0.0<< 0.0<<
00195          0.0<< 0.0<< 0.0<< 0.0<< 0.0<< 0.0<<
00196          0.0<< 0.0<< 0.0<< 0.0<< 0.0<< 0.0<<
00197          0.0<< 0.0<< 0.0<< 0.0<< 0.0<< 0.0<<
00198          0.0<< 0.0<< 0.0<< 0.0<< 0.0<< 0.0;
00199   return RelPoseFactory::FRelPose(parent, m, cov);
00200 #endif /*NO_LO_SERVICE_AVAILABLE*/
00201 }
00202 
00203 
00204 RelPose* RelPoseFactory::CloneRelPose(RelPose* pose)
00205 {
00206 #ifdef NO_LO_SERVICE_AVAILABLE
00207   if(pose->m_uniqueID == ID_WORLD)
00208     return pose;
00209   RelPose* relpose = new RelPose(pose->m_relation, pose->GetMatrix(), pose->GetCovariance());
00210   relpose = GetRelPoseIndex(SetRelPose(relpose));
00211   return relpose;
00212 #else /*NO_LO_SERVICE_AVAILABLE*/
00213   if(pose == NULL)
00214     throw "Invalid pose to clone!";
00215   return new RelPose(*pose);
00216 #endif /*NO_LO_SERVICE_AVAILABLE*/
00217 }
00218 
00219 RelPose* RelPoseFactory::CloneRelPose(LocatedObjectID_t uniqueID)
00220 {
00221 #ifdef NO_LO_SERVICE_AVAILABLE
00222   RelPose* pose = GetRelPose(uniqueID);
00223 
00224   if(pose != NULL)
00225   {
00226     try
00227     {
00228       return CloneRelPose(pose);
00229     }
00230     catch(...)
00231     {
00232       pose = NULL;
00233     }
00234   }
00235   return pose;
00236 #else /*NO_LO_SERVICE_AVAILABLE*/
00237   return GetRelPose(uniqueID);
00238 #endif /*NO_LO_SERVICE_AVAILABLE*/
00239 }
00240 
00241 RelPose* RelPoseFactory::FRelPose(RelPose* pose, Matrix m, Matrix cov)
00242 {
00243 #ifdef NO_LO_SERVICE_AVAILABLE
00244   return GetRelPoseIndex(SetRelPose(new RelPose(pose, m, cov)));
00245 #else /*NO_LO_SERVICE_AVAILABLE*/
00246   return s_loService->CreateNewPose(pose, &m, &cov);
00247 #endif /*NO_LO_SERVICE_AVAILABLE*/
00248 }
00249 
00250 
00251 RelPose* RelPoseFactory::FRelPose(LocatedObjectID_t parent, Matrix m, Matrix cov)
00252 {
00253   #ifdef NO_LO_SERVICE_AVAILABLE
00254   throw "Not yet implemeted";
00255 #else /*NO_LO_SERVICE_AVAILABLE*/
00256   return s_loService->CreateNewPose(parent, &m, &cov);
00257 #endif /*NO_LO_SERVICE_AVAILABLE*/
00258 
00259 }
00260 
00261 RelPose* RelPoseFactory::FRelPose(RelPose* pose, LocatedObjectID_t parent, Matrix m, Matrix cov)
00262 {
00263 #ifdef NO_LO_SERVICE_AVAILABLE
00264   throw "Error: not yet implemented";
00265 #else /*NO_LO_SERVICE_AVAILABLE*/
00266   return s_loService->UpdatePose(pose,parent, &m, &cov);
00267 #endif /*NO_LO_SERVICE_AVAILABLE*/
00268 }
00269 
00270 
00271 
00272 RelPose* RelPoseFactory::FRelPose(LocatedObjectID_t id)
00273 {
00274   return GetRelPose(id);
00275 }
00276 
00277 void RelPoseFactory::FreeRelPose(RelPose** pose, bool temporary )
00278 {
00279   if(pose == NULL || *pose == NULL)
00280      return ;
00281   int id = (*pose)->m_uniqueID;
00282   if(id != ID_WORLD)
00283   {
00284 #ifdef NO_LO_SERVICE_AVAILABLE
00285     for(std::vector<RelPose*>::iterator iter = s_relPoses.begin();
00286     iter != s_relPoses.end(); iter++)
00287     {
00288       if((*iter)->m_uniqueID == id)
00289       {
00290         printf("Release relpose id: %d\n", id);
00291         delete (*iter);
00292         *pose = NULL;
00293         s_relPoses.erase(iter);
00294         break;
00295       }
00296     }
00297 #else /*NO_LO_SERVICE_AVAILABLE*/
00298 
00299     if(!s_loService->FreePose((*pose)->m_uniqueID))
00300     {
00301       if(temporary)
00302        {
00303 //    ROS_ERROR("A pose (%ld) marked as temporary could not be deleted", (*p
00304        }
00305     }
00306 #endif  /*NO_LO_SERVICE_AVAILABLE*/
00307   }
00308   if((*pose) != NULL && id != ID_WORLD)
00309   {
00310     delete (*pose);
00311     (*pose) = NULL;
00312   }
00313 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


cognitive_perception
Author(s): Ulrich F Klank
autogenerated on Thu May 23 2013 07:38:34