Skip to content

Cleanups in buffer_core.cpp. #301

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
clalancette merged 1 commit into from
Sep 4, 2020
Merged

Cleanups in buffer_core.cpp. #301

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
45 changes: 1 addition & 44 deletions tf2/include/tf2/buffer_core.h
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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