|
From: <tf...@us...> - 2008-04-02 22:10:34
|
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.
|