Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[2822] pkg/trunk
|
From: <tf...@us...> - 2008-08-08 21:32:32
|
Revision: 2822
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2822&view=rev
Author: tfoote
Date: 2008-08-08 21:32:36 +0000 (Fri, 08 Aug 2008)
Log Message:
-----------
upgrading libTF exceptions to use std::runtime_error as a base and now there is a frame id associated with extrapolation exceptions
Modified Paths:
--------------
pkg/trunk/exec_trex/ROSNode.cc
pkg/trunk/nav/wavefront_player/wavefront_player.cc
pkg/trunk/simulators/rosgazebo/test/groundtruthtransform.cpp
pkg/trunk/util/libTF/include/libTF/Pose3DCache.h
pkg/trunk/util/libTF/include/libTF/libTF.h
pkg/trunk/util/libTF/src/Pose3DCache.cpp
pkg/trunk/util/libTF/src/libTF.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/laser_scan_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/marker_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/point_cloud_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_model_visualizer.cpp
pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp
pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
Modified: pkg/trunk/exec_trex/ROSNode.cc
===================================================================
--- pkg/trunk/exec_trex/ROSNode.cc 2008-08-08 21:10:11 UTC (rev 2821)
+++ pkg/trunk/exec_trex/ROSNode.cc 2008-08-08 21:32:36 UTC (rev 2822)
@@ -408,7 +408,7 @@
} catch(libTF::TransformReference::LookupException& ex) {
debugMsg("ROSNode::VS", "no global->local Tx yet");
return NULL;
- } catch(libTF::Pose3DCache::ExtrapolateException& ex) {
+ } catch(libTF::TransformReference::ExtrapolateException& ex) {
debugMsg("ROSNode::VS",
"libTF::Quaternion3D::ExtrapolateException occured");
}
@@ -751,7 +751,7 @@
delete[] pts.pts;
return;
}
- catch(libTF::Pose3DCache::ExtrapolateException& ex)
+ catch(libTF::TransformReference::ExtrapolateException& ex)
{
puts("extrapolation required");
delete[] pts.pts;
Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-08-08 21:10:11 UTC (rev 2821)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-08-08 21:32:36 UTC (rev 2822)
@@ -447,7 +447,7 @@
puts("no global->local Tx yet");
return;
}
- catch(libTF::Pose3DCache::ExtrapolateException& ex)
+ catch(libTF::TransformReference::ExtrapolateException& ex)
{
// puts("extrapolation required");
continue;
@@ -587,7 +587,7 @@
this->stopRobot();
return;
}
- catch(libTF::Pose3DCache::ExtrapolateException& ex)
+ catch(libTF::TransformReference::ExtrapolateException& ex)
{
// this should never happen
puts("WARNING: extrapolation failed!");
Modified: pkg/trunk/simulators/rosgazebo/test/groundtruthtransform.cpp
===================================================================
--- pkg/trunk/simulators/rosgazebo/test/groundtruthtransform.cpp 2008-08-08 21:10:11 UTC (rev 2821)
+++ pkg/trunk/simulators/rosgazebo/test/groundtruthtransform.cpp 2008-08-08 21:32:36 UTC (rev 2822)
@@ -32,7 +32,7 @@
std::cerr << "LookupException in lookup(\"FRAMEID_ROBOT\"): " << ex.what() << "\n";
std::cout << "LookupException in lookup(\"FRAMEID_ROBOT\"): " << ex.what();
return;
- } catch(libTF::Pose3DCache::ExtrapolateException& ex) {
+ } catch(libTF::TransformReference::ExtrapolateException& ex) {
std::cerr << "ExtrapolateException in lookup(\"FRAMEID_ROBOT\"): " << ex.what() << "\n";
std::cout << "ExtrapolateException in lookup(\"FRAMEID_ROBOT\"): " << ex.what();
return;
@@ -50,7 +50,7 @@
std::cerr << "LookupException in transformPose(\"FRAMEID_ODOM\", robotPose): " << ex.what() << "\n";
std::cout << "LookupException in transformPose(\"FRAMEID_ODOM\", robotPose): " << ex.what();
return;
- } catch(libTF::Pose3DCache::ExtrapolateException& ex) {
+ } catch(libTF::TransformReference::ExtrapolateException& ex) {
std::cerr << "ExtrapolateException in transformPose(\"FRAMEID_ODOM\", robotPose): " << ex.what() << "\n";
std::cout << "ExtrapolateException in transformPose(\"FRAMEID_ODOM\", robotPose): " << ex.what();
return;
Modified: pkg/trunk/util/libTF/include/libTF/Pose3DCache.h
===================================================================
--- pkg/trunk/util/libTF/include/libTF/Pose3DCache.h 2008-08-08 21:10:11 UTC (rev 2821)
+++ pkg/trunk/util/libTF/include/libTF/Pose3DCache.h 2008-08-08 21:32:36 UTC (rev 2822)
@@ -36,6 +36,7 @@
#include <iostream>
#include <sstream>
+#include <stdexcept>
#include <newmat10/newmat.h>
#include <newmat10/newmatio.h>
#include <cmath>
@@ -79,15 +80,10 @@
/** \brief An exception class to notify that the requested value would have required extrapolation, and extrapolation is not allowed.
*
*/
- class ExtrapolateException : public std::exception
+ class ExtrapolationException : public std::runtime_error
{
public:
- ExtrapolateException(const std::string errorDescription) {
- errorDescription_ = new std::string(errorDescription);
- };
- std::string * errorDescription_;
- ~ExtrapolateException() throw() { delete errorDescription_; };
- virtual const char* what() const throw() { return errorDescription_->c_str(); };
+ ExtrapolationException(const std::string errorDescription) : std::runtime_error(errorDescription) { ; };
};
/** \brief The constructor
Modified: pkg/trunk/util/libTF/include/libTF/libTF.h
===================================================================
--- pkg/trunk/util/libTF/include/libTF/libTF.h 2008-08-08 21:10:11 UTC (rev 2821)
+++ pkg/trunk/util/libTF/include/libTF/libTF.h 2008-08-08 21:32:36 UTC (rev 2822)
@@ -32,6 +32,7 @@
#ifndef LIBTF_HH
#define LIBTF_HH
+#include <stdexcept>
#include <iostream>
#include <iomanip>
#include <newmat10/newmat.h>
@@ -291,18 +292,12 @@
* This is an exception class to be thrown in the case
* that the Reference Frame tree is not connected between
* the frames requested. */
- class ConnectivityException : public std::exception
+ class ConnectivityException : public std::runtime_error
{
public:
- ConnectivityException(const std::string errorDescription) {
- errorDescription_ = new std::string(errorDescription);
- };
- std::string * errorDescription_;
- ~ConnectivityException() throw() { delete errorDescription_; };
- virtual const char* what() const throw() { return errorDescription_->c_str(); };
- private:
+ ConnectivityException(const std::string errorDescription) : std::runtime_error(errorDescription) { ; };
};
-
+
/** \brief An exception class to notify that the search for connectivity descended too deep.
*
* This is an exception class which will be thrown if the tree search
@@ -310,19 +305,21 @@
* infinitely looping in the case that a tree was malformed and
* became cyclic.
*/
- class MaxDepthException : public std::exception
+ class MaxDepthException : public std::runtime_error
{
public:
- MaxDepthException(const std::string errorDescription) {
- errorDescription_ = new std::string(errorDescription);
- };
- std::string * errorDescription_;
- ~MaxDepthException() throw() { delete errorDescription_; };
- virtual const char* what() const throw() { return errorDescription_->c_str(); };
- private:
+ MaxDepthException(const std::string errorDescription): std::runtime_error(errorDescription) { ; };
};
+
+ /** \brief An exception class to notify that the requested value would have required extrapolation, and extrapolation is not allowed.
+ *
+ */
+ class ExtrapolateException : public std::runtime_error
+ {
+ public:
+ ExtrapolateException(const std::string &errorDescription): std::runtime_error(errorDescription) { ; };
+ };
-
private:
/** \brief The internal storage class for ReferenceTransform.
Modified: pkg/trunk/util/libTF/src/Pose3DCache.cpp
===================================================================
--- pkg/trunk/util/libTF/src/Pose3DCache.cpp 2008-08-08 21:10:11 UTC (rev 2821)
+++ pkg/trunk/util/libTF/src/Pose3DCache.cpp 2008-08-08 21:32:36 UTC (rev 2822)
@@ -159,16 +159,17 @@
bool retval = false;
pthread_mutex_lock(&linked_list_mutex);
- num_nodes = findClosest(p_temp_1,p_temp_2, time, time_diff);
-
- if (num_nodes == 0)
- retval= false;
- else if (num_nodes == 1)
+ try
+ {
+ num_nodes = findClosest(p_temp_1,p_temp_2, time, time_diff);
+ if (num_nodes == 0)
+ retval= false;
+ else if (num_nodes == 1)
{
memcpy(&buff, &p_temp_1, sizeof(Pose3DStorage));
retval = true;
}
- else
+ else
{
if(interpolating)
interpolate(p_temp_1, p_temp_2, time, buff);
@@ -176,7 +177,13 @@
buff = p_temp_1;
retval = true;
}
-
+ }
+ catch (ExtrapolationException &ex)
+ {
+ pthread_mutex_unlock(&linked_list_mutex);
+ throw ex;
+ }
+
pthread_mutex_unlock(&linked_list_mutex);
@@ -377,8 +384,8 @@
std::stringstream ss;
ss << "Extrapolation Too Far: target_time = "<< target_time <<", closest data at "
<< two.time << " and " << one.time <<" which are farther away than max_extrapolation_time "
- << max_extrapolation_time <<".";
- throw ExtrapolateException(ss.str());
+ << max_extrapolation_time <<" at "<< target_time - two.time<< " and " << target_time - one.time <<" respectively.";
+ throw ExtrapolationException(ss.str());
}
return 2;
Modified: pkg/trunk/util/libTF/src/libTF.cpp
===================================================================
--- pkg/trunk/util/libTF/src/libTF.cpp 2008-08-08 21:10:11 UTC (rev 2821)
+++ pkg/trunk/util/libTF/src/libTF.cpp 2008-08-08 21:32:36 UTC (rev 2822)
@@ -394,11 +394,27 @@
for (unsigned int i = 0; i < lists.inverseTransforms.size(); i++)
{
- retMat *= getFrame(lists.inverseTransforms[lists.inverseTransforms.size() -1 - i])->getMatrix(time); //Reverse to get left multiply
+ try {
+ retMat *= getFrame(lists.inverseTransforms[lists.inverseTransforms.size() -1 - i])->getMatrix(time); //Reverse to get left multiply
+ }
+ catch (libTF::Pose3DCache::ExtrapolationException &ex)
+ {
+ std::stringstream ss;
+ ss << "Frame "<< lists.inverseTransforms[lists.inverseTransforms.size() -1 - i] << " is out of date. " << ex.what();
+ throw ExtrapolateException(ss.str());
+ }
}
for (unsigned int i = 0; i < lists.forwardTransforms.size(); i++)
{
- retMat *= getFrame(lists.forwardTransforms[i])->getInverseMatrix(time); //Do this list backwards(from backwards) for it was generated traveling the wrong way
+ try {
+ retMat *= getFrame(lists.forwardTransforms[i])->getInverseMatrix(time); //Do this list backwards(from backwards) for it was generated traveling the wrong way
+ }
+ catch (libTF::Pose3DCache::ExtrapolationException &ex)
+ {
+ std::stringstream ss;
+ ss << "Frame "<< lists.forwardTransforms[i] << " is out of date. " << ex.what();
+ throw ExtrapolateException(ss.str());
+ }
}
return retMat;
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/laser_scan_visualizer.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/laser_scan_visualizer.cpp 2008-08-08 21:10:11 UTC (rev 2821)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/laser_scan_visualizer.cpp 2008-08-08 21:32:36 UTC (rev 2822)
@@ -199,7 +199,7 @@
{
printf( "Error transforming laser scan '%s': %s\n", m_Name.c_str(), e.what() );
}
- catch(libTF::Pose3DCache::ExtrapolateException& e)
+ catch(libTF::TransformReference::ExtrapolateException& e)
{
printf( "Error transforming laser scan '%s': %s\n", m_Name.c_str(), e.what() );
}
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/marker_visualizer.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/marker_visualizer.cpp 2008-08-08 21:10:11 UTC (rev 2821)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/marker_visualizer.cpp 2008-08-08 21:32:36 UTC (rev 2822)
@@ -233,7 +233,7 @@
{
printf( "Error transforming marker '%d': %s\n", message.id, e.what() );
}
- catch(libTF::Pose3DCache::ExtrapolateException& e)
+ catch(libTF::TransformReference::ExtrapolateException& e)
{
printf( "Error transforming marker '%d': %s\n", message.id, e.what() );
}
@@ -256,7 +256,7 @@
{
printf( "Error transforming marker '%d': %s\n", message.id, e.what() );
}
- catch(libTF::Pose3DCache::ExtrapolateException& e)
+ catch(libTF::TransformReference::ExtrapolateException& e)
{
printf( "Error transforming marker '%d': %s\n", message.id, e.what() );
}
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/point_cloud_visualizer.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/point_cloud_visualizer.cpp 2008-08-08 21:10:11 UTC (rev 2821)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/point_cloud_visualizer.cpp 2008-08-08 21:32:36 UTC (rev 2822)
@@ -162,7 +162,7 @@
{
printf( "Error transforming point cloud '%s': %s\n", m_Name.c_str(), e.what() );
}
- catch(libTF::Pose3DCache::ExtrapolateException& e)
+ catch(libTF::TransformReference::ExtrapolateException& e)
{
printf( "Error transforming point cloud '%s': %s\n", m_Name.c_str(), e.what() );
}
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_model_visualizer.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_model_visualizer.cpp 2008-08-08 21:10:11 UTC (rev 2821)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_model_visualizer.cpp 2008-08-08 21:32:36 UTC (rev 2822)
@@ -212,7 +212,7 @@
{
printf( "Error transforming from frame '%s' to frame '%s': %s\n", name.c_str(), m_TargetFrame.c_str(), e.what() );
}
- catch(libTF::Pose3DCache::ExtrapolateException& e)
+ catch(libTF::TransformReference::ExtrapolateException& e)
{
printf( "Error transforming from frame '%s' to frame '%s': %s\n", name.c_str(), m_TargetFrame.c_str(), e.what() );
}
Modified: pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp 2008-08-08 21:10:11 UTC (rev 2821)
+++ pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp 2008-08-08 21:32:36 UTC (rev 2822)
@@ -157,7 +157,7 @@
puts("no global->local Tx yet");
return;
}
- catch(libTF::Pose3DCache::ExtrapolateException& ex)
+ catch(libTF::TransformReference::ExtrapolateException& ex)
{
puts("extrapolation required");
return;
Modified: pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
===================================================================
--- pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-08-08 21:10:11 UTC (rev 2821)
+++ pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-08-08 21:32:36 UTC (rev 2822)
@@ -253,7 +253,7 @@
fprintf(stderr, "Discarding pointcloud: Transform reference lookup exception\n");
success = false;
}
- catch(libTF::Pose3DCache::ExtrapolateException& ex)
+ catch(libTF::TransformReference::ExtrapolateException& ex)
{
fprintf(stderr, "Discarding pointcloud: Extrapolation exception: %s\n", ex.what());
success = false;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
