From 369dc840837c5ed1e990f63efa2738976cf331ee Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 30 Nov 2022 10:44:08 -0800 Subject: [PATCH] Port tf2_2d to ROS 2 (#5) * Build sytem changes for ROS 2 Signed-off-by: Shane Loretz * Code changes for ROS 2 Signed-off-by: Shane Loretz * Don't need int main(); because linking to GTEST_MAIN_LIBRARIES Signed-off-by: Shane Loretz * Skip ament_cmake_copyright Signed-off-by: Shane Loretz * Move .h headers to .hpp Signed-off-by: Shane Loretz * Add .h headers with warning for backwards compatability Signed-off-by: Shane Loretz * Linter fixes: Satisfy Uncrustify Signed-off-by: Shane Loretz * Linter fixes: Satisfy cpplint Signed-off-by: Shane Loretz * Minimum CMake 3.14.4 Signed-off-by: Shane Loretz * ${tf2_geometry_msgs_TARGETS} -> tf2_geometry_msgs::tf2_geometry_msgs Signed-off-by: Shane Loretz * Bump copywrite date on redirection headers Signed-off-by: Shane Loretz * Remove Scalar.h include Signed-off-by: Shane Loretz Signed-off-by: Shane Loretz --- CMakeLists.txt | 109 +++--- include/tf2_2d/rotation.h | 247 +------------ include/tf2_2d/rotation.hpp | 277 ++++++++++++++ .../{rotation_impl.h => rotation_impl.hpp} | 81 ++--- include/tf2_2d/tf2_2d.h | 287 +-------------- include/tf2_2d/tf2_2d.hpp | 341 +++++++++++++++++ include/tf2_2d/transform.h | 199 +--------- include/tf2_2d/transform.hpp | 229 ++++++++++++ .../{transform_impl.h => transform_impl.hpp} | 72 ++-- include/tf2_2d/vector2.h | 313 +--------------- include/tf2_2d/vector2.hpp | 343 ++++++++++++++++++ .../{vector2_impl.h => vector2_impl.hpp} | 64 ++-- package.xml | 16 +- test/test_conversions.cpp | 216 ++++++----- test/test_rotation.cpp | 12 +- test/test_transform.cpp | 17 +- test/test_vector2.cpp | 10 +- 17 files changed, 1513 insertions(+), 1320 deletions(-) create mode 100644 include/tf2_2d/rotation.hpp rename include/tf2_2d/{rotation_impl.h => rotation_impl.hpp} (70%) create mode 100644 include/tf2_2d/tf2_2d.hpp create mode 100644 include/tf2_2d/transform.hpp rename include/tf2_2d/{transform_impl.h => transform_impl.hpp} (64%) create mode 100644 include/tf2_2d/vector2.hpp rename include/tf2_2d/{vector2_impl.h => vector2_impl.hpp} (72%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 92660c6..fc272d8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,103 +1,96 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.14.4) project(tf2_2d) -add_compile_options(-std=c++14) -add_compile_options(-Wall) -add_compile_options(-Werror) - -find_package(catkin REQUIRED COMPONENTS - roscpp - tf2 - tf2_geometry_msgs -) +find_package(ament_cmake REQUIRED) +find_package(Boost REQUIRED) find_package(Eigen3 REQUIRED) - -catkin_package( - INCLUDE_DIRS - include - CATKIN_DEPENDS - roscpp - tf2 - tf2_geometry_msgs - DEPENDS - EIGEN3 -) +find_package(rclcpp REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) ########### ## Build ## ########### -# Header-only package. Nothing to build. +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Werror) +endif() + +add_library(tf2_2d INTERFACE) +target_compile_features(tf2_2d INTERFACE cxx_std_14) +target_include_directories(tf2_2d INTERFACE + "$" + "$") +target_link_libraries(tf2_2d INTERFACE + Boost::boost + Eigen3::Eigen + rclcpp::rclcpp + tf2::tf2 + tf2_ros::tf2_ros + tf2_geometry_msgs::tf2_geometry_msgs) ############# ## Install ## ############# -# Mark cpp header files for installation -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# Install header files +install(DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install( + TARGETS tf2_2d EXPORT tf2_2d-export + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) +# Export modern CMake target +ament_export_targets(tf2_2d-export) + ############# ## Testing ## ############# -if(CATKIN_ENABLE_TESTING) +if(BUILD_TESTING) + # Exclude copyright check because it requires a CONTRIBUTING.md file + list(APPEND AMENT_LINT_AUTO_EXCLUDE "ament_cmake_copyright") - include_directories( - include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} - ) - - find_package(roslint REQUIRED) - - # Lint tests - set(ROSLINT_CPP_OPTS "--filter=-build/c++11,-runtime/references") - roslint_cpp() - roslint_add_test() + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() # Conversion Tests - catkin_add_gtest(test_conversions + ament_add_gtest(test_conversions test/test_conversions.cpp ) - add_dependencies(test_conversions - ${catkin_EXPORTED_TARGETS} - ) target_link_libraries(test_conversions - ${catkin_LIBRARIES} + tf2_2d ) # Rotation Tests - catkin_add_gtest(test_rotation + ament_add_gtest(test_rotation test/test_rotation.cpp ) - add_dependencies(test_rotation - ${catkin_EXPORTED_TARGETS} - ) target_link_libraries(test_rotation - ${catkin_LIBRARIES} + tf2_2d ) # Transform Tests - catkin_add_gtest(test_transform + ament_add_gtest(test_transform test/test_transform.cpp ) - add_dependencies(test_transform - ${catkin_EXPORTED_TARGETS} - ) target_link_libraries(test_transform - ${catkin_LIBRARIES} + tf2_2d ) # Vector2 Tests - catkin_add_gtest(test_vector2 + ament_add_gtest(test_vector2 test/test_vector2.cpp ) - add_dependencies(test_vector2 - ${catkin_EXPORTED_TARGETS} - ) target_link_libraries(test_vector2 - ${catkin_LIBRARIES} + tf2_2d ) endif() + +ament_package() diff --git a/include/tf2_2d/rotation.h b/include/tf2_2d/rotation.h index b9c8a3f..2195c8c 100644 --- a/include/tf2_2d/rotation.h +++ b/include/tf2_2d/rotation.h @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2019, Locus Robotics + * Copyright (c) 2022, Locus Robotics * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -31,247 +31,12 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef TF2_2D_ROTATION_H -#define TF2_2D_ROTATION_H -#include -#include +#ifndef TF2_2D__ROTATION_H_ +#define TF2_2D__ROTATION_H_ -#include +#warning This header is obsolete, please include tf2_2d/rotation.hpp instead +#include -namespace tf2_2d -{ - -/** - * @brief A class representing a planar rotation - * - * The rotation angle is always within the range [-PI, PI]. This class mimics the tf2::Matrix3x3/tf2::Quaternion - * classes to the extent possible. - */ -class Rotation -{ -public: - /** - * @brief No initialization constructor - */ - Rotation(); - - /** - * @brief Constructor from scalars - * - * @param angle The rotation angle in radians - */ - explicit Rotation(const tf2Scalar angle); - - /** - * @brief Return the reverse rotation - */ - Rotation operator-() const; - - /** - * @brief Add a rotation to this one - * - * @param rhs The rotation to add to this one - */ - Rotation& operator+=(const Rotation& rhs); - - /** - * @brief Subtract a rotation from this one - * - * @param rhs The rotation to subtract - */ - Rotation& operator-=(const Rotation& rhs); - - /** - * @brief Scale the rotation - * - * @param rhs Scale factor - */ - Rotation& operator*=(const tf2Scalar rhs); - - /** - * @brief Inversely scale the rotation - * - * @param rhs Scale factor to divide by - */ - Rotation& operator/=(const tf2Scalar rhs); - - /** - * @brief Check if two rotations are equal - */ - bool operator==(const Rotation& other) const; - - /** - * @brief Check if two vectors are not equal - */ - bool operator!=(const Rotation& other) const; - - /** - * @brief Return the distance squared between this and another rotation - */ - tf2Scalar distance2(const Rotation& other) const; - - /** - * @brief Return the distance between this and another rotation - */ - tf2Scalar distance(const Rotation& other) const; - - /** - * @brief Rotate a vector by this rotation - * - * @param vec The vector to rotate - */ - Vector2 rotate(const Vector2& vec) const; - - /** - * @brief Rotate a vector by the inverse of this rotation - * - * @param vec The vector to rotate - */ - Vector2 unrotate(const Vector2& vec) const; - - /** - * @brief Return the inverse of this rotation - */ - Rotation inverse() const; - - /** - * @brief Return a rotation with the absolute values of each element - */ - Rotation absolute() const; - - /** - * @brief Return the linear interpolation between this and another rotation - * - * The interpolation always uses the shortest path between this and other - * - * @param other The other rotation - * @param ratio The ratio of this to other (ratio=0 => return this, ratio=1 => return other) - */ - Rotation lerp(const Rotation& other, const tf2Scalar ratio) const; - - /** - * @brief Return the angle value in radians - */ - const tf2Scalar& getAngle() const; - const tf2Scalar& angle() const { return getAngle(); } - - /** - * @brief Set the angle value in radians - */ - void setAngle(const tf2Scalar angle); - - /** - * @brief Set the angle to the max of this and another rotation - * - * @param other The other Rotation to compare with - */ - void setMax(const Rotation& other); - - /** - * @brief Set the angle to the min of this and another rotation - * - * @param other The other Rotation to compare with - */ - void setMin(const Rotation& other); - - /** - * @brief Set the rotation to the provided angle in radians - * - * @param angle The angle value in radians - */ - void setValue(const tf2Scalar angle); - - /** - * Set the rotation to zero - */ - void setZero(); - - /** - * Check if all the elements of this vector are zero - */ - bool isZero() const; - - /** - * Check if all the elements of this vector close to zero - */ - bool fuzzyZero() const; - - /** - * @brief Get a 2x2 rotation matrix - */ - Eigen::Matrix2d getRotationMatrix() const; - - /** - * @brief Get a 3x3 homogeneous transformation matrix with just the rotation portion populated - */ - Eigen::Matrix3d getHomogeneousMatrix() const; - -private: - tf2Scalar angle_; //!< Storage for the angle in radians - mutable tf2Scalar cos_angle_; //!< Storage for cos(angle) so we only compute it once - mutable tf2Scalar sin_angle_; //!< Storage for sin(angle) so we only compute it once - - /** - * @brief Private constructor that allows populating the trig cache in special cases when it is known at construction - * - * @param[IN] angle The rotation angle in radians - * @param[IN] cos_angle The cos(angle) value; this is assumed to be correct - * @param[IN] sin_angle The sin(angle) value; this is assumed to be correct - */ - Rotation(const tf2Scalar angle, tf2Scalar cos_angle, tf2Scalar sin_angle); - - /** - * @brief Wrap the rotation to the range [-Pi, Pi) - */ - Rotation& wrap(); - - /** - * @brief Populate the cos/sin cache if it is not already populated. - * - * This only modifies mutable variables, so it is logically const. - */ - void populateTrigCache() const; -}; - -/** - * @brief Add two rotations - */ -Rotation operator+(Rotation lhs, const Rotation& rhs); - -/** - * @brief Subtract two rotations - */ -Rotation operator-(Rotation lhs, const Rotation& rhs); - -/** - * @brief Scale a rotation - */ -Rotation operator*(Rotation lhs, const tf2Scalar rhs); - -/** - * @brief Scale a rotation - */ -Rotation operator*(const tf2Scalar lhs, Rotation rhs); - -/** - * @brief Inversely scale a rotation - */ -Rotation operator/(Rotation lhs, const tf2Scalar rhs); - -/** - * @brief Rotate a vector by this rotation - */ -Vector2 operator*(const Rotation& lhs, const Vector2& rhs); - -/** - * @brief Stream the rotation in human-readable format - */ -std::ostream& operator<<(std::ostream& stream, const Rotation& rotation); - -} // namespace tf2_2d - -#include - -#endif // TF2_2D_ROTATION_H +#endif // TF2_2D__ROTATION_H_ diff --git a/include/tf2_2d/rotation.hpp b/include/tf2_2d/rotation.hpp new file mode 100644 index 0000000..b63b8d8 --- /dev/null +++ b/include/tf2_2d/rotation.hpp @@ -0,0 +1,277 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef TF2_2D__ROTATION_HPP_ +#define TF2_2D__ROTATION_HPP_ + +#include +#include + +#include + + +namespace tf2_2d +{ + +/** + * @brief A class representing a planar rotation + * + * The rotation angle is always within the range [-PI, PI]. This class mimics the tf2::Matrix3x3/tf2::Quaternion + * classes to the extent possible. + */ +class Rotation +{ +public: + /** + * @brief No initialization constructor + */ + Rotation(); + + /** + * @brief Constructor from scalars + * + * @param angle The rotation angle in radians + */ + explicit Rotation(const tf2Scalar angle); + + /** + * @brief Return the reverse rotation + */ + Rotation operator-() const; + + /** + * @brief Add a rotation to this one + * + * @param rhs The rotation to add to this one + */ + Rotation & operator+=(const Rotation & rhs); + + /** + * @brief Subtract a rotation from this one + * + * @param rhs The rotation to subtract + */ + Rotation & operator-=(const Rotation & rhs); + + /** + * @brief Scale the rotation + * + * @param rhs Scale factor + */ + Rotation & operator*=(const tf2Scalar rhs); + + /** + * @brief Inversely scale the rotation + * + * @param rhs Scale factor to divide by + */ + Rotation & operator/=(const tf2Scalar rhs); + + /** + * @brief Check if two rotations are equal + */ + bool operator==(const Rotation & other) const; + + /** + * @brief Check if two vectors are not equal + */ + bool operator!=(const Rotation & other) const; + + /** + * @brief Return the distance squared between this and another rotation + */ + tf2Scalar distance2(const Rotation & other) const; + + /** + * @brief Return the distance between this and another rotation + */ + tf2Scalar distance(const Rotation & other) const; + + /** + * @brief Rotate a vector by this rotation + * + * @param vec The vector to rotate + */ + Vector2 rotate(const Vector2 & vec) const; + + /** + * @brief Rotate a vector by the inverse of this rotation + * + * @param vec The vector to rotate + */ + Vector2 unrotate(const Vector2 & vec) const; + + /** + * @brief Return the inverse of this rotation + */ + Rotation inverse() const; + + /** + * @brief Return a rotation with the absolute values of each element + */ + Rotation absolute() const; + + /** + * @brief Return the linear interpolation between this and another rotation + * + * The interpolation always uses the shortest path between this and other + * + * @param other The other rotation + * @param ratio The ratio of this to other (ratio=0 => return this, ratio=1 => return other) + */ + Rotation lerp(const Rotation & other, const tf2Scalar ratio) const; + + /** + * @brief Return the angle value in radians + */ + const tf2Scalar & getAngle() const; + const tf2Scalar & angle() const {return getAngle();} + + /** + * @brief Set the angle value in radians + */ + void setAngle(const tf2Scalar angle); + + /** + * @brief Set the angle to the max of this and another rotation + * + * @param other The other Rotation to compare with + */ + void setMax(const Rotation & other); + + /** + * @brief Set the angle to the min of this and another rotation + * + * @param other The other Rotation to compare with + */ + void setMin(const Rotation & other); + + /** + * @brief Set the rotation to the provided angle in radians + * + * @param angle The angle value in radians + */ + void setValue(const tf2Scalar angle); + + /** + * Set the rotation to zero + */ + void setZero(); + + /** + * Check if all the elements of this vector are zero + */ + bool isZero() const; + + /** + * Check if all the elements of this vector close to zero + */ + bool fuzzyZero() const; + + /** + * @brief Get a 2x2 rotation matrix + */ + Eigen::Matrix2d getRotationMatrix() const; + + /** + * @brief Get a 3x3 homogeneous transformation matrix with just the rotation portion populated + */ + Eigen::Matrix3d getHomogeneousMatrix() const; + +private: + tf2Scalar angle_; //!< Storage for the angle in radians + mutable tf2Scalar cos_angle_; //!< Storage for cos(angle) so we only compute it once + mutable tf2Scalar sin_angle_; //!< Storage for sin(angle) so we only compute it once + + /** + * @brief Private constructor that allows populating the trig cache in special cases when it is known at construction + * + * @param[IN] angle The rotation angle in radians + * @param[IN] cos_angle The cos(angle) value; this is assumed to be correct + * @param[IN] sin_angle The sin(angle) value; this is assumed to be correct + */ + Rotation(const tf2Scalar angle, tf2Scalar cos_angle, tf2Scalar sin_angle); + + /** + * @brief Wrap the rotation to the range [-Pi, Pi) + */ + Rotation & wrap(); + + /** + * @brief Populate the cos/sin cache if it is not already populated. + * + * This only modifies mutable variables, so it is logically const. + */ + void populateTrigCache() const; +}; + +/** + * @brief Add two rotations + */ +Rotation operator+(Rotation lhs, const Rotation & rhs); + +/** + * @brief Subtract two rotations + */ +Rotation operator-(Rotation lhs, const Rotation & rhs); + +/** + * @brief Scale a rotation + */ +Rotation operator*(Rotation lhs, const tf2Scalar rhs); + +/** + * @brief Scale a rotation + */ +Rotation operator*(const tf2Scalar lhs, Rotation rhs); + +/** + * @brief Inversely scale a rotation + */ +Rotation operator/(Rotation lhs, const tf2Scalar rhs); + +/** + * @brief Rotate a vector by this rotation + */ +Vector2 operator*(const Rotation & lhs, const Vector2 & rhs); + +/** + * @brief Stream the rotation in human-readable format + */ +std::ostream & operator<<(std::ostream & stream, const Rotation & rotation); + +} // namespace tf2_2d + +#include + +#endif // TF2_2D__ROTATION_HPP_ diff --git a/include/tf2_2d/rotation_impl.h b/include/tf2_2d/rotation_impl.hpp similarity index 70% rename from include/tf2_2d/rotation_impl.h rename to include/tf2_2d/rotation_impl.hpp index 62d0b67..e8b7096 100644 --- a/include/tf2_2d/rotation_impl.h +++ b/include/tf2_2d/rotation_impl.hpp @@ -31,17 +31,16 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef TF2_2D_ROTATION_IMPL_H -#define TF2_2D_ROTATION_IMPL_H - -#include // NOLINT: The tf2 MinMax.h file does not include all requirements. Consequently, -#include // NOLINT: the order of the headers here is important. -#include +#ifndef TF2_2D__ROTATION_IMPL_HPP_ +#define TF2_2D__ROTATION_IMPL_HPP_ #include +#include + #include +#include namespace tf2_2d { @@ -56,8 +55,8 @@ inline Rotation::Rotation(const tf2Scalar angle) setValue(angle); } -inline Rotation::Rotation(const tf2Scalar angle, tf2Scalar cos_angle, tf2Scalar sin_angle) : - angle_(angle), +inline Rotation::Rotation(const tf2Scalar angle, tf2Scalar cos_angle, tf2Scalar sin_angle) +: angle_(angle), cos_angle_(cos_angle), sin_angle_(sin_angle) { @@ -68,61 +67,63 @@ inline Rotation Rotation::operator-() const return inverse(); } -inline Rotation& Rotation::operator+=(const Rotation& rhs) +inline Rotation & Rotation::operator+=(const Rotation & rhs) { setValue(angle_ + rhs.angle_); return *this; } -inline Rotation& Rotation::operator-=(const Rotation& rhs) +inline Rotation & Rotation::operator-=(const Rotation & rhs) { setValue(angle_ - rhs.angle_); return *this; } -inline Rotation& Rotation::operator*=(const tf2Scalar rhs) +inline Rotation & Rotation::operator*=(const tf2Scalar rhs) { setValue(angle_ * rhs); return *this; } -inline Rotation& Rotation::operator/=(const tf2Scalar rhs) +inline Rotation & Rotation::operator/=(const tf2Scalar rhs) { tf2FullAssert(rhs != tf2Scalar(0.0)); return *this *= tf2Scalar(1.0) / rhs; } -inline bool Rotation::operator==(const Rotation& other) const +inline bool Rotation::operator==(const Rotation & other) const { - return (angle_ == other.angle_); + return angle_ == other.angle_; } -inline bool Rotation::operator!=(const Rotation& other) const +inline bool Rotation::operator!=(const Rotation & other) const { return !operator==(other); } -inline tf2Scalar Rotation::distance2(const Rotation& other) const +inline tf2Scalar Rotation::distance2(const Rotation & other) const { tf2Scalar d = distance(other); return d * d; } -inline tf2Scalar Rotation::distance(const Rotation& other) const +inline tf2Scalar Rotation::distance(const Rotation & other) const { return (other - *this).angle(); } -inline Vector2 Rotation::rotate(const Vector2& vec) const +inline Vector2 Rotation::rotate(const Vector2 & vec) const { populateTrigCache(); - return Vector2(vec.x() * cos_angle_ - vec.y() * sin_angle_, - vec.x() * sin_angle_ + vec.y() * cos_angle_); + return Vector2( + vec.x() * cos_angle_ - vec.y() * sin_angle_, + vec.x() * sin_angle_ + vec.y() * cos_angle_); } -inline Vector2 Rotation::unrotate(const Vector2& vec) const +inline Vector2 Rotation::unrotate(const Vector2 & vec) const { - // Populate the cache before inverse() is called. The cache is propagated to inverse() temporary object, and we get + // Populate the cache before inverse() is called. + // The cache is propagated to inverse() temporary object, and we get // the benefit of having the cache populated for future calls using this object. populateTrigCache(); return inverse().rotate(vec); @@ -137,22 +138,19 @@ inline Rotation Rotation::inverse() const inline Rotation Rotation::absolute() const { // A little effort to keep the trig cache populated - if (angle_ < 0) - { + if (angle_ < 0) { return inverse(); - } - else - { + } else { return *this; } } -inline Rotation Rotation::lerp(const Rotation& other, const tf2Scalar ratio) const +inline Rotation Rotation::lerp(const Rotation & other, const tf2Scalar ratio) const { return Rotation(angle_ + ratio * (other - *this).angle()); } -inline const tf2Scalar& Rotation::getAngle() const +inline const tf2Scalar & Rotation::getAngle() const { return angle_; } @@ -162,18 +160,16 @@ inline void Rotation::setAngle(const tf2Scalar angle) setValue(angle); } -inline void Rotation::setMax(const Rotation& other) +inline void Rotation::setMax(const Rotation & other) { - if (other.angle_ > angle_) - { + if (other.angle_ > angle_) { *this = other; } } -inline void Rotation::setMin(const Rotation& other) +inline void Rotation::setMin(const Rotation & other) { - if (other.angle_ < angle_) - { + if (other.angle_ < angle_) { *this = other; } } @@ -220,7 +216,7 @@ inline Eigen::Matrix3d Rotation::getHomogeneousMatrix() const return matrix; } -inline Rotation& Rotation::wrap() +inline Rotation & Rotation::wrap() { // Handle the 2*Pi roll-over angle_ -= TF2SIMD_2_PI * std::floor((angle_ + TF2SIMD_PI) / TF2SIMD_2_PI); @@ -229,20 +225,19 @@ inline Rotation& Rotation::wrap() inline void Rotation::populateTrigCache() const { - if ((cos_angle_ == 0) && (sin_angle_ == 0)) - { + if ((cos_angle_ == 0) && (sin_angle_ == 0)) { cos_angle_ = tf2Cos(angle_); sin_angle_ = tf2Sin(angle_); } } -inline Rotation operator+(Rotation lhs, const Rotation& rhs) +inline Rotation operator+(Rotation lhs, const Rotation & rhs) { lhs += rhs; return lhs; } -inline Rotation operator-(Rotation lhs, const Rotation& rhs) +inline Rotation operator-(Rotation lhs, const Rotation & rhs) { lhs -= rhs; return lhs; @@ -266,16 +261,16 @@ inline Rotation operator/(Rotation lhs, const tf2Scalar rhs) return lhs; } -inline Vector2 operator*(const Rotation& lhs, const Vector2& rhs) +inline Vector2 operator*(const Rotation & lhs, const Vector2 & rhs) { return lhs.rotate(rhs); } -inline std::ostream& operator<<(std::ostream& stream, const Rotation& rotation) +inline std::ostream & operator<<(std::ostream & stream, const Rotation & rotation) { return stream << "angle: " << rotation.angle(); } } // namespace tf2_2d -#endif // TF2_2D_ROTATION_IMPL_H +#endif // TF2_2D__ROTATION_IMPL_HPP_ diff --git a/include/tf2_2d/tf2_2d.h b/include/tf2_2d/tf2_2d.h index d2a17dd..e964460 100644 --- a/include/tf2_2d/tf2_2d.h +++ b/include/tf2_2d/tf2_2d.h @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2019, Locus Robotics + * Copyright (c) 2022, Locus Robotics * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -31,287 +31,12 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef TF2_2D_TF2_2D_H -#define TF2_2D_TF2_2D_H -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#ifndef TF2_2D__TF2_2D_H_ +#define TF2_2D__TF2_2D_H_ -#include -#include +#warning This header is obsolete, please include tf2_2d/tf2_2d.hpp instead -#include -#include +#include -/** - * This file contains conversion functions from standard ROS message types to/from the 2D geometry objects. These are - * designed to work with the tf2 data conversion system: http://wiki.ros.org/tf2/Tutorials/Migration/DataConversions - */ - -namespace tf2 -{ - -inline void toMsg(const tf2_2d::Vector2& in, geometry_msgs::Vector3& msg) -{ - msg.x = in.x(); - msg.y = in.y(); - msg.z = 0; -} - -inline void toMsg(const tf2_2d::Vector2& in, geometry_msgs::Point& msg) -{ - msg.x = in.x(); - msg.y = in.y(); - msg.z = 0; -} - -inline void toMsg(const tf2_2d::Vector2& in, geometry_msgs::Point32& msg) -{ - msg.x = in.x(); - msg.y = in.y(); - msg.z = 0; -} - -// You can only choose one destination message type with this signature -inline geometry_msgs::Vector3 toMsg(const tf2_2d::Vector2& in) -{ - geometry_msgs::Vector3 msg; - toMsg(in, msg); - return msg; -} - -inline void fromMsg(const geometry_msgs::Vector3& msg, tf2_2d::Vector2& out) -{ - out.setX(msg.x); - out.setY(msg.y); -} - -inline void fromMsg(const geometry_msgs::Point& msg, tf2_2d::Vector2& out) -{ - out.setX(msg.x); - out.setY(msg.y); -} - -inline void fromMsg(const geometry_msgs::Point32& msg, tf2_2d::Vector2& out) -{ - out.setX(msg.x); - out.setY(msg.y); -} - -inline void toMsg(const tf2::Stamped& in, geometry_msgs::Vector3Stamped& msg) -{ - msg.header.stamp = in.stamp_; - msg.header.frame_id = in.frame_id_; - toMsg(static_cast(in), msg.vector); -} - -inline void toMsg(const tf2::Stamped& in, geometry_msgs::PointStamped& msg) -{ - msg.header.stamp = in.stamp_; - msg.header.frame_id = in.frame_id_; - toMsg(static_cast(in), msg.point); -} - -inline geometry_msgs::Vector3Stamped toMsg(const tf2::Stamped& in) -{ - geometry_msgs::Vector3Stamped msg; - toMsg(in, msg); - return msg; -} - -inline void fromMsg(const geometry_msgs::Vector3Stamped& msg, tf2::Stamped& out) -{ - out.stamp_ = msg.header.stamp; - out.frame_id_ = msg.header.frame_id; - fromMsg(msg.vector, static_cast(out)); -} - -inline void fromMsg(const geometry_msgs::PointStamped& msg, tf2::Stamped& out) -{ - out.stamp_ = msg.header.stamp; - out.frame_id_ = msg.header.frame_id; - fromMsg(msg.point, static_cast(out)); -} - -inline void toMsg(const tf2_2d::Rotation& in, geometry_msgs::Quaternion& msg) -{ - msg.x = 0; - msg.y = 0; - msg.z = std::sin(0.5 * in.angle()); - msg.w = std::cos(0.5 * in.angle()); -} - -inline geometry_msgs::Quaternion toMsg(const tf2_2d::Rotation& in) -{ - geometry_msgs::Quaternion msg; - toMsg(in, msg); - return msg; -} - -inline void fromMsg(const geometry_msgs::Quaternion& msg, tf2_2d::Rotation& out) -{ - out.setAngle(tf2::getYaw(msg)); -} - -inline void toMsg(const tf2::Stamped& in, geometry_msgs::QuaternionStamped& msg) -{ - msg.header.stamp = in.stamp_; - msg.header.frame_id = in.frame_id_; - toMsg(static_cast(in), msg.quaternion); -} - -inline geometry_msgs::QuaternionStamped toMsg(const tf2::Stamped& in) -{ - geometry_msgs::QuaternionStamped msg; - toMsg(in, msg); - return msg; -} - -inline void fromMsg(const geometry_msgs::QuaternionStamped& msg, tf2::Stamped& out) -{ - out.stamp_ = msg.header.stamp; - out.frame_id_ = msg.header.frame_id; - fromMsg(msg.quaternion, static_cast(out)); -} - -inline void toMsg(const tf2_2d::Transform& in, geometry_msgs::Transform& msg) -{ - toMsg(in.translation(), msg.translation); - toMsg(in.rotation(), msg.rotation); -} - -inline void toMsg(const tf2_2d::Transform& in, geometry_msgs::Pose& msg) -{ - toMsg(in.translation(), msg.position); - toMsg(in.rotation(), msg.orientation); -} - -inline void toMsg(const tf2_2d::Transform& in, geometry_msgs::Pose2D& msg) -{ - msg.x = in.x(); - msg.y = in.y(); - msg.theta = in.theta(); -} - -inline geometry_msgs::Transform toMsg(const tf2_2d::Transform& in) -{ - geometry_msgs::Transform msg; - toMsg(in, msg); - return msg; -} - -inline void fromMsg(const geometry_msgs::Transform& msg, tf2_2d::Transform& out) -{ - tf2_2d::Rotation rotation; - fromMsg(msg.rotation, rotation); - out.setRotation(rotation); - tf2_2d::Vector2 translation; - fromMsg(msg.translation, translation); - out.setTranslation(translation); -} - -inline void fromMsg(const geometry_msgs::Pose& msg, tf2_2d::Transform& out) -{ - tf2_2d::Rotation rotation; - fromMsg(msg.orientation, rotation); - out.setRotation(rotation); - tf2_2d::Vector2 translation; - fromMsg(msg.position, translation); - out.setTranslation(translation); -} - -inline void fromMsg(const geometry_msgs::Pose2D& msg, tf2_2d::Transform& out) -{ - out.setAngle(msg.theta); - out.setX(msg.x); - out.setY(msg.y); -} - -inline void toMsg(const tf2::Stamped& in, geometry_msgs::TransformStamped& msg) -{ - msg.header.stamp = in.stamp_; - msg.header.frame_id = in.frame_id_; - toMsg(static_cast(in), msg.transform); -} - -inline void toMsg(const tf2::Stamped& in, geometry_msgs::PoseStamped& msg) -{ - msg.header.stamp = in.stamp_; - msg.header.frame_id = in.frame_id_; - toMsg(static_cast(in), msg.pose); -} - -inline geometry_msgs::TransformStamped toMsg(const tf2::Stamped& in) -{ - geometry_msgs::TransformStamped msg; - msg.header.stamp = in.stamp_; - msg.header.frame_id = in.frame_id_; - msg.transform = toMsg(static_cast(in)); - return msg; -} - -inline void fromMsg(const geometry_msgs::TransformStamped& msg, tf2::Stamped& out) -{ - out.stamp_ = msg.header.stamp; - out.frame_id_ = msg.header.frame_id; - fromMsg(msg.transform, static_cast(out)); -} - -inline void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped& out) -{ - out.stamp_ = msg.header.stamp; - out.frame_id_ = msg.header.frame_id; - fromMsg(msg.pose, static_cast(out)); -} - - - -inline -Eigen::Matrix3d transformCovariance(const Eigen::Matrix3d& cov_in, const tf2_2d::Transform& transform) -{ - Eigen::Matrix3d R = transform.rotation().getHomogeneousMatrix(); - return R * cov_in * R.transpose(); -} - -inline -std::array transformCovariance(const std::array& cov_in, const tf2_2d::Transform& transform) -{ - Eigen::Matrix3d R = transform.rotation().getHomogeneousMatrix(); - std::array cov_out; - Eigen::Map> cov_out_map(cov_out.data()); - Eigen::Map> cov_in_map(cov_in.data()); - cov_out_map = R * cov_in_map * R.transpose(); - return cov_out; -} - -inline -boost::array transformCovariance(const boost::array& cov_in, const tf2_2d::Transform& transform) -{ - Eigen::Matrix3d R = transform.rotation().getHomogeneousMatrix(); - boost::array cov_out; - Eigen::Map> cov_out_map(cov_out.data()); - Eigen::Map> cov_in_map(cov_in.data()); - cov_out_map = R * cov_in_map * R.transpose(); - return cov_out; -} - -} // namespace tf2 - -#endif // TF2_2D_TF2_2D_H +#endif // TF2_2D__TF2_2D_H_ diff --git a/include/tf2_2d/tf2_2d.hpp b/include/tf2_2d/tf2_2d.hpp new file mode 100644 index 0000000..514070e --- /dev/null +++ b/include/tf2_2d/tf2_2d.hpp @@ -0,0 +1,341 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef TF2_2D__TF2_2D_HPP_ +#define TF2_2D__TF2_2D_HPP_ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +/** + * This file contains conversion functions from standard ROS message types to/from the 2D geometry objects. These are + * designed to work with the tf2 data conversion system: http://wiki.ros.org/tf2/Tutorials/Migration/DataConversions + */ + +namespace tf2 +{ + +inline void toMsg(const tf2_2d::Vector2 & in, geometry_msgs::msg::Vector3 & msg) +{ + msg.x = in.x(); + msg.y = in.y(); + msg.z = 0; +} + +inline void toMsg(const tf2_2d::Vector2 & in, geometry_msgs::msg::Point & msg) +{ + msg.x = in.x(); + msg.y = in.y(); + msg.z = 0; +} + +inline void toMsg(const tf2_2d::Vector2 & in, geometry_msgs::msg::Point32 & msg) +{ + msg.x = in.x(); + msg.y = in.y(); + msg.z = 0; +} + +// You can only choose one destination message type with this signature +inline geometry_msgs::msg::Vector3 toMsg(const tf2_2d::Vector2 & in) +{ + geometry_msgs::msg::Vector3 msg; + toMsg(in, msg); + return msg; +} + +inline void fromMsg(const geometry_msgs::msg::Vector3 & msg, tf2_2d::Vector2 & out) +{ + out.setX(msg.x); + out.setY(msg.y); +} + +inline void fromMsg(const geometry_msgs::msg::Point & msg, tf2_2d::Vector2 & out) +{ + out.setX(msg.x); + out.setY(msg.y); +} + +inline void fromMsg(const geometry_msgs::msg::Point32 & msg, tf2_2d::Vector2 & out) +{ + out.setX(msg.x); + out.setY(msg.y); +} + +inline void toMsg( + const tf2::Stamped & in, + geometry_msgs::msg::Vector3Stamped & msg) +{ + msg.header.stamp = tf2_ros::toMsg(in.stamp_); + msg.header.frame_id = in.frame_id_; + toMsg(static_cast(in), msg.vector); +} + +inline void toMsg(const tf2::Stamped & in, geometry_msgs::msg::PointStamped & msg) +{ + msg.header.stamp = tf2_ros::toMsg(in.stamp_); + msg.header.frame_id = in.frame_id_; + toMsg(static_cast(in), msg.point); +} + +inline geometry_msgs::msg::Vector3Stamped toMsg(const tf2::Stamped & in) +{ + geometry_msgs::msg::Vector3Stamped msg; + toMsg(in, msg); + return msg; +} + +inline void fromMsg( + const geometry_msgs::msg::Vector3Stamped & msg, + tf2::Stamped & out) +{ + out.stamp_ = tf2_ros::fromMsg(msg.header.stamp); + out.frame_id_ = msg.header.frame_id; + fromMsg(msg.vector, static_cast(out)); +} + +inline void fromMsg( + const geometry_msgs::msg::PointStamped & msg, + tf2::Stamped & out) +{ + out.stamp_ = tf2_ros::fromMsg(msg.header.stamp); + out.frame_id_ = msg.header.frame_id; + fromMsg(msg.point, static_cast(out)); +} + +inline void toMsg(const tf2_2d::Rotation & in, geometry_msgs::msg::Quaternion & msg) +{ + msg.x = 0; + msg.y = 0; + msg.z = std::sin(0.5 * in.angle()); + msg.w = std::cos(0.5 * in.angle()); +} + +inline geometry_msgs::msg::Quaternion toMsg(const tf2_2d::Rotation & in) +{ + geometry_msgs::msg::Quaternion msg; + toMsg(in, msg); + return msg; +} + +inline void fromMsg(const geometry_msgs::msg::Quaternion & msg, tf2_2d::Rotation & out) +{ + out.setAngle(tf2::getYaw(msg)); +} + +inline void toMsg( + const tf2::Stamped & in, + geometry_msgs::msg::QuaternionStamped & msg) +{ + msg.header.stamp = tf2_ros::toMsg(in.stamp_); + msg.header.frame_id = in.frame_id_; + toMsg(static_cast(in), msg.quaternion); +} + +inline geometry_msgs::msg::QuaternionStamped toMsg(const tf2::Stamped & in) +{ + geometry_msgs::msg::QuaternionStamped msg; + toMsg(in, msg); + return msg; +} + +inline void fromMsg( + const geometry_msgs::msg::QuaternionStamped & msg, + tf2::Stamped & out) +{ + out.stamp_ = tf2_ros::fromMsg(msg.header.stamp); + out.frame_id_ = msg.header.frame_id; + fromMsg(msg.quaternion, static_cast(out)); +} + +inline void toMsg(const tf2_2d::Transform & in, geometry_msgs::msg::Transform & msg) +{ + toMsg(in.translation(), msg.translation); + toMsg(in.rotation(), msg.rotation); +} + +inline void toMsg(const tf2_2d::Transform & in, geometry_msgs::msg::Pose & msg) +{ + toMsg(in.translation(), msg.position); + toMsg(in.rotation(), msg.orientation); +} + +inline void toMsg(const tf2_2d::Transform & in, geometry_msgs::msg::Pose2D & msg) +{ + msg.x = in.x(); + msg.y = in.y(); + msg.theta = in.theta(); +} + +inline geometry_msgs::msg::Transform toMsg(const tf2_2d::Transform & in) +{ + geometry_msgs::msg::Transform msg; + toMsg(in, msg); + return msg; +} + +inline void fromMsg(const geometry_msgs::msg::Transform & msg, tf2_2d::Transform & out) +{ + tf2_2d::Rotation rotation; + fromMsg(msg.rotation, rotation); + out.setRotation(rotation); + tf2_2d::Vector2 translation; + fromMsg(msg.translation, translation); + out.setTranslation(translation); +} + +inline void fromMsg(const geometry_msgs::msg::Pose & msg, tf2_2d::Transform & out) +{ + tf2_2d::Rotation rotation; + fromMsg(msg.orientation, rotation); + out.setRotation(rotation); + tf2_2d::Vector2 translation; + fromMsg(msg.position, translation); + out.setTranslation(translation); +} + +inline void fromMsg(const geometry_msgs::msg::Pose2D & msg, tf2_2d::Transform & out) +{ + out.setAngle(msg.theta); + out.setX(msg.x); + out.setY(msg.y); +} + +inline void toMsg( + const tf2::Stamped & in, + geometry_msgs::msg::TransformStamped & msg) +{ + msg.header.stamp = tf2_ros::toMsg(in.stamp_); + msg.header.frame_id = in.frame_id_; + toMsg(static_cast(in), msg.transform); +} + +inline void toMsg(const tf2::Stamped & in, geometry_msgs::msg::PoseStamped & msg) +{ + msg.header.stamp = tf2_ros::toMsg(in.stamp_); + msg.header.frame_id = in.frame_id_; + toMsg(static_cast(in), msg.pose); +} + +inline geometry_msgs::msg::TransformStamped toMsg(const tf2::Stamped & in) +{ + geometry_msgs::msg::TransformStamped msg; + msg.header.stamp = tf2_ros::toMsg(in.stamp_); + msg.header.frame_id = in.frame_id_; + msg.transform = toMsg(static_cast(in)); + return msg; +} + +inline void fromMsg( + const geometry_msgs::msg::TransformStamped & msg, + tf2::Stamped & out) +{ + out.stamp_ = tf2_ros::fromMsg(msg.header.stamp); + out.frame_id_ = msg.header.frame_id; + fromMsg(msg.transform, static_cast(out)); +} + +inline void fromMsg( + const geometry_msgs::msg::PoseStamped & msg, + tf2::Stamped & out) +{ + out.stamp_ = tf2_ros::fromMsg(msg.header.stamp); + out.frame_id_ = msg.header.frame_id; + fromMsg(msg.pose, static_cast(out)); +} + + +inline +Eigen::Matrix3d transformCovariance( + const Eigen::Matrix3d & cov_in, + const tf2_2d::Transform & transform) +{ + Eigen::Matrix3d R = transform.rotation().getHomogeneousMatrix(); + return R * cov_in * R.transpose(); +} + +inline +std::array transformCovariance( + const std::array & cov_in, + const tf2_2d::Transform & transform) +{ + Eigen::Matrix3d R = transform.rotation().getHomogeneousMatrix(); + std::array cov_out; + Eigen::Map> cov_out_map(cov_out.data()); + Eigen::Map> cov_in_map(cov_in.data()); + cov_out_map = R * cov_in_map * R.transpose(); + return cov_out; +} + +inline +boost::array transformCovariance( + const boost::array & cov_in, + const tf2_2d::Transform & transform) +{ + Eigen::Matrix3d R = transform.rotation().getHomogeneousMatrix(); + boost::array cov_out; + Eigen::Map> cov_out_map(cov_out.data()); + Eigen::Map> cov_in_map(cov_in.data()); + cov_out_map = R * cov_in_map * R.transpose(); + return cov_out; +} + +} // namespace tf2 + +#endif // TF2_2D__TF2_2D_HPP_ diff --git a/include/tf2_2d/transform.h b/include/tf2_2d/transform.h index d6c3778..5936b77 100644 --- a/include/tf2_2d/transform.h +++ b/include/tf2_2d/transform.h @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2019, Locus Robotics + * Copyright (c) 2022, Locus Robotics * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -31,199 +31,12 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef TF2_2D_TRANSFORM_H -#define TF2_2D_TRANSFORM_H -#include -#include -#include -#include +#ifndef TF2_2D__TRANSFORM_H_ +#define TF2_2D__TRANSFORM_H_ -#include +#warning This header is obsolete, please include tf2_2d/transform.hpp instead +#include -namespace tf2_2d -{ - -/** - * @brief A class representing a 2D frame transformation or 2D pose - * - * This class mimics the tf2::Transform class to the extent possible. - */ -class Transform -{ -public: - /** - * @brief No initialization constructor - */ - Transform(); - - /** - * @brief Constructor from a rotation and translation - * - * @param rotation Rotation - * @param translation Translation - */ - Transform(const Rotation& rotation, const Vector2& translation); - - /** - * @brief Constructor from individual x, y, yaw elements - * - * @param x The X value - * @param y The Y value - * @param yaw The yaw value in radians - */ - Transform(const tf2Scalar x, const tf2Scalar y, const tf2Scalar yaw); - - /** - * @brief Constructor from a tf2 Transform object - * - * @param transform - The tf2 Transform object from which we will initialize this object - */ - explicit Transform(const tf2::Transform &transform); - - /** - * @brief Compose this transform with another transform on the right - */ - Transform& operator*=(const Transform& rhs); - - /** - * @brief Test if two transforms have all elements equal - */ - bool operator==(const Transform& rhs); - - /** - * @brief Test if two transforms are not equal - */ - bool operator!=(const Transform& rhs); - - /** - * @brief Return the linear interpolation between this and another transform - * - * @param other The other transform - * @param ratio The ratio of this to other (ratio=0 => return this, ratio=1 => return other) - */ - Transform lerp(const Transform& other, const tf2Scalar ratio) const; - - /** - * @brief Return the rotation for this transform - */ - const Rotation& getRotation() const; - const Rotation& rotation() const { return getRotation(); } - - /** - * @brief Return the translation for this transform - */ - const Vector2& getTranslation() const; - const Vector2& translation() const { return getTranslation(); } - - /** - * @brief Set the rotation for this transform - */ - void setRotation(const Rotation& other); - - /** - * @brief Set the translation for this transform - */ - void setTranslation(const Vector2& other); - - /** - * @brief Return the X value - */ - const tf2Scalar& getX() const; - const tf2Scalar& x() const { return getX(); } - - /** - * @brief Return the Y value - */ - const tf2Scalar& getY() const; - const tf2Scalar& y() const { return getY(); } - - /** - * @brief Return the yaw value - */ - const tf2Scalar& getYaw() const; - const tf2Scalar& yaw() const { return getYaw(); } - // There are a lot of common aliases for yaw - const tf2Scalar& getAngle() const { return getYaw(); } - const tf2Scalar& angle() const { return getYaw(); } - const tf2Scalar& getHeading() const { return getYaw(); } - const tf2Scalar& heading() const { return getYaw(); } - const tf2Scalar& getTheta() const { return getYaw(); } - const tf2Scalar& theta() const { return getYaw(); } - - /** - * @brief Set the X value - */ - void setX(const tf2Scalar x); - - /** - * @brief Set the Y value - */ - void setY(const tf2Scalar y); - - /** - * @brief Set the yaw value - */ - void setYaw(const tf2Scalar yaw); - // There are a lot of common aliases for yaw - void setAngle(const tf2Scalar angle) { setYaw(angle); } - void setHeading(const tf2Scalar heading) { setYaw(heading); } - void setTheta(const tf2Scalar theta) { setYaw(theta); } - - /** - * @brief Set this transformation to the identity - */ - void setIdentity(); - - /** - * @brief Return the inverse of this transform - */ - Transform inverse() const; - - /** - * @brief Return the inverse of this transform times the other transform - * - * returns this.inverse() * other - * - * @param other The other transform - */ - Transform inverseTimes(const Transform& other) const; - - /** - * @brief Get a 3x3 homogeneous transformation matrix - */ - Eigen::Matrix3d getHomogeneousMatrix() const; - -private: - Rotation rotation_; //!< Storage for the rotation - Vector2 translation_; //!< Storage for the translation -}; - -/** - * @brief Compose two transforms - */ -Transform operator*(Transform lhs, const Transform& rhs); - -/** - * @brief Return the transformed vector - * - * Output = This * Vector - */ -Vector2 operator*(const Transform& lhs, const Vector2& rhs); - -/** - * @brief Return the transformed rotation - */ -Rotation operator*(const Transform& lhs, const Rotation& rhs); - -/** - * @brief Stream the transformation in human-readable format - */ -std::ostream& operator<<(std::ostream& stream, const Transform& transform); - -} // namespace tf2_2d - -#include - -#endif // TF2_2D_TRANSFORM_H +#endif // TF2_2D__TRANSFORM_H_ diff --git a/include/tf2_2d/transform.hpp b/include/tf2_2d/transform.hpp new file mode 100644 index 0000000..4846ff0 --- /dev/null +++ b/include/tf2_2d/transform.hpp @@ -0,0 +1,229 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef TF2_2D__TRANSFORM_HPP_ +#define TF2_2D__TRANSFORM_HPP_ + +#include + +#include +#include +#include +#include + + +namespace tf2_2d +{ + +/** + * @brief A class representing a 2D frame transformation or 2D pose + * + * This class mimics the tf2::Transform class to the extent possible. + */ +class Transform +{ +public: + /** + * @brief No initialization constructor + */ + Transform(); + + /** + * @brief Constructor from a rotation and translation + * + * @param rotation Rotation + * @param translation Translation + */ + Transform(const Rotation & rotation, const Vector2 & translation); + + /** + * @brief Constructor from individual x, y, yaw elements + * + * @param x The X value + * @param y The Y value + * @param yaw The yaw value in radians + */ + Transform(const tf2Scalar x, const tf2Scalar y, const tf2Scalar yaw); + + /** + * @brief Constructor from a tf2 Transform object + * + * @param transform - The tf2 Transform object from which we will initialize this object + */ + explicit Transform(const tf2::Transform & transform); + + /** + * @brief Compose this transform with another transform on the right + */ + Transform & operator*=(const Transform & rhs); + + /** + * @brief Test if two transforms have all elements equal + */ + bool operator==(const Transform & rhs); + + /** + * @brief Test if two transforms are not equal + */ + bool operator!=(const Transform & rhs); + + /** + * @brief Return the linear interpolation between this and another transform + * + * @param other The other transform + * @param ratio The ratio of this to other (ratio=0 => return this, ratio=1 => return other) + */ + Transform lerp(const Transform & other, const tf2Scalar ratio) const; + + /** + * @brief Return the rotation for this transform + */ + const Rotation & getRotation() const; + const Rotation & rotation() const {return getRotation();} + + /** + * @brief Return the translation for this transform + */ + const Vector2 & getTranslation() const; + const Vector2 & translation() const {return getTranslation();} + + /** + * @brief Set the rotation for this transform + */ + void setRotation(const Rotation & other); + + /** + * @brief Set the translation for this transform + */ + void setTranslation(const Vector2 & other); + + /** + * @brief Return the X value + */ + const tf2Scalar & getX() const; + const tf2Scalar & x() const {return getX();} + + /** + * @brief Return the Y value + */ + const tf2Scalar & getY() const; + const tf2Scalar & y() const {return getY();} + + /** + * @brief Return the yaw value + */ + const tf2Scalar & getYaw() const; + const tf2Scalar & yaw() const {return getYaw();} + // There are a lot of common aliases for yaw + const tf2Scalar & getAngle() const {return getYaw();} + const tf2Scalar & angle() const {return getYaw();} + const tf2Scalar & getHeading() const {return getYaw();} + const tf2Scalar & heading() const {return getYaw();} + const tf2Scalar & getTheta() const {return getYaw();} + const tf2Scalar & theta() const {return getYaw();} + + /** + * @brief Set the X value + */ + void setX(const tf2Scalar x); + + /** + * @brief Set the Y value + */ + void setY(const tf2Scalar y); + + /** + * @brief Set the yaw value + */ + void setYaw(const tf2Scalar yaw); + // There are a lot of common aliases for yaw + void setAngle(const tf2Scalar angle) {setYaw(angle);} + void setHeading(const tf2Scalar heading) {setYaw(heading);} + void setTheta(const tf2Scalar theta) {setYaw(theta);} + + /** + * @brief Set this transformation to the identity + */ + void setIdentity(); + + /** + * @brief Return the inverse of this transform + */ + Transform inverse() const; + + /** + * @brief Return the inverse of this transform times the other transform + * + * returns this.inverse() * other + * + * @param other The other transform + */ + Transform inverseTimes(const Transform & other) const; + + /** + * @brief Get a 3x3 homogeneous transformation matrix + */ + Eigen::Matrix3d getHomogeneousMatrix() const; + +private: + Rotation rotation_; //!< Storage for the rotation + Vector2 translation_; //!< Storage for the translation +}; + +/** + * @brief Compose two transforms + */ +Transform operator*(Transform lhs, const Transform & rhs); + +/** + * @brief Return the transformed vector + * + * Output = This * Vector + */ +Vector2 operator*(const Transform & lhs, const Vector2 & rhs); + +/** + * @brief Return the transformed rotation + */ +Rotation operator*(const Transform & lhs, const Rotation & rhs); + +/** + * @brief Stream the transformation in human-readable format + */ +std::ostream & operator<<(std::ostream & stream, const Transform & transform); + +} // namespace tf2_2d + +#include + +#endif // TF2_2D__TRANSFORM_HPP_ diff --git a/include/tf2_2d/transform_impl.h b/include/tf2_2d/transform_impl.hpp similarity index 64% rename from include/tf2_2d/transform_impl.h rename to include/tf2_2d/transform_impl.hpp index 12f1bf1..638e50f 100644 --- a/include/tf2_2d/transform_impl.h +++ b/include/tf2_2d/transform_impl.hpp @@ -31,18 +31,17 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef TF2_2D_TRANSFORM_IMPL_H -#define TF2_2D_TRANSFORM_IMPL_H - -#include // NOLINT: The tf2 MinMax.h file does not include all requirements. Consequently, -#include // NOLINT: the order of the headers here is important. -#include // -#include // -#include -#include +#ifndef TF2_2D__TRANSFORM_IMPL_HPP_ +#define TF2_2D__TRANSFORM_IMPL_HPP_ #include +#include +#include +#include +#include +#include + namespace tf2_2d { @@ -50,79 +49,81 @@ inline Transform::Transform() { } -inline Transform::Transform(const Rotation& rotation, const Vector2& translation) : - rotation_(rotation), +inline Transform::Transform(const Rotation & rotation, const Vector2 & translation) +: rotation_(rotation), translation_(translation) { } -inline Transform::Transform(const tf2Scalar x, const tf2Scalar y, const tf2Scalar yaw) : - rotation_(yaw), +inline Transform::Transform(const tf2Scalar x, const tf2Scalar y, const tf2Scalar yaw) +: rotation_(yaw), translation_(x, y) { } -inline Transform::Transform(const tf2::Transform &transform) : - rotation_(tf2::getYaw(transform.getRotation())), +inline Transform::Transform(const tf2::Transform & transform) +: rotation_(tf2::getYaw(transform.getRotation())), translation_(transform.getOrigin().getX(), transform.getOrigin().getY()) { } -inline Transform& Transform::operator*=(const Transform& rhs) +inline Transform & Transform::operator*=(const Transform & rhs) { translation_ = translation_ + rotation_.rotate(rhs.translation_); rotation_ = rotation_ + rhs.rotation_; return *this; } -inline bool Transform::operator==(const Transform& rhs) +inline bool Transform::operator==(const Transform & rhs) { - return ((rotation_ == rhs.rotation_) && (translation_ == rhs.translation_)); + return (rotation_ == rhs.rotation_) && (translation_ == rhs.translation_); } -inline bool Transform::operator!=(const Transform& rhs) +inline bool Transform::operator!=(const Transform & rhs) { return !operator==(rhs); } -inline Transform Transform::lerp(const Transform& other, const tf2Scalar ratio) const +inline Transform Transform::lerp(const Transform & other, const tf2Scalar ratio) const { // Following the tf2 3D implementation, interpolation of translation and rotation // is performed independently of each other - return Transform(rotation_.lerp(other.rotation_, ratio), translation_.lerp(other.translation_, ratio)); + return Transform( + rotation_.lerp(other.rotation_, ratio), + translation_.lerp(other.translation_, ratio)); } -inline const Rotation& Transform::getRotation() const +inline const Rotation & Transform::getRotation() const { return rotation_; } -inline const Vector2& Transform::getTranslation() const +inline const Vector2 & Transform::getTranslation() const { return translation_; } -inline const tf2Scalar& Transform::getX() const +inline const tf2Scalar & Transform::getX() const { return translation_.getX(); } -inline const tf2Scalar& Transform::getY() const +inline const tf2Scalar & Transform::getY() const { return translation_.getY(); } -inline const tf2Scalar& Transform::getYaw() const +inline const tf2Scalar & Transform::getYaw() const { return rotation_.getAngle(); } -inline void Transform::setRotation(const Rotation& other) +inline void Transform::setRotation(const Rotation & other) { rotation_ = other; } -inline void Transform::setTranslation(const Vector2& other) +inline void Transform::setTranslation(const Vector2 & other) { translation_ = other; } @@ -153,7 +154,7 @@ inline Transform Transform::inverse() const return Transform(rotation_.inverse(), rotation_.unrotate(-translation_)); } -inline Transform Transform::inverseTimes(const Transform& other) const +inline Transform Transform::inverseTimes(const Transform & other) const { return inverse() * other; } @@ -167,27 +168,28 @@ inline Eigen::Matrix3d Transform::getHomogeneousMatrix() const return matrix; } -inline Transform operator*(Transform lhs, const Transform& rhs) +inline Transform operator*(Transform lhs, const Transform & rhs) { lhs *= rhs; return lhs; } -inline Vector2 operator*(const Transform& lhs, const Vector2& rhs) +inline Vector2 operator*(const Transform & lhs, const Vector2 & rhs) { return lhs.rotation().rotate(rhs) + lhs.translation(); } -inline Rotation operator*(const Transform& lhs, const Rotation& rhs) +inline Rotation operator*(const Transform & lhs, const Rotation & rhs) { return lhs.rotation() + rhs; } -inline std::ostream& operator<<(std::ostream& stream, const Transform& transform) +inline std::ostream & operator<<(std::ostream & stream, const Transform & transform) { - return stream << "x: " << transform.x() << ", y: " << transform.y() << ", yaw: " << transform.yaw(); + return stream << "x: " << transform.x() << ", y: " << transform.y() << ", yaw: " << + transform.yaw(); } } // namespace tf2_2d -#endif // TF2_2D_TRANSFORM_IMPL_H +#endif // TF2_2D__TRANSFORM_IMPL_HPP_ diff --git a/include/tf2_2d/vector2.h b/include/tf2_2d/vector2.h index 0eeaeff..e6c0da7 100644 --- a/include/tf2_2d/vector2.h +++ b/include/tf2_2d/vector2.h @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2019, Locus Robotics + * Copyright (c) 2022, Locus Robotics * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -31,313 +31,12 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef TF2_2D_VECTOR2_H -#define TF2_2D_VECTOR2_H -#include +#ifndef TF2_2D__VECTOR2_H_ +#define TF2_2D__VECTOR2_H_ -#include +#warning This header is obsolete, please include tf2_2d/vector2.hpp instead +#include -namespace tf2_2d -{ - -/** - * @brief A class representing a 2D vector or 2D point - * - * This class mimics the tf2::Vector3 class to the extent possible. - */ -class Vector2 -{ -public: - /** - * @brief Axis constants - */ - enum Axis - { - X = 0, //!< X axis - Y = 1 //!< Y axis - }; - - /** - * @brief No initialization constructor - */ - Vector2(); - - /** - * @brief Constructor from scalars - * - * @param x X value - * @param y Y value - */ - Vector2(const tf2Scalar x, const tf2Scalar y); - - /** - * @brief Return the negative of the vector - */ - Vector2 operator-() const; - - /** - * @brief Add a vector to this one - * - * @param rhs The vector to add to this one - */ - Vector2& operator+=(const Vector2& rhs); - - /** - * @brief Subtract a vector from this one - * - * @param rhs The vector to subtract - */ - Vector2& operator-=(const Vector2& rhs); - - /** - * @brief Scale the vector - * - * @param rhs Scale factor - */ - Vector2& operator*=(const tf2Scalar rhs); - - /** - * @brief Element-wise multiply this vector by the other - * - * @param v The other vector - */ - Vector2& operator*=(const Vector2& rhs); - - /** - * @brief Inversely scale the vector - * - * @param rhs Scale factor to divide by - */ - Vector2& operator/=(const tf2Scalar rhs); - - /** - * @brief Element-wise divide this vector by the other - * - * @param rhs The other vector - */ - Vector2& operator/=(const Vector2& rhs); - - /** - * @brief Check if two vectors are equal - */ - bool operator==(const Vector2& other) const; - - /** - * @brief Check if two vectors are not equal - */ - bool operator!=(const Vector2& other) const; - - /** - * @brief Return the dot product - * - * @param v The other vector in the dot product - */ - tf2Scalar dot(const Vector2& other) const; - - /** - * @brief Return the length of the vector squared - */ - tf2Scalar length2() const; - - /** - * @brief Return the length of the vector - */ - tf2Scalar length() const; - - /** - * @brief Return the distance squared between the ends of this and another vector - * - * This is semantically treating the vector like a point - */ - tf2Scalar distance2(const Vector2& other) const; - - /** - * @brief Return the distance between the ends of this and another vector - * - * This is semantically treating the vector like a point - */ - tf2Scalar distance(const Vector2& other) const; - - /** - * @brief Normalize this vector - * - * x^2 + y^2 = 1 - */ - Vector2& normalize(); - - /** - * @brief Return a normalized version of this vector - */ - Vector2 normalized() const; - - /** - * @brief Return the angle between this and another vector - * - * @param other The other vector - */ - tf2Scalar angle(const Vector2& other) const; - - /** - * @brief Return a vector with the absolute values of each element - */ - Vector2 absolute() const; - - /** - * @brief Return the axis with the smallest value - * - * Note return values are 0,1 for x or y - */ - Axis minAxis() const; - - /** - * @brief Return the axis with the largest value - * - * Note return values are 0,1 for x or y - */ - Axis maxAxis() const; - - /** - * @brief Returns the axis with the smallest absolute value - * - * Note return values are 0,1 for x or y - * Seems like a poorly named function, but mimicking the tf2::Vector3 class - */ - Axis furthestAxis() const; - - /** - * @brief Returns the axis with the largest absolute value - * - * Note return values are 0,1 for x or y - * Seems like a poorly named function, but mimicking the tf2::Vector3 class - */ - Axis closestAxis() const; - - /** - * @brief Return the linear interpolation between this and another vector - * - * @param other The other vector - * @param ratio The ratio of this to other (ratio=0 => return this, ratio=1 => return other) - */ - Vector2 lerp(const Vector2& other, const tf2Scalar ratio) const; - - /** - * @brief Return the x value - */ - const tf2Scalar& getX() const; - const tf2Scalar& x() const { return getX(); } - - /** - * @brief Return the y value - */ - const tf2Scalar& getY() const; - const tf2Scalar& y() const { return getY(); } - - /** - * @brief Set the x value - */ - void setX(const tf2Scalar x); - - /** - * @brief Set the y value - */ - void setY(const tf2Scalar y); - - /** - * @brief Set each element to the max of the current values and the values of another Vector3 - * - * @param other The other Vector3 to compare with - */ - void setMax(const Vector2& other); - - /** - * @brief Set each element to the min of the current values and the values of another Vector3 - * - * @param other The other Vector3 to compare with - */ - void setMin(const Vector2& other); - - /** - * @brief Set each element to the provided value - * - * @param x The X value - * @param y The Y value - */ - void setValue(const tf2Scalar x, const tf2Scalar y); - - /** - * Set all elements of this vector to zero - */ - void setZero(); - - /** - * Check if all the elements of this vector are zero - */ - bool isZero() const; - - /** - * Check if all the elements of this vector close to zero - */ - bool fuzzyZero() const; - - /** - * @brief Get an Eigen vector representation - */ - Eigen::Vector2d getVector() const; - - /** - * @brief Get a 3x3 homogeneous transformation matrix with just the translation portion populated - */ - Eigen::Matrix3d getHomogeneousMatrix() const; - -private: - tf2Scalar x_; //!< Storage for the X value - tf2Scalar y_; //!< Storage for the Y value -}; - -/** - * @brief Add two vectors - */ -Vector2 operator+(Vector2 lhs, const Vector2& rhs); - -/** - * @brief Subtract two vectors - */ -Vector2 operator-(Vector2 lhs, const Vector2& rhs); - -/** - * @brief Scale a vector - */ -Vector2 operator*(Vector2 lhs, const tf2Scalar rhs); - -/** - * @brief Scale a vector - */ -Vector2 operator*(const tf2Scalar lhs, Vector2 rhs); - -/** - * @brief Element-wise multiplication of two vectors - */ -Vector2 operator*(Vector2 lhs, const Vector2& rhs); - -/** - * @brief Inversely scale a vector - */ -Vector2 operator/(Vector2 lhs, const tf2Scalar rhs); - -/** - * @brief Element-wise division of two vectors - */ -Vector2 operator/(Vector2 lhs, const Vector2& rhs); - -/** - * @brief Stream the vector in human-readable format - */ -std::ostream& operator<<(std::ostream& stream, const Vector2& vector); - -} // namespace tf2_2d - -#include - -#endif // TF2_2D_VECTOR2_H +#endif // TF2_2D__VECTOR2_H_ diff --git a/include/tf2_2d/vector2.hpp b/include/tf2_2d/vector2.hpp new file mode 100644 index 0000000..f497568 --- /dev/null +++ b/include/tf2_2d/vector2.hpp @@ -0,0 +1,343 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef TF2_2D__VECTOR2_HPP_ +#define TF2_2D__VECTOR2_HPP_ + +#include + +#include + + +namespace tf2_2d +{ + +/** + * @brief A class representing a 2D vector or 2D point + * + * This class mimics the tf2::Vector3 class to the extent possible. + */ +class Vector2 +{ +public: + /** + * @brief Axis constants + */ + enum Axis + { + X = 0, //!< X axis + Y = 1 //!< Y axis + }; + + /** + * @brief No initialization constructor + */ + Vector2(); + + /** + * @brief Constructor from scalars + * + * @param x X value + * @param y Y value + */ + Vector2(const tf2Scalar x, const tf2Scalar y); + + /** + * @brief Return the negative of the vector + */ + Vector2 operator-() const; + + /** + * @brief Add a vector to this one + * + * @param rhs The vector to add to this one + */ + Vector2 & operator+=(const Vector2 & rhs); + + /** + * @brief Subtract a vector from this one + * + * @param rhs The vector to subtract + */ + Vector2 & operator-=(const Vector2 & rhs); + + /** + * @brief Scale the vector + * + * @param rhs Scale factor + */ + Vector2 & operator*=(const tf2Scalar rhs); + + /** + * @brief Element-wise multiply this vector by the other + * + * @param v The other vector + */ + Vector2 & operator*=(const Vector2 & rhs); + + /** + * @brief Inversely scale the vector + * + * @param rhs Scale factor to divide by + */ + Vector2 & operator/=(const tf2Scalar rhs); + + /** + * @brief Element-wise divide this vector by the other + * + * @param rhs The other vector + */ + Vector2 & operator/=(const Vector2 & rhs); + + /** + * @brief Check if two vectors are equal + */ + bool operator==(const Vector2 & other) const; + + /** + * @brief Check if two vectors are not equal + */ + bool operator!=(const Vector2 & other) const; + + /** + * @brief Return the dot product + * + * @param v The other vector in the dot product + */ + tf2Scalar dot(const Vector2 & other) const; + + /** + * @brief Return the length of the vector squared + */ + tf2Scalar length2() const; + + /** + * @brief Return the length of the vector + */ + tf2Scalar length() const; + + /** + * @brief Return the distance squared between the ends of this and another vector + * + * This is semantically treating the vector like a point + */ + tf2Scalar distance2(const Vector2 & other) const; + + /** + * @brief Return the distance between the ends of this and another vector + * + * This is semantically treating the vector like a point + */ + tf2Scalar distance(const Vector2 & other) const; + + /** + * @brief Normalize this vector + * + * x^2 + y^2 = 1 + */ + Vector2 & normalize(); + + /** + * @brief Return a normalized version of this vector + */ + Vector2 normalized() const; + + /** + * @brief Return the angle between this and another vector + * + * @param other The other vector + */ + tf2Scalar angle(const Vector2 & other) const; + + /** + * @brief Return a vector with the absolute values of each element + */ + Vector2 absolute() const; + + /** + * @brief Return the axis with the smallest value + * + * Note return values are 0,1 for x or y + */ + Axis minAxis() const; + + /** + * @brief Return the axis with the largest value + * + * Note return values are 0,1 for x or y + */ + Axis maxAxis() const; + + /** + * @brief Returns the axis with the smallest absolute value + * + * Note return values are 0,1 for x or y + * Seems like a poorly named function, but mimicking the tf2::Vector3 class + */ + Axis furthestAxis() const; + + /** + * @brief Returns the axis with the largest absolute value + * + * Note return values are 0,1 for x or y + * Seems like a poorly named function, but mimicking the tf2::Vector3 class + */ + Axis closestAxis() const; + + /** + * @brief Return the linear interpolation between this and another vector + * + * @param other The other vector + * @param ratio The ratio of this to other (ratio=0 => return this, ratio=1 => return other) + */ + Vector2 lerp(const Vector2 & other, const tf2Scalar ratio) const; + + /** + * @brief Return the x value + */ + const tf2Scalar & getX() const; + const tf2Scalar & x() const {return getX();} + + /** + * @brief Return the y value + */ + const tf2Scalar & getY() const; + const tf2Scalar & y() const {return getY();} + + /** + * @brief Set the x value + */ + void setX(const tf2Scalar x); + + /** + * @brief Set the y value + */ + void setY(const tf2Scalar y); + + /** + * @brief Set each element to the max of the current values and the values of another Vector3 + * + * @param other The other Vector3 to compare with + */ + void setMax(const Vector2 & other); + + /** + * @brief Set each element to the min of the current values and the values of another Vector3 + * + * @param other The other Vector3 to compare with + */ + void setMin(const Vector2 & other); + + /** + * @brief Set each element to the provided value + * + * @param x The X value + * @param y The Y value + */ + void setValue(const tf2Scalar x, const tf2Scalar y); + + /** + * Set all elements of this vector to zero + */ + void setZero(); + + /** + * Check if all the elements of this vector are zero + */ + bool isZero() const; + + /** + * Check if all the elements of this vector close to zero + */ + bool fuzzyZero() const; + + /** + * @brief Get an Eigen vector representation + */ + Eigen::Vector2d getVector() const; + + /** + * @brief Get a 3x3 homogeneous transformation matrix with just the translation portion populated + */ + Eigen::Matrix3d getHomogeneousMatrix() const; + +private: + tf2Scalar x_; //!< Storage for the X value + tf2Scalar y_; //!< Storage for the Y value +}; + +/** + * @brief Add two vectors + */ +Vector2 operator+(Vector2 lhs, const Vector2 & rhs); + +/** + * @brief Subtract two vectors + */ +Vector2 operator-(Vector2 lhs, const Vector2 & rhs); + +/** + * @brief Scale a vector + */ +Vector2 operator*(Vector2 lhs, const tf2Scalar rhs); + +/** + * @brief Scale a vector + */ +Vector2 operator*(const tf2Scalar lhs, Vector2 rhs); + +/** + * @brief Element-wise multiplication of two vectors + */ +Vector2 operator*(Vector2 lhs, const Vector2 & rhs); + +/** + * @brief Inversely scale a vector + */ +Vector2 operator/(Vector2 lhs, const tf2Scalar rhs); + +/** + * @brief Element-wise division of two vectors + */ +Vector2 operator/(Vector2 lhs, const Vector2 & rhs); + +/** + * @brief Stream the vector in human-readable format + */ +std::ostream & operator<<(std::ostream & stream, const Vector2 & vector); + +} // namespace tf2_2d + +#include + +#endif // TF2_2D__VECTOR2_HPP_ diff --git a/include/tf2_2d/vector2_impl.h b/include/tf2_2d/vector2_impl.hpp similarity index 72% rename from include/tf2_2d/vector2_impl.h rename to include/tf2_2d/vector2_impl.hpp index 17e3894..f57de60 100644 --- a/include/tf2_2d/vector2_impl.h +++ b/include/tf2_2d/vector2_impl.hpp @@ -31,14 +31,13 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef TF2_2D_VECTOR2_IMPL_H -#define TF2_2D_VECTOR2_IMPL_H - -#include // NOLINT: The tf2 MinMax.h file does not include all requirements. Consequently, -#include // NOLINT: the order of the headers here is important. +#ifndef TF2_2D__VECTOR2_IMPL_HPP_ +#define TF2_2D__VECTOR2_IMPL_HPP_ #include +#include + namespace tf2_2d { @@ -59,58 +58,58 @@ inline Vector2 Vector2::operator-() const return Vector2(-x_, -y_); } -inline Vector2& Vector2::operator+=(const Vector2& rhs) +inline Vector2 & Vector2::operator+=(const Vector2 & rhs) { x_ += rhs.x_; y_ += rhs.y_; return *this; } -inline Vector2& Vector2::operator-=(const Vector2& rhs) +inline Vector2 & Vector2::operator-=(const Vector2 & rhs) { x_ -= rhs.x_; y_ -= rhs.y_; return *this; } -inline Vector2& Vector2::operator*=(const tf2Scalar s) +inline Vector2 & Vector2::operator*=(const tf2Scalar s) { x_ *= s; y_ *= s; return *this; } -inline Vector2& Vector2::operator*=(const Vector2& rhs) +inline Vector2 & Vector2::operator*=(const Vector2 & rhs) { x_ *= rhs.x_; y_ *= rhs.y_; return *this; } -inline Vector2& Vector2::operator/=(const tf2Scalar rhs) +inline Vector2 & Vector2::operator/=(const tf2Scalar rhs) { tf2FullAssert(rhs != tf2Scalar(0.0)); return *this *= tf2Scalar(1.0) / rhs; } -inline Vector2& Vector2::operator/=(const Vector2& rhs) +inline Vector2 & Vector2::operator/=(const Vector2 & rhs) { x_ /= rhs.x_; y_ /= rhs.y_; return *this; } -inline bool Vector2::operator==(const Vector2& other) const +inline bool Vector2::operator==(const Vector2 & other) const { - return ((x_ == other.x_) && (y_ == other.y_)); + return (x_ == other.x_) && (y_ == other.y_); } -inline bool Vector2::operator!=(const Vector2& other) const +inline bool Vector2::operator!=(const Vector2 & other) const { return !operator==(other); } -inline tf2Scalar Vector2::dot(const Vector2& other) const +inline tf2Scalar Vector2::dot(const Vector2 & other) const { return x_ * other.x_ + y_ * other.y_; } @@ -125,17 +124,17 @@ inline tf2Scalar Vector2::length() const return tf2Sqrt(length2()); } -inline tf2Scalar Vector2::distance2(const Vector2& other) const +inline tf2Scalar Vector2::distance2(const Vector2 & other) const { return (other - *this).length2(); } -inline tf2Scalar Vector2::distance(const Vector2& other) const +inline tf2Scalar Vector2::distance(const Vector2 & other) const { return (other - *this).length(); } -inline Vector2& Vector2::normalize() +inline Vector2 & Vector2::normalize() { return *this /= length(); } @@ -145,7 +144,7 @@ inline Vector2 Vector2::normalized() const return *this / length(); } -inline tf2Scalar Vector2::angle(const Vector2& other) const +inline tf2Scalar Vector2::angle(const Vector2 & other) const { tf2Scalar s = tf2Sqrt(length2() * other.length2()); tf2FullAssert(s != tf2Scalar(0)); @@ -177,18 +176,19 @@ inline Vector2::Axis Vector2::closestAxis() const return absolute().maxAxis(); } -inline Vector2 Vector2::lerp(const Vector2& other, const tf2Scalar ratio) const +inline Vector2 Vector2::lerp(const Vector2 & other, const tf2Scalar ratio) const { - return Vector2(x_ + (other.x_ - x_) * ratio, - y_ + (other.y_ - y_) * ratio); + return Vector2( + x_ + (other.x_ - x_) * ratio, + y_ + (other.y_ - y_) * ratio); } -inline const tf2Scalar& Vector2::getX() const +inline const tf2Scalar & Vector2::getX() const { return x_; } -inline const tf2Scalar& Vector2::getY() const +inline const tf2Scalar & Vector2::getY() const { return y_; } @@ -203,13 +203,13 @@ inline void Vector2::setY(const tf2Scalar y) y_ = y; } -inline void Vector2::setMax(const Vector2& other) +inline void Vector2::setMax(const Vector2 & other) { tf2SetMax(x_, other.x_); tf2SetMax(y_, other.y_); } -inline void Vector2::setMin(const Vector2& other) +inline void Vector2::setMin(const Vector2 & other) { tf2SetMin(x_, other.x_); tf2SetMin(y_, other.y_); @@ -250,13 +250,13 @@ inline Eigen::Matrix3d Vector2::getHomogeneousMatrix() const return matrix; } -inline Vector2 operator+(Vector2 lhs, const Vector2& rhs) +inline Vector2 operator+(Vector2 lhs, const Vector2 & rhs) { lhs += rhs; return lhs; } -inline Vector2 operator-(Vector2 lhs, const Vector2& rhs) +inline Vector2 operator-(Vector2 lhs, const Vector2 & rhs) { lhs -= rhs; return lhs; @@ -274,7 +274,7 @@ inline Vector2 operator*(const tf2Scalar lhs, Vector2 rhs) return rhs; } -inline Vector2 operator*(Vector2 lhs, const Vector2& rhs) +inline Vector2 operator*(Vector2 lhs, const Vector2 & rhs) { lhs *= rhs; return lhs; @@ -286,17 +286,17 @@ inline Vector2 operator/(Vector2 lhs, const tf2Scalar rhs) return lhs; } -inline Vector2 operator/(Vector2 lhs, const Vector2& rhs) +inline Vector2 operator/(Vector2 lhs, const Vector2 & rhs) { lhs /= rhs; return lhs; } -inline std::ostream& operator<<(std::ostream& stream, const Vector2& vector) +inline std::ostream & operator<<(std::ostream & stream, const Vector2 & vector) { return stream << "x: " << vector.x() << ", y: " << vector.y(); } } // namespace tf2_2d -#endif // TF2_2D_VECTOR2_IMPL_H +#endif // TF2_2D__VECTOR2_IMPL_HPP_ diff --git a/package.xml b/package.xml index 5438f22..b491f0f 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ tf2_2d - 0.6.4 + 1.0.0 A set of 2D geometry classes modeled after the 3D geometry classes in tf2. @@ -11,12 +11,20 @@ BSD - catkin + ament_cmake + libboost-dev eigen - roscpp + rclcpp tf2 tf2_geometry_msgs + tf2_ros - roslint + ament_cmake_gtest + ament_lint_auto + ament_lint_common + + + ament_cmake + diff --git a/test/test_conversions.cpp b/test/test_conversions.cpp index 5e3cd23..0494d4b 100644 --- a/test/test_conversions.cpp +++ b/test/test_conversions.cpp @@ -31,27 +31,29 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include #include -#include -#include -#include -#include +#include #include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include TEST(Conversions, Vector2) @@ -59,9 +61,9 @@ TEST(Conversions, Vector2) // Test conversion to/from the Vector2 type { tf2_2d::Vector2 in(1.0, 2.0); - geometry_msgs::Vector3 actual; + geometry_msgs::msg::Vector3 actual; tf2::toMsg(in, actual); - geometry_msgs::Vector3 expected; + geometry_msgs::msg::Vector3 expected; expected.x = 1.0; expected.y = 2.0; expected.z = 0.0; @@ -71,9 +73,9 @@ TEST(Conversions, Vector2) } { tf2_2d::Vector2 in(1.0, 2.0); - geometry_msgs::Point actual; + geometry_msgs::msg::Point actual; tf2::toMsg(in, actual); - geometry_msgs::Point expected; + geometry_msgs::msg::Point expected; expected.x = 1.0; expected.y = 2.0; expected.z = 0.0; @@ -83,9 +85,9 @@ TEST(Conversions, Vector2) } { tf2_2d::Vector2 in(1.0, 2.0); - geometry_msgs::Point32 actual; + geometry_msgs::msg::Point32 actual; tf2::toMsg(in, actual); - geometry_msgs::Point32 expected; + geometry_msgs::msg::Point32 expected; expected.x = 1.0; expected.y = 2.0; expected.z = 0.0; @@ -95,8 +97,8 @@ TEST(Conversions, Vector2) } { tf2_2d::Vector2 in(1.0, 2.0); - geometry_msgs::Vector3 actual = tf2::toMsg(in); - geometry_msgs::Vector3 expected; + geometry_msgs::msg::Vector3 actual = tf2::toMsg(in); + geometry_msgs::msg::Vector3 expected; expected.x = 1.0; expected.y = 2.0; expected.z = 0.0; @@ -105,7 +107,7 @@ TEST(Conversions, Vector2) EXPECT_EQ(expected.z, actual.z); } { - geometry_msgs::Vector3 msg; + geometry_msgs::msg::Vector3 msg; msg.x = 1.0; msg.y = 2.0; msg.z = 3.0; @@ -116,7 +118,7 @@ TEST(Conversions, Vector2) EXPECT_EQ(expected.y(), actual.y()); } { - geometry_msgs::Point msg; + geometry_msgs::msg::Point msg; msg.x = 1.0; msg.y = 2.0; msg.z = 3.0; @@ -127,7 +129,7 @@ TEST(Conversions, Vector2) EXPECT_EQ(expected.y(), actual.y()); } { - geometry_msgs::Point32 msg; + geometry_msgs::msg::Point32 msg; msg.x = 1.0; msg.y = 2.0; msg.z = 3.0; @@ -138,11 +140,12 @@ TEST(Conversions, Vector2) EXPECT_EQ(expected.y(), actual.y()); } { - tf2::Stamped in(tf2_2d::Vector2(1.0, 2.0), ros::Time(1234, 5678), "test_frame"); - geometry_msgs::Vector3Stamped actual; + tf2::Stamped in(tf2_2d::Vector2(1.0, 2.0), + tf2_ros::fromRclcpp(rclcpp::Time(1234, 5678)), "test_frame"); + geometry_msgs::msg::Vector3Stamped actual; tf2::toMsg(in, actual); - geometry_msgs::Vector3Stamped expected; - expected.header.stamp = ros::Time(1234, 5678); + geometry_msgs::msg::Vector3Stamped expected; + expected.header.stamp = rclcpp::Time(1234, 5678); expected.header.frame_id = "test_frame"; expected.vector.x = 1.0; expected.vector.y = 2.0; @@ -154,11 +157,12 @@ TEST(Conversions, Vector2) EXPECT_EQ(expected.vector.z, actual.vector.z); } { - tf2::Stamped in(tf2_2d::Vector2(1.0, 2.0), ros::Time(1234, 5678), "test_frame"); - geometry_msgs::PointStamped actual; + tf2::Stamped in(tf2_2d::Vector2(1.0, 2.0), + tf2_ros::fromRclcpp(rclcpp::Time(1234, 5678)), "test_frame"); + geometry_msgs::msg::PointStamped actual; tf2::toMsg(in, actual); - geometry_msgs::PointStamped expected; - expected.header.stamp = ros::Time(1234, 5678); + geometry_msgs::msg::PointStamped expected; + expected.header.stamp = rclcpp::Time(1234, 5678); expected.header.frame_id = "test_frame"; expected.point.x = 1.0; expected.point.y = 2.0; @@ -170,10 +174,11 @@ TEST(Conversions, Vector2) EXPECT_EQ(expected.point.z, actual.point.z); } { - tf2::Stamped in(tf2_2d::Vector2(1.0, 2.0), ros::Time(1234, 5678), "test_frame"); - geometry_msgs::Vector3Stamped actual = tf2::toMsg(in); - geometry_msgs::Vector3Stamped expected; - expected.header.stamp = ros::Time(1234, 5678); + tf2::Stamped in(tf2_2d::Vector2(1.0, 2.0), + tf2_ros::fromRclcpp(rclcpp::Time(1234, 5678)), "test_frame"); + geometry_msgs::msg::Vector3Stamped actual = tf2::toMsg(in); + geometry_msgs::msg::Vector3Stamped expected; + expected.header.stamp = rclcpp::Time(1234, 5678); expected.header.frame_id = "test_frame"; expected.vector.x = 1.0; expected.vector.y = 2.0; @@ -185,30 +190,32 @@ TEST(Conversions, Vector2) EXPECT_EQ(expected.vector.z, actual.vector.z); } { - geometry_msgs::Vector3Stamped msg; - msg.header.stamp = ros::Time(1234, 5678); + geometry_msgs::msg::Vector3Stamped msg; + msg.header.stamp = rclcpp::Time(1234, 5678); msg.header.frame_id = "test_frame"; msg.vector.x = 1.0; msg.vector.y = 2.0; msg.vector.z = 3.0; tf2::Stamped actual; tf2::fromMsg(msg, actual); - tf2::Stamped expected(tf2_2d::Vector2(1.0, 2.0), ros::Time(1234, 5678), "test_frame"); + tf2::Stamped expected(tf2_2d::Vector2(1.0, 2.0), + tf2_ros::fromRclcpp(rclcpp::Time(1234, 5678)), "test_frame"); EXPECT_EQ(expected.stamp_, actual.stamp_); EXPECT_EQ(expected.frame_id_, actual.frame_id_); EXPECT_EQ(expected.x(), actual.x()); EXPECT_EQ(expected.y(), actual.y()); } { - geometry_msgs::PointStamped msg; - msg.header.stamp = ros::Time(1234, 5678); + geometry_msgs::msg::PointStamped msg; + msg.header.stamp = rclcpp::Time(1234, 5678); msg.header.frame_id = "test_frame"; msg.point.x = 1.0; msg.point.y = 2.0; msg.point.z = 3.0; tf2::Stamped actual; tf2::fromMsg(msg, actual); - tf2::Stamped expected(tf2_2d::Vector2(1.0, 2.0), ros::Time(1234, 5678), "test_frame"); + tf2::Stamped expected(tf2_2d::Vector2(1.0, 2.0), + tf2_ros::fromRclcpp(rclcpp::Time(1234, 5678)), "test_frame"); EXPECT_EQ(expected.stamp_, actual.stamp_); EXPECT_EQ(expected.frame_id_, actual.frame_id_); EXPECT_EQ(expected.x(), actual.x()); @@ -221,9 +228,9 @@ TEST(Conversions, Rotation) // Test conversion to/from the Rotation type { tf2_2d::Rotation in(2.0); - geometry_msgs::Quaternion actual; + geometry_msgs::msg::Quaternion actual; tf2::toMsg(in, actual); - geometry_msgs::Quaternion expected; + geometry_msgs::msg::Quaternion expected; expected.x = 0.0; expected.y = 0.0; expected.z = 0.841470984807897; @@ -235,8 +242,8 @@ TEST(Conversions, Rotation) } { tf2_2d::Rotation in(2.0); - geometry_msgs::Quaternion actual = tf2::toMsg(in); - geometry_msgs::Quaternion expected; + geometry_msgs::msg::Quaternion actual = tf2::toMsg(in); + geometry_msgs::msg::Quaternion expected; expected.x = 0.0; expected.y = 0.0; expected.z = 0.841470984807897; @@ -247,7 +254,7 @@ TEST(Conversions, Rotation) EXPECT_NEAR(expected.w, actual.w, 1.0e-9); } { - geometry_msgs::Quaternion msg; + geometry_msgs::msg::Quaternion msg; msg.x = 0.0; msg.y = 0.0; msg.z = 0.841470984807897; @@ -258,11 +265,12 @@ TEST(Conversions, Rotation) EXPECT_NEAR(expected.angle(), actual.angle(), 1.0e-9); } { - tf2::Stamped in(tf2_2d::Rotation(2.0), ros::Time(1234, 5678), "test_frame"); - geometry_msgs::QuaternionStamped actual; + tf2::Stamped in(tf2_2d::Rotation(2.0), + tf2_ros::fromRclcpp(rclcpp::Time(1234, 5678)), "test_frame"); + geometry_msgs::msg::QuaternionStamped actual; tf2::toMsg(in, actual); - geometry_msgs::QuaternionStamped expected; - expected.header.stamp = ros::Time(1234, 5678); + geometry_msgs::msg::QuaternionStamped expected; + expected.header.stamp = rclcpp::Time(1234, 5678); expected.header.frame_id = "test_frame"; expected.quaternion.x = 0.0; expected.quaternion.y = 0.0; @@ -276,10 +284,11 @@ TEST(Conversions, Rotation) EXPECT_NEAR(expected.quaternion.w, actual.quaternion.w, 1.0e-9); } { - tf2::Stamped in(tf2_2d::Rotation(2.0), ros::Time(1234, 5678), "test_frame"); - geometry_msgs::QuaternionStamped actual = tf2::toMsg(in); - geometry_msgs::QuaternionStamped expected; - expected.header.stamp = ros::Time(1234, 5678); + tf2::Stamped in(tf2_2d::Rotation(2.0), + tf2_ros::fromRclcpp(rclcpp::Time(1234, 5678)), "test_frame"); + geometry_msgs::msg::QuaternionStamped actual = tf2::toMsg(in); + geometry_msgs::msg::QuaternionStamped expected; + expected.header.stamp = rclcpp::Time(1234, 5678); expected.header.frame_id = "test_frame"; expected.quaternion.x = 0.0; expected.quaternion.y = 0.0; @@ -293,8 +302,8 @@ TEST(Conversions, Rotation) EXPECT_NEAR(expected.quaternion.w, actual.quaternion.w, 1.0e-9); } { - geometry_msgs::QuaternionStamped msg; - msg.header.stamp = ros::Time(1234, 5678); + geometry_msgs::msg::QuaternionStamped msg; + msg.header.stamp = rclcpp::Time(1234, 5678); msg.header.frame_id = "test_frame"; msg.quaternion.x = 0.0; msg.quaternion.y = 0.0; @@ -302,7 +311,8 @@ TEST(Conversions, Rotation) msg.quaternion.w = 0.540302305868140; tf2::Stamped actual; tf2::fromMsg(msg, actual); - tf2::Stamped expected(tf2_2d::Rotation(2.0), ros::Time(1234, 5678), "test_frame"); + tf2::Stamped expected(tf2_2d::Rotation(2.0), + tf2_ros::fromRclcpp(rclcpp::Time(1234, 5678)), "test_frame"); EXPECT_EQ(expected.stamp_, actual.stamp_); EXPECT_EQ(expected.frame_id_, actual.frame_id_); EXPECT_NEAR(expected.angle(), actual.angle(), 1.0e-9); @@ -314,9 +324,9 @@ TEST(Conversions, Transform) // Test conversion to/from the Transform type { tf2_2d::Transform in(1.0, 1.5, 2.0); - geometry_msgs::Transform actual; + geometry_msgs::msg::Transform actual; tf2::toMsg(in, actual); - geometry_msgs::Transform expected; + geometry_msgs::msg::Transform expected; expected.translation.x = 1.0; expected.translation.y = 1.5; expected.translation.z = 0; @@ -334,9 +344,9 @@ TEST(Conversions, Transform) } { tf2_2d::Transform in(1.0, 1.5, 2.0); - geometry_msgs::Pose actual; + geometry_msgs::msg::Pose actual; tf2::toMsg(in, actual); - geometry_msgs::Pose expected; + geometry_msgs::msg::Pose expected; expected.position.x = 1.0; expected.position.y = 1.5; expected.position.z = 0; @@ -354,9 +364,9 @@ TEST(Conversions, Transform) } { tf2_2d::Transform in(1.0, 1.5, 2.0); - geometry_msgs::Pose2D actual; + geometry_msgs::msg::Pose2D actual; tf2::toMsg(in, actual); - geometry_msgs::Pose2D expected; + geometry_msgs::msg::Pose2D expected; expected.x = 1.0; expected.y = 1.5; expected.theta = 2.0; @@ -366,8 +376,8 @@ TEST(Conversions, Transform) } { tf2_2d::Transform in(1.0, 1.5, 2.0); - geometry_msgs::Transform actual = tf2::toMsg(in); - geometry_msgs::Transform expected; + geometry_msgs::msg::Transform actual = tf2::toMsg(in); + geometry_msgs::msg::Transform expected; expected.translation.x = 1.0; expected.translation.y = 1.5; expected.translation.z = 0; @@ -384,7 +394,7 @@ TEST(Conversions, Transform) EXPECT_NEAR(expected.rotation.w, actual.rotation.w, 1.0e-9); } { - geometry_msgs::Transform msg; + geometry_msgs::msg::Transform msg; msg.translation.x = 1.0; msg.translation.y = 1.5; msg.translation.z = 0; @@ -400,7 +410,7 @@ TEST(Conversions, Transform) EXPECT_NEAR(expected.angle(), actual.angle(), 1.0e-9); } { - geometry_msgs::Pose msg; + geometry_msgs::msg::Pose msg; msg.position.x = 1.0; msg.position.y = 1.5; msg.position.z = 0; @@ -416,7 +426,7 @@ TEST(Conversions, Transform) EXPECT_NEAR(expected.angle(), actual.angle(), 1.0e-9); } { - geometry_msgs::Pose2D msg; + geometry_msgs::msg::Pose2D msg; msg.x = 1.0; msg.y = 1.5; msg.theta = 2.0; @@ -428,11 +438,12 @@ TEST(Conversions, Transform) EXPECT_EQ(expected.angle(), actual.angle()); } { - tf2::Stamped in(tf2_2d::Transform(1.0, 1.5, 2.0), ros::Time(1234, 5678), "test_frame"); - geometry_msgs::TransformStamped actual; + tf2::Stamped in(tf2_2d::Transform(1.0, 1.5, 2.0), + tf2_ros::fromRclcpp(rclcpp::Time(1234, 5678)), "test_frame"); + geometry_msgs::msg::TransformStamped actual; tf2::toMsg(in, actual); - geometry_msgs::TransformStamped expected; - expected.header.stamp = ros::Time(1234, 5678); + geometry_msgs::msg::TransformStamped expected; + expected.header.stamp = rclcpp::Time(1234, 5678); expected.header.frame_id = "test_frame"; expected.transform.translation.x = 1.0; expected.transform.translation.y = 1.5; @@ -452,11 +463,12 @@ TEST(Conversions, Transform) EXPECT_NEAR(expected.transform.rotation.w, actual.transform.rotation.w, 1.0e-9); } { - tf2::Stamped in(tf2_2d::Transform(1.0, 1.5, 2.0), ros::Time(1234, 5678), "test_frame"); - geometry_msgs::PoseStamped actual; + tf2::Stamped in(tf2_2d::Transform(1.0, 1.5, 2.0), + tf2_ros::fromRclcpp(rclcpp::Time(1234, 5678)), "test_frame"); + geometry_msgs::msg::PoseStamped actual; tf2::toMsg(in, actual); - geometry_msgs::PoseStamped expected; - expected.header.stamp = ros::Time(1234, 5678); + geometry_msgs::msg::PoseStamped expected; + expected.header.stamp = rclcpp::Time(1234, 5678); expected.header.frame_id = "test_frame"; expected.pose.position.x = 1.0; expected.pose.position.y = 1.5; @@ -476,10 +488,11 @@ TEST(Conversions, Transform) EXPECT_NEAR(expected.pose.orientation.w, actual.pose.orientation.w, 1.0e-9); } { - tf2::Stamped in(tf2_2d::Transform(1.0, 1.5, 2.0), ros::Time(1234, 5678), "test_frame"); - geometry_msgs::TransformStamped actual = tf2::toMsg(in); - geometry_msgs::TransformStamped expected; - expected.header.stamp = ros::Time(1234, 5678); + tf2::Stamped in(tf2_2d::Transform(1.0, 1.5, 2.0), + tf2_ros::fromRclcpp(rclcpp::Time(1234, 5678)), "test_frame"); + geometry_msgs::msg::TransformStamped actual = tf2::toMsg(in); + geometry_msgs::msg::TransformStamped expected; + expected.header.stamp = rclcpp::Time(1234, 5678); expected.header.frame_id = "test_frame"; expected.transform.translation.x = 1.0; expected.transform.translation.y = 1.5; @@ -499,8 +512,8 @@ TEST(Conversions, Transform) EXPECT_NEAR(expected.transform.rotation.w, actual.transform.rotation.w, 1.0e-9); } { - geometry_msgs::TransformStamped msg; - msg.header.stamp = ros::Time(1234, 5678); + geometry_msgs::msg::TransformStamped msg; + msg.header.stamp = rclcpp::Time(1234, 5678); msg.header.frame_id = "test_frame"; msg.transform.translation.x = 1.0; msg.transform.translation.y = 1.5; @@ -511,7 +524,8 @@ TEST(Conversions, Transform) msg.transform.rotation.w = 0.540302305868140; tf2::Stamped actual; tf2::fromMsg(msg, actual); - tf2::Stamped expected(tf2_2d::Transform(1.0, 1.5, 2.0), ros::Time(1234, 5678), "test_frame"); + tf2::Stamped expected(tf2_2d::Transform(1.0, 1.5, 2.0), + tf2_ros::fromRclcpp(rclcpp::Time(1234, 5678)), "test_frame"); EXPECT_EQ(expected.stamp_, actual.stamp_); EXPECT_EQ(expected.frame_id_, actual.frame_id_); EXPECT_NEAR(expected.x(), actual.x(), 1.0e-9); @@ -519,8 +533,8 @@ TEST(Conversions, Transform) EXPECT_NEAR(expected.angle(), actual.angle(), 1.0e-9); } { - geometry_msgs::PoseStamped msg; - msg.header.stamp = ros::Time(1234, 5678); + geometry_msgs::msg::PoseStamped msg; + msg.header.stamp = rclcpp::Time(1234, 5678); msg.header.frame_id = "test_frame"; msg.pose.position.x = 1.0; msg.pose.position.y = 1.5; @@ -531,7 +545,8 @@ TEST(Conversions, Transform) msg.pose.orientation.w = 0.540302305868140; tf2::Stamped actual; tf2::fromMsg(msg, actual); - tf2::Stamped expected(tf2_2d::Transform(1.0, 1.5, 2.0), ros::Time(1234, 5678), "test_frame"); + tf2::Stamped expected(tf2_2d::Transform(1.0, 1.5, 2.0), + tf2_ros::fromRclcpp(rclcpp::Time(1234, 5678)), "test_frame"); EXPECT_EQ(expected.stamp_, actual.stamp_); EXPECT_EQ(expected.frame_id_, actual.frame_id_); EXPECT_NEAR(expected.x(), actual.x(), 1.0e-9); @@ -544,15 +559,19 @@ TEST(Conversions, TransformCovariance) { { Eigen::Matrix3d cov_in; + // *INDENT-OFF* cov_in << 1.0, 0.2, 0.3, 0.2, 2.0, 0.4, 0.3, 0.4, 3.0; + // *INDENT-ON* tf2_2d::Transform trans(1.0, 1.5, 2.0); Eigen::Matrix3d actual = tf2::transformCovariance(cov_in, trans); Eigen::Matrix3d expected; + // *INDENT-OFF* expected << 1.978182309493392, 0.247672523481242, -0.488563021694415, 0.247672523481242, 1.021817690506609, 0.106330493428848, -0.488563021694415, 0.106330493428848, 3.000000000000000; + // *INDENT-ON* EXPECT_NEAR(expected(0, 0), actual(0, 0), 1.0e-9); EXPECT_NEAR(expected(0, 1), actual(0, 1), 1.0e-9); EXPECT_NEAR(expected(0, 2), actual(0, 2), 1.0e-9); @@ -564,14 +583,18 @@ TEST(Conversions, TransformCovariance) EXPECT_NEAR(expected(2, 2), actual(2, 2), 1.0e-9); } { + // *INDENT-OFF* std::array cov_in{1.0, 0.2, 0.3, // NOLINT(whitespace/braces) 0.2, 2.0, 0.4, 0.3, 0.4, 3.0}; // NOLINT(whitespace/braces) + // *INDENT-ON* tf2_2d::Transform trans(1.0, 1.5, 2.0); std::array actual = tf2::transformCovariance(cov_in, trans); + // *INDENT-OFF* std::array expected{1.978182309493392, 0.247672523481242, -0.488563021694415, // NOLINT 0.247672523481242, 1.021817690506609, 0.106330493428848, -0.488563021694415, 0.106330493428848, 3.000000000000000}; // NOLINT + // *INDENT-ON* EXPECT_NEAR(expected[0], actual[0], 1.0e-9); EXPECT_NEAR(expected[1], actual[1], 1.0e-9); EXPECT_NEAR(expected[2], actual[2], 1.0e-9); @@ -583,14 +606,18 @@ TEST(Conversions, TransformCovariance) EXPECT_NEAR(expected[8], actual[8], 1.0e-9); } { + // *INDENT-OFF* boost::array cov_in{1.0, 0.2, 0.3, // NOLINT(whitespace/braces) 0.2, 2.0, 0.4, 0.3, 0.4, 3.0}; // NOLINT(whitespace/braces) + // *INDENT-ON* tf2_2d::Transform trans(1.0, 1.5, 2.0); boost::array actual = tf2::transformCovariance(cov_in, trans); + // *INDENT-OFF* boost::array expected{1.978182309493392, 0.247672523481242, -0.488563021694415, // NOLINT 0.247672523481242, 1.021817690506609, 0.106330493428848, -0.488563021694415, 0.106330493428848, 3.000000000000000}; // NOLINT + // *INDENT-ON* EXPECT_NEAR(expected[0], actual[0], 1.0e-9); EXPECT_NEAR(expected[1], actual[1], 1.0e-9); EXPECT_NEAR(expected[2], actual[2], 1.0e-9); @@ -602,8 +629,3 @@ TEST(Conversions, TransformCovariance) EXPECT_NEAR(expected[8], actual[8], 1.0e-9); } } -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/test/test_rotation.cpp b/test/test_rotation.cpp index 71df2ce..a21b8bf 100644 --- a/test/test_rotation.cpp +++ b/test/test_rotation.cpp @@ -31,12 +31,12 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#include -#include - #include #include +#include +#include + TEST(Rotation, Constructor) { @@ -407,9 +407,3 @@ TEST(Rotation, Stream) std::cout << r << std::endl; SUCCEED(); } - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/test/test_transform.cpp b/test/test_transform.cpp index 1aff1e7..0908937 100644 --- a/test/test_transform.cpp +++ b/test/test_transform.cpp @@ -31,17 +31,16 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ +#include +#include #include #include #include -#include -#include -#include - -#include -#include #include +#include +#include +#include TEST(Transform, Constructor) @@ -272,9 +271,3 @@ TEST(Transform, Stream) std::cout << t << std::endl; SUCCEED(); } - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/test/test_vector2.cpp b/test/test_vector2.cpp index cee0b3e..5b445db 100644 --- a/test/test_vector2.cpp +++ b/test/test_vector2.cpp @@ -31,11 +31,11 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#include - #include #include +#include + TEST(Vector2, Constructor) { @@ -606,9 +606,3 @@ TEST(Vector2, Stream) std::cout << v << std::endl; SUCCEED(); } - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -}