Skip to content

Commit

Permalink
Add doTransform support for Point32, Polygon and PolygonStamped (back…
Browse files Browse the repository at this point in the history 
…port #616) (#619)

* Add doTransform support for Point32, Polygon and PolygonStamped (#616)

Co-authored-by: Guillaume Doisy <[email protected]>
(cherry picked from commit 70d8c95)
Co-authored-by: Alejandro Hernández Cordero <[email protected]>
  • Loading branch information
@mergify @ahcorde
3 people authored Aug 21, 2023
1 parent 6681aa3 commit 8b5440a
Show file tree
Hide file tree
Showing 2 changed files with 230 additions and 0 deletions.
169 changes: 169 additions & 0 deletions tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,9 +43,11 @@
#include "geometry_msgs/msg/vector3_stamped.hpp"
#include "geometry_msgs/msg/pose.hpp"
#include "geometry_msgs/msg/point.hpp"
#include "geometry_msgs/msg/point32.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/pose_with_covariance.hpp"
#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
#include "geometry_msgs/msg/polygon_stamped.hpp"
#include "geometry_msgs/msg/wrench.hpp"
#include "geometry_msgs/msg/wrench_stamped.hpp"
#include "kdl/frames.hpp"
Expand Down Expand Up @@ -243,6 +245,54 @@ void fromMsg(const geometry_msgs::msg::Point & in, tf2::Vector3 & out)
out = tf2::Vector3(in.x, in.y, in.z);
}

/*************/
/** Point32 **/
/*************/

/** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Point32 type.
* This function is a specialization of the doTransform template defined in tf2/convert.h.
* \param t_in The point to transform, as a Point32 message.
* \param t_out The transformed point, as a Point32 message.
* \param transform The timestamped transform to apply, as a TransformStamped message.
*/
template<>
inline
void doTransform(
const geometry_msgs::msg::Point32 & t_in,
geometry_msgs::msg::Point32 & t_out,
const geometry_msgs::msg::TransformStamped & transform)
{
KDL::Vector v_out = gmTransformToKDL(transform) * KDL::Vector(t_in.x, t_in.y, t_in.z);
t_out.x = static_cast<float>(v_out[0]);
t_out.y = static_cast<float>(v_out[1]);
t_out.z = static_cast<float>(v_out[2]);
}

/** \brief Convert a tf2 Vector3 type to its equivalent geometry_msgs representation.
* This function is a specialization of the toMsg template defined in tf2/convert.h.
* \param in A tf2 Vector3 object.
* \return The Vector3 converted to a geometry_msgs message type.
*/
inline
geometry_msgs::msg::Point32 & toMsg(const tf2::Vector3 & in, geometry_msgs::msg::Point32 & out)
{
out.x = static_cast<float>(in.getX());
out.y = static_cast<float>(in.getY());
out.z = static_cast<float>(in.getZ());
return out;
}

/** \brief Convert a Vector3 message to its equivalent tf2 representation.
* This function is a specialization of the fromMsg template defined in tf2/convert.h.
* \param in A Vector3 message type.
* \param out The Vector3 converted to a tf2 type.
*/
inline
void fromMsg(const geometry_msgs::msg::Point32 & in, tf2::Vector3 & out)
{
out = tf2::Vector3(in.x, in.y, in.z);
}

/******************/
/** PointStamped **/
/******************/
Expand Down Expand Up @@ -395,6 +445,125 @@ void fromMsg(const geometry_msgs::msg::PoseStamped & msg, geometry_msgs::msg::Po
out = msg;
}

/*************/
/** Polygon **/
/*************/

/** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs PolygonStamped type.
* This function is a specialization of the doTransform template defined in tf2/convert.h.
* \param poly_in The Polygon to transform.
* \param poly_out The transformed Polygon.
* \param transform The timestamped transform to apply, as a TransformStamped message.
*/
template<>
inline
void doTransform(
const geometry_msgs::msg::Polygon & poly_in,
geometry_msgs::msg::Polygon & poly_out,
const geometry_msgs::msg::TransformStamped & transform)
{
poly_out.points.clear();
for (auto & point : poly_in.points) {
geometry_msgs::msg::Point32 point_transformed;
doTransform(point, point_transformed, transform);
poly_out.points.push_back(point_transformed);
}
}

/** \brief Trivial "conversion" function for Polygon message type.
* This function is a specialization of the toMsg template defined in tf2/convert.h.
* \param in A Polygon message.
* \return The input argument.
*/
inline
geometry_msgs::msg::Polygon toMsg(const geometry_msgs::msg::Polygon & in)
{
return in;
}

/** \brief Trivial "conversion" function for Polygon message type.
* This function is a specialization of the toMsg template defined in tf2/convert.h.
* \param msg A Polygon message.
* \param out The input argument.
*/
inline
void fromMsg(
const geometry_msgs::msg::Polygon & msg,
geometry_msgs::msg::Polygon & out)
{
out = msg;
}

/********************/
/** PolygonStamped **/
/********************/

/** \brief Extract a timestamp from the header of a PolygonStamped message.
* This function is a specialization of the getTimestamp template defined in tf2/convert.h.
* \param t PolygonStamped message to extract the timestamp from.
* \return The timestamp of the message.
*/
template<>
inline
tf2::TimePoint getTimestamp(const geometry_msgs::msg::PolygonStamped & t)
{
return tf2_ros::fromMsg(t.header.stamp);
}

/** \brief Extract a frame ID from the header of a PolygonStamped message.
* This function is a specialization of the getFrameId template defined in tf2/convert.h.
* \param t PoseStamped message to extract the frame ID from.
* \return A string containing the frame ID of the message.
*/
template<>
inline
std::string getFrameId(const geometry_msgs::msg::PolygonStamped & t)
{
return t.header.frame_id;
}

/** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs PolygonStamped type.
* This function is a specialization of the doTransform template defined in tf2/convert.h.
* \param poly_in The PolygonStamped to transform.
* \param poly_out The transformed PolygonStamped.
* \param transform The timestamped transform to apply, as a TransformStamped message.
*/
template<>
inline
void doTransform(
const geometry_msgs::msg::PolygonStamped & poly_in,
geometry_msgs::msg::PolygonStamped & poly_out,
const geometry_msgs::msg::TransformStamped & transform)
{
doTransform(poly_in.polygon, poly_out.polygon, transform);

poly_out.header.stamp = transform.header.stamp;
poly_out.header.frame_id = transform.header.frame_id;
}

/** \brief Trivial "conversion" function for Polygon message type.
* This function is a specialization of the toMsg template defined in tf2/convert.h.
* \param in A PoseStamped message.
* \return The input argument.
*/
inline
geometry_msgs::msg::PolygonStamped toMsg(const geometry_msgs::msg::PolygonStamped & in)
{
return in;
}

/** \brief Trivial "conversion" function for Polygon message type.
* This function is a specialization of the toMsg template defined in tf2/convert.h.
* \param msg A PoseStamped message.
* \param out The input argument.
*/
inline
void fromMsg(
const geometry_msgs::msg::PolygonStamped & msg,
geometry_msgs::msg::PolygonStamped & out)
{
out = msg;
}

/************************/
/** PoseWithCovariance **/
Expand Down
61 changes: 61 additions & 0 deletions tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -397,6 +397,21 @@ TEST(TfGeometry, Point)
EXPECT_NEAR(res.z, 27, EPS);
}

// non-stamped 32
{
geometry_msgs::msg::Point32 v1, res;
v1.x = 1;
v1.y = 2;
v1.z = 3;

geometry_msgs::msg::TransformStamped t = generate_stamped_transform();

tf2::doTransform(v1, res, t);
EXPECT_NEAR(res.x, 11, EPS);
EXPECT_NEAR(res.y, 18, EPS);
EXPECT_NEAR(res.z, 27, EPS);
}

// stamped
{
geometry_msgs::msg::PointStamped v1, res;
Expand All @@ -422,6 +437,52 @@ TEST(TfGeometry, Point)
}
}

TEST(TfGeometry, Polygon)
{
// non-stamped
{
geometry_msgs::msg::Polygon v1, res;
geometry_msgs::msg::Point32 p;
p.x = 1;
p.y = 2;
p.z = 3;
v1.points.push_back(p);

geometry_msgs::msg::TransformStamped t = generate_stamped_transform();

tf2::doTransform(v1, res, t);
EXPECT_NEAR(res.points[0].x, 11, EPS);
EXPECT_NEAR(res.points[0].y, 18, EPS);
EXPECT_NEAR(res.points[0].z, 27, EPS);
}

// stamped
{
geometry_msgs::msg::PolygonStamped v1, res;
geometry_msgs::msg::Point32 p;
p.x = 1;
p.y = 2;
p.z = 3;
v1.polygon.points.push_back(p);
v1.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(2));
v1.header.frame_id = "A";

// simple api
geometry_msgs::msg::PolygonStamped v_simple =
tf_buffer->transform(v1, "B", tf2::durationFromSec(2.0));
EXPECT_NEAR(v_simple.polygon.points[0].x, -9, EPS);
EXPECT_NEAR(v_simple.polygon.points[0].y, 18, EPS);
EXPECT_NEAR(v_simple.polygon.points[0].z, 27, EPS);

// advanced api
geometry_msgs::msg::PolygonStamped v_advanced =
tf_buffer->transform(v1, "B", tf2::timeFromSec(2.0), "A", tf2::durationFromSec(3.0));
EXPECT_NEAR(v_simple.polygon.points[0].x, -9, EPS);
EXPECT_NEAR(v_simple.polygon.points[0].y, 18, EPS);
EXPECT_NEAR(v_simple.polygon.points[0].z, 27, EPS);
}
}

TEST(TfGeometry, Quaternion)
{
// rotated by -90° around y
Expand Down

0 comments on commit 8b5440a

Please sign in to comment.