Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots: [72] pkg/trunk/libTF/simple
|
From: <tf...@us...> - 2008-03-31 20:56:47
|
Revision: 72
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=72&view=rev
Author: tfoote
Date: 2008-03-31 13:56:54 -0700 (Mon, 31 Mar 2008)
Log Message:
-----------
adding dynamic allocation of frames. and exceptions if an uninitialized frame is tried to be accessed. ticket:4
Modified Paths:
--------------
pkg/trunk/libTF/simple/Makefile
pkg/trunk/libTF/simple/libTF.cc
pkg/trunk/libTF/simple/libTF.hh
pkg/trunk/libTF/simple/main.cc
Modified: pkg/trunk/libTF/simple/Makefile
===================================================================
--- pkg/trunk/libTF/simple/Makefile 2008-03-31 17:43:56 UTC (rev 71)
+++ pkg/trunk/libTF/simple/Makefile 2008-03-31 20:56:54 UTC (rev 72)
@@ -6,7 +6,7 @@
test: main.cc libTF.o
- g++ -O2 $^ -o test -lnewmat
+ g++ -g -O2 $^ -o test -lnewmat
clean:
Modified: pkg/trunk/libTF/simple/libTF.cc
===================================================================
--- pkg/trunk/libTF/simple/libTF.cc 2008-03-31 17:43:56 UTC (rev 71)
+++ pkg/trunk/libTF/simple/libTF.cc 2008-03-31 20:56:54 UTC (rev 72)
@@ -33,16 +33,27 @@
}
-/*TransformReference::TransformReference()
- *{
- * return;
- *}
- */
+TransformReference::TransformReference()
+{
+ /* initialize pointers to NULL */
+ for (unsigned int i = 0; i < MAX_NUM_FRAMES; i++)
+ {
+ frames[i] = NULL;
+ }
+ return;
+}
+
void TransformReference::set(unsigned int frameID, unsigned int parentID, double a,double b,double c,double d,double e,double f)
{
- frames[frameID].setParent(parentID);
- frames[frameID].setParams(a,b,c,d,e,f);
+ if (frameID > MAX_NUM_FRAMES || parentID > MAX_NUM_FRAMES)
+ throw InvalidFrame;
+
+ if (frames[frameID] == NULL)
+ frames[frameID] = new RefFrame();
+
+ getFrame(frameID)->setParent(parentID);
+ getFrame(frameID)->setParams(a,b,c,d,e,f);
}
@@ -110,19 +121,19 @@
unsigned int frame = target_frame;
while (frame != 0)
{
- if (frames[frame].getParent() > 100) exit(-1); //todo cleanup
+ if (getFrame(frame)->getParent() > MAX_NUM_FRAMES) throw InvalidFrame;
mTfLs.inverseTransforms.push_back(frame);
// std::cout <<"Frame: " << frame <<std::endl;
- frame = frames[frame].getParent();
+ frame = getFrame(frame)->getParent();
}
mTfLs.inverseTransforms.push_back(0);
frame = source_frame;
while (frame != 0)
{
- if (frames[frame].getParent() > 100) exit(-1); //todo cleanup
+ if (getFrame(frame)->getParent() > MAX_NUM_FRAMES) throw InvalidFrame;
mTfLs.forwardTransforms.push_back(frame);
- frame = frames[frame].getParent();
+ frame = getFrame(frame)->getParent();
}
mTfLs.forwardTransforms.push_back(0);
@@ -152,13 +163,13 @@
for (unsigned int i = 0; i < lists.inverseTransforms.size(); i++)
{
- retMat *= frames[lists.inverseTransforms[i]].getInverseMatrix();
+ retMat *= getFrame(lists.inverseTransforms[i])->getInverseMatrix();
// std::cout <<"Multiplying by " << std::endl << frames[lists.inverseTransforms[i]].getInverseMatrix() << std::endl;
//std::cout <<"Result "<<std::endl << retMat << std::endl;
}
for (unsigned int i = 0; i < lists.forwardTransforms.size(); i++)
{
- retMat *= frames[lists.forwardTransforms[lists.forwardTransforms.size() -1 - i]].getMatrix(); //Do this list backwards for it was generated traveling the wrong way
+ retMat *= getFrame(lists.forwardTransforms[lists.forwardTransforms.size() -1 - i])->getMatrix(); //Do this list backwards for it was generated traveling the wrong way
// std::cout <<"Multiplying by "<<std::endl << frames[lists.forwardTransforms[i]].getMatrix() << std::endl;
//std::cout <<"Result "<<std::endl << retMat << std::endl;
}
@@ -175,7 +186,7 @@
for (unsigned int i = 0; i < lists.inverseTransforms.size(); i++)
{
std::cout << lists.inverseTransforms[i];
- // retMat *= frames[lists.inverseTransforms[i]].getInverseMatrix();
+ // retMat *= getFrame(lists.inverseTransforms[i])->getInverseMatrix();
}
std::cout << std::endl;
@@ -183,7 +194,7 @@
for (unsigned int i = 0; i < lists.forwardTransforms.size(); i++)
{
std::cout << lists.forwardTransforms[i];
- // retMat *= frames[lists.inverseTransforms[lists.forwardTransforms.size() -1 - i]].getMatrix(); //Do this list backwards for it was generated traveling the wrong way
+ // retMat *= getFrame(lists.inverseTransforms[lists.forwardTransforms.size() -1 - i])->getMatrix(); //Do this list backwards for it was generated traveling the wrong way
}
std::cout << std::endl;
Modified: pkg/trunk/libTF/simple/libTF.hh
===================================================================
--- pkg/trunk/libTF/simple/libTF.hh 2008-03-31 17:43:56 UTC (rev 71)
+++ pkg/trunk/libTF/simple/libTF.hh 2008-03-31 20:56:54 UTC (rev 72)
@@ -10,6 +10,7 @@
class RefFrame
{
public:
+
/* The type of frame
* This determines how many different parameters to expect to be updated, versus fixed */
// static enum FrameType {SIXDOF, DH //how to tell which mode it's in //todo add this process i'm going to start with 6dof only
@@ -46,7 +47,9 @@
class TransformReference
{
- public:
+public:
+ static const unsigned int MAX_NUM_FRAMES = 100;
+
/* Set a new frame or update an old one. */
void set(unsigned int framid, unsigned int parentid, double,double,double,double,double,double);
@@ -54,14 +57,29 @@
NEWMAT::Matrix get(unsigned int target_frame, unsigned int source_frame);
/* Constructor */
- // TransformReference();
+ TransformReference();
/* Debugging function that will print to std::cout the transformation matrix */
void view(unsigned int target_frame, unsigned int source_frame);
-
- private:
+
+ /* An exception class to notify of bad frame number */
+ class LookupException : public std::exception
+ {
+ public:
+ virtual const char* what() const throw() { return "InvalidFrame"; }
+ } InvalidFrame;
+ /* An exception class to notify of no connection */
+ class ConnectivityException : public std::exception
+ {
+ public:
+ virtual const char* what() const throw() { return "No connection between frames"; }
+ private:
+ } NoFrameConnectivity;
+private:
+
+ inline RefFrame* getFrame(unsigned int frame_number) { if (frames[frame_number] == NULL) throw InvalidFrame; else return frames[frame_number];};
/* The frames that the tree can be made of */
- RefFrame frames[100];
+ RefFrame* frames[MAX_NUM_FRAMES];
/* This struct is how the list of transforms are stored before being generated. */
Modified: pkg/trunk/libTF/simple/main.cc
===================================================================
--- pkg/trunk/libTF/simple/main.cc 2008-03-31 17:43:56 UTC (rev 71)
+++ pkg/trunk/libTF/simple/main.cc 2008-03-31 20:56:54 UTC (rev 72)
@@ -36,7 +36,17 @@
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);
- mTR.view(1,8);
+ try
+ {
+ mTR.view(1,9);
+ }
+ catch (TransformReference::LookupException &ex)
+ {
+ std::cout << "Caught " << ex.what()<<std::endl;
+ mTR.view(1,8);
+ }
+
+
std::cout <<"Calling get(1,8)"<<std::endl;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
