Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots: [575] pkg/trunk
|
From: <ge...@us...> - 2008-05-30 21:33:39
|
Revision: 575
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=575&view=rev
Author: gerkey
Date: 2008-05-30 14:33:41 -0700 (Fri, 30 May 2008)
Log Message:
-----------
added extrapolate exception
Modified Paths:
--------------
pkg/trunk/nav/wavefront_player/wavefront_player.cc
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/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-05-30 21:27:11 UTC (rev 574)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-05-30 21:33:41 UTC (rev 575)
@@ -201,7 +201,7 @@
// Map update paramters (for adding obstacles)
double laser_maxrange;
ros::Duration laser_buffer_time;
- //std::list<MsgLaserScan*> buffered_laser_scans;
+ std::list<MsgLaserScan*> buffered_laser_scans;
std::list<laser_pts_t> laser_scans;
double* laser_hitpts;
size_t laser_hitpts_len, laser_hitpts_size;
@@ -298,7 +298,7 @@
avmax(DTOR(80.0)),
amin(DTOR(10.0)),
amax(DTOR(40.0)),
- tf(*this, true)
+ tf(*this, true, false)
{
// TODO: get map via RPC
char* mapdata;
@@ -386,54 +386,81 @@
void
WavefrontNode::laserReceived()
{
- this->lock.lock();
+ // Copy and push this scan into the list of buffered scans
+ MsgLaserScan* newscan = new MsgLaserScan(laserMsg);
+ assert(newscan);
+ // Do a deep copy of the range data
+ newscan->set_ranges_size(laserMsg.get_ranges_size());
+ memcpy(newscan->ranges,laserMsg.ranges,laserMsg.get_ranges_size()*sizeof(float));
+ newscan->set_intensities_size(0);
+ this->buffered_laser_scans.push_back(newscan);
- // For each beam, convert to cartesian in the laser's frame, then convert
- // to the map frame and store the result in the the laser_scans list
+ // Iterate through the buffered scans, trying to interpolate each one
+ for(std::list<MsgLaserScan*>::iterator it = this->buffered_laser_scans.begin();
+ it != this->buffered_laser_scans.end();
+ it++)
+ {
+ // For each beam, convert to cartesian in the laser's frame, then convert
+ // to the map frame and store the result in the the laser_scans list
- laser_pts_t pts;
- pts.pts_num = laserMsg.get_ranges_size();
- pts.pts = new double[pts.pts_num*2];
- assert(pts.pts);
- pts.ts = laserMsg.header.stamp;
+ laser_pts_t pts;
+ pts.pts_num = (*it)->get_ranges_size();
+ pts.pts = new double[pts.pts_num*2];
+ assert(pts.pts);
+ pts.ts = (*it)->header.stamp;
- libTF::TFPose2D local,global;
- //local.frame = FRAMEID_LASER;
- local.frame = FRAMEID_ROBOT;
- local.time = laserMsg.header.stamp.sec * 1000000000ULL +
- laserMsg.header.stamp.nsec;
- float b=laserMsg.angle_min;
- float* r=laserMsg.ranges;
- for(unsigned int i=0;i<laserMsg.get_ranges_size();
- i++,r++,b+=laserMsg.angle_increment)
- {
- // TODO: take out the bogus epsilon range_max check, after the
- // hokuyourg_player node is fixed
- if(((*r) >= this->laser_maxrange) ||
- ((laserMsg.range_max > 0.1) && ((*r) >= laserMsg.range_max)) ||
- ((*r) <= 0.01))
- continue;
+ libTF::TFPose2D local,global;
+ local.frame = FRAMEID_LASER;
+ //local.frame = FRAMEID_ROBOT;
+ local.time = (*it)->header.stamp.sec * 1000000000ULL +
+ (*it)->header.stamp.nsec;
+ float b=(*it)->angle_min;
+ float* r=(*it)->ranges;
+ unsigned int i;
+ for(i=0;i<(*it)->get_ranges_size();
+ i++,r++,b+=(*it)->angle_increment)
+ {
+ // TODO: take out the bogus epsilon range_max check, after the
+ // hokuyourg_player node is fixed
+ if(((*r) >= this->laser_maxrange) ||
+ (((*it)->range_max > 0.1) && ((*r) >= (*it)->range_max)) ||
+ ((*r) <= 0.01))
+ continue;
- local.x = (*r)*cos(b);
- local.y = (*r)*sin(b);
- local.yaw = 0;
- try
- {
- global = this->tf.transformPose2D(FRAMEID_MAP, local);
+ local.x = (*r)*cos(b);
+ local.y = (*r)*sin(b);
+ local.yaw = 0;
+ try
+ {
+ global = this->tf.transformPose2D(FRAMEID_MAP, local);
+ }
+ catch(libTF::TransformReference::LookupException& ex)
+ {
+ puts("no global->local Tx yet");
+ delete[] pts.pts;
+ return;
+ }
+ catch(libTF::Quaternion3D::ExtrapolateException& ex)
+ {
+ //puts("extrapolation required");
+ delete[] pts.pts;
+ break;
+ }
+
+ // Copy in the result
+ pts.pts[2*i] = global.x;
+ pts.pts[2*i+1] = global.y;
}
- catch(libTF::TransformReference::LookupException& ex)
+ // Did we break early?
+ if(i < (*it)->get_ranges_size())
+ continue;
+ else
{
- puts("no global->local Tx yet");
- delete[] pts.pts;
- this->lock.unlock();
- return;
+ this->laser_scans.push_back(pts);
+ it = this->buffered_laser_scans.erase(it);
+ it--;
}
-
- // Copy in the result
- pts.pts[2*i] = global.x;
- pts.pts[2*i+1] = global.y;
}
- this->laser_scans.push_back(pts);
// Remove anything that's too old from the laser_scans list
// Also count how many points we have
@@ -452,6 +479,9 @@
hitpt_cnt += it->pts_num;
}
+ // Lock here because we're operating on the laser_hitpts array, which is
+ // used in another thread.
+ this->lock.lock();
// allocate more space as necessary
if(this->laser_hitpts_size < hitpt_cnt)
{
@@ -474,6 +504,7 @@
it->pts, it->pts_num * 2 * sizeof(double));
this->laser_hitpts_len += it->pts_num;
}
+ this->lock.unlock();
// Draw the points
@@ -488,8 +519,6 @@
this->pointcloudMsg.points[i].y = this->laser_hitpts[2*i+1];
}
publish("gui_laser",this->pointcloudMsg);
-
- this->lock.unlock();
}
void
@@ -547,6 +576,10 @@
this->stopRobot();
return;
}
+ catch(libTF::Quaternion3D::ExtrapolateException& ex)
+ {
+ // no big deal.
+ }
this->lock.lock();
switch(this->planner_state)
Modified: pkg/trunk/util/transforms/libTF/include/libTF/Quaternion3D.h
===================================================================
--- pkg/trunk/util/transforms/libTF/include/libTF/Quaternion3D.h 2008-05-30 21:27:11 UTC (rev 574)
+++ pkg/trunk/util/transforms/libTF/include/libTF/Quaternion3D.h 2008-05-30 21:33:41 UTC (rev 575)
@@ -43,7 +43,6 @@
namespace libTF{
-
struct PoseYPR
{
double x,y,z,yaw, pitch, roll;
@@ -150,10 +149,21 @@
Quaternion3DStorage & operator=(const Quaternion3DStorage & input);
unsigned long long time; //nanoseconds since 1970
};
+
+ /** \brief An exception class to notify that the requested value would have required extrapolation, and extrapolation is not allowed.
+ *
+ */
+ class ExtrapolateException : public std::exception
+ {
+ public:
+ virtual const char* what() const throw() { return "Disallowed extrapolation required."; }
+ };
/** Constructors **/
// Standard constructor max_cache_time is how long to cache transform data
- Quaternion3D(bool caching = true, unsigned long long max_cache_time = DEFAULT_MAX_STORAGE_TIME);
+ Quaternion3D(bool caching = true,
+ unsigned long long max_cache_time = DEFAULT_MAX_STORAGE_TIME,
+ bool extrapolate = true);
~Quaternion3D();
/** Mutators **/
@@ -198,6 +208,7 @@
void add_value(const Quaternion3DStorage&);//todo fixme finish implementing this
+ ExtrapolateException NoExtrapolation;
// insert a node into the sorted linked list
void insertNode(const Quaternion3DStorage & );
@@ -220,6 +231,8 @@
unsigned long long max_storage_time;
//Max length of linked list
unsigned long long max_length_linked_list;
+ //Whether to allow extrapolation
+ bool extrapolate;
//A mutex to prevent linked list collisions
pthread_mutex_t linked_list_mutex;
Modified: pkg/trunk/util/transforms/libTF/include/libTF/libTF.h
===================================================================
--- pkg/trunk/util/transforms/libTF/include/libTF/libTF.h 2008-05-30 21:27:11 UTC (rev 574)
+++ pkg/trunk/util/transforms/libTF/include/libTF/libTF.h 2008-05-30 21:33:41 UTC (rev 575)
@@ -202,7 +202,9 @@
/** Constructor
* \param How long to keep a history of transforms in nanoseconds
*/
- TransformReference(bool caching = true, ULLtime cache_time = DEFAULT_CACHE_TIME);
+ TransformReference(bool caching = true,
+ ULLtime cache_time = DEFAULT_CACHE_TIME,
+ bool extrapolate = true);
~TransformReference();
/********** Mutators **************/
@@ -264,7 +266,6 @@
*/
static ULLtime gettime(void);
-
/************ Possible Exceptions ****************************/
/** \brief An exception class to notify of bad frame number
@@ -307,6 +308,7 @@
private:
} MaxSearchDepth;
+
private:
/** RefFrame *******
@@ -323,7 +325,9 @@
public:
/** Constructor */
- RefFrame(bool caching = true, unsigned long long max_cache_time = DEFAULT_MAX_STORAGE_TIME);
+ RefFrame(bool caching = true,
+ unsigned long long max_cache_time = DEFAULT_MAX_STORAGE_TIME,
+ bool extrapolate = true);
/** \brief Get the parent nodeID */
inline unsigned int getParent(){return parent;};
@@ -339,7 +343,7 @@
/** Internal storage of the parent */
unsigned int parent;
-
+
};
/******************** Internal Storage ****************/
@@ -353,6 +357,9 @@
/// whether or not to cache
bool caching;
+
+ /// whether or not to allow extrapolation
+ bool extrapolate;
public:
/** \brief An internal representation of transform chains
Modified: pkg/trunk/util/transforms/libTF/src/simple/Quaternion3D.cpp
===================================================================
--- pkg/trunk/util/transforms/libTF/src/simple/Quaternion3D.cpp 2008-05-30 21:27:11 UTC (rev 574)
+++ pkg/trunk/util/transforms/libTF/src/simple/Quaternion3D.cpp 2008-05-30 21:33:41 UTC (rev 575)
@@ -45,9 +45,12 @@
};
-Quaternion3D::Quaternion3D(bool caching, unsigned long long max_cache_time):
+Quaternion3D::Quaternion3D(bool caching,
+ unsigned long long max_cache_time,
+ bool _extrapolate):
max_storage_time(max_cache_time),
max_length_linked_list(MAX_LENGTH_LINKED_LIST),
+ extrapolate(_extrapolate),
first(NULL),
last(NULL),
list_length(0)
@@ -677,6 +680,17 @@
else
{
//Two or more elements
+
+ // Are we allowed to extrapolate?
+ if(!extrapolate)
+ {
+ if(target_time > first->data.time)
+ {
+ pthread_mutex_unlock(&linked_list_mutex);
+ throw NoExtrapolation;
+ }
+ }
+
//Find the one that just exceeds the time or hits the end
//and then take the previous one
p_current = first->next; //Start on the 2nd element so if we fail we fall back to the first one
Modified: pkg/trunk/util/transforms/libTF/src/simple/libTF.cpp
===================================================================
--- pkg/trunk/util/transforms/libTF/src/simple/libTF.cpp 2008-05-30 21:27:11 UTC (rev 574)
+++ pkg/trunk/util/transforms/libTF/src/simple/libTF.cpp 2008-05-30 21:33:41 UTC (rev 575)
@@ -35,17 +35,22 @@
using namespace libTF;
-TransformReference::RefFrame::RefFrame(bool caching, unsigned long long max_cache_time) :
- Quaternion3D(caching, max_cache_time),
+TransformReference::RefFrame::RefFrame(bool caching,
+ unsigned long long max_cache_time,
+ bool extrapolate) :
+ Quaternion3D(caching, max_cache_time, extrapolate),
parent(TransformReference::NO_PARENT)
{
return;
}
-TransformReference::TransformReference(bool caching, ULLtime cache_time):
+TransformReference::TransformReference(bool caching,
+ ULLtime cache_time,
+ bool extrapolate):
cache_time(cache_time),
- caching (caching)
+ caching (caching),
+ extrapolate(extrapolate)
{
frames = new RefFrame*[MAX_NUM_FRAMES];
@@ -78,7 +83,7 @@
throw InvalidFrame;
if (frames[frameID] == NULL)
- frames[frameID] = new RefFrame(caching);
+ frames[frameID] = new RefFrame(caching, cache_time, extrapolate);
getFrame(frameID)->setParent(parentID);
getFrame(frameID)->addFromEuler(a,b,c,d,e,f,time);
@@ -90,7 +95,7 @@
throw InvalidFrame;
if (frames[frameID] == NULL)
- frames[frameID] = new RefFrame(caching);
+ frames[frameID] = new RefFrame(caching, cache_time, extrapolate);
getFrame(frameID)->setParent(parentID);
getFrame(frameID)->addFromDH(a,b,c,d,time);
@@ -104,7 +109,7 @@
//TODO check and throw exception if matrix wrong size
if (frames[frameID] == NULL)
- frames[frameID] = new RefFrame(caching);
+ frames[frameID] = new RefFrame(caching, cache_time, extrapolate);
getFrame(frameID)->setParent(parentID);
getFrame(frameID)->addFromMatrix(matrix_in,time);
@@ -117,7 +122,7 @@
throw InvalidFrame;
if (frames[frameID] == NULL)
- frames[frameID] = new RefFrame(caching);
+ frames[frameID] = new RefFrame(caching, cache_time, extrapolate);
getFrame(frameID)->setParent(parentID);
getFrame(frameID)->addFromQuaternion(xt, yt, zt, xr, yr, zr, w,time);
Modified: pkg/trunk/util/transforms/rosTF/include/rosTF/rosTF.h
===================================================================
--- pkg/trunk/util/transforms/rosTF/include/rosTF/rosTF.h 2008-05-30 21:27:11 UTC (rev 574)
+++ pkg/trunk/util/transforms/rosTF/include/rosTF/rosTF.h 2008-05-30 21:33:41 UTC (rev 575)
@@ -61,7 +61,7 @@
{
public:
//Constructor
- rosTFClient(ros::node & rosnode, bool caching = true);
+ rosTFClient(ros::node & rosnode, bool caching = true, bool extrapolate = true);
//Call back functions
void receiveEuler();
Modified: pkg/trunk/util/transforms/rosTF/src/rosTF/rosTF.cpp
===================================================================
--- pkg/trunk/util/transforms/rosTF/src/rosTF/rosTF.cpp 2008-05-30 21:27:11 UTC (rev 574)
+++ pkg/trunk/util/transforms/rosTF/src/rosTF/rosTF.cpp 2008-05-30 21:33:41 UTC (rev 575)
@@ -32,8 +32,12 @@
#include "rosTF/rosTF.h"
-rosTFClient::rosTFClient(ros::node & rosnode, bool caching):
- TransformReference(caching),
+rosTFClient::rosTFClient(ros::node & rosnode,
+ bool caching,
+ bool extrapolate):
+ TransformReference(caching,
+ TransformReference::DEFAULT_CACHE_TIME,
+ extrapolate),
myNode(rosnode)
{
myNode.subscribe("TransformEuler", eulerIn, &rosTFClient::receiveEuler, this,100);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|