diff --git a/tf2/include/tf2/buffer_core.h b/tf2/include/tf2/buffer_core.h index 56e36ab54..3ff0a7dea 100644 --- a/tf2/include/tf2/buffer_core.h +++ b/tf2/include/tf2/buffer_core.h @@ -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 @@ -365,7 +322,7 @@ class BufferCore : public BufferCoreInterface typedef std::unordered_map M_StringToCompactFrameID; M_StringToCompactFrameID frameIDs_; /** \brief A map from CompactFrameID frame_id_numbers to string for debugging and output */ - std::vector frameIDs_reverse; + std::vector frameIDs_reverse_; /** \brief A map to lookup the most recent authority for a given frame */ std::map frame_authority_; diff --git a/tf2/include/tf2/transform_storage.h b/tf2/include/tf2/transform_storage.h index 34d87e6b1..112778694 100644 --- a/tf2/include/tf2/transform_storage.h +++ b/tf2/include/tf2/transform_storage.h @@ -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; } diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index a129d737c..f46102f8a 100644 --- a/tf2/src/buffer_core.cpp +++ b/tf2/src/buffer_core.cpp @@ -28,9 +28,8 @@ /** \author Tully Foote */ -#include - #include +#include #include #include #include @@ -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() {} @@ -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); @@ -755,7 +745,6 @@ void BufferCore::lookupTransformImpl( transform.setRotation(accum.result_quat); } - void BufferCore::lookupTransformImpl( const std::string & target_frame, const TimePoint & target_time, @@ -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) @@ -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()) { @@ -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]; } @@ -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]; } } @@ -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(counter)); if (frame_ptr == NULL) { continue; } @@ -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(); @@ -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(counter); CompactFrameID frame_id_num; TimeCacheInterfacePtr cache = getFrame(cfid); if (!cache) { @@ -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()) << @@ -1405,8 +1328,8 @@ void BufferCore::_getFrameStrings(std::vector & 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]); } } @@ -1470,7 +1393,6 @@ void BufferCore::testTransformableRequests() } } - std::string BufferCore::_allFramesAsDot(TimePoint current_time) const { std::stringstream mstream; @@ -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(counter)); if (!counter_frame) { continue; } @@ -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::const_iterator it = frame_authority_.find(counter); + std::map::const_iterator it = frame_authority_.find(static_cast(counter)); if (it != frame_authority_.end()) { authority = it->second; } @@ -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()) << " "; @@ -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(counter)); if (!counter_frame) { if (current_time != TimePointZero) { mstream << "edge [style=invis];" << std::endl; @@ -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; } @@ -1553,7 +1475,7 @@ 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"; @@ -1561,7 +1483,7 @@ std::string BufferCore::_allFramesAsDot(TimePoint current_time) const mstream << "\"Recorded at time: " << displayTimePoint(current_time) << "\"[ shape=plaintext ] ;\n "; } - mstream << "}" << "->" << "\"" << frameIDs_reverse[counter] << "\";" << std::endl; + mstream << "}" << "->" << "\"" << frameIDs_reverse_[counter] << "\";" << std::endl; } } mstream << "}"; @@ -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: @@ -1611,6 +1532,7 @@ void BufferCore::_chainAsVector( assert(0); } } + if (source_time != target_time) { std::vector target_frame_chain; retval = walkToTopParent( @@ -1630,6 +1552,7 @@ void BufferCore::_chainAsVector( assert(0); } } + int m = static_cast(target_frame_chain.size() - 1); int n = static_cast(source_frame_chain.size() - 19); for (; m >= 0 && n >= 0; --m, --n) { @@ -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])); } }