Skip to content
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

Add Tf2 versions of lookupTransform, lookupVelocity, setTransform #728

Open
wants to merge 5 commits into
base: rolling
Choose a base branch
from
Open

Add Tf2 versions of lookupTransform, lookupVelocity, setTransform #728

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
eee73eb
Add generic interface to tf2
kyle-basis Nov 11, 2024
62b005d
add *Tf2 versions of functions
kyle-basis Nov 12, 2024
13104aa
uncrustify
kyle-basis Nov 13, 2024
ba44b56
docs
kyle-basis Nov 13, 2024
78b086f
remove pointless virtual
kyle-basis Nov 13, 2024
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
65 changes: 63 additions & 2 deletions tf2/include/tf2/buffer_core.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -124,11 +124,28 @@ class BufferCore : public BufferCoreInterface

/*********** Accessors *************/

/**
* \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).
* \return The transform between the frames.
*
* Possible exceptions tf2::LookupException, tf2::ConnectivityException,
* tf2::ExtrapolationException, tf2::InvalidArgumentException
*/
TF2_PUBLIC
virtual tf2::Stamped<tf2::Transform>
lookupTransformTf2(
const std::string & target_frame,
const std::string & source_frame,
const tf2::TimePoint & time) const override;

/** \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)
* \return The transform between the frames
* \return The transform between the frames as a ROS type.
*
* Possible exceptions tf2::LookupException, tf2::ConnectivityException,
* tf2::ExtrapolationException, tf2::InvalidArgumentException
Expand All @@ -139,13 +156,35 @@ class BufferCore : public BufferCoreInterface
const std::string & target_frame, const std::string & source_frame,
const TimePoint & time) const override;

/**
* \brief Get the transform between two frames by frame ID assuming fixed frame.
* \param target_frame The frame to which data should be transformed.
* \param target_time The time to which the data should be transformed (0 will get the latest).
* \param source_frame The frame where the data originated.
* \param source_time The time at which the source_frame should be evaluated
* (0 will get the latest).
* \param fixed_frame The frame in which to assume the transform is constant in time.
* \return The transform between the frames.
*
* Possible exceptions tf2::LookupException, tf2::ConnectivityException,
* tf2::ExtrapolationException, tf2::InvalidArgumentException
*/
TF2_PUBLIC
virtual tf2::Stamped<tf2::Transform>
lookupTransformTf2(
const std::string & target_frame,
const tf2::TimePoint & target_time,
const std::string & source_frame,
const tf2::TimePoint & source_time,
const std::string & fixed_frame) const override;

/** \brief Get the transform between two frames by frame ID assuming fixed frame.
* \param target_frame The frame to which data should be transformed
* \param target_time The time to which the data should be transformed. (0 will get the latest)
* \param source_frame The frame where the data originated
* \param source_time The time at which the source_frame should be evaluated. (0 will get the latest)
* \param fixed_frame The frame in which to assume the transform is constant in time.
* \return The transform between the frames
* \return The transform between the frames as a ROS type.
*
* Possible exceptions tf2::LookupException, tf2::ConnectivityException,
* tf2::ExtrapolationException, tf2::InvalidArgumentException
Expand All @@ -158,11 +197,33 @@ class BufferCore : public BufferCoreInterface
const std::string & source_frame, const TimePoint & source_time,
const std::string & fixed_frame) const override;

TF2_PUBLIC
tf2::Stamped<std::pair<tf2::Vector3, tf2::Vector3>> lookupVelocityTf2(
const std::string & tracking_frame, const std::string & observation_frame,
const TimePoint & time, const tf2::Duration & averaging_interval) const;

TF2_PUBLIC
geometry_msgs::msg::VelocityStamped lookupVelocity(
const std::string & tracking_frame, const std::string & observation_frame,
const TimePoint & time, const tf2::Duration & averaging_interval) const;

/** \brief Lookup the velocity of the moving_frame in the reference_frame
* \param reference_frame The frame in which to track
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

update docs

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think I've fixed this

* \param moving_frame The frame to track
* \param time The time at which to get the velocity
* \param duration The period over which to average
* \param velocity The velocity output
*
* Possible exceptions TransformReference::LookupException, TransformReference::ConnectivityException,
* TransformReference::MaxDepthException
*/
TF2_PUBLIC
tf2::Stamped<std::pair<tf2::Vector3, tf2::Vector3>> lookupVelocityTf2(
const std::string & tracking_frame, const std::string & observation_frame,
const std::string & reference_frame, const tf2::Vector3 & reference_point,
const std::string & reference_point_frame,
const TimePoint & time, const tf2::Duration & duration) const;

/** \brief Lookup the velocity of the moving_frame in the reference_frame
* \param reference_frame The frame in which to track
* \param moving_frame The frame to track
Expand Down
34 changes: 34 additions & 0 deletions tf2/include/tf2/buffer_core_interface.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,7 @@
#include "geometry_msgs/msg/transform_stamped.hpp"

#include "tf2/time.h"
#include "tf2/transform_datatypes.h"
#include "tf2/visibility_control.h"

namespace tf2
Expand Down Expand Up @@ -67,6 +68,20 @@ class BufferCoreInterface
* \return The transform between the frames.
*/
TF2_PUBLIC
virtual tf2::Stamped<tf2::Transform>
lookupTransformTf2(
const std::string & target_frame,
const std::string & source_frame,
const tf2::TimePoint & time) const = 0;

/**
* \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).
* \return The transform between the frames as a ROS type.
*/
TF2_PUBLIC
virtual geometry_msgs::msg::TransformStamped
lookupTransform(
const std::string & target_frame,
Expand All @@ -84,6 +99,25 @@ class BufferCoreInterface
* \return The transform between the frames.
*/
TF2_PUBLIC
virtual tf2::Stamped<tf2::Transform>
lookupTransformTf2(
const std::string & target_frame,
const tf2::TimePoint & target_time,
const std::string & source_frame,
const tf2::TimePoint & source_time,
const std::string & fixed_frame) const = 0;

/**
* \brief Get the transform between two frames by frame ID assuming fixed frame.
* \param target_frame The frame to which data should be transformed.
* \param target_time The time to which the data should be transformed (0 will get the latest).
* \param source_frame The frame where the data originated.
* \param source_time The time at which the source_frame should be evaluated
* (0 will get the latest).
* \param fixed_frame The frame in which to assume the transform is constant in time.
* \return The transform between the frames as a ROS type.
*/
TF2_PUBLIC
virtual geometry_msgs::msg::TransformStamped
lookupTransform(
const std::string & target_frame,
Expand Down
160 changes: 103 additions & 57 deletions tf2/src/buffer_core.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -98,6 +98,29 @@ void fillOrWarnMessageForInvalidFrame(
}
}

/*
* remove when geometry_msgs dependency is broken, tf2_geometry_msgs provides a better implementation
*/
geometry_msgs::msg::TransformStamped toMsg(const tf2::Stamped<tf2::Transform> & stamped_transform)
{
geometry_msgs::msg::TransformStamped msg;
msg.transform.translation.x = stamped_transform.getOrigin().x();
msg.transform.translation.y = stamped_transform.getOrigin().y();
msg.transform.translation.z = stamped_transform.getOrigin().z();
msg.transform.rotation.x = stamped_transform.getRotation().x();
msg.transform.rotation.y = stamped_transform.getRotation().y();
msg.transform.rotation.z = stamped_transform.getRotation().z();
msg.transform.rotation.w = stamped_transform.getRotation().w();
std::chrono::nanoseconds ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
stamped_transform.stamp_.time_since_epoch());
std::chrono::seconds s = std::chrono::duration_cast<std::chrono::seconds>(
stamped_transform.stamp_.time_since_epoch());
msg.header.stamp.sec = static_cast<int32_t>(s.count());
msg.header.stamp.nanosec = static_cast<uint32_t>(ns.count() % 1000000000ull);
msg.header.frame_id = stamped_transform.frame_id_;
return msg;
}

} // anonymous namespace

CompactFrameID BufferCore::validateFrameId(
Expand Down Expand Up @@ -576,6 +599,18 @@ struct TransformAccum
tf2::Vector3 result_vec;
};

tf2::Stamped<std::pair<tf2::Vector3, tf2::Vector3>> BufferCore::lookupVelocityTf2(
const std::string & tracking_frame, const std::string & observation_frame,
const TimePoint & time, const tf2::Duration & averaging_interval) const
{
// ref point is origin of tracking_frame, ref_frame = obs_frame
return lookupVelocityTf2(
tracking_frame, observation_frame, observation_frame, tf2::Vector3(
0, 0,
0), tracking_frame, time,
averaging_interval);
}

geometry_msgs::msg::VelocityStamped BufferCore::lookupVelocity(
const std::string & tracking_frame, const std::string & observation_frame,
const TimePoint & time, const tf2::Duration & averaging_interval) const
Expand All @@ -588,7 +623,7 @@ geometry_msgs::msg::VelocityStamped BufferCore::lookupVelocity(
averaging_interval);
}

geometry_msgs::msg::VelocityStamped BufferCore::lookupVelocity(
tf2::Stamped<std::pair<tf2::Vector3, tf2::Vector3>> BufferCore::lookupVelocityTf2(
const std::string & tracking_frame, const std::string & observation_frame,
const std::string & reference_frame, const tf2::Vector3 & reference_point,
const std::string & reference_point_frame,
Expand Down Expand Up @@ -673,80 +708,91 @@ geometry_msgs::msg::VelocityStamped BufferCore::lookupVelocity(
tf2::Vector3 delta = rp_desired - rp_orig;
out_vel = out_vel + out_rot * delta;

std::chrono::nanoseconds ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
tf2::timeFromSec(start_time + averaging_interval_seconds * 0.5).time_since_epoch());
std::chrono::seconds s = std::chrono::duration_cast<std::chrono::seconds>(
tf2::timeFromSec(start_time + averaging_interval_seconds * 0.5).time_since_epoch());
geometry_msgs::msg::VelocityStamped velocity;
velocity.header.stamp.sec = static_cast<int32_t>(s.count());
velocity.header.stamp.nanosec = static_cast<uint32_t>(ns.count() % 1000000000ull);
velocity.header.frame_id = reference_frame;
velocity.body_frame_id = tracking_frame;

velocity.velocity.linear.x = out_vel.x();
velocity.velocity.linear.y = out_vel.y();
velocity.velocity.linear.z = out_vel.z();
velocity.velocity.angular.x = out_rot.x();
velocity.velocity.angular.y = out_rot.y();
velocity.velocity.angular.z = out_rot.z();

return velocity;
const tf2::TimePoint out_time = tf2::timeFromSec(start_time + averaging_interval_seconds * 0.5);

return tf2::Stamped<std::pair<tf2::Vector3, tf2::Vector3>>({out_vel, out_rot}, out_time, reference_frame);
}

geometry_msgs::msg::VelocityStamped BufferCore::lookupVelocity(
const std::string & tracking_frame, const std::string & observation_frame,
const std::string & reference_frame, const tf2::Vector3 & reference_point,
const std::string & reference_point_frame,
const TimePoint & time, const tf2::Duration & averaging_interval) const
{
const tf2::Stamped<std::pair<tf2::Vector3, tf2::Vector3>> stamped_velocity = lookupVelocityTf2(tracking_frame, observation_frame, reference_frame,
reference_point, reference_point_frame, time, averaging_interval);

geometry_msgs::msg::VelocityStamped msg;
const std::chrono::nanoseconds ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
stamped_velocity.stamp_.time_since_epoch());
const std::chrono::seconds s = std::chrono::duration_cast<std::chrono::seconds>(
stamped_velocity.stamp_.time_since_epoch());
msg.header.stamp.sec = static_cast<int32_t>(s.count());
msg.header.stamp.nanosec = static_cast<uint32_t>(ns.count() % 1000000000ull);
msg.header.frame_id = reference_frame;
msg.body_frame_id = tracking_frame;

msg.velocity.linear.x = stamped_velocity.first.x();
msg.velocity.linear.y = stamped_velocity.first.y();
msg.velocity.linear.z = stamped_velocity.first.z();
msg.velocity.angular.x = stamped_velocity.second.x();
msg.velocity.angular.y = stamped_velocity.second.y();
msg.velocity.angular.z = stamped_velocity.second.z();

return msg;
}

tf2::Stamped<tf2::Transform>
BufferCore::lookupTransformTf2(
const std::string & target_frame, const std::string & source_frame,
const TimePoint & time) const
{
tf2::Stamped<tf2::Transform> stamped_transform;
lookupTransformImpl(target_frame, source_frame, time, stamped_transform, stamped_transform.stamp_);
stamped_transform.frame_id_ = target_frame;
return stamped_transform;
}


geometry_msgs::msg::TransformStamped
BufferCore::lookupTransform(
const std::string & target_frame, const std::string & source_frame,
const TimePoint & time) const
{
tf2::Transform transform;
TimePoint time_out;
lookupTransformImpl(target_frame, source_frame, time, transform, time_out);
geometry_msgs::msg::TransformStamped msg;
msg.transform.translation.x = transform.getOrigin().x();
msg.transform.translation.y = transform.getOrigin().y();
msg.transform.translation.z = transform.getOrigin().z();
msg.transform.rotation.x = transform.getRotation().x();
msg.transform.rotation.y = transform.getRotation().y();
msg.transform.rotation.z = transform.getRotation().z();
msg.transform.rotation.w = transform.getRotation().w();
std::chrono::nanoseconds ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
time_out.time_since_epoch());
std::chrono::seconds s = std::chrono::duration_cast<std::chrono::seconds>(
time_out.time_since_epoch());
msg.header.stamp.sec = static_cast<int32_t>(s.count());
msg.header.stamp.nanosec = static_cast<uint32_t>(ns.count() % 1000000000ull);
msg.header.frame_id = target_frame;
const tf2::Stamped<tf2::Transform> stamped_transform = lookupTransformTf2(target_frame, source_frame, time);

geometry_msgs::msg::TransformStamped msg = toMsg(stamped_transform);
msg.child_frame_id = source_frame;

return msg;
}

geometry_msgs::msg::TransformStamped
BufferCore::lookupTransform(

tf2::Stamped<tf2::Transform>
BufferCore::lookupTransformTf2(
const std::string & target_frame, const TimePoint & target_time,
const std::string & source_frame, const TimePoint & source_time,
const std::string & fixed_frame) const
{
tf2::Transform transform;
TimePoint time_out;
tf2::Stamped<tf2::Transform> stamped_transform;

lookupTransformImpl(
target_frame, target_time, source_frame, source_time,
fixed_frame, transform, time_out);
geometry_msgs::msg::TransformStamped msg;
msg.transform.translation.x = transform.getOrigin().x();
msg.transform.translation.y = transform.getOrigin().y();
msg.transform.translation.z = transform.getOrigin().z();
msg.transform.rotation.x = transform.getRotation().x();
msg.transform.rotation.y = transform.getRotation().y();
msg.transform.rotation.z = transform.getRotation().z();
msg.transform.rotation.w = transform.getRotation().w();
std::chrono::nanoseconds ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
time_out.time_since_epoch());
std::chrono::seconds s = std::chrono::duration_cast<std::chrono::seconds>(
time_out.time_since_epoch());
msg.header.stamp.sec = static_cast<int32_t>(s.count());
msg.header.stamp.nanosec = static_cast<uint32_t>(ns.count() % 1000000000ull);
msg.header.frame_id = target_frame;
fixed_frame, stamped_transform, stamped_transform.stamp_);

return stamped_transform;
}


geometry_msgs::msg::TransformStamped
BufferCore::lookupTransform(
const std::string & target_frame, const TimePoint & target_time,
const std::string & source_frame, const TimePoint & source_time,
const std::string & fixed_frame) const
{
const tf2::Stamped<tf2::Transform> stamped_transform = lookupTransformTf2(target_frame, target_time, source_frame, source_time, fixed_frame);

geometry_msgs::msg::TransformStamped msg = toMsg(stamped_transform);
msg.child_frame_id = source_frame;

return msg;
Expand Down