Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots: [769] pkg/trunk
|
From: <tf...@us...> - 2008-06-12 01:13:29
|
Revision: 769
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=769&view=rev
Author: tfoote
Date: 2008-06-11 18:13:36 -0700 (Wed, 11 Jun 2008)
Log Message:
-----------
adding zero timestamp = latest and updating constructor
Modified Paths:
--------------
pkg/trunk/nav/wavefront_player/wavefront_player.cc
pkg/trunk/util/transforms/libTF/doc/libTF_Manual.tex
pkg/trunk/util/transforms/libTF/include/libTF/Quaternion3D.h
pkg/trunk/util/transforms/libTF/include/libTF/libTF.h
pkg/trunk/util/transforms/libTF/src/simple/Quaternion3D.cpp
pkg/trunk/util/transforms/libTF/src/simple/libTF.cpp
pkg/trunk/util/transforms/libTF/testtf.cc
pkg/trunk/util/transforms/rosTF/include/rosTF/rosTF.h
pkg/trunk/util/transforms/rosTF/src/rosTF/rosTF.cpp
Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-06-12 00:57:25 UTC (rev 768)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-06-12 01:13:36 UTC (rev 769)
@@ -298,7 +298,7 @@
avmax(DTOR(80.0)),
amin(DTOR(10.0)),
amax(DTOR(40.0)),
- tf(*this, true)
+ tf(*this, true, 200000000ULL, 200000000ULL) //nanoseconds
{
// TODO: get map via RPC
char* mapdata;
@@ -570,9 +570,9 @@
robotPose.y = 0;
robotPose.yaw = 0;
robotPose.frame = FRAMEID_ROBOT;
- //robotPose.time = 0; // request most recent pose
- robotPose.time = laserMsg.header.stamp.sec * 1000000000ULL +
- laserMsg.header.stamp.nsec; ///HACKE FIXME we should be able to get time somewhere else
+ robotPose.time = 0; // request most recent pose
+ //robotPose.time = laserMsg.header.stamp.sec * 1000000000ULL +
+ // laserMsg.header.stamp.nsec; ///HACKE FIXME we should be able to get time somewhere else
try
{
global_pose = this->tf.transformPose2D(FRAMEID_MAP, robotPose);
Modified: pkg/trunk/util/transforms/libTF/doc/libTF_Manual.tex
===================================================================
--- pkg/trunk/util/transforms/libTF/doc/libTF_Manual.tex 2008-06-12 00:57:25 UTC (rev 768)
+++ pkg/trunk/util/transforms/libTF/doc/libTF_Manual.tex 2008-06-12 01:13:36 UTC (rev 769)
@@ -242,7 +242,10 @@
\subsubsection{Constructor}
\index{libTF API!Constructor}
\begin{verbatim}
-TransformReference(ULLtime cache_time = DEFAULT_CACHE_TIME);
+TransformReference(bool interpolating = true,
+ ULLtime cache_time = DEFAULT_CACHE_TIME,
+ unsigned long long max_extrapolation_distance = DEFAULT_MAX_EXTRAPOLATION_DISTANCE);
+
\end{verbatim}
This is the constructor for the class. It's optional argument is
how long to keep a history of the transforms. It is expressed in
@@ -292,6 +295,8 @@
unsigned int source_frame,
unsigned long long time);
\end{verbatim}
+getMatrix will return the homogeneous transformation matrix between the souce frame and the target frame.
+The standard approach will return the linearly interpolated
\paragraph{transform[DATA\_TYPE]}
\index{libTF API!transform[DATA\_TYPE]}
Modified: pkg/trunk/util/transforms/libTF/include/libTF/Quaternion3D.h
===================================================================
--- pkg/trunk/util/transforms/libTF/include/libTF/Quaternion3D.h 2008-06-12 00:57:25 UTC (rev 768)
+++ pkg/trunk/util/transforms/libTF/include/libTF/Quaternion3D.h 2008-06-12 01:13:36 UTC (rev 769)
@@ -161,7 +161,7 @@
/** Constructors **/
// Standard constructor max_cache_time is how long to cache transform data
- Quaternion3D(bool caching = true,
+ Quaternion3D(bool interpolating = true,
unsigned long long max_cache_time = DEFAULT_MAX_STORAGE_TIME,
unsigned long long max_extrapolation_time = DEFAULT_MAX_EXTRAPOLATION_TIME);
~Quaternion3D();
@@ -243,6 +243,9 @@
unsigned int list_length;
+ //Whether or not to interpolate
+ bool interpolating;
+
};
Modified: pkg/trunk/util/transforms/libTF/include/libTF/libTF.h
===================================================================
--- pkg/trunk/util/transforms/libTF/include/libTF/libTF.h 2008-06-12 00:57:25 UTC (rev 768)
+++ pkg/trunk/util/transforms/libTF/include/libTF/libTF.h 2008-06-12 01:13:36 UTC (rev 769)
@@ -203,7 +203,7 @@
/** Constructor
* \param How long to keep a history of transforms in nanoseconds
*/
- TransformReference(bool caching = true,
+ TransformReference(bool interpolating = true,
ULLtime cache_time = DEFAULT_CACHE_TIME,
unsigned long long max_extrapolation_distance = DEFAULT_MAX_EXTRAPOLATION_DISTANCE);
~TransformReference();
@@ -319,7 +319,7 @@
public:
/** Constructor */
- RefFrame(bool caching = true,
+ RefFrame(bool interpolating = true,
unsigned long long max_cache_time = DEFAULT_MAX_STORAGE_TIME,
unsigned long long max_extrapolation_time = DEFAULT_MAX_EXTRAPOLATION_TIME);
@@ -349,8 +349,8 @@
/// How long to cache transform history
ULLtime cache_time;
- /// whether or not to cache
- bool caching;
+ /// whether or not to interpolate or extrapolate
+ bool interpolating;
/// whether or not to allow extrapolation
unsigned long long max_extrapolation_distance;
Modified: pkg/trunk/util/transforms/libTF/src/simple/Quaternion3D.cpp
===================================================================
--- pkg/trunk/util/transforms/libTF/src/simple/Quaternion3D.cpp 2008-06-12 00:57:25 UTC (rev 768)
+++ pkg/trunk/util/transforms/libTF/src/simple/Quaternion3D.cpp 2008-06-12 01:13:36 UTC (rev 769)
@@ -45,7 +45,7 @@
};
-Quaternion3D::Quaternion3D(bool caching,
+Quaternion3D::Quaternion3D(bool interpolating,
unsigned long long max_cache_time,
unsigned long long _max_extrapolation_time):
max_storage_time(max_cache_time),
@@ -53,11 +53,12 @@
max_extrapolation_time(_max_extrapolation_time),
first(NULL),
last(NULL),
- list_length(0)
+ list_length(0),
+ interpolating(interpolating)
{
//Turn of caching, this should only keep a liked list of lenth 1
// Thus returning only the latest
- if (!caching) max_length_linked_list = 1;
+ // if (!caching) max_length_linked_list = 1; //Removed, simply use 0 time to get first value
pthread_mutex_init( &linked_list_mutex, NULL);
//fixme Normalize();
@@ -504,7 +505,10 @@
}
else
{
- interpolate(p_temp_1, p_temp_2, time, buff);
+ if(interpolating)
+ interpolate(p_temp_1, p_temp_2, time, buff);
+ else
+ buff = p_temp_1;
retval = true;
}
@@ -669,8 +673,8 @@
return 0;
}
- //Case one element list
- else if (first->next == NULL)
+ //Case one element list or latest value is wanted.
+ else if (first->next == NULL || target_time == 0)
{
one = first->data;
time_diff = target_time - first->data.time;
Modified: pkg/trunk/util/transforms/libTF/src/simple/libTF.cpp
===================================================================
--- pkg/trunk/util/transforms/libTF/src/simple/libTF.cpp 2008-06-12 00:57:25 UTC (rev 768)
+++ pkg/trunk/util/transforms/libTF/src/simple/libTF.cpp 2008-06-12 01:13:36 UTC (rev 769)
@@ -35,21 +35,21 @@
using namespace libTF;
-TransformReference::RefFrame::RefFrame(bool caching,
+TransformReference::RefFrame::RefFrame(bool interpolating,
unsigned long long max_cache_time,
unsigned long long max_extrapolation_distance) :
- Quaternion3D(caching, max_cache_time, max_extrapolation_distance),
+ Quaternion3D(interpolating, max_cache_time, max_extrapolation_distance),
parent(TransformReference::NO_PARENT)
{
return;
}
-TransformReference::TransformReference(bool caching,
+TransformReference::TransformReference(bool interpolating,
ULLtime cache_time,
unsigned long long max_extrapolation_distance):
cache_time(cache_time),
- caching (caching),
+ interpolating (interpolating),
max_extrapolation_distance(max_extrapolation_distance)
{
@@ -83,7 +83,7 @@
throw InvalidFrame;
if (frames[frameID] == NULL)
- frames[frameID] = new RefFrame(caching, cache_time, max_extrapolation_distance);
+ frames[frameID] = new RefFrame(interpolating, cache_time, max_extrapolation_distance);
getFrame(frameID)->setParent(parentID);
getFrame(frameID)->addFromEuler(a,b,c,d,e,f,time);
@@ -95,7 +95,7 @@
throw InvalidFrame;
if (frames[frameID] == NULL)
- frames[frameID] = new RefFrame(caching, cache_time, max_extrapolation_distance);
+ frames[frameID] = new RefFrame(interpolating, cache_time, max_extrapolation_distance);
getFrame(frameID)->setParent(parentID);
getFrame(frameID)->addFromDH(a,b,c,d,time);
@@ -109,7 +109,7 @@
//TODO check and throw exception if matrix wrong size
if (frames[frameID] == NULL)
- frames[frameID] = new RefFrame(caching, cache_time, max_extrapolation_distance);
+ frames[frameID] = new RefFrame(interpolating, cache_time, max_extrapolation_distance);
getFrame(frameID)->setParent(parentID);
getFrame(frameID)->addFromMatrix(matrix_in,time);
@@ -122,7 +122,7 @@
throw InvalidFrame;
if (frames[frameID] == NULL)
- frames[frameID] = new RefFrame(caching, cache_time, max_extrapolation_distance);
+ frames[frameID] = new RefFrame(interpolating, cache_time, max_extrapolation_distance);
getFrame(frameID)->setParent(parentID);
getFrame(frameID)->addFromQuaternion(xt, yt, zt, xr, yr, zr, w,time);
Modified: pkg/trunk/util/transforms/libTF/testtf.cc
===================================================================
--- pkg/trunk/util/transforms/libTF/testtf.cc 2008-06-12 00:57:25 UTC (rev 768)
+++ pkg/trunk/util/transforms/libTF/testtf.cc 2008-06-12 01:13:36 UTC (rev 769)
@@ -11,7 +11,7 @@
int
main(void)
{
- libTF::TransformReference mTR(false);
+ libTF::TransformReference mTR(true, 0);
timeval temp_time_struct;
gettimeofday(&temp_time_struct,NULL);
unsigned long long atime = temp_time_struct.tv_sec * 1000000000ULL + (unsigned long long)temp_time_struct.tv_usec * 1000ULL;
Modified: pkg/trunk/util/transforms/rosTF/include/rosTF/rosTF.h
===================================================================
--- pkg/trunk/util/transforms/rosTF/include/rosTF/rosTF.h 2008-06-12 00:57:25 UTC (rev 768)
+++ pkg/trunk/util/transforms/rosTF/include/rosTF/rosTF.h 2008-06-12 01:13:36 UTC (rev 769)
@@ -62,7 +62,7 @@
{
public:
//Constructor
- rosTFClient(ros::node & rosnode, bool caching = true, unsigned long long max_extrapolation_distance = libTF::TransformReference::DEFAULT_MAX_EXTRAPOLATION_DISTANCE);
+ rosTFClient(ros::node & rosnode, bool interpolating = true, unsigned long long max_cache_time = libTF::TransformReference::DEFAULT_CACHE_TIME, unsigned long long max_extrapolation_distance = libTF::TransformReference::DEFAULT_MAX_EXTRAPOLATION_DISTANCE);
// PointCloudFloat32 transformPointCloud(unsigned int target_frame, const PointCloudFloat32 & cloudIn); // todo switch after ticket:232
std_msgs::PointCloudFloat32 transformPointCloud(unsigned int target_frame, std_msgs::PointCloudFloat32 & cloudIn);
Modified: pkg/trunk/util/transforms/rosTF/src/rosTF/rosTF.cpp
===================================================================
--- pkg/trunk/util/transforms/rosTF/src/rosTF/rosTF.cpp 2008-06-12 00:57:25 UTC (rev 768)
+++ pkg/trunk/util/transforms/rosTF/src/rosTF/rosTF.cpp 2008-06-12 01:13:36 UTC (rev 769)
@@ -33,10 +33,11 @@
#include "rosTF/rosTF.h"
rosTFClient::rosTFClient(ros::node & rosnode,
- bool caching,
+ bool interpolating,
+ unsigned long long max_cache_time,
unsigned long long max_extrapolation_distance):
- TransformReference(caching,
- TransformReference::DEFAULT_CACHE_TIME,
+ TransformReference(interpolating,
+ max_cache_time,
max_extrapolation_distance),
myNode(rosnode)
{
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
