Skip to content
This repository has been archived by the owner on Jul 23, 2024. It is now read-only.

Commit

Permalink
Cleanups in buffer_core.cpp. (ros#301)
Browse files Browse the repository at this point in the history 
This does a few things:
1.  Removes commented out code.  This code has been commented
out for a decade, so I think it is safe to say nobody is
using it.
2.  Uses size_t everywhere we are calling vector.size()
3.  Ensures all member variables are suffixed with _

Signed-off-by: Chris Lalancette <[email protected]>
  • Loading branch information
@clalancette
clalancette authored Sep 4, 2020
1 parent a4f152f commit cab7c8c
Show file tree
Hide file tree
Showing 3 changed files with 31 additions and 153 deletions.
45 changes: 1 addition & 44 deletions tf2/include/tf2/buffer_core.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -156,49 +156,6 @@ class BufferCore : public BufferCoreInterface
const std::string & source_frame, const TimePoint & source_time,
const std::string & fixed_frame) const override;

/** \brief Lookup the twist of the tracking_frame with respect to the observation frame in the reference_frame using the reference point
* \param tracking_frame The frame to track
* \param observation_frame The frame from which to measure the twist
* \param reference_frame The reference frame in which to express the twist
* \param reference_point The reference point with which to express the twist
* \param reference_point_frame The frame_id in which the reference point is expressed
* \param time The time at which to get the velocity
* \param duration The period over which to average
* \return twist The twist output
*
* This will compute the average velocity on the interval
* (time - duration/2, time+duration/2). If that is too close to the most
* recent reading, in which case it will shift the interval up to
* duration/2 to prevent extrapolation.
*
* Possible exceptions tf2::LookupException, tf2::ConnectivityException,
* tf2::ExtrapolationException, tf2::InvalidArgumentException
*
* New in geometry 1.1
*/
/*
geometry_msgs::Twist
lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, const std::string& reference_frame,
const tf::Point & reference_point, const std::string& reference_point_frame,
const tf2::TimePoint& time, const tf2::Duration& averaging_interval) const;
*/
/** \brief lookup the twist of the tracking frame with respect to the observational frame
*
* This is a simplified version of
* lookupTwist with it assumed that the reference point is the
* origin of the tracking frame, and the reference frame is the
* observation frame.
*
* Possible exceptions tf2::LookupException, tf2::ConnectivityException,
* tf2::ExtrapolationException, tf2::InvalidArgumentException
*
* New in geometry 1.1
*/
/*
geometry_msgs::Twist
lookupTwist(const std::string& tracking_frame, const std::string& observation_frame,
const tf2::TimePoint& time, const tf2::Duration& averaging_interval) const;
*/
/** \brief Test if a transform is possible
* \param target_frame The frame into which to transform
* \param source_frame The frame from which to transform
Expand Down Expand Up @@ -365,7 +322,7 @@ class BufferCore : public BufferCoreInterface
typedef std::unordered_map<std::string, CompactFrameID> M_StringToCompactFrameID;
M_StringToCompactFrameID frameIDs_;
/** \brief A map from CompactFrameID frame_id_numbers to string for debugging and output */
std::vector<std::string> frameIDs_reverse;
std::vector<std::string> frameIDs_reverse_;
/** \brief A map to lookup the most recent authority for a given frame */
std::map<CompactFrameID, std::string> frame_authority_;

Expand Down
2 changes: 0 additions & 2 deletions tf2/include/tf2/transform_storage.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,13 +59,11 @@ class TransformStorage
TF2_PUBLIC
TransformStorage & operator=(const TransformStorage & rhs)
{
#if 01
rotation_ = rhs.rotation_;
translation_ = rhs.translation_;
stamp_ = rhs.stamp_;
frame_id_ = rhs.frame_id_;
child_frame_id_ = rhs.child_frame_id_;
#endif
return *this;
}

Expand Down
137 changes: 30 additions & 107 deletions tf2/src/buffer_core.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,9 +28,8 @@

/** \author Tully Foote */

#include <assert.h>

#include <algorithm>
#include <cassert>
#include <map>
#include <mutex>
#include <string>
Expand Down Expand Up @@ -219,7 +218,7 @@ BufferCore::BufferCore(tf2::Duration cache_time)
{
frameIDs_["NO_PARENT"] = 0;
frames_.push_back(TimeCacheInterfacePtr());
frameIDs_reverse.push_back("NO_PARENT");
frameIDs_reverse_.push_back("NO_PARENT");
}

BufferCore::~BufferCore() {}
Expand Down Expand Up @@ -265,15 +264,6 @@ bool BufferCore::setTransformImpl(
const std::string child_frame_id, const TimePoint stamp,
const std::string & authority, bool is_static)
{
// BACKWARDS COMPATABILITY
/* tf::StampedTransform tf_transform;
tf::transformStampedMsgToTF(transform_in, tf_transform);
if (!old_tf_.setTransform(tf_transform, authority))
{
printf("Warning old setTransform Failed but was not caught\n");
}*/

/////// New implementation
std::string stripped_frame_id = stripSlash(frame_id);
std::string stripped_child_frame_id = stripSlash(child_frame_id);

Expand Down Expand Up @@ -755,7 +745,6 @@ void BufferCore::lookupTransformImpl(
transform.setRotation(accum.result_quat);
}


void BufferCore::lookupTransformImpl(
const std::string & target_frame,
const TimePoint & target_time,
Expand All @@ -776,71 +765,6 @@ void BufferCore::lookupTransformImpl(
transform = tf2 * tf1;
}


/*
geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame,
const std::string& observation_frame,
const tf2::TimePoint& time,
const tf2::Duration& averaging_interval) const
{
try
{
geometry_msgs::Twist t;
old_tf_.lookupTwist(tracking_frame, observation_frame,
time, averaging_interval, t);
return t;
}
catch (tf::LookupException& ex)
{
throw tf2::LookupException(ex.what());
}
catch (tf::ConnectivityException& ex)
{
throw tf2::ConnectivityException(ex.what());
}
catch (tf::ExtrapolationException& ex)
{
throw tf2::ExtrapolationException(ex.what());
}
catch (tf::InvalidArgument& ex)
{
throw tf2::InvalidArgumentException(ex.what());
}
}
geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame,
const std::string& observation_frame,
const std::string& reference_frame,
const tf2::Point & reference_point,
const std::string& reference_point_frame,
const tf2::TimePoint& time,
const tf2::Duration& averaging_interval) const
{
try{
geometry_msgs::Twist t;
old_tf_.lookupTwist(tracking_frame, observation_frame, reference_frame, reference_point, reference_point_frame,
time, averaging_interval, t);
return t;
}
catch (tf::LookupException& ex)
{
throw tf2::LookupException(ex.what());
}
catch (tf::ConnectivityException& ex)
{
throw tf2::ConnectivityException(ex.what());
}
catch (tf::ExtrapolationException& ex)
{
throw tf2::ExtrapolationException(ex.what());
}
catch (tf::InvalidArgument& ex)
{
throw tf2::InvalidArgumentException(ex.what());
}
}
*/

struct CanTransformAccum
{
CompactFrameID gather(TimeCacheInterfacePtr cache, TimePoint time, std::string * error_string)
Expand Down Expand Up @@ -939,7 +863,6 @@ bool BufferCore::canTransform(
canTransformInternal(fixed_id, source_id, source_time, error_msg);
}


tf2::TimeCacheInterfacePtr BufferCore::getFrame(CompactFrameID frame_id) const
{
if (frame_id >= frames_.size()) {
Expand Down Expand Up @@ -970,7 +893,7 @@ CompactFrameID BufferCore::lookupOrInsertFrameNumber(const std::string & frameid
// Just a place holder for iteration
frames_.push_back(TimeCacheInterfacePtr());
frameIDs_[frameid_str] = retval;
frameIDs_reverse.push_back(frameid_str);
frameIDs_reverse_.push_back(frameid_str);
} else {
retval = frameIDs_[frameid_str];
}
Expand All @@ -979,12 +902,12 @@ CompactFrameID BufferCore::lookupOrInsertFrameNumber(const std::string & frameid

const std::string & BufferCore::lookupFrameString(CompactFrameID frame_id_num) const
{
if (frame_id_num >= frameIDs_reverse.size()) {
if (frame_id_num >= frameIDs_reverse_.size()) {
std::stringstream ss;
ss << "Reverse lookup of frame id " << frame_id_num << " failed!";
throw tf2::LookupException(ss.str());
} else {
return frameIDs_reverse[frame_id_num];
return frameIDs_reverse_[frame_id_num];
}
}

Expand Down Expand Up @@ -1021,8 +944,8 @@ std::string BufferCore::allFramesAsStringNoLock() const
TransformStorage temp;

// regular transforms
for (unsigned int counter = 1; counter < frames_.size(); counter++) {
TimeCacheInterfacePtr frame_ptr = getFrame(CompactFrameID(counter));
for (size_t counter = 1; counter < frames_.size(); counter++) {
TimeCacheInterfacePtr frame_ptr = getFrame(static_cast<CompactFrameID>(counter));
if (frame_ptr == NULL) {
continue;
}
Expand All @@ -1032,8 +955,8 @@ std::string BufferCore::allFramesAsStringNoLock() const
} else {
frame_id_num = 0;
}
mstream << "Frame " << frameIDs_reverse[counter] << " exists with parent " <<
frameIDs_reverse[frame_id_num] << "." << std::endl;
mstream << "Frame " << frameIDs_reverse_[counter] << " exists with parent " <<
frameIDs_reverse_[frame_id_num] << "." << std::endl;
}

return mstream.str();
Expand Down Expand Up @@ -1220,8 +1143,8 @@ std::string BufferCore::allFramesAsYAML(TimePoint current_time) const
mstream.setf(std::ios::fixed, std::ios::floatfield);

// one referenced for 0 is no frame
for (unsigned int counter = 1; counter < frames_.size(); counter++) {
CompactFrameID cfid = CompactFrameID(counter);
for (size_t counter = 1; counter < frames_.size(); counter++) {
CompactFrameID cfid = static_cast<CompactFrameID>(counter);
CompactFrameID frame_id_num;
TimeCacheInterfacePtr cache = getFrame(cfid);
if (!cache) {
Expand Down Expand Up @@ -1254,8 +1177,8 @@ std::string BufferCore::allFramesAsYAML(TimePoint current_time) const

mstream << std::fixed; // fixed point notation
mstream.precision(3); // 3 decimal places
mstream << frameIDs_reverse[cfid] << ": " << std::endl;
mstream << " parent: '" << frameIDs_reverse[frame_id_num] << "'" << std::endl;
mstream << frameIDs_reverse_[cfid] << ": " << std::endl;
mstream << " parent: '" << frameIDs_reverse_[frame_id_num] << "'" << std::endl;
mstream << " broadcaster: '" << authority << "'" << std::endl;
mstream << " rate: " << rate << std::endl;
mstream << " most_recent_transform: " << displayTimePoint(cache->getLatestTimestamp()) <<
Expand Down Expand Up @@ -1405,8 +1328,8 @@ void BufferCore::_getFrameStrings(std::vector<std::string> & vec) const

TransformStorage temp;

for (unsigned int counter = 1; counter < frameIDs_reverse.size(); counter++) {
vec.push_back(frameIDs_reverse[counter]);
for (size_t counter = 1; counter < frameIDs_reverse_.size(); counter++) {
vec.push_back(frameIDs_reverse_[counter]);
}
}

Expand Down Expand Up @@ -1470,7 +1393,6 @@ void BufferCore::testTransformableRequests()
}
}


std::string BufferCore::_allFramesAsDot(TimePoint current_time) const
{
std::stringstream mstream;
Expand All @@ -1485,9 +1407,9 @@ std::string BufferCore::_allFramesAsDot(TimePoint current_time) const
mstream.precision(3);
mstream.setf(std::ios::fixed, std::ios::floatfield);
// one referenced for 0 is no frame
for (unsigned int counter = 1; counter < frames_.size(); counter++) {
unsigned int frame_id_num;
TimeCacheInterfacePtr counter_frame = getFrame(counter);
for (size_t counter = 1; counter < frames_.size(); counter++) {
CompactFrameID frame_id_num;
TimeCacheInterfacePtr counter_frame = getFrame(static_cast<CompactFrameID>(counter));
if (!counter_frame) {
continue;
}
Expand All @@ -1497,7 +1419,7 @@ std::string BufferCore::_allFramesAsDot(TimePoint current_time) const
frame_id_num = temp.frame_id_;
}
std::string authority = "no recorded authority";
std::map<unsigned int, std::string>::const_iterator it = frame_authority_.find(counter);
std::map<CompactFrameID, std::string>::const_iterator it = frame_authority_.find(static_cast<CompactFrameID>(counter));
if (it != frame_authority_.end()) {
authority = it->second;
}
Expand All @@ -1516,8 +1438,8 @@ std::string BufferCore::_allFramesAsDot(TimePoint current_time) const

mstream << std::fixed; // fixed point notation
mstream.precision(3); // 3 decimal places
mstream << "\"" << frameIDs_reverse[frame_id_num] << "\"" << " -> " <<
"\"" << frameIDs_reverse[counter] << "\"" << "[label=\"" <<
mstream << "\"" << frameIDs_reverse_[frame_id_num] << "\"" << " -> " <<
"\"" << frameIDs_reverse_[counter] << "\"" << "[label=\"" <<
"Broadcaster: " << authority << "\\n" <<
"Average rate: " << rate << " Hz\\n" <<
"Most recent transform: " << displayTimePoint(counter_frame->getLatestTimestamp()) << " ";
Expand All @@ -1532,9 +1454,9 @@ std::string BufferCore::_allFramesAsDot(TimePoint current_time) const
}

// one referenced for 0 is no frame
for (unsigned int counter = 1; counter < frames_.size(); counter++) {
unsigned int frame_id_num;
TimeCacheInterfacePtr counter_frame = getFrame(counter);
for (size_t counter = 1; counter < frames_.size(); counter++) {
CompactFrameID frame_id_num;
TimeCacheInterfacePtr counter_frame = getFrame(static_cast<CompactFrameID>(counter));
if (!counter_frame) {
if (current_time != TimePointZero) {
mstream << "edge [style=invis];" << std::endl;
Expand All @@ -1543,7 +1465,7 @@ std::string BufferCore::_allFramesAsDot(TimePoint current_time) const
<<
"\"Recorded at time: " << displayTimePoint(current_time) <<
"\"[ shape=plaintext ] ;\n " <<
"}" << "->" << "\"" << frameIDs_reverse[counter] << "\";" << std::endl;
"}" << "->" << "\"" << frameIDs_reverse_[counter] << "\";" << std::endl;
}
continue;
}
Expand All @@ -1553,15 +1475,15 @@ std::string BufferCore::_allFramesAsDot(TimePoint current_time) const
frame_id_num = 0;
}

if (frameIDs_reverse[frame_id_num] == "NO_PARENT") {
if (frameIDs_reverse_[frame_id_num] == "NO_PARENT") {
mstream << "edge [style=invis];" << std::endl;
mstream <<
" subgraph cluster_legend { style=bold; color=black; label =\"view_frames Result\";\n";
if (current_time != TimePointZero) {
mstream << "\"Recorded at time: " << displayTimePoint(current_time) <<
"\"[ shape=plaintext ] ;\n ";
}
mstream << "}" << "->" << "\"" << frameIDs_reverse[counter] << "\";" << std::endl;
mstream << "}" << "->" << "\"" << frameIDs_reverse_[counter] << "\";" << std::endl;
}
}
mstream << "}";
Expand Down Expand Up @@ -1597,7 +1519,6 @@ void BufferCore::_chainAsVector(
tf2::TF2Error retval = walkToTopParent(
accum, source_time, fixed_id, source_id, &error_string,
&source_frame_chain);

if (retval != tf2::TF2Error::NO_ERROR) {
switch (retval) {
case tf2::TF2Error::CONNECTIVITY_ERROR:
Expand All @@ -1611,6 +1532,7 @@ void BufferCore::_chainAsVector(
assert(0);
}
}

if (source_time != target_time) {
std::vector<CompactFrameID> target_frame_chain;
retval = walkToTopParent(
Expand All @@ -1630,6 +1552,7 @@ void BufferCore::_chainAsVector(
assert(0);
}
}

int m = static_cast<int>(target_frame_chain.size() - 1);
int n = static_cast<int>(source_frame_chain.size() - 19);
for (; m >= 0 && n >= 0; --m, --n) {
Expand All @@ -1650,7 +1573,7 @@ void BufferCore::_chainAsVector(
}

// Write each element of source_frame_chain as string
for (unsigned int i = 0; i < source_frame_chain.size(); ++i) {
for (size_t i = 0; i < source_frame_chain.size(); ++i) {
output.push_back(lookupFrameString(source_frame_chain[i]));
}
}
Expand Down

0 comments on commit cab7c8c

Please sign in to comment.