Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[18030] pkg/trunk/stacks/geometry/tf
|
From: <jfa...@us...> - 2009-06-30 23:43:34
|
Revision: 18030
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18030&view=rev
Author: jfaustwg
Date: 2009-06-30 23:43:31 +0000 (Tue, 30 Jun 2009)
Log Message:
-----------
Add ability to be notified whenever TF data changes through a callback
Modified Paths:
--------------
pkg/trunk/stacks/geometry/tf/CMakeLists.txt
pkg/trunk/stacks/geometry/tf/include/tf/message_notifier.h
pkg/trunk/stacks/geometry/tf/include/tf/tf.h
pkg/trunk/stacks/geometry/tf/src/tf.cpp
Modified: pkg/trunk/stacks/geometry/tf/CMakeLists.txt
===================================================================
--- pkg/trunk/stacks/geometry/tf/CMakeLists.txt 2009-06-30 23:38:51 UTC (rev 18029)
+++ pkg/trunk/stacks/geometry/tf/CMakeLists.txt 2009-06-30 23:43:31 UTC (rev 18030)
@@ -8,8 +8,8 @@
genmsg()
gensrv()
-rospack_add_library(tf src/tf.cpp src/transform_listener.cpp src/cache.cpp src/message_notifier.cpp src/transform_broadcaster.cpp)
-rospack_link_boost(tf thread)
+rospack_add_library(tf src/tf.cpp src/transform_listener.cpp src/cache.cpp src/message_notifier.cpp src/message_filter.cpp src/transform_broadcaster.cpp)
+rospack_link_boost(tf thread signals)
rospack_add_executable(bin/tf_echo src/tf_echo.cpp)
target_link_libraries(bin/tf_echo tf)
Modified: pkg/trunk/stacks/geometry/tf/include/tf/message_notifier.h
===================================================================
--- pkg/trunk/stacks/geometry/tf/include/tf/message_notifier.h 2009-06-30 23:38:51 UTC (rev 18029)
+++ pkg/trunk/stacks/geometry/tf/include/tf/message_notifier.h 2009-06-30 23:43:31 UTC (rev 18030)
@@ -29,8 +29,8 @@
/** \author Josh Faust */
-#ifndef MESSAGE_NOTIFIER_H
-#define MESSAGE_NOTIFIER_H
+#ifndef TF_MESSAGE_NOTIFIER_H
+#define TF_MESSAGE_NOTIFIER_H
#include <ros/ros.h>
#include <tf/tf.h>
@@ -111,7 +111,7 @@
* \param callback The function to call when a message is ready to be transformed
* \param topic The topic to listen on
* \param target_frame The frame we need to be able to transform to before a message is ready
- * \param queue_size The number of messages to keep around waiting for transform data.
+ * \param queue_size The number of messages to keep around waiting for transform data.
* \note A queue size of 0 means infinite, which can be dangerous
*/
MessageNotifier(Transformer& tf, Callback callback,
@@ -135,7 +135,7 @@
target_frames_.resize(1);
target_frames_[0] = target_frame;
target_frames_string_ = target_frame;
-
+
setTopic(topic);
tf_subscriber_ = node_.subscribe<tfMessage>("/tf_message", 1,
@@ -170,7 +170,7 @@
target_frames_[0] = target_frame;
setTopic(topic);
-
+
tf_subscriber_ = node_.subscribe<tfMessage>("/tf_message", 1,
boost::bind(&MessageNotifier::incomingTFMessage, this, _1));
@@ -186,8 +186,8 @@
successful_transform_count_, failed_transform_count_, failed_out_the_back_count_, transform_message_count_, incoming_message_count_, dropped_message_count_);
unsubscribeFromMessage();
-
+
// Tell the worker thread that we're destructing
destructing_ = true;
new_data_.notify_all();
@@ -293,7 +293,7 @@
{
if (!topic_.empty())
{
- subscriber_ = node_.subscribe<MessageT>(topic_, queue_size_,
+ subscriber_ = node_.subscribe<MessageT>(topic_, queue_size_,
boost::bind(&MessageNotifier::incomingMessage, this, _1));
}
}
Modified: pkg/trunk/stacks/geometry/tf/include/tf/tf.h
===================================================================
--- pkg/trunk/stacks/geometry/tf/include/tf/tf.h 2009-06-30 23:38:51 UTC (rev 18029)
+++ pkg/trunk/stacks/geometry/tf/include/tf/tf.h 2009-06-30 23:43:31 UTC (rev 18030)
@@ -1,10 +1,10 @@
/*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
- *
+ *
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
- *
+ *
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
@@ -13,7 +13,7 @@
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
- *
+ *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
@@ -42,8 +42,8 @@
#include <tf/exceptions.h>
#include "tf/time_cache.h"
#include <boost/thread/mutex.hpp>
+#include <boost/signals.hpp>
-
namespace tf
{
/** \brief remap names \todo document me */
@@ -52,30 +52,30 @@
enum ErrorValues { NO_ERROR = 0, LOOKUP_ERROR, CONNECTIVITY_ERROR, EXTRAPOLATION_ERROR};
/** \brief An internal representation of transform chains
- *
+ *
* This struct is how the list of transforms are stored before being passed to computeTransformFromList. */
-typedef struct
+typedef struct
{
std::vector<TransformStorage > inverseTransforms;
std::vector<TransformStorage > forwardTransforms;
} TransformLists;
-/** \brief A Class which provides coordinate transforms between any two frames in a system.
- *
- * This class provides a simple interface to allow recording and lookup of
+/** \brief A Class which provides coordinate transforms between any two frames in a system.
+ *
+ * This class provides a simple interface to allow recording and lookup of
* relationships between arbitrary frames of the system.
- *
- * libTF assumes that there is a tree of coordinate frame transforms which define the relationship between all coordinate frames.
- * For example your typical robot would have a transform from global to real world. And then from base to hand, and from base to head.
- * But Base to Hand really is composed of base to shoulder to elbow to wrist to hand.
- * libTF is designed to take care of all the intermediate steps for you.
- *
- * Internal Representation
+ *
+ * libTF assumes that there is a tree of coordinate frame transforms which define the relationship between all coordinate frames.
+ * For example your typical robot would have a transform from global to real world. And then from base to hand, and from base to head.
+ * But Base to Hand really is composed of base to shoulder to elbow to wrist to hand.
+ * libTF is designed to take care of all the intermediate steps for you.
+ *
+ * Internal Representation
* libTF will store frames with the parameters necessary for generating the transform into that frame from it's parent and a reference to the parent frame.
* Frames are designated using an std::string
* 0 is a frame without a parent (the top of a tree)
- * The positions of frames over time must be pushed in.
- *
+ * The positions of frames over time must be pushed in.
+ *
* All function calls which pass frame ids can potentially throw the exception TransformReference::LookupException
*/
class Transformer
@@ -87,55 +87,55 @@
static const int64_t DEFAULT_MAX_EXTRAPOLATION_DISTANCE = 0ULL; //!< The default amount of time to extrapolate
- /** Constructor
+ /** 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
- *
+ *
*/
- Transformer(bool interpolating = true,
+ Transformer(bool interpolating = true,
ros::Duration cache_time_ = ros::Duration(DEFAULT_CACHE_TIME));
virtual ~Transformer(void);
/** \brief Clear all data */
void clear();
- /** \brief Add transform information to the tf data structure
+ /** \brief Add transform information to the tf data structure
* \param transform The transform to store
- * \param authority The source of the information for this transform
+ * \param authority The source of the information for this transform
* returns true unless an error occured
*/
bool setTransform(const Stamped<btTransform>& transform, const std::string & authority = "default_authority");
/*********** Accessors *************/
- /** \brief Get the transform between two frames by frame ID.
+ /** \brief Get the transform between two frames by frame ID.
* \param target_frame The frame to which data should be transformed
* \param source_frame The frame where the data originated
* \param time The time at which the value of the transform is desired. (0 will get the latest)
- *
- * Possible exceptions TransformReference::LookupException, TransformReference::ConnectivityException,
+ *
+ * Possible exceptions TransformReference::LookupException, TransformReference::ConnectivityException,
* TransformReference::MaxDepthException
*/
- void lookupTransform(const std::string& target_frame, const std::string& source_frame,
+ void lookupTransform(const std::string& target_frame, const std::string& source_frame,
const ros::Time& time, Stamped<btTransform>& transform) const;
//time traveling version
- void lookupTransform(const std::string& target_frame, const ros::Time& target_time,
- const std::string& source_frame, const ros::Time& source_time,
- const std::string& fixed_frame, Stamped<btTransform>& transform) const;
- bool canTransform(const std::string& target_frame, const std::string& source_frame,
+ void lookupTransform(const std::string& target_frame, const ros::Time& target_time,
+ const std::string& source_frame, const ros::Time& source_time,
+ const std::string& fixed_frame, Stamped<btTransform>& transform) const;
+ bool canTransform(const std::string& target_frame, const std::string& source_frame,
const ros::Time& time, const ros::Duration& timeout, const ros::Duration& polling_sleep_duration = ros::Duration(0.01)) const;
- bool canTransform(const std::string& target_frame, const std::string& source_frame,
+ bool canTransform(const std::string& target_frame, const std::string& source_frame,
const ros::Time& time) const;
//time traveling version
- bool canTransform(const std::string& target_frame, const ros::Time& target_time,
- const std::string& source_frame, const ros::Time& source_time,
- const std::string& fixed_frame) const;
- bool canTransform(const std::string& target_frame, const ros::Time& target_time,
- const std::string& source_frame, const ros::Time& source_time,
- const std::string& fixed_frame,
- const ros::Duration& timeout, const ros::Duration& polling_sleep_duration = ros::Duration(0.01)) const;
+ bool canTransform(const std::string& target_frame, const ros::Time& target_time,
+ const std::string& source_frame, const ros::Time& source_time,
+ const std::string& fixed_frame) const;
+ bool canTransform(const std::string& target_frame, const ros::Time& target_time,
+ const std::string& source_frame, const ros::Time& source_time,
+ const std::string& fixed_frame,
+ const ros::Duration& timeout, const ros::Duration& polling_sleep_duration = ros::Duration(0.01)) const;
- /**@brief Return the latest rostime which is common across the spanning set
+ /**@brief Return the latest rostime which is common across the spanning set
* zero if fails to cross */
int getLatestCommonTime(const std::string& source, const std::string& dest, ros::Time& time, std::string * error_string) const;
@@ -150,60 +150,60 @@
void transformPose(const std::string& target_frame, const Stamped<tf::Pose>& stamped_in, Stamped<tf::Pose>& stamped_out) const;
/** \brief Transform a Stamped Quaternion into the target frame */
- void transformQuaternion(const std::string& target_frame, const ros::Time& target_time,
- const Stamped<tf::Quaternion>& stamped_in,
- const std::string& fixed_frame,
+ void transformQuaternion(const std::string& target_frame, const ros::Time& target_time,
+ const Stamped<tf::Quaternion>& stamped_in,
+ const std::string& fixed_frame,
Stamped<tf::Quaternion>& stamped_out) const;
/** \brief Transform a Stamped Vector3 into the target frame */
- void transformVector(const std::string& target_frame, const ros::Time& target_time,
- const Stamped<tf::Vector3>& stamped_in,
- const std::string& fixed_frame,
+ void transformVector(const std::string& target_frame, const ros::Time& target_time,
+ const Stamped<tf::Vector3>& stamped_in,
+ const std::string& fixed_frame,
Stamped<tf::Vector3>& stamped_out) const;
- /** \brief Transform a Stamped Point into the target frame
+ /** \brief Transform a Stamped Point into the target frame
* \todo document*/
- void transformPoint(const std::string& target_frame, const ros::Time& target_time,
- const Stamped<tf::Point>& stamped_in,
- const std::string& fixed_frame,
+ void transformPoint(const std::string& target_frame, const ros::Time& target_time,
+ const Stamped<tf::Point>& stamped_in,
+ const std::string& fixed_frame,
Stamped<tf::Point>& stamped_out) const;
- /** \brief Transform a Stamped Pose into the target frame
+ /** \brief Transform a Stamped Pose into the target frame
* \todo document*/
- void transformPose(const std::string& target_frame, const ros::Time& target_time,
- const Stamped<tf::Pose>& stamped_in,
+ void transformPose(const std::string& target_frame, const ros::Time& target_time,
+ const Stamped<tf::Pose>& stamped_in,
const std::string& fixed_frame,
Stamped<tf::Pose>& stamped_out) const;
/** \brief Debugging function that will print the spanning chain of transforms.
- * Possible exceptions TransformReference::LookupException, TransformReference::ConnectivityException,
+ * Possible exceptions TransformReference::LookupException, TransformReference::ConnectivityException,
* TransformReference::MaxDepthException
*/
std::string chainAsString(const std::string & target_frame, ros::Time target_time, const std::string & source_frame, ros::Time source_time, const std::string & fixed_frame) const;
/** \brief Debugging function that will print the spanning chain of transforms.
- * Possible exceptions TransformReference::LookupException, TransformReference::ConnectivityException,
+ * Possible exceptions TransformReference::LookupException, TransformReference::ConnectivityException,
* TransformReference::MaxDepthException
*/
void chainAsVector(const std::string & target_frame, ros::Time target_time, const std::string & source_frame, ros::Time source_time, const std::string & fixed_frame, std::vector<std::string>& output) const;
- /** \brief A way to see what frames have been cached
- * Useful for debugging
+ /** \brief A way to see what frames have been cached
+ * Useful for debugging
*/
std::string allFramesAsString() const;
- /** \brief A way to see what frames have been cached
- * Useful for debugging
+ /** \brief A way to see what frames have been cached
+ * Useful for debugging
*/
std::string allFramesAsDot() const;
/** \brief A way to get a std::vector of available frame ids */
void getFrameStrings(std::vector<std::string>& ids) const;
- /**@brief Fill the parent of a frame.
+ /**@brief Fill the parent of a frame.
* @param frame_id The frame id of the frame in question
* @param parent The reference to the string to fill the parent
* Returns true unless "NO_PARENT" */
bool getParent(const std::string& frame_id, ros::Time time, std::string& parent) const;
- /**@brief Check if a frame exists in the tree
+ /**@brief Check if a frame exists in the tree
* @param frame_id_str The frame id in question */
bool frameExists(const std::string& frame_id_str) const
{
@@ -225,19 +225,28 @@
/**@brief Get the duration over which this transformer will cache */
ros::Duration getCacheLength() { return cache_time;}
+ /**
+ * \brief Add a callback that happens when a new transform has arrived
+ *
+ * \param callback The callback, of the form void func();
+ * \return A boost::signals::connection object that can be used to remove this
+ * listener
+ */
+ boost::signals::connection addTransformChangedListener(boost::function<void(void)> callback);
+
protected:
- /** \brief The internal storage class for ReferenceTransform.
- *
+ /** \brief The internal storage class for ReferenceTransform.
+ *
* An instance of this class is created for each frame in the system.
- * This class natively handles the relationship between frames.
+ * This class natively handles the relationship between frames.
*
* The derived class Pose3DCache provides a buffered history of positions
* with interpolation.
- *
+ *
*/
-
+
/******************** Internal Storage ****************/
/** \brief The pointers to potential frames that the tree can be made of.
@@ -250,25 +259,30 @@
std::map<std::string, unsigned int> frameIDs_;
std::map<unsigned int, std::string> frame_authority_;
std::vector<std::string> frameIDs_reverse;
-
+
/// How long to cache transform history
ros::Duration cache_time;
/// whether or not to interpolate or extrapolate
bool interpolating;
-
+
/// whether or not to allow extrapolation
ros::Duration max_extrapolation_distance_;
- /// transform prefix to apply as necessary
+ /// transform prefix to apply as necessary
std::string tf_prefix_;
+ typedef boost::signal<void(void)> TransformsChangedSignal;
+ /// Signal which is fired whenever new transform data has arrived, from the thread the data arrived in
+ TransformsChangedSignal transforms_changed_;
+ boost::mutex transforms_changed_mutex_;
+
/************************* Internal Functions ****************************/
- /** \brief An accessor to get a frame, which will throw an exception if the frame is no there.
+ /** \brief An accessor to get a frame, which will throw an exception if the frame is no there.
* \param frame_number The frameID of the desired Reference Frame
- *
+ *
* This is an internal function which will get the pointer to the frame associated with the frame id
* Possible Exception: TransformReference::LookupException
*/
@@ -317,7 +331,7 @@
ss << "Reverse lookup of frame id " << frame_id_num << " failed!";
throw LookupException(ss.str());
}
- else
+ else
return frameIDs_reverse[frame_id_num];
};
@@ -327,7 +341,7 @@
int lookupLists(unsigned int target_frame, ros::Time time, unsigned int source_frame, TransformLists & lists, std::string* error_string) const;
bool test_extrapolation(const ros::Time& target_time, const TransformLists& t_lists, std::string * error_string) const;
-
+
/** Compute the transform based on the list of frames */
btTransform computeTransformFromList(const TransformLists & list) const;
Modified: pkg/trunk/stacks/geometry/tf/src/tf.cpp
===================================================================
--- pkg/trunk/stacks/geometry/tf/src/tf.cpp 2009-06-30 23:38:51 UTC (rev 18029)
+++ pkg/trunk/stacks/geometry/tf/src/tf.cpp 2009-06-30 23:43:31 UTC (rev 18030)
@@ -121,7 +121,7 @@
ROS_ERROR("TF_SELF_TRANSFORM: Ignoring transform from authority \"%s\" with parent_id and frame_id \"%s\" because they are the same", authority.c_str(), transform.frame_id_.c_str());
error_exists = true;
}
-
+
if (transform.frame_id_ == "")
{
ROS_ERROR("TF_NO_FRAME_ID: Ignoring transform from authority \"%s\" because frame_id not set ", authority.c_str());
@@ -137,13 +137,13 @@
if (std::isnan(transform.getOrigin().x()) || std::isnan(transform.getOrigin().y()) || std::isnan(transform.getOrigin().z())||
std::isnan(transform.getRotation().x()) || std::isnan(transform.getRotation().y()) || std::isnan(transform.getRotation().z()) || std::isnan(transform.getRotation().w()))
{
- ROS_ERROR("TF_NAN_INPUT: Ignoring transform for frame_id \"%s\" from authority \"%s\" because of a nan value in the transform (%f %f %f) (%f %f %f %f)",
+ ROS_ERROR("TF_NAN_INPUT: Ignoring transform for frame_id \"%s\" from authority \"%s\" because of a nan value in the transform (%f %f %f) (%f %f %f %f)",
transform.frame_id_.c_str(), authority.c_str(),
- transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z(),
+ transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z(),
transform.getRotation().x(), transform.getRotation().y(), transform.getRotation().z(), transform.getRotation().w()
);
error_exists = true;
- }
+ }
if (error_exists)
return false;
@@ -152,11 +152,17 @@
{
frame_authority_[frame_number] = authority;
}
- else
+ else
{
ROS_WARN("TF_OLD_DATA ignoring data from the past for frame %s at time %g according to authority %s\nPossible reasons are listed at ", transform.frame_id_.c_str(), transform.stamp_.toSec(), authority.c_str());
- return false;
+ return false;
}
+
+ {
+ boost::mutex::scoped_lock lock(transforms_changed_mutex_);
+ transforms_changed_();
+ }
+
return true;
};
@@ -179,7 +185,7 @@
transform.frame_id_ = target_frame;
return;
- }
+ }
// printf("Mapped Source: %s \nMapped Target: %s\n", mapped_source_frame.c_str(), mapped_target_frame.c_str());
int retval = NO_ERROR;
@@ -194,7 +200,7 @@
TransformLists t_list;
if (retval == NO_ERROR)
- try
+ try
{
retval = lookupLists(lookupFrameNumber( mapped_target_frame), temp_time, lookupFrameNumber( mapped_source_frame), t_list, &error_string);
}
@@ -219,8 +225,8 @@
if (time == ros::Time())// Using latest common time if we extrapolate this means that one of the links is out of date
{
ss << "Could not find a common time " << mapped_source_frame << " and " << mapped_target_frame <<".";
- throw ConnectivityException(ss.str());
- }
+ throw ConnectivityException(ss.str());
+ }
else
{
ss << " When trying to transform between " << mapped_source_frame << " and " << mapped_target_frame <<".";
@@ -250,7 +256,7 @@
bool Transformer::canTransform(const std::string& target_frame, const std::string& source_frame,
- const ros::Time& time,
+ const ros::Time& time,
const ros::Duration& timeout, const ros::Duration& polling_sleep_duration) const
{
ros::Time start_time = ros::Time::now();
@@ -274,7 +280,7 @@
//break out early if no op transform
if (mapped_source_frame == mapped_target_frame) return true;
-
+
if (local_time == ros::Time())
if (NO_ERROR != getLatestCommonTime(mapped_source_frame, mapped_target_frame, local_time, NULL)) // set time if zero
{
@@ -286,7 +292,7 @@
TransformLists t_list;
///\todo check return
int retval;
- try
+ try
{
retval = lookupLists(lookupFrameNumber( mapped_target_frame), local_time, lookupFrameNumber( mapped_source_frame), t_list, NULL);
}
@@ -294,8 +300,8 @@
{
return false;
}
-
+
///\todo WRITE HELPER FUNCITON TO RETHROW
if (retval != NO_ERROR)
{
@@ -313,7 +319,7 @@
{
return false;
}
-
+
return true;
};
@@ -324,7 +330,7 @@
};
bool Transformer::canTransform(const std::string& target_frame,const ros::Time& target_time, const std::string& source_frame,
- const ros::Time& source_time, const std::string& fixed_frame,
+ const ros::Time& source_time, const std::string& fixed_frame,
const ros::Duration& timeout, const ros::Duration& polling_sleep_duration) const
{
return canTransform(target_frame, fixed_frame, target_time, timeout, polling_sleep_duration) && canTransform(fixed_frame, source_frame, source_time, timeout, polling_sleep_duration);
@@ -343,7 +349,7 @@
{
return false;
}
-
+
TransformStorage temp;
if (! cache->getData(time, temp)) return false;
if (temp.parent_id_ == "NO_PARENT") return false;
@@ -352,7 +358,7 @@
};
-void Transformer::setExtrapolationLimit(const ros::Duration& distance)
+void Transformer::setExtrapolationLimit(const ros::Duration& distance)
{
max_extrapolation_distance_ = distance;
}
@@ -362,7 +368,7 @@
time = ros::Time(UINT_MAX, 999999999);///\todo replace with ros::TIME_MAX when it is merged from stable
int retval;
TransformLists lists;
- try
+ try
{
retval = lookupLists(lookupFrameNumber(dest), ros::Time(), lookupFrameNumber(source), lists, error_string);
}
@@ -380,7 +386,7 @@
time = ros::Time::now();
return retval;
}
-
+
for (unsigned int i = 0; i < lists.inverseTransforms.size(); i++)
{
if (time > lists.inverseTransforms[i].stamp_)
@@ -562,7 +568,7 @@
return CONNECTIVITY_ERROR;//throw(ConnectivityException(ss.str()));
}
- try
+ try
{
if (lookupFrameNumber(lists.inverseTransforms.back().parent_id_) != target_frame)
{
@@ -593,7 +599,7 @@
return CONNECTIVITY_ERROR;// throw(ConnectivityException(ss.str()));
}
/* Make sure that we don't have a no parent at the top */
- try
+ try
{
if (lookupFrameNumber(lists.inverseTransforms.back().frame_id_) == 0 || lookupFrameNumber( lists.forwardTransforms.back().frame_id_) == 0)
{
@@ -652,7 +658,7 @@
if ( max_extrapolation_distance_ > ros::Duration(0))
{
ss << "This is greater than the max_extrapolation_distance of "
- << (max_extrapolation_distance_).toSec() <<".";
+ << (max_extrapolation_distance_).toSec() <<".";
}
}
}
@@ -669,7 +675,7 @@
if ( max_extrapolation_distance_ > ros::Duration(0))
{
ss << "This is greater than the max_extrapolation_distance of "
- << (max_extrapolation_distance_).toSec() <<".";
+ << (max_extrapolation_distance_).toSec() <<".";
}
}
}
@@ -686,7 +692,7 @@
if ( max_extrapolation_distance_ > ros::Duration(0))
{
ss << "This is greater than the max_extrapolation_distance of "
- << (max_extrapolation_distance_).toSec() <<".";
+ << (max_extrapolation_distance_).toSec() <<".";
}
}
}
@@ -706,7 +712,7 @@
if ( max_extrapolation_distance_ > ros::Duration(0))
{
ss << "This is greater than the max_extrapolation_distance of "
- << (max_extrapolation_distance_).toSec() <<".";
+ << (max_extrapolation_distance_).toSec() <<".";
}
}
}
@@ -723,7 +729,7 @@
if ( max_extrapolation_distance_ > ros::Duration(0))
{
ss << "This is greater than the max_extrapolation_distance of "
- << (max_extrapolation_distance_).toSec() <<".";
+ << (max_extrapolation_distance_).toSec() <<".";
}
}
}
@@ -739,14 +745,14 @@
if ( max_extrapolation_distance_ > ros::Duration(0))
{
ss << "This is greater than the max_extrapolation_distance of "
- << (max_extrapolation_distance_).toSec() <<".";
+ << (max_extrapolation_distance_).toSec() <<".";
}
}
}
}
-
+
if (error_string) ss << " See http://pr.willowgarage.com/pr-docs/ros-packages/tf/html/faq.html for more info.";
-
+
if (error_string) *error_string = ss.str();
return retval;
@@ -861,7 +867,7 @@
TransformStorage temp;
ros::Time current_time = ros::Time::now();
-
+
if (frames_.size() ==1)
mstream <<"\"no tf data recieved\"";
@@ -882,21 +888,21 @@
if (it != frame_authority_.end())
authority = it->second;
- double rate = getFrame(counter)->getListLength() / std::max((getFrame(counter)->getLatestTimestamp().toSec() -
+ double rate = getFrame(counter)->getListLength() / std::max((getFrame(counter)->getLatestTimestamp().toSec() -
getFrame(counter)->getOldestTimestamp().toSec() ), 0.0001);
mstream << std::fixed; //fixed point notation
mstream.precision(3); //3 decimal places
- mstream << "\"" << frameIDs_reverse[counter] << "\"" << " -> "
+ mstream << "\"" << frameIDs_reverse[counter] << "\"" << " -> "
<< "\"" << frameIDs_reverse[parent_id] << "\"" << "[label=\""
<< "Authority: " << authority << "\\n"
<< getFrame(counter)->getListLength() << " Readings averaging " << rate <<" Hz\\n"
- << " Latest reading: \\n" << getFrame(counter)->getLatestTimestamp().toSec()
+ << " Latest reading: \\n" << getFrame(counter)->getLatestTimestamp().toSec()
<< " ( " << (current_time - getFrame(counter)->getLatestTimestamp()).toSec()
<<" seconds ago )\\n"
- << " Oldest reading:\\n"
+ << " Oldest reading:\\n"
<< getFrame(counter)->getOldestTimestamp().toSec()
- << " ( " << (current_time - getFrame(counter)->getOldestTimestamp()).toSec()
+ << " ( " << (current_time - getFrame(counter)->getOldestTimestamp()).toSec()
<<" seconds ago )\\n"
<<"\"];" <<std::endl;
}
@@ -1051,8 +1057,12 @@
// stamped_out.parent_id_ = stamped_in.parent_id_;//only useful for transforms
};
+boost::signals::connection Transformer::addTransformChangedListener(boost::function<void(void)> callback)
+{
+ boost::mutex::scoped_lock lock(transforms_changed_mutex_);
+ return transforms_changed_.connect(callback);
+}
-
/*
void Transformer::transformTransform(const std::string& target_frame,
const robot_msgs::TransformStamped& msg_in,
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|