|
From: <tf...@us...> - 2008-11-07 06:40:38
|
Revision: 6371
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6371&view=rev
Author: tfoote
Date: 2008-11-07 06:40:35 +0000 (Fri, 07 Nov 2008)
Log Message:
-----------
removing extrapolation from tf constructors
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp
pkg/trunk/nav/slam_gmapping/src/slam_gmapping.cpp
pkg/trunk/nav/slam_gmapping/src/tftest.cpp
pkg/trunk/nav/wavefront_player/wavefront_player.cc
pkg/trunk/util/tf/include/tf/tf.h
pkg/trunk/util/tf/include/tf/transform_listener.h
pkg/trunk/util/tf/src/tf.cpp
pkg/trunk/util/tf/test/tf_unittest_future.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.cpp
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp 2008-11-07 04:23:22 UTC (rev 6370)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp 2008-11-07 06:40:35 UTC (rev 6371)
@@ -160,7 +160,7 @@
ROS_REGISTER_CONTROLLER(HeadPanTiltControllerNode)
HeadPanTiltControllerNode::HeadPanTiltControllerNode()
-: Controller(), node_(ros::node::instance()), TF(*ros::node::instance(),false, 10000000000ULL, 1000000000ULL)
+: Controller(), node_(ros::node::instance()), TF(*ros::node::instance(),false, 10000000000ULL)
{
c_ = new HeadPanTiltController();
}
Modified: pkg/trunk/nav/slam_gmapping/src/slam_gmapping.cpp
===================================================================
--- pkg/trunk/nav/slam_gmapping/src/slam_gmapping.cpp 2008-11-07 04:23:22 UTC (rev 6370)
+++ pkg/trunk/nav/slam_gmapping/src/slam_gmapping.cpp 2008-11-07 06:40:35 UTC (rev 6371)
@@ -56,7 +56,8 @@
/// @todo Disable extrapolation, and implement scan-buffering. This is
/// not urgent for a robot like the PR2, where odometry is being
/// published several times faster than laser scans.
- tf_ = new tf::TransformListener(*node_, true, 10000000000ULL, 200000000ULL);
+ tf_ = new tf::TransformListener(*node_, true, 10000000000ULL);
+ tf_->setExtrapolationLimit((int64_t) 200000000ULL );
ROS_ASSERT(tf_);
gsp_laser_ = NULL;
Modified: pkg/trunk/nav/slam_gmapping/src/tftest.cpp
===================================================================
--- pkg/trunk/nav/slam_gmapping/src/tftest.cpp 2008-11-07 04:23:22 UTC (rev 6370)
+++ pkg/trunk/nav/slam_gmapping/src/tftest.cpp 2008-11-07 06:40:35 UTC (rev 6371)
@@ -9,7 +9,8 @@
{
node_ = new ros::node("test");
tf_ = new tf::TransformListener(*node_, true,
- 10000000000ULL, 200000000ULL);
+ 10000000000ULL);
+ tf_->setExtrapolationLimit( ros::Duration((int64_t)200000000ULL));
node_->subscribe("base_scan", scan_, &Test::laser_cb, this, -1);
}
Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-11-07 04:23:22 UTC (rev 6370)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-11-07 06:40:35 UTC (rev 6371)
@@ -289,7 +289,7 @@
avmax(DTOR(80.0)),
amin(DTOR(10.0)),
amax(DTOR(40.0)),
- tf(*this, true, 10000000000ULL, 0)// cache for 10 sec, no extrapolation
+ tf(*this, true, 10000000000ULL)// cache for 10 sec, no extrapolation
//tf(*this, true, 200000000ULL, 200000000ULL) //nanoseconds
{
// Initialize global pose. Will be set in control loop based on actual data.
@@ -376,8 +376,8 @@
/* this->tf.setWithEulers("base_laser",
"base",
laser_x_offset, 0.0, 0.0, 0.0, 0.0, 0.0, 0);*/
- this->tf.setTransform(tf::Stamped<btTransform>(btTransform(btQuaternion(0,0,0), btVector3(laser_x_offset, 0,0)), 0, "base_laser", "base"));
- this->tf.setTransform(tf::Stamped<btTransform>(btTransform(btQuaternion(0,0,0), btVector3(laser_x_offset, 0,0)), 0, "map", "other"));///\todo fixme hack to get around short list edge case
+ this->tf.setTransform(tf::Stamped<btTransform>(btTransform(btQuaternion(0,0,0), btVector3(laser_x_offset, 0,0)), ros::Time(0ULL), "base_laser", "base"));
+ this->tf.setTransform(tf::Stamped<btTransform>(btTransform(btQuaternion(0,0,0), btVector3(laser_x_offset, 0,0)), ros::Time(0ULL), "map", "other"));///\todo fixme hack to get around short list edge case
advertise<std_msgs::Planner2DState>("state",1);
@@ -611,7 +611,7 @@
tf::Stamped<tf::Pose> robotPose;
robotPose.setIdentity();
robotPose.frame_id_ = "base";
- robotPose.stamp_ = 0; // request most recent pose
+ robotPose.stamp_ = ros::Time(0ULL); // 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
Modified: pkg/trunk/util/tf/include/tf/tf.h
===================================================================
--- pkg/trunk/util/tf/include/tf/tf.h 2008-11-07 04:23:22 UTC (rev 6370)
+++ pkg/trunk/util/tf/include/tf/tf.h 2008-11-07 06:40:35 UTC (rev 6371)
@@ -85,11 +85,10 @@
/** Constructor
* \param interpolating Whether to interpolate, if this is false the closest value will be returned
* \param cache_time How long to keep a history of transforms in nanoseconds
- * \param max_extrapolation_distance How far to extrapolate before throwing an exception
+ *
*/
Transformer(bool interpolating = true,
- ros::Duration cache_time_ = ros::Duration(DEFAULT_CACHE_TIME),
- ros::Duration max_extrapolation_distance_ = ros::Duration(DEFAULT_MAX_EXTRAPOLATION_DISTANCE));
+ ros::Duration cache_time_ = ros::Duration(DEFAULT_CACHE_TIME));
virtual ~Transformer(void);
/** \brief Clear all data */
@@ -167,6 +166,11 @@
* Returns true unless "NO_PARENT" */
bool getParent(const std::string& frame_id, ros::Time time, std::string& parent);
+ /**@brief Set the distance which tf is allow to extrapolate
+ * \param distance How far to extrapolate before throwing an exception
+ * default is zero */
+ void setExtrapolationLimit(const ros::Duration& distance);
+
protected:
/** \brief The internal storage class for ReferenceTransform.
Modified: pkg/trunk/util/tf/include/tf/transform_listener.h
===================================================================
--- pkg/trunk/util/tf/include/tf/transform_listener.h 2008-11-07 04:23:22 UTC (rev 6370)
+++ pkg/trunk/util/tf/include/tf/transform_listener.h 2008-11-07 06:40:35 UTC (rev 6371)
@@ -55,13 +55,15 @@
rosTF::TransformArray tfArrayIn;
public:
+ /**@brief Constructor for transform listener
+ * \param rosnode A reference to an instance of a ros::node for communication
+ * \param interpolating Whether to interpolate or return the closest
+ * \param max_cache_time How long to store transform information */
TransformListener(ros::node & rosnode,
- bool interpolating = true,
- int64_t max_cache_time = DEFAULT_CACHE_TIME,
- int64_t max_extrapolation_distance = DEFAULT_MAX_EXTRAPOLATION_DISTANCE):
+ bool interpolating = true,
+ int64_t max_cache_time = DEFAULT_CACHE_TIME):
Transformer(interpolating,
- max_cache_time,
- max_extrapolation_distance),
+ max_cache_time),
node_(rosnode)
{
// printf("Constructed rosTF\n");
Modified: pkg/trunk/util/tf/src/tf.cpp
===================================================================
--- pkg/trunk/util/tf/src/tf.cpp 2008-11-07 04:23:22 UTC (rev 6370)
+++ pkg/trunk/util/tf/src/tf.cpp 2008-11-07 06:40:35 UTC (rev 6371)
@@ -34,11 +34,10 @@
using namespace tf;
Transformer::Transformer(bool interpolating,
- ros::Duration cache_time,
- ros::Duration max_extrapolation_distance):
+ ros::Duration cache_time):
cache_time(cache_time),
interpolating (interpolating),
- max_extrapolation_distance_(max_extrapolation_distance)
+ max_extrapolation_distance_(DEFAULT_MAX_EXTRAPOLATION_DISTANCE)
{
frameIDs_["NO_PARENT"] = 0;
frames_.push_back(NULL);// new TimeCache(interpolating, cache_time, max_extrapolation_distance));//unused but needed for iteration over all elements
@@ -130,6 +129,10 @@
};
+void Transformer::setExtrapolationLimit(const ros::Duration& distance)
+{
+ max_extrapolation_distance_ = distance;
+}
TransformLists Transformer::lookupLists(unsigned int target_frame, ros::Time time, unsigned int source_frame)
Modified: pkg/trunk/util/tf/test/tf_unittest_future.cpp
===================================================================
--- pkg/trunk/util/tf/test/tf_unittest_future.cpp 2008-11-07 04:23:22 UTC (rev 6370)
+++ pkg/trunk/util/tf/test/tf_unittest_future.cpp 2008-11-07 06:40:35 UTC (rev 6371)
@@ -17,7 +17,7 @@
TEST(tf, NoExtrapolationExceptionFromParent)
{
- tf::Transformer mTR(true, ros::Duration((int64_t)1000000LL), ros::Duration((int64_t)0LL));
+ tf::Transformer mTR(true, ros::Duration((int64_t)1000000LL));
@@ -54,7 +54,7 @@
TEST(tf, ExtrapolationFromOneValue)
{
- tf::Transformer mTR(true, ros::Duration((int64_t)1000000LL), ros::Duration((int64_t)0LL));
+ tf::Transformer mTR(true, ros::Duration((int64_t)1000000LL));
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.cpp 2008-11-07 04:23:22 UTC (rev 6370)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.cpp 2008-11-07 06:40:35 UTC (rev 6371)
@@ -86,7 +86,8 @@
}
ROS_ASSERT( ros_node_ );
- tf_ = new tf::TransformListener( *ros_node_, true, 10000000000ULL, 1000000000ULL );
+ tf_ = new tf::TransformListener( *ros_node_, true, 10000000000ULL);
+ tf_->setExtrapolationLimit( ros::Duration((int64_t)1000000000ULL) );
scene_manager_ = ogre_root_->createSceneManager( Ogre::ST_GENERIC );
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|