[Personalrobots-commit] SF.net SVN: personalrobots: [75] pkg/trunk/libTF/simple

[<tf...@us...>]

Revision: 75
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=75&view=rev
Author:   tfoote
Date:     2008-04-02 15:10:38 -0700 (Wed, 02 Apr 2008)

Log Message:
-----------
successfully catching loops and unconnected frames

Modified Paths:
--------------
    pkg/trunk/libTF/simple/libTF.cc
    pkg/trunk/libTF/simple/main.cc

Modified: pkg/trunk/libTF/simple/libTF.cc
===================================================================
--- pkg/trunk/libTF/simple/libTF.cc	2008-04-02 20:48:56 UTC (rev 74)
+++ pkg/trunk/libTF/simple/libTF.cc	2008-04-02 22:10:38 UTC (rev 75)
@@ -46,7 +46,7 @@
 
 void TransformReference::set(unsigned int frameID, unsigned int parentID, double a,double b,double c,double d,double e,double f)
 {
-  if (frameID > MAX_NUM_FRAMES || parentID > MAX_NUM_FRAMES)
+  if (frameID > MAX_NUM_FRAMES || parentID > MAX_NUM_FRAMES || frameID == NO_PARENT || frameID == ROOT_FRAME)
     throw InvalidFrame;
   
   if (frames[frameID] == NULL)
@@ -120,36 +120,45 @@
 
   unsigned int frame = target_frame;
   unsigned int counter = 0;  //A counter to keep track of how deep we've descended
-  while (frame != NO_PARENT  && frame != ROOT_FRAME) //Descend until we reach the root node or don't have a parent
+  while (true)
     {
+      mTfLs.inverseTransforms.push_back(frame);
+      if (frame == NO_PARENT  || frame == ROOT_FRAME) //Descend until we reach the root node or don't have a parent
+	break;
       if (getFrame(frame)->getParent() > MAX_NUM_FRAMES) throw InvalidFrame;
-      mTfLs.inverseTransforms.push_back(frame);
       //   std::cout <<"Frame: " << frame <<std::endl;
       frame = getFrame(frame)->getParent();
       /* Check if we've gone too deep.  Probably a loop */
       if (counter++ > MAX_GRAPH_DEPTH)
 	throw(MaxSearchDepth);
     }
-  mTfLs.inverseTransforms.push_back(0);
+  //  mTfLs.inverseTransforms.push_back(0);
   
   frame = source_frame;
   counter = 0;
-  while (frame != NO_PARENT  && frame != ROOT_FRAME) //Descend until we reach the root node or don't have a parent
+  while (true)//(frame != NO_PARENT  && frame != ROOT_FRAME) //Descend until we reach the root node or don't have a parent
     {
+      mTfLs.forwardTransforms.push_back(frame);
+      if (frame == NO_PARENT  || frame == ROOT_FRAME) //Descend until we reach the root node or don't have a parent
+	break;
       if (getFrame(frame)->getParent() > MAX_NUM_FRAMES) throw InvalidFrame;
-      mTfLs.forwardTransforms.push_back(frame);
       frame = getFrame(frame)->getParent();
       /* Check if we've gone too deep.  Probably a loop */
       if (counter++ > MAX_GRAPH_DEPTH)
 	throw(MaxSearchDepth);
     }
-  mTfLs.forwardTransforms.push_back(0);
+  //  mTfLs.forwardTransforms.push_back(0);
   
   
   /* Make sure the end of the search shares a parent. */
   if (mTfLs.inverseTransforms.back() != mTfLs.forwardTransforms.back())
     throw(NoFrameConnectivity);
 
+  /* Make sure that we don't have a no parent at the top */
+  std::cout << "Back = " << mTfLs.inverseTransforms.back()<<" " << mTfLs.forwardTransforms.back();
+  //  if (mTfLs.inverseTransforms.back() == NO_PARENT ||  mTfLs.forwardTransforms.back() == NO_PARENT)
+  //   throw(NoFrameConnectivity);
+
   while (mTfLs.inverseTransforms.back() == mTfLs.forwardTransforms.back())
     {
       //      std::cout << "removing " << mTfLs.inverseTransforms.back() << std::endl;

Modified: pkg/trunk/libTF/simple/main.cc
===================================================================
--- pkg/trunk/libTF/simple/main.cc	2008-04-02 20:48:56 UTC (rev 74)
+++ pkg/trunk/libTF/simple/main.cc	2008-04-02 22:10:38 UTC (rev 75)
@@ -32,7 +32,7 @@
       mTR.set(10,2,1,1,1,dyaw,dp,dr);
       mTR.set(2,3,1,1,1,dyaw,dp,dr);
       mTR.set(3,5,dx,dy,dz,dyaw,dp,dr);
-      mTR.set(5,0,dx,dy,dz,dyaw,dp,dr);
+      mTR.set(5,1,dx,dy,dz,dyaw,dp,dr);
       mTR.set(6,5,dx,dy,dz,dyaw,dp,dr);
       mTR.set(7,6,1,1,1,dyaw,dp,dr);
       mTR.set(8,7,1,1,1,dyaw,dp,dr);
@@ -43,10 +43,11 @@
       catch (TransformReference::LookupException &ex)
 	{
 	  std::cout << "Caught " << ex.what()<<std::endl;
-	  mTR.view(10,8);
 	}
-	  
 	
+      std::cout<<"Viewing (10,8)";  
+      mTR.view(10,8);
+	
 
 
       std::cout <<"Calling get(10,8)"<<std::endl;
@@ -54,6 +55,34 @@
       NEWMAT::Matrix mat = mTR.get(10,8);
 
       std::cout << "Result" << std::endl << mat<< std::endl;
+
+
+
+
+      mTR.set(6,7,dx,dy,dz,dyaw,dp,dr);
+      
+      try {
+	mTR.view(10,8);
+      }
+      catch (TransformReference::MaxDepthException &ex)
+	{
+	  std::cout <<"caught loop in graph"<<std::endl;
+	}
+
+      mTR.set(6,0,dx,dy,dz,dyaw,dp,dr);
+      
+      try {
+	mTR.view(10,8);
+      }
+      catch (TransformReference::ConnectivityException &ex)
+	{
+	  std::cout <<"caught unconnected frame"<<std::endl;
+	}
+
+
+
+
+
 #ifdef DONOTUSE
 
       //      NEWMAT::Matrix mat(4,4);


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.