From b1f807edc3e111438f3983d912b5443f58b66367 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 26 Jan 2024 19:24:18 +0100 Subject: [PATCH 01/12] Enable Twist interpolator MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- tf2/include/tf2/buffer_core.h | 15 ++++++ tf2/include/tf2/convert.h | 6 +++ tf2/src/buffer_core.cpp | 52 +++++++++++++++++++ .../tf2_geometry_msgs/tf2_geometry_msgs.hpp | 46 ++++++++++++++++ .../test/test_tf2_geometry_msgs.cpp | 14 +++++ tf2_ros/include/tf2_ros/buffer_interface.h | 15 ++++++ 6 files changed, 148 insertions(+) diff --git a/tf2/include/tf2/buffer_core.h b/tf2/include/tf2/buffer_core.h index bc62c2dd1..fe95f91d4 100644 --- a/tf2/include/tf2/buffer_core.h +++ b/tf2/include/tf2/buffer_core.h @@ -46,6 +46,7 @@ #include "LinearMath/Transform.h" #include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/velocity_stamped.hpp" #include "tf2/buffer_core_interface.h" #include "tf2/exceptions.h" #include "tf2/transform_storage.h" @@ -156,6 +157,20 @@ class BufferCore : public BufferCoreInterface const std::string & source_frame, const TimePoint & source_time, const std::string & fixed_frame) const override; + /** \brief Lookup the velocity of the moving_frame in the reference_frame + * \param reference_frame The frame in which to track + * \param moving_frame The frame to track + * \param time The time at which to get the velocity + * \param duration The period over which to average + * \param velocity The velocity output + * + * Possible exceptions TransformReference::LookupException, TransformReference::ConnectivityException, + * TransformReference::MaxDepthException + */ + geometry_msgs::msg::VelocityStamped lookupVelocity( + const std::string & reference_frame, const std::string & moving_frame, + const TimePoint & time, const tf2::Duration & duration) const; + /** \brief Test if a transform is possible * \param target_frame The frame into which to transform * \param source_frame The frame from which to transform diff --git a/tf2/include/tf2/convert.h b/tf2/include/tf2/convert.h index f81a3c65d..85ffab5d7 100644 --- a/tf2/include/tf2/convert.h +++ b/tf2/include/tf2/convert.h @@ -59,6 +59,12 @@ void doTransform( const T & data_in, T & data_out, const geometry_msgs::msg::TransformStamped & transform); +template +void doTransform( + const T & data_in, T & data_out, + const geometry_msgs::msg::TransformStamped & transform, + double duration); + /**\brief Get the timestamp from data * \param[in] t The data input. * \return The timestamp associated with the data. diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index f0c61b3ce..d26cabab0 100644 --- a/tf2/src/buffer_core.cpp +++ b/tf2/src/buffer_core.cpp @@ -38,6 +38,8 @@ #include #include +#include + #include "tf2/buffer_core.h" #include "tf2/time_cache.h" #include "tf2/exceptions.h" @@ -572,6 +574,56 @@ struct TransformAccum tf2::Vector3 result_vec; }; +geometry_msgs::msg::VelocityStamped BufferCore::lookupVelocity( + const std::string & reference_frame, const std::string & moving_frame, + const TimePoint & time, const tf2::Duration & duration) const +{ + tf2::TimePoint latest_time; + // TODO(anyone): This is incorrect, but better than nothing. Really we want the latest time for + // any of the frames + getLatestCommonTime( + lookupFrameNumber(reference_frame), + lookupFrameNumber(moving_frame), + latest_time, + 0); + + auto time_seconds = tf2::timeToSec(time); + auto duration_seconds = std::chrono::duration(duration).count(); + + auto end_time = std::min(time_seconds + duration_seconds * 0.5 , tf2::timeToSec(latest_time)); + std::cout << end_time <<" latest " << tf2::timeToSec(latest_time) << " input " << time_seconds << std::endl; + auto start_time = end_time - duration_seconds; + std::cout << start_time << " start time" << std::endl; + + tf2::Transform start, end; + TimePoint time_out; + lookupTransformImpl(moving_frame, reference_frame, tf2::timeFromSec(start_time), start, time_out); + lookupTransformImpl(moving_frame, reference_frame, tf2::timeFromSec(end_time), end, time_out); + + auto temp = start.getBasis().inverse() * end.getBasis(); + tf2::Quaternion quat_temp; + temp.getRotation(quat_temp); + auto o = start.getBasis() * quat_temp.getAxis(); + auto ang = quat_temp.getAngle(); + + std::chrono::nanoseconds ns = std::chrono::duration_cast( + tf2::timeFromSec(start_time + duration_seconds * 0.5).time_since_epoch()); + std::chrono::seconds s = std::chrono::duration_cast( + tf2::timeFromSec(start_time + duration_seconds * 0.5).time_since_epoch()); + geometry_msgs::msg::VelocityStamped velocity; + velocity.header.stamp.sec = static_cast(s.count()); + velocity.header.stamp.nanosec = static_cast(ns.count() % 1000000000ull); + velocity.header.frame_id = reference_frame; + velocity.body_frame_id = moving_frame; + velocity.velocity.linear.x = (end.getOrigin().getX() - start.getOrigin().getX()) / duration_seconds; + velocity.velocity.linear.y = (end.getOrigin().getY() - start.getOrigin().getY()) / duration_seconds; + velocity.velocity.linear.z = (end.getOrigin().getZ() - start.getOrigin().getZ()) / duration_seconds; + velocity.velocity.angular.x = o.x() * ang / duration_seconds; + velocity.velocity.angular.y = o.y() * ang / duration_seconds; + velocity.velocity.angular.z = o.z() * ang / duration_seconds; + return velocity; +} + geometry_msgs::msg::TransformStamped BufferCore::lookupTransform( const std::string & target_frame, const std::string & source_frame, diff --git a/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp b/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp index 48c36b4a6..4bf34cba2 100644 --- a/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp +++ b/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp @@ -48,6 +48,7 @@ #include "geometry_msgs/msg/pose_with_covariance.hpp" #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" #include "geometry_msgs/msg/polygon_stamped.hpp" +#include "geometry_msgs/msg/velocity_stamped.hpp" #include "geometry_msgs/msg/wrench.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" #include "kdl/frames.hpp" @@ -1313,6 +1314,51 @@ void fromMsg(const geometry_msgs::msg::Pose & in, tf2::Transform & out) tf2::Quaternion(in.orientation.x, in.orientation.y, in.orientation.z, in.orientation.w)); } +/*********************/ +/** VelocityStamped **/ +/*********************/ + +/** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs VelocityStamped type. + * This function is a specialization of the doTransform template defined in tf2/convert.h. + * \param t_in The point to transform, as a VelocityStamped message. + * \param t_out The transformed point, as a VelocityStamped message. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template<> +inline +void doTransform( + const geometry_msgs::msg::VelocityStamped & t_in, + geometry_msgs::msg::VelocityStamped & t_out, + const geometry_msgs::msg::TransformStamped & transform, + double duration) +{ +} + +// /** \brief Convert a tf2 Vector3 type to its equivalent geometry_msgs representation. +// * This function is a specialization of the toMsg template defined in tf2/convert.h. +// * \param in A tf2 Vector3 object. +// * \return The Vector3 converted to a geometry_msgs message type. +// */ +// inline +// geometry_msgs::msg::Point32 & toMsg(const tf2::Vector3 & in, geometry_msgs::msg::Point32 & out) +// { +// out.x = static_cast(in.getX()); +// out.y = static_cast(in.getY()); +// out.z = static_cast(in.getZ()); +// return out; +// } + +// /** \brief Convert a Vector3 message to its equivalent tf2 representation. +// * This function is a specialization of the fromMsg template defined in tf2/convert.h. +// * \param in A Vector3 message type. +// * \param out The Vector3 converted to a tf2 type. +// */ +// inline +// void fromMsg(const geometry_msgs::msg::Point32 & in, tf2::Vector3 & out) +// { +// out = tf2::Vector3(in.x, in.y, in.z); +// } + /**********************/ /*** WrenchStamped ****/ /**********************/ diff --git a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp index 4d699b265..3f6d520cc 100644 --- a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp +++ b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp @@ -46,6 +46,8 @@ #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" +#include + std::unique_ptr tf_buffer = nullptr; static const double EPS = 1e-3; @@ -625,6 +627,18 @@ TEST(TfGeometry, Wrench) EXPECT_NEAR(res.torque.z, 5, EPS); } +TEST(TfGeometry, Velocity) +{ + geometry_msgs::msg::VelocityStamped v1, res; + v1.header.frame_id = "world"; + v1.body_frame_id = "base_link"; + + + geometry_msgs::msg::TransformStamped trafo; + + tf2::doTransform(v1, res, trafo, 0.1); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/tf2_ros/include/tf2_ros/buffer_interface.h b/tf2_ros/include/tf2_ros/buffer_interface.h index 4f5e4c31f..5ae160fd2 100644 --- a/tf2_ros/include/tf2_ros/buffer_interface.h +++ b/tf2_ros/include/tf2_ros/buffer_interface.h @@ -216,6 +216,21 @@ class BufferInterface return out; } + template + T & transform( + const T & in, T & out, + const std::string & target_frame, + double duration, + tf2::Duration timeout = tf2::durationFromSec(0.0)) const + { + // do the transform + tf2::doTransform( + in, out, + lookupVelocity(target_frame, tf2::getFrameId(in), tf2::getTimestamp(in), timeout), + duration); + return out; + } + /** \brief Transform an input into the target frame. * This function is templated and can take as input any valid mathematical object that tf knows * how to apply a transform to, by way of the templated math conversions interface. From 9cf307e9325fa104a841f2fd0c113f03b1467031 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 8 Feb 2024 23:22:00 +0100 Subject: [PATCH 02/12] added velocity tests MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- tf2/include/tf2/buffer_core.h | 8 +- tf2/src/buffer_core.cpp | 125 ++++++-- tf2_ros/CMakeLists.txt | 8 + tf2_ros/test/test_buffer.cpp | 95 ++++++ tf2_ros/test/velocity_test.cpp | 529 +++++++++++++++++++++++++++++++++ 5 files changed, 744 insertions(+), 21 deletions(-) create mode 100644 tf2_ros/test/velocity_test.cpp diff --git a/tf2/include/tf2/buffer_core.h b/tf2/include/tf2/buffer_core.h index fe95f91d4..58d62f58e 100644 --- a/tf2/include/tf2/buffer_core.h +++ b/tf2/include/tf2/buffer_core.h @@ -157,6 +157,10 @@ class BufferCore : public BufferCoreInterface const std::string & source_frame, const TimePoint & source_time, const std::string & fixed_frame) const override; + geometry_msgs::msg::VelocityStamped lookupVelocity( + const std::string& tracking_frame, const std::string& observation_frame, + const TimePoint & time, const tf2::Duration & averaging_interval) const; + /** \brief Lookup the velocity of the moving_frame in the reference_frame * \param reference_frame The frame in which to track * \param moving_frame The frame to track @@ -168,7 +172,9 @@ class BufferCore : public BufferCoreInterface * TransformReference::MaxDepthException */ geometry_msgs::msg::VelocityStamped lookupVelocity( - const std::string & reference_frame, const std::string & moving_frame, + const std::string & tracking_frame, const std::string & observation_frame, + const std::string & reference_frame, const tf2::Vector3 & reference_point, + const std::string & reference_point_frame, const TimePoint & time, const tf2::Duration & duration) const; /** \brief Test if a transform is possible diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index d26cabab0..d95c6d1d2 100644 --- a/tf2/src/buffer_core.cpp +++ b/tf2/src/buffer_core.cpp @@ -575,30 +575,62 @@ struct TransformAccum }; geometry_msgs::msg::VelocityStamped BufferCore::lookupVelocity( - const std::string & reference_frame, const std::string & moving_frame, - const TimePoint & time, const tf2::Duration & duration) const + const std::string& tracking_frame, const std::string& observation_frame, + const TimePoint & time, const tf2::Duration & averaging_interval) const +{ + // ref point is origin of tracking_frame, ref_frame = obs_frame + return lookupVelocity(tracking_frame, observation_frame, observation_frame, tf2::Vector3(0,0,0), tracking_frame, time, averaging_interval); +} + +geometry_msgs::msg::VelocityStamped BufferCore::lookupVelocity( + const std::string & tracking_frame, const std::string & observation_frame, + const std::string & reference_frame, const tf2::Vector3 & reference_point, + const std::string & reference_point_frame, + const TimePoint & time, const tf2::Duration & averaging_interval) const { tf2::TimePoint latest_time; // TODO(anyone): This is incorrect, but better than nothing. Really we want the latest time for // any of the frames getLatestCommonTime( - lookupFrameNumber(reference_frame), - lookupFrameNumber(moving_frame), + lookupFrameNumber(observation_frame), + lookupFrameNumber(tracking_frame), latest_time, 0); auto time_seconds = tf2::timeToSec(time); - auto duration_seconds = std::chrono::duration(duration).count(); + auto averaging_interval_seconds = std::chrono::duration(averaging_interval).count(); + + auto end_time = std::min(time_seconds + averaging_interval_seconds * 0.5 , tf2::timeToSec(latest_time)); + + // std::cout << std::fixed << end_time <<" latest " << tf2::timeToSec(latest_time) << " input " << time_seconds << std::endl; + // auto start_time = end_time - averaging_interval_seconds; + auto start_time = std::max(0.00001 + averaging_interval_seconds, end_time) - averaging_interval_seconds; + auto corrected_averaging_interval = end_time - start_time; //correct for the possiblity that start time was truncated above. - auto end_time = std::min(time_seconds + duration_seconds * 0.5 , tf2::timeToSec(latest_time)); - std::cout << end_time <<" latest " << tf2::timeToSec(latest_time) << " input " << time_seconds << std::endl; - auto start_time = end_time - duration_seconds; - std::cout << start_time << " start time" << std::endl; + // std::cout << start_time << " start time" << std::endl; tf2::Transform start, end; TimePoint time_out; - lookupTransformImpl(moving_frame, reference_frame, tf2::timeFromSec(start_time), start, time_out); - lookupTransformImpl(moving_frame, reference_frame, tf2::timeFromSec(end_time), end, time_out); + lookupTransformImpl(observation_frame, tracking_frame, tf2::timeFromSec(start_time), start, time_out); + lookupTransformImpl(observation_frame, tracking_frame, tf2::timeFromSec(end_time), end, time_out); + + // std::cout << "start.getOrigin().getX() " << start.getOrigin().getX() << std::endl; + // std::cout << "start.getOrigin().getY() " << start.getOrigin().getY() << std::endl; + // std::cout << "start.getOrigin().getZ() " << start.getOrigin().getZ() << std::endl; + + // std::cout << "start.getRotation().getX() " << start.getRotation().x() << std::endl; + // std::cout << "start.getRotation().getY() " << start.getRotation().y() << std::endl; + // std::cout << "start.getRotation().getZ() " << start.getRotation().z() << std::endl; + // std::cout << "start.getRotation().getZ() " << start.getRotation().w() << std::endl; + + // std::cout << "end.getOrigin().getX() " << end.getOrigin().getX() << std::endl; + // std::cout << "end.getOrigin().getY() " << end.getOrigin().getY() << std::endl; + // std::cout << "end.getOrigin().getZ() " << end.getOrigin().getZ() << std::endl; + + // std::cout << "end.getRotation().getX() " << end.getRotation().x() << std::endl; + // std::cout << "end.getRotation().getY() " << end.getRotation().y() << std::endl; + // std::cout << "end.getRotation().getZ() " << end.getRotation().z() << std::endl; + // std::cout << "end.getRotation().getZ() " << end.getRotation().w() << std::endl; auto temp = start.getBasis().inverse() * end.getBasis(); tf2::Quaternion quat_temp; @@ -606,21 +638,74 @@ geometry_msgs::msg::VelocityStamped BufferCore::lookupVelocity( auto o = start.getBasis() * quat_temp.getAxis(); auto ang = quat_temp.getAngle(); + double delta_x = end.getOrigin().getX() - start.getOrigin().getX(); + double delta_y = end.getOrigin().getY() - start.getOrigin().getY(); + double delta_z = end.getOrigin().getZ() - start.getOrigin().getZ(); + + tf2::Vector3 twist_vel ((delta_x)/corrected_averaging_interval, + (delta_y)/corrected_averaging_interval, + (delta_z)/corrected_averaging_interval); + tf2::Vector3 twist_rot = o * (ang / corrected_averaging_interval); + + // correct for the position of the reference frame + tf2::Transform inverse; + lookupTransformImpl(reference_frame, tracking_frame, tf2::timeFromSec(time_seconds), inverse, time_out); + tf2::Vector3 out_rot = inverse.getBasis() * twist_rot; + tf2::Vector3 out_vel = inverse.getBasis()* twist_vel + inverse.getOrigin().cross(out_rot); + + auto transform_point = [this]( + const std::string & target_frame, + const std::string & source_frame, + const tf2::Vector3 & point_in, + double time_transform) + { + // transform point + tf2::Transform transform; + tf2::TimePoint time_out; + lookupTransformImpl( + target_frame, source_frame, tf2::timeFromSec(time_transform), transform, time_out); + + tf2::Vector3 out; + out = transform * point_in; + return out; + }; + + // Rereference the twist about a new reference point + // Start by computing the original reference point in the reference frame: + // tf2::Stamped rp_orig(tf::Point(0,0,0), target_time, tracking_frame); + tf2::Vector3 p = tf2::Vector3(0,0,0); + tf2::Vector3 rp_orig = transform_point( + reference_frame, tracking_frame, p, time_seconds); + + tf2::Vector3 rp_desired = transform_point( + reference_frame, reference_point_frame, reference_point, time_seconds); + + tf2::Vector3 delta = rp_desired - rp_orig; + out_vel = out_vel + out_rot * delta; + + // std::cout << "out_vel " << out_vel.x() << std::endl; + // std::cout << "out_vel " << out_vel.y() << std::endl; + // std::cout << "out_vel " << out_vel.z() << std::endl; + + // std::cout << "out_angular " << out_rot.x() << std::endl; + // std::cout << "out_angular " << out_rot.y() << std::endl; + // std::cout << "out_angular " << out_rot.z() << std::endl; + std::chrono::nanoseconds ns = std::chrono::duration_cast( - tf2::timeFromSec(start_time + duration_seconds * 0.5).time_since_epoch()); + tf2::timeFromSec(start_time + averaging_interval_seconds * 0.5).time_since_epoch()); std::chrono::seconds s = std::chrono::duration_cast( - tf2::timeFromSec(start_time + duration_seconds * 0.5).time_since_epoch()); + tf2::timeFromSec(start_time + averaging_interval_seconds * 0.5).time_since_epoch()); geometry_msgs::msg::VelocityStamped velocity; velocity.header.stamp.sec = static_cast(s.count()); velocity.header.stamp.nanosec = static_cast(ns.count() % 1000000000ull); velocity.header.frame_id = reference_frame; - velocity.body_frame_id = moving_frame; - velocity.velocity.linear.x = (end.getOrigin().getX() - start.getOrigin().getX()) / duration_seconds; - velocity.velocity.linear.y = (end.getOrigin().getY() - start.getOrigin().getY()) / duration_seconds; - velocity.velocity.linear.z = (end.getOrigin().getZ() - start.getOrigin().getZ()) / duration_seconds; - velocity.velocity.angular.x = o.x() * ang / duration_seconds; - velocity.velocity.angular.y = o.y() * ang / duration_seconds; - velocity.velocity.angular.z = o.z() * ang / duration_seconds; + velocity.body_frame_id = tracking_frame; + velocity.velocity.linear.x = (end.getOrigin().getX() - start.getOrigin().getX()) / averaging_interval_seconds; + velocity.velocity.linear.y = (end.getOrigin().getY() - start.getOrigin().getY()) / averaging_interval_seconds; + velocity.velocity.linear.z = (end.getOrigin().getZ() - start.getOrigin().getZ()) / averaging_interval_seconds; + velocity.velocity.angular.x = o.x() * ang / averaging_interval_seconds; + velocity.velocity.angular.y = o.y() * ang / averaging_interval_seconds; + velocity.velocity.angular.z = o.z() * ang / averaging_interval_seconds; return velocity; } diff --git a/tf2_ros/CMakeLists.txt b/tf2_ros/CMakeLists.txt index 26d7d9f10..98aa08115 100644 --- a/tf2_ros/CMakeLists.txt +++ b/tf2_ros/CMakeLists.txt @@ -203,6 +203,14 @@ if(BUILD_TESTING) # rclcpp::rclcpp ) + ament_add_gtest(${PROJECT_NAME}_test_velocity test/velocity_test.cpp) + target_link_libraries(${PROJECT_NAME}_test_velocity + ${PROJECT_NAME} + # Used, but not linked to test tf2_ros's exports: + # ${builtin_interfaces_TARGETS} + # rclcpp::rclcpp + ) + ament_add_gtest(${PROJECT_NAME}_test_listener test/listener_unittest.cpp) target_link_libraries(${PROJECT_NAME}_test_listener ${PROJECT_NAME} diff --git a/tf2_ros/test/test_buffer.cpp b/tf2_ros/test/test_buffer.cpp index c50e32a30..a64b61adf 100644 --- a/tf2_ros/test/test_buffer.cpp +++ b/tf2_ros/test/test_buffer.cpp @@ -184,6 +184,101 @@ TEST(test_buffer, can_transform_valid_transform) EXPECT_DOUBLE_EQ(transform.transform.translation.z, output_rclcpp.transform.translation.z); } +TEST(test_buffer, velocity_transform) +{ + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + tf2_ros::Buffer buffer(clock); + // Silence error about dedicated thread's being necessary + buffer.setUsingDedicatedThread(true); + + rclcpp::Time rclcpp_time = clock->now(); + tf2::TimePoint tf2_time(std::chrono::nanoseconds(rclcpp_time.nanoseconds())); + + geometry_msgs::msg::TransformStamped transform; + transform.header.frame_id = "foo"; + transform.header.stamp = builtin_interfaces::msg::Time(rclcpp_time - rclcpp::Duration(0, 1e+9)); + transform.child_frame_id = "bar"; + transform.transform.translation.x = 0; + transform.transform.translation.y = 0; + transform.transform.translation.z = 0.0; + transform.transform.rotation.w = 1.0; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; + + EXPECT_TRUE(buffer.setTransform(transform, "unittest")); + + transform.header.frame_id = "foo"; + transform.header.stamp = builtin_interfaces::msg::Time(rclcpp_time + rclcpp::Duration(0, 1e+9)); + transform.child_frame_id = "bar"; + transform.transform.translation.x = 2.0; + transform.transform.translation.y = 0; + transform.transform.translation.z = 0.0; + transform.transform.rotation.w = 1.0; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; + + EXPECT_TRUE(buffer.setTransform(transform, "unittest")); + + EXPECT_TRUE(buffer.canTransform("bar", "foo", tf2_time)); + EXPECT_TRUE(buffer.canTransform("bar", "foo", rclcpp_time)); + + geometry_msgs::msg::VelocityStamped output = + buffer.lookupVelocity("bar", "foo", tf2_time, tf2::durationFromSec(0.1)); + + output = + buffer.lookupVelocity( + "bar", "foo", + "bar", {0, 0, 0}, "bar", + tf2_time, tf2::durationFromSec(0.1)); +} + + +TEST(test_buffer, test_twist) +{ + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + tf2_ros::Buffer buffer(clock); + // Silence error about dedicated thread's being necessary + buffer.setUsingDedicatedThread(true); + + rclcpp::Time rclcpp_time = clock->now(); + tf2::TimePoint tf2_time(std::chrono::nanoseconds(rclcpp_time.nanoseconds())); + + std::cout << "seconds " << rclcpp_time.seconds() << std::endl; + + float vel = 0.3; + for (int i = -10; i < 5; ++i) + { + geometry_msgs::msg::TransformStamped transform; + transform.header.frame_id = "PARENT"; + if (i < 0) + { + transform.header.stamp = builtin_interfaces::msg::Time(rclcpp_time - rclcpp::Duration(std::fabs(i), 0)); + std::cout << "seconds " << (rclcpp_time - rclcpp::Duration(std::fabs(i), 0)).seconds() << std::endl; + + } else { + transform.header.stamp = builtin_interfaces::msg::Time(rclcpp_time + rclcpp::Duration(i, 0)); + std::cout << "seconds " << (rclcpp_time + rclcpp::Duration(i, 0)).seconds() << std::endl; + } + transform.child_frame_id = "THISFRAME"; + transform.transform.translation.x = i * vel; + transform.transform.translation.y = 0; + transform.transform.translation.z = 0.0; + transform.transform.rotation.w = 1.0; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; + EXPECT_TRUE(buffer.setTransform(transform, "unittest")); + } + + auto tw0 = buffer.lookupVelocity("THISFRAME", "PARENT", tf2_time, tf2::durationFromSec(4.001)); + std::cout << "tw0 : " << tw0.velocity.linear.x << std::endl; + + auto tw1 = buffer.lookupVelocity("THISFRAME", "PARENT", "PARENT", {0, 0, 0}, "THISFRAME", tf2_time, tf2::durationFromSec(4.001)); + std::cout << "tw1 : " << tw1.velocity.linear.x << std::endl; +} + TEST(test_buffer, can_transform_without_dedicated_thread) { rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); diff --git a/tf2_ros/test/velocity_test.cpp b/tf2_ros/test/velocity_test.cpp new file mode 100644 index 000000000..1ef0ffc10 --- /dev/null +++ b/tf2_ros/test/velocity_test.cpp @@ -0,0 +1,529 @@ +/* + * Copyright (c) 2024, Open Source Robotics Foundation, Inc. + * 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 Willow Garage, Inc. 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 OWNER 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. + */ + +#include + +#include +#include + +#include "tf2_ros/buffer.h" +#include "tf2_ros/create_timer_interface.h" +#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/transform_listener.h" + +// #include "tf/LinearMath/Vector3.h" + +// The fixture for testing class Foo. +class LinearVelocitySquareTest : public ::testing::Test +{ +protected: + // You can remove any or all of the following functions if its body + // is empty. + + LinearVelocitySquareTest() + { + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_STEADY_TIME); + buffer_ = std::make_shared(clock); + + rclcpp::Time rclcpp_time = clock->now(); + tf2_time_ = tf2::TimePoint(std::chrono::nanoseconds(rclcpp_time.nanoseconds())); + + double x = -.1; + double y = 0; + double z = 0; + for (double t = 0.1; t <= 6; t += 0.1) { + if (t < 1) { + x += .1; + } else if (t < 2) { + y += .1; + } else if (t < 3) { + x -= .1; + } else if (t < 4) { + y -= .1; + } else if (t < 5) { + z += .1; + } else { + z -= .1; + } + + { + geometry_msgs::msg::TransformStamped transform; + transform.transform.rotation.w = 1.0; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; + transform.transform.translation.x = x; + transform.transform.translation.y = y; + transform.transform.translation.z = z; + transform.header.frame_id = "foo"; + transform.child_frame_id = "bar"; + transform.header.stamp = + builtin_interfaces::msg::Time(rclcpp_time + rclcpp::Duration::from_seconds(t)); + buffer_->setTransform(transform, "unittest"); + } + + { + geometry_msgs::msg::TransformStamped transform; + transform.transform.rotation.w = 1.0; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; + transform.transform.translation.x = 1; + transform.transform.translation.y = 0; + transform.transform.translation.z = 0; + transform.header.frame_id = "foo"; + transform.child_frame_id = "stationary_offset_child"; + transform.header.stamp = + builtin_interfaces::msg::Time(rclcpp_time + rclcpp::Duration::from_seconds(t)); + buffer_->setTransform(transform, "unittest"); + } + + { + geometry_msgs::msg::TransformStamped transform; + transform.transform.rotation.w = 1.0; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; + transform.transform.translation.x = 0; + transform.transform.translation.y = 0; + transform.transform.translation.z = 1; + transform.header.frame_id = "stationary_offset_parent"; + transform.child_frame_id = "foo"; + transform.header.stamp = + builtin_interfaces::msg::Time(rclcpp_time + rclcpp::Duration::from_seconds(t)); + buffer_->setTransform(transform, "unittest"); + } + } + + // You can do set-up work for each test here. + } + + virtual ~LinearVelocitySquareTest() + { + // You can do clean-up work that doesn't throw exceptions here. + } + + // If the constructor and destructor are not enough for setting up + // and cleaning up each test, you can define the following methods: + + virtual void SetUp() + { + // Code here will be called immediately after the constructor (right + // before each test). + } + + virtual void TearDown() + { + // Code here will be called immediately after each test (right + // before the destructor). + } + + // Objects declared here can be used by all tests in the test case for LinearVelocity. + std::shared_ptr buffer_; + tf2::TimePoint tf2_time_; +}; + +// The fixture for testing class Foo. +class AngularVelocitySquareTest : public ::testing::Test +{ +protected: + // You can remove any or all of the following functions if its body + // is empty. + + AngularVelocitySquareTest() + { + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_STEADY_TIME); + buffer_ = std::make_shared(clock); + + rclcpp::Time rclcpp_time = clock->now(); + tf2_time_ = tf2::TimePoint(std::chrono::nanoseconds(rclcpp_time.nanoseconds())); + + double x = -.1; + double y = 0; + double z = 0; + for (double t = 0.1; t < 6; t += 0.1) + { + if (t < 1) x += .1; + else if (t < 2) x -= .1; + else if (t < 3) y += .1; + else if (t < 4) y -= .1; + else if (t < 5) z += .1; + else z -= .1; + + tf2::Quaternion quat; + quat.setRPY(x, y, z); + + { + geometry_msgs::msg::TransformStamped transform; + transform.transform.rotation.w = quat.w(); + transform.transform.rotation.x = quat.x(); + transform.transform.rotation.y = quat.y(); + transform.transform.rotation.z = quat.z(); + transform.transform.translation.x = 0.0; + transform.transform.translation.y = 0.0; + transform.transform.translation.z = 0.0; + transform.header.frame_id = "foo"; + transform.child_frame_id = "bar"; + transform.header.stamp = + builtin_interfaces::msg::Time(rclcpp_time + rclcpp::Duration::from_seconds(t)); + buffer_->setTransform(transform, "unittest"); + } + + { + geometry_msgs::msg::TransformStamped transform; + transform.transform.rotation.w = quat.w(); + transform.transform.rotation.x = quat.x(); + transform.transform.rotation.y = quat.y(); + transform.transform.rotation.z = quat.z(); + transform.transform.translation.x = 1.0; + transform.transform.translation.y = 0.0; + transform.transform.translation.z = 0.0; + transform.header.frame_id = "foo"; + transform.child_frame_id = "stationary_offset_child"; + transform.header.stamp = + builtin_interfaces::msg::Time(rclcpp_time + rclcpp::Duration::from_seconds(t)); + buffer_->setTransform(transform, "unittest"); + } + + { + geometry_msgs::msg::TransformStamped transform; + transform.transform.rotation.w = quat.w(); + transform.transform.rotation.x = quat.x(); + transform.transform.rotation.y = quat.y(); + transform.transform.rotation.z = quat.z(); + transform.transform.translation.x = 0.0; + transform.transform.translation.y = 0.0; + transform.transform.translation.z = -1.0; + transform.header.frame_id = "stationary_offset_parent"; + transform.child_frame_id = "foo"; + transform.header.stamp = + builtin_interfaces::msg::Time(rclcpp_time + rclcpp::Duration::from_seconds(t)); + buffer_->setTransform(transform, "unittest"); + } + } + + // You can do set-up work for each test here. + } + + virtual ~AngularVelocitySquareTest() + { + // You can do clean-up work that doesn't throw exceptions here. + } + + // If the constructor and destructor are not enough for setting up + // and cleaning up each test, you can define the following methods: + + virtual void SetUp() + { + // Code here will be called immediately after the constructor (right + // before each test). + } + + virtual void TearDown() + { + // Code here will be called immediately after each test (right + // before the destructor). + } + + // Objects declared here can be used by all tests in the test case for AngularVelocity. + std::shared_ptr buffer_; + tf2::TimePoint tf2_time_; +}; + +TEST_F(LinearVelocitySquareTest, LinearVelocityToThreeFrames) +{ + std::vector offset_frames; + + offset_frames.push_back("foo"); + offset_frames.push_back("stationary_offset_child"); + offset_frames.push_back("stationary_offset_parent"); + + for (std::vector::iterator it = offset_frames.begin(); it != offset_frames.end(); ++it) + { + try + { + tf2::TimePoint check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 0.5); + + auto twist = buffer_->lookupVelocity( + "bar", *it, check_time, tf2::durationFromSec(0.1)); + EXPECT_FLOAT_EQ(twist.velocity.linear.x, 1.0); + EXPECT_FLOAT_EQ(twist.velocity.linear.y, 0.0); + EXPECT_FLOAT_EQ(twist.velocity.linear.z, 0.0); + EXPECT_FLOAT_EQ(twist.velocity.angular.x, 0.0); + EXPECT_FLOAT_EQ(twist.velocity.angular.y, 0.0); + EXPECT_FLOAT_EQ(twist.velocity.angular.z, 0.0); + + check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 1.5); + twist = buffer_->lookupVelocity("bar", *it, check_time, tf2::durationFromSec(0.1)); + EXPECT_FLOAT_EQ(twist.velocity.linear.x, 0.0); + EXPECT_FLOAT_EQ(twist.velocity.linear.y, 1.0); + EXPECT_FLOAT_EQ(twist.velocity.linear.z, 0.0); + EXPECT_FLOAT_EQ(twist.velocity.angular.x, 0.0); + EXPECT_FLOAT_EQ(twist.velocity.angular.y, 0.0); + EXPECT_FLOAT_EQ(twist.velocity.angular.z, 0.0); + + check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 2.5); + twist = buffer_->lookupVelocity("bar", *it, check_time, tf2::durationFromSec(0.1)); + EXPECT_FLOAT_EQ(twist.velocity.linear.x, -1.0); + EXPECT_FLOAT_EQ(twist.velocity.linear.y, 0.0); + EXPECT_FLOAT_EQ(twist.velocity.linear.z, 0.0); + EXPECT_FLOAT_EQ(twist.velocity.angular.x, 0.0); + EXPECT_FLOAT_EQ(twist.velocity.angular.y, 0.0); + EXPECT_FLOAT_EQ(twist.velocity.angular.z, 0.0); + + check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 3.5); + twist = buffer_->lookupVelocity("bar", *it, check_time, tf2::durationFromSec(0.1)); + EXPECT_FLOAT_EQ(twist.velocity.linear.x, 0.0); + EXPECT_FLOAT_EQ(twist.velocity.linear.y, -1.0); + EXPECT_FLOAT_EQ(twist.velocity.linear.z, 0.0); + EXPECT_FLOAT_EQ(twist.velocity.angular.x, 0.0); + EXPECT_FLOAT_EQ(twist.velocity.angular.y, 0.0); + EXPECT_FLOAT_EQ(twist.velocity.angular.z, 0.0); + + check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 4.5); + twist = buffer_->lookupVelocity("bar", *it, check_time, tf2::durationFromSec(0.1)); + EXPECT_FLOAT_EQ(twist.velocity.linear.x, 0.0); + EXPECT_FLOAT_EQ(twist.velocity.linear.y, 0.0); + EXPECT_FLOAT_EQ(twist.velocity.linear.z, 1.0); + EXPECT_FLOAT_EQ(twist.velocity.angular.x, 0.0); + EXPECT_FLOAT_EQ(twist.velocity.angular.y, 0.0); + EXPECT_FLOAT_EQ(twist.velocity.angular.z, 0.0); + + check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 5.5); + twist = buffer_->lookupVelocity("bar", *it, check_time, tf2::durationFromSec(0.1)); + EXPECT_FLOAT_EQ(twist.velocity.linear.x, 0.0); + EXPECT_FLOAT_EQ(twist.velocity.linear.y, 0.0); + EXPECT_FLOAT_EQ(twist.velocity.linear.z, -1.0); + EXPECT_FLOAT_EQ(twist.velocity.angular.x, 0.0); + EXPECT_FLOAT_EQ(twist.velocity.angular.y, 0.0); + EXPECT_FLOAT_EQ(twist.velocity.angular.z, 0.0); + } + catch(tf2::TransformException &ex) + { + EXPECT_STREQ("", ex.what()); + } + } +} + +TEST_F(AngularVelocitySquareTest, AngularVelocityAlone) +{ + double epsilon = 1e-6; + try + { + tf2::TimePoint check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 0.5); + auto twist = buffer_->lookupVelocity("bar", "foo", check_time, tf2::durationFromSec(0.1)); + EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.y, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.x, 1.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.y, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.z, 0.0, epsilon); + + check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 1.5); + twist = buffer_->lookupVelocity("bar", "foo", check_time, tf2::durationFromSec(0.1)); + EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.y, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.x, -1.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.y, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.z, 0.0, epsilon); + + check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 2.5); + twist = buffer_->lookupVelocity("bar", "foo", check_time, tf2::durationFromSec(0.1)); + EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.y, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.y, 1.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.z, 0.0, epsilon); + + check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 3.5); + twist = buffer_->lookupVelocity("bar", "foo", check_time, tf2::durationFromSec(0.1)); + EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.y, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.y, -1.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.z, 0.0, epsilon); + + check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 4.5); + twist = buffer_->lookupVelocity("bar", "foo", check_time, tf2::durationFromSec(0.1)); + EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.y, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.y, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.z, 1.0, epsilon); + + check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 5.5); + twist = buffer_->lookupVelocity("bar", "foo", check_time, tf2::durationFromSec(0.1)); + EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.y, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.y, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.z, -1.0, epsilon); + } + catch(tf2::TransformException &ex) + { + EXPECT_STREQ("", ex.what()); + } +} + +TEST_F(AngularVelocitySquareTest, AngularVelocityOffsetChildFrameInX) +{ + double epsilon = 1e-6; + try + { + // tf2::TimePoint check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 0.5); + // auto twist = buffer_->lookupVelocity("bar", "stationary_offset_child", check_time, tf2::durationFromSec(0.1)); + // EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); + // EXPECT_NEAR(twist.velocity.linear.y, 0.0, epsilon); + // EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); + // EXPECT_NEAR(twist.velocity.angular.x, 1.0, epsilon); + // EXPECT_NEAR(twist.velocity.angular.y, 0.0, epsilon); + // EXPECT_NEAR(twist.velocity.angular.z, 0.0, epsilon); + + // check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 1.5); + // twist = buffer_->lookupVelocity("bar", "stationary_offset_child", check_time, tf2::durationFromSec(0.1)); + // EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); + // EXPECT_NEAR(twist.velocity.linear.y, 0.0, epsilon); + // EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); + // EXPECT_NEAR(twist.velocity.angular.x, -1.0, epsilon); + // EXPECT_NEAR(twist.velocity.angular.y, 0.0, epsilon); + // EXPECT_NEAR(twist.velocity.angular.z, 0.0, epsilon); + + // check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 2.5); + // twist = buffer_->lookupVelocity("bar", "stationary_offset_child", check_time, tf2::durationFromSec(0.1)); + // EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); + // EXPECT_NEAR(twist.velocity.linear.y, 0.0, epsilon); + // EXPECT_NEAR(twist.velocity.linear.z, -1.0, epsilon); + // EXPECT_NEAR(twist.velocity.angular.x, 0.0, epsilon); + // EXPECT_NEAR(twist.velocity.angular.y, 1.0, epsilon); + // EXPECT_NEAR(twist.velocity.angular.z, 0.0, epsilon); + + // check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 3.5); + // twist = buffer_->lookupVelocity("bar", "stationary_offset_child", check_time, tf2::durationFromSec(0.1)); + // EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); + // EXPECT_NEAR(twist.velocity.linear.y, 0.0, epsilon); + // EXPECT_NEAR(twist.velocity.linear.z, 1.0, epsilon); + // EXPECT_NEAR(twist.velocity.angular.x, 0.0, epsilon); + // EXPECT_NEAR(twist.velocity.angular.y, -1.0, epsilon); + // EXPECT_NEAR(twist.velocity.angular.z, 0.0, epsilon); + + // check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 4.5); + // twist = buffer_->lookupVelocity("bar", "stationary_offset_child", check_time, tf2::durationFromSec(0.1)); + // EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); + // EXPECT_NEAR(twist.velocity.linear.y, 1.0, epsilon); + // EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); + // EXPECT_NEAR(twist.velocity.angular.x, 0.0, epsilon); + // EXPECT_NEAR(twist.velocity.angular.y, 0.0, epsilon); + // EXPECT_NEAR(twist.velocity.angular.z, 1.0, epsilon); + + // check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 5.5); + // twist = buffer_->lookupVelocity("bar", "stationary_offset_child", check_time, tf2::durationFromSec(0.1)); + // EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); + // EXPECT_NEAR(twist.velocity.linear.y, -1.0, epsilon); + // EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); + // EXPECT_NEAR(twist.velocity.angular.x, 0.0, epsilon); + // EXPECT_NEAR(twist.velocity.angular.y, 0.0, epsilon); + // EXPECT_NEAR(twist.velocity.angular.z, -1.0, epsilon); + } + catch(tf2::TransformException &ex) + { + EXPECT_STREQ("", ex.what()); + } +} + +TEST_F(AngularVelocitySquareTest, AngularVelocityOffsetParentFrameInZ) +{ + double epsilon = 1e-6; + try + { + tf2::TimePoint check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 0.5); + +// auto twist = buffer_->lookupVelocity("bar", "stationary_offset_parent", check_time, tf2::durationFromSec(0.1)); +// EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); +// EXPECT_NEAR(twist.velocity.linear.y, -1.0, epsilon); +// EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); +// EXPECT_NEAR(twist.velocity.angular.x, 1.0, epsilon); +// EXPECT_NEAR(twist.velocity.angular.y, 0.0, epsilon); +// EXPECT_NEAR(twist.velocity.angular.z, 0.0, epsilon); + +// buffer_->lookupVelocity("bar", "stationary_offset_parent", ros::Time(1.5), tf2::durationFromSec(0.1)); +// EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); +// EXPECT_NEAR(twist.velocity.linear.y, 1.0, epsilon); +// EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); +// EXPECT_NEAR(twist.velocity.angular.x, -1.0, epsilon); +// EXPECT_NEAR(twist.velocity.angular.y, 0.0, epsilon); +// EXPECT_NEAR(twist.velocity.angular.z, 0.0, epsilon); + +// buffer_->lookupVelocity("bar", "stationary_offset_parent", ros::Time(2.5), tf2::durationFromSec(0.1)); +// EXPECT_NEAR(twist.velocity.linear.x, 1.0, epsilon); +// EXPECT_NEAR(twist.velocity.linear.y, 0.0, epsilon); +// EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); +// EXPECT_NEAR(twist.velocity.angular.x, 0.0, epsilon); +// EXPECT_NEAR(twist.velocity.angular.y, 1.0, epsilon); +// EXPECT_NEAR(twist.velocity.angular.z, 0.0, epsilon); + +// buffer_->lookupVelocity("bar", "stationary_offset_parent", ros::Time(3.5), tf2::durationFromSec(0.1)); +// EXPECT_NEAR(twist.velocity.linear.x, -1.0, epsilon); +// EXPECT_NEAR(twist.velocity.linear.y, 0.0, epsilon); +// EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); +// EXPECT_NEAR(twist.velocity.angular.x, 0.0, epsilon); +// EXPECT_NEAR(twist.velocity.angular.y, -1.0, epsilon); +// EXPECT_NEAR(twist.velocity.angular.z, 0.0, epsilon); + +// buffer_->lookupVelocity("bar", "stationary_offset_parent", ros::Time(4.5), tf2::durationFromSec(0.1)); +// EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); +// EXPECT_NEAR(twist.velocity.linear.y, 0.0, epsilon); +// EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); +// EXPECT_NEAR(twist.velocity.angular.x, 0.0, epsilon); +// EXPECT_NEAR(twist.velocity.angular.y, 0.0, epsilon); +// EXPECT_NEAR(twist.velocity.angular.z, 1.0, epsilon); + +// buffer_->lookupVelocity("bar", "stationary_offset_parent", ros::Time(5.5), tf2::durationFromSec(0.1)); +// EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); +// EXPECT_NEAR(twist.velocity.linear.y, 0.0, epsilon); +// EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); +// EXPECT_NEAR(twist.velocity.angular.x, 0.0, epsilon); +// EXPECT_NEAR(twist.velocity.angular.y, 0.0, epsilon); +// EXPECT_NEAR(twist.velocity.angular.z, -1.0, epsilon); + } + catch(tf2::TransformException &ex) + { + EXPECT_STREQ("", ex.what()); + } +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From da7fe847f37602b2b75469f0754407fce2b2c0a1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 9 Feb 2024 11:10:56 +0100 Subject: [PATCH 03/12] Fixed velocity test MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- tf2/include/tf2/buffer_core.h | 2 +- tf2/src/buffer_core.cpp | 108 +++++------- tf2_ros/test/test_buffer.cpp | 22 +-- tf2_ros/test/velocity_test.cpp | 291 +++++++++++++++++---------------- 4 files changed, 211 insertions(+), 212 deletions(-) diff --git a/tf2/include/tf2/buffer_core.h b/tf2/include/tf2/buffer_core.h index 58d62f58e..8562fa140 100644 --- a/tf2/include/tf2/buffer_core.h +++ b/tf2/include/tf2/buffer_core.h @@ -158,7 +158,7 @@ class BufferCore : public BufferCoreInterface const std::string & fixed_frame) const override; geometry_msgs::msg::VelocityStamped lookupVelocity( - const std::string& tracking_frame, const std::string& observation_frame, + const std::string & tracking_frame, const std::string & observation_frame, const TimePoint & time, const tf2::Duration & averaging_interval) const; /** \brief Lookup the velocity of the moving_frame in the reference_frame diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index d95c6d1d2..7b6a4429c 100644 --- a/tf2/src/buffer_core.cpp +++ b/tf2/src/buffer_core.cpp @@ -575,18 +575,22 @@ struct TransformAccum }; geometry_msgs::msg::VelocityStamped BufferCore::lookupVelocity( - const std::string& tracking_frame, const std::string& observation_frame, + const std::string & tracking_frame, const std::string & observation_frame, const TimePoint & time, const tf2::Duration & averaging_interval) const { // ref point is origin of tracking_frame, ref_frame = obs_frame - return lookupVelocity(tracking_frame, observation_frame, observation_frame, tf2::Vector3(0,0,0), tracking_frame, time, averaging_interval); + return lookupVelocity( + tracking_frame, observation_frame, observation_frame, tf2::Vector3( + 0, 0, + 0), tracking_frame, time, + averaging_interval); } geometry_msgs::msg::VelocityStamped BufferCore::lookupVelocity( - const std::string & tracking_frame, const std::string & observation_frame, - const std::string & reference_frame, const tf2::Vector3 & reference_point, - const std::string & reference_point_frame, - const TimePoint & time, const tf2::Duration & averaging_interval) const + const std::string & tracking_frame, const std::string & observation_frame, + const std::string & reference_frame, const tf2::Vector3 & reference_point, + const std::string & reference_point_frame, + const TimePoint & time, const tf2::Duration & averaging_interval) const { tf2::TimePoint latest_time; // TODO(anyone): This is incorrect, but better than nothing. Really we want the latest time for @@ -600,38 +604,21 @@ geometry_msgs::msg::VelocityStamped BufferCore::lookupVelocity( auto time_seconds = tf2::timeToSec(time); auto averaging_interval_seconds = std::chrono::duration(averaging_interval).count(); - auto end_time = std::min(time_seconds + averaging_interval_seconds * 0.5 , tf2::timeToSec(latest_time)); - - // std::cout << std::fixed << end_time <<" latest " << tf2::timeToSec(latest_time) << " input " << time_seconds << std::endl; - // auto start_time = end_time - averaging_interval_seconds; - auto start_time = std::max(0.00001 + averaging_interval_seconds, end_time) - averaging_interval_seconds; - auto corrected_averaging_interval = end_time - start_time; //correct for the possiblity that start time was truncated above. + auto end_time = + std::min(time_seconds + averaging_interval_seconds * 0.5, tf2::timeToSec(latest_time)); - // std::cout << start_time << " start time" << std::endl; + auto start_time = + std::max(0.00001 + averaging_interval_seconds, end_time) - averaging_interval_seconds; + // correct for the possiblity that start time was truncated above. + auto corrected_averaging_interval = end_time - start_time; tf2::Transform start, end; TimePoint time_out; - lookupTransformImpl(observation_frame, tracking_frame, tf2::timeFromSec(start_time), start, time_out); + lookupTransformImpl( + observation_frame, tracking_frame, tf2::timeFromSec( + start_time), start, time_out); lookupTransformImpl(observation_frame, tracking_frame, tf2::timeFromSec(end_time), end, time_out); - // std::cout << "start.getOrigin().getX() " << start.getOrigin().getX() << std::endl; - // std::cout << "start.getOrigin().getY() " << start.getOrigin().getY() << std::endl; - // std::cout << "start.getOrigin().getZ() " << start.getOrigin().getZ() << std::endl; - - // std::cout << "start.getRotation().getX() " << start.getRotation().x() << std::endl; - // std::cout << "start.getRotation().getY() " << start.getRotation().y() << std::endl; - // std::cout << "start.getRotation().getZ() " << start.getRotation().z() << std::endl; - // std::cout << "start.getRotation().getZ() " << start.getRotation().w() << std::endl; - - // std::cout << "end.getOrigin().getX() " << end.getOrigin().getX() << std::endl; - // std::cout << "end.getOrigin().getY() " << end.getOrigin().getY() << std::endl; - // std::cout << "end.getOrigin().getZ() " << end.getOrigin().getZ() << std::endl; - - // std::cout << "end.getRotation().getX() " << end.getRotation().x() << std::endl; - // std::cout << "end.getRotation().getY() " << end.getRotation().y() << std::endl; - // std::cout << "end.getRotation().getZ() " << end.getRotation().z() << std::endl; - // std::cout << "end.getRotation().getZ() " << end.getRotation().w() << std::endl; - auto temp = start.getBasis().inverse() * end.getBasis(); tf2::Quaternion quat_temp; temp.getRotation(quat_temp); @@ -642,38 +629,39 @@ geometry_msgs::msg::VelocityStamped BufferCore::lookupVelocity( double delta_y = end.getOrigin().getY() - start.getOrigin().getY(); double delta_z = end.getOrigin().getZ() - start.getOrigin().getZ(); - tf2::Vector3 twist_vel ((delta_x)/corrected_averaging_interval, - (delta_y)/corrected_averaging_interval, - (delta_z)/corrected_averaging_interval); + tf2::Vector3 twist_vel((delta_x) / corrected_averaging_interval, + (delta_y) / corrected_averaging_interval, + (delta_z) / corrected_averaging_interval); tf2::Vector3 twist_rot = o * (ang / corrected_averaging_interval); // correct for the position of the reference frame tf2::Transform inverse; - lookupTransformImpl(reference_frame, tracking_frame, tf2::timeFromSec(time_seconds), inverse, time_out); + lookupTransformImpl( + reference_frame, tracking_frame, tf2::timeFromSec( + time_seconds), inverse, time_out); tf2::Vector3 out_rot = inverse.getBasis() * twist_rot; - tf2::Vector3 out_vel = inverse.getBasis()* twist_vel + inverse.getOrigin().cross(out_rot); + tf2::Vector3 out_vel = inverse.getBasis() * twist_vel + inverse.getOrigin().cross(out_rot); auto transform_point = [this]( const std::string & target_frame, const std::string & source_frame, const tf2::Vector3 & point_in, double time_transform) - { - // transform point - tf2::Transform transform; - tf2::TimePoint time_out; - lookupTransformImpl( - target_frame, source_frame, tf2::timeFromSec(time_transform), transform, time_out); + { + // transform point + tf2::Transform transform; + tf2::TimePoint time_out; + lookupTransformImpl( + target_frame, source_frame, tf2::timeFromSec(time_transform), transform, time_out); - tf2::Vector3 out; - out = transform * point_in; - return out; - }; + tf2::Vector3 out; + out = transform * point_in; + return out; + }; // Rereference the twist about a new reference point // Start by computing the original reference point in the reference frame: - // tf2::Stamped rp_orig(tf::Point(0,0,0), target_time, tracking_frame); - tf2::Vector3 p = tf2::Vector3(0,0,0); + tf2::Vector3 p = tf2::Vector3(0, 0, 0); tf2::Vector3 rp_orig = transform_point( reference_frame, tracking_frame, p, time_seconds); @@ -683,14 +671,6 @@ geometry_msgs::msg::VelocityStamped BufferCore::lookupVelocity( tf2::Vector3 delta = rp_desired - rp_orig; out_vel = out_vel + out_rot * delta; - // std::cout << "out_vel " << out_vel.x() << std::endl; - // std::cout << "out_vel " << out_vel.y() << std::endl; - // std::cout << "out_vel " << out_vel.z() << std::endl; - - // std::cout << "out_angular " << out_rot.x() << std::endl; - // std::cout << "out_angular " << out_rot.y() << std::endl; - // std::cout << "out_angular " << out_rot.z() << std::endl; - std::chrono::nanoseconds ns = std::chrono::duration_cast( tf2::timeFromSec(start_time + averaging_interval_seconds * 0.5).time_since_epoch()); std::chrono::seconds s = std::chrono::duration_cast( @@ -700,12 +680,14 @@ geometry_msgs::msg::VelocityStamped BufferCore::lookupVelocity( velocity.header.stamp.nanosec = static_cast(ns.count() % 1000000000ull); velocity.header.frame_id = reference_frame; velocity.body_frame_id = tracking_frame; - velocity.velocity.linear.x = (end.getOrigin().getX() - start.getOrigin().getX()) / averaging_interval_seconds; - velocity.velocity.linear.y = (end.getOrigin().getY() - start.getOrigin().getY()) / averaging_interval_seconds; - velocity.velocity.linear.z = (end.getOrigin().getZ() - start.getOrigin().getZ()) / averaging_interval_seconds; - velocity.velocity.angular.x = o.x() * ang / averaging_interval_seconds; - velocity.velocity.angular.y = o.y() * ang / averaging_interval_seconds; - velocity.velocity.angular.z = o.z() * ang / averaging_interval_seconds; + + velocity.velocity.linear.x = out_vel.x(); + velocity.velocity.linear.y = out_vel.y(); + velocity.velocity.linear.z = out_vel.z(); + velocity.velocity.angular.x = out_rot.x(); + velocity.velocity.angular.y = out_rot.y(); + velocity.velocity.angular.z = out_rot.z(); + return velocity; } diff --git a/tf2_ros/test/test_buffer.cpp b/tf2_ros/test/test_buffer.cpp index a64b61adf..f25101ee7 100644 --- a/tf2_ros/test/test_buffer.cpp +++ b/tf2_ros/test/test_buffer.cpp @@ -229,9 +229,9 @@ TEST(test_buffer, velocity_transform) output = buffer.lookupVelocity( - "bar", "foo", - "bar", {0, 0, 0}, "bar", - tf2_time, tf2::durationFromSec(0.1)); + "bar", "foo", + "bar", {0, 0, 0}, "bar", + tf2_time, tf2::durationFromSec(0.1)); } @@ -248,14 +248,14 @@ TEST(test_buffer, test_twist) std::cout << "seconds " << rclcpp_time.seconds() << std::endl; float vel = 0.3; - for (int i = -10; i < 5; ++i) - { + for (int i = -10; i < 5; ++i) { geometry_msgs::msg::TransformStamped transform; transform.header.frame_id = "PARENT"; - if (i < 0) - { - transform.header.stamp = builtin_interfaces::msg::Time(rclcpp_time - rclcpp::Duration(std::fabs(i), 0)); - std::cout << "seconds " << (rclcpp_time - rclcpp::Duration(std::fabs(i), 0)).seconds() << std::endl; + if (i < 0) { + transform.header.stamp = + builtin_interfaces::msg::Time(rclcpp_time - rclcpp::Duration(std::fabs(i), 0)); + std::cout << "seconds " << + (rclcpp_time - rclcpp::Duration(std::fabs(i), 0)).seconds() << std::endl; } else { transform.header.stamp = builtin_interfaces::msg::Time(rclcpp_time + rclcpp::Duration(i, 0)); @@ -275,7 +275,9 @@ TEST(test_buffer, test_twist) auto tw0 = buffer.lookupVelocity("THISFRAME", "PARENT", tf2_time, tf2::durationFromSec(4.001)); std::cout << "tw0 : " << tw0.velocity.linear.x << std::endl; - auto tw1 = buffer.lookupVelocity("THISFRAME", "PARENT", "PARENT", {0, 0, 0}, "THISFRAME", tf2_time, tf2::durationFromSec(4.001)); + auto tw1 = buffer.lookupVelocity( + "THISFRAME", "PARENT", "PARENT", {0, 0, 0}, "THISFRAME", + tf2_time, tf2::durationFromSec(4.001)); std::cout << "tw1 : " << tw1.velocity.linear.x << std::endl; } diff --git a/tf2_ros/test/velocity_test.cpp b/tf2_ros/test/velocity_test.cpp index 1ef0ffc10..92aeecafd 100644 --- a/tf2_ros/test/velocity_test.cpp +++ b/tf2_ros/test/velocity_test.cpp @@ -167,17 +167,14 @@ class AngularVelocitySquareTest : public ::testing::Test double x = -.1; double y = 0; double z = 0; - for (double t = 0.1; t < 6; t += 0.1) - { - if (t < 1) x += .1; - else if (t < 2) x -= .1; - else if (t < 3) y += .1; - else if (t < 4) y -= .1; - else if (t < 5) z += .1; - else z -= .1; + for (double t = 0.1; t < 6; t += 0.1) { + if (t < 1) {x += .1;} else if (t < 2) {x -= .1;} else if (t < 3) {y += .1;} else if (t < 4) { + y -= .1; + } else if (t < 5) {z += .1;} else {z -= .1;} tf2::Quaternion quat; quat.setRPY(x, y, z); + quat = quat.normalize(); { geometry_msgs::msg::TransformStamped transform; @@ -197,10 +194,10 @@ class AngularVelocitySquareTest : public ::testing::Test { geometry_msgs::msg::TransformStamped transform; - transform.transform.rotation.w = quat.w(); - transform.transform.rotation.x = quat.x(); - transform.transform.rotation.y = quat.y(); - transform.transform.rotation.z = quat.z(); + transform.transform.rotation.w = 1.0; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; transform.transform.translation.x = 1.0; transform.transform.translation.y = 0.0; transform.transform.translation.z = 0.0; @@ -213,10 +210,10 @@ class AngularVelocitySquareTest : public ::testing::Test { geometry_msgs::msg::TransformStamped transform; - transform.transform.rotation.w = quat.w(); - transform.transform.rotation.x = quat.x(); - transform.transform.rotation.y = quat.y(); - transform.transform.rotation.z = quat.z(); + transform.transform.rotation.w = 1.0; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; transform.transform.translation.x = 0.0; transform.transform.translation.y = 0.0; transform.transform.translation.z = -1.0; @@ -264,10 +261,10 @@ TEST_F(LinearVelocitySquareTest, LinearVelocityToThreeFrames) offset_frames.push_back("stationary_offset_child"); offset_frames.push_back("stationary_offset_parent"); - for (std::vector::iterator it = offset_frames.begin(); it != offset_frames.end(); ++it) + for (std::vector::iterator it = offset_frames.begin(); it != offset_frames.end(); + ++it) { - try - { + try { tf2::TimePoint check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 0.5); auto twist = buffer_->lookupVelocity( @@ -323,9 +320,7 @@ TEST_F(LinearVelocitySquareTest, LinearVelocityToThreeFrames) EXPECT_FLOAT_EQ(twist.velocity.angular.x, 0.0); EXPECT_FLOAT_EQ(twist.velocity.angular.y, 0.0); EXPECT_FLOAT_EQ(twist.velocity.angular.z, 0.0); - } - catch(tf2::TransformException &ex) - { + } catch (tf2::TransformException & ex) { EXPECT_STREQ("", ex.what()); } } @@ -334,8 +329,7 @@ TEST_F(LinearVelocitySquareTest, LinearVelocityToThreeFrames) TEST_F(AngularVelocitySquareTest, AngularVelocityAlone) { double epsilon = 1e-6; - try - { + try { tf2::TimePoint check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 0.5); auto twist = buffer_->lookupVelocity("bar", "foo", check_time, tf2::durationFromSec(0.1)); EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); @@ -356,7 +350,7 @@ TEST_F(AngularVelocitySquareTest, AngularVelocityAlone) check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 2.5); twist = buffer_->lookupVelocity("bar", "foo", check_time, tf2::durationFromSec(0.1)); - EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); EXPECT_NEAR(twist.velocity.linear.y, 0.0, epsilon); EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); EXPECT_NEAR(twist.velocity.angular.x, 0.0, epsilon); @@ -389,9 +383,7 @@ TEST_F(AngularVelocitySquareTest, AngularVelocityAlone) EXPECT_NEAR(twist.velocity.angular.x, 0.0, epsilon); EXPECT_NEAR(twist.velocity.angular.y, 0.0, epsilon); EXPECT_NEAR(twist.velocity.angular.z, -1.0, epsilon); - } - catch(tf2::TransformException &ex) - { + } catch (tf2::TransformException & ex) { EXPECT_STREQ("", ex.what()); } } @@ -399,64 +391,73 @@ TEST_F(AngularVelocitySquareTest, AngularVelocityAlone) TEST_F(AngularVelocitySquareTest, AngularVelocityOffsetChildFrameInX) { double epsilon = 1e-6; - try - { - // tf2::TimePoint check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 0.5); - // auto twist = buffer_->lookupVelocity("bar", "stationary_offset_child", check_time, tf2::durationFromSec(0.1)); - // EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); - // EXPECT_NEAR(twist.velocity.linear.y, 0.0, epsilon); - // EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); - // EXPECT_NEAR(twist.velocity.angular.x, 1.0, epsilon); - // EXPECT_NEAR(twist.velocity.angular.y, 0.0, epsilon); - // EXPECT_NEAR(twist.velocity.angular.z, 0.0, epsilon); - - // check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 1.5); - // twist = buffer_->lookupVelocity("bar", "stationary_offset_child", check_time, tf2::durationFromSec(0.1)); - // EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); - // EXPECT_NEAR(twist.velocity.linear.y, 0.0, epsilon); - // EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); - // EXPECT_NEAR(twist.velocity.angular.x, -1.0, epsilon); - // EXPECT_NEAR(twist.velocity.angular.y, 0.0, epsilon); - // EXPECT_NEAR(twist.velocity.angular.z, 0.0, epsilon); - - // check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 2.5); - // twist = buffer_->lookupVelocity("bar", "stationary_offset_child", check_time, tf2::durationFromSec(0.1)); - // EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); - // EXPECT_NEAR(twist.velocity.linear.y, 0.0, epsilon); - // EXPECT_NEAR(twist.velocity.linear.z, -1.0, epsilon); - // EXPECT_NEAR(twist.velocity.angular.x, 0.0, epsilon); - // EXPECT_NEAR(twist.velocity.angular.y, 1.0, epsilon); - // EXPECT_NEAR(twist.velocity.angular.z, 0.0, epsilon); - - // check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 3.5); - // twist = buffer_->lookupVelocity("bar", "stationary_offset_child", check_time, tf2::durationFromSec(0.1)); - // EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); - // EXPECT_NEAR(twist.velocity.linear.y, 0.0, epsilon); - // EXPECT_NEAR(twist.velocity.linear.z, 1.0, epsilon); - // EXPECT_NEAR(twist.velocity.angular.x, 0.0, epsilon); - // EXPECT_NEAR(twist.velocity.angular.y, -1.0, epsilon); - // EXPECT_NEAR(twist.velocity.angular.z, 0.0, epsilon); - - // check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 4.5); - // twist = buffer_->lookupVelocity("bar", "stationary_offset_child", check_time, tf2::durationFromSec(0.1)); - // EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); - // EXPECT_NEAR(twist.velocity.linear.y, 1.0, epsilon); - // EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); - // EXPECT_NEAR(twist.velocity.angular.x, 0.0, epsilon); - // EXPECT_NEAR(twist.velocity.angular.y, 0.0, epsilon); - // EXPECT_NEAR(twist.velocity.angular.z, 1.0, epsilon); - - // check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 5.5); - // twist = buffer_->lookupVelocity("bar", "stationary_offset_child", check_time, tf2::durationFromSec(0.1)); - // EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); - // EXPECT_NEAR(twist.velocity.linear.y, -1.0, epsilon); - // EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); - // EXPECT_NEAR(twist.velocity.angular.x, 0.0, epsilon); - // EXPECT_NEAR(twist.velocity.angular.y, 0.0, epsilon); - // EXPECT_NEAR(twist.velocity.angular.z, -1.0, epsilon); - } - catch(tf2::TransformException &ex) - { + try { + tf2::TimePoint check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 0.5); + auto twist = buffer_->lookupVelocity( + "bar", "stationary_offset_child", check_time, tf2::durationFromSec( + 0.1)); + EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.y, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.x, 1.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.y, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.z, 0.0, epsilon); + + check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 1.5); + twist = buffer_->lookupVelocity( + "bar", "stationary_offset_child", check_time, tf2::durationFromSec( + 0.1)); + EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.y, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.x, -1.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.y, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.z, 0.0, epsilon); + + check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 2.5); + twist = buffer_->lookupVelocity( + "bar", "stationary_offset_child", check_time, tf2::durationFromSec( + 0.1)); + EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.y, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.z, -1.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.y, 1.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.z, 0.0, epsilon); + + check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 3.5); + twist = buffer_->lookupVelocity( + "bar", "stationary_offset_child", check_time, tf2::durationFromSec( + 0.1)); + EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.y, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.z, 1.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.y, -1.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.z, 0.0, epsilon); + + check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 4.5); + twist = buffer_->lookupVelocity( + "bar", "stationary_offset_child", check_time, tf2::durationFromSec( + 0.1)); + EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.y, 1.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.y, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.z, 1.0, epsilon); + + check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 5.5); + twist = buffer_->lookupVelocity( + "bar", "stationary_offset_child", check_time, tf2::durationFromSec( + 0.1)); + EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.y, -1.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.y, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.z, -1.0, epsilon); + } catch (tf2::TransformException & ex) { EXPECT_STREQ("", ex.what()); } } @@ -464,60 +465,74 @@ TEST_F(AngularVelocitySquareTest, AngularVelocityOffsetChildFrameInX) TEST_F(AngularVelocitySquareTest, AngularVelocityOffsetParentFrameInZ) { double epsilon = 1e-6; - try - { + try { tf2::TimePoint check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 0.5); -// auto twist = buffer_->lookupVelocity("bar", "stationary_offset_parent", check_time, tf2::durationFromSec(0.1)); -// EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); -// EXPECT_NEAR(twist.velocity.linear.y, -1.0, epsilon); -// EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); -// EXPECT_NEAR(twist.velocity.angular.x, 1.0, epsilon); -// EXPECT_NEAR(twist.velocity.angular.y, 0.0, epsilon); -// EXPECT_NEAR(twist.velocity.angular.z, 0.0, epsilon); - -// buffer_->lookupVelocity("bar", "stationary_offset_parent", ros::Time(1.5), tf2::durationFromSec(0.1)); -// EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); -// EXPECT_NEAR(twist.velocity.linear.y, 1.0, epsilon); -// EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); -// EXPECT_NEAR(twist.velocity.angular.x, -1.0, epsilon); -// EXPECT_NEAR(twist.velocity.angular.y, 0.0, epsilon); -// EXPECT_NEAR(twist.velocity.angular.z, 0.0, epsilon); - -// buffer_->lookupVelocity("bar", "stationary_offset_parent", ros::Time(2.5), tf2::durationFromSec(0.1)); -// EXPECT_NEAR(twist.velocity.linear.x, 1.0, epsilon); -// EXPECT_NEAR(twist.velocity.linear.y, 0.0, epsilon); -// EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); -// EXPECT_NEAR(twist.velocity.angular.x, 0.0, epsilon); -// EXPECT_NEAR(twist.velocity.angular.y, 1.0, epsilon); -// EXPECT_NEAR(twist.velocity.angular.z, 0.0, epsilon); - -// buffer_->lookupVelocity("bar", "stationary_offset_parent", ros::Time(3.5), tf2::durationFromSec(0.1)); -// EXPECT_NEAR(twist.velocity.linear.x, -1.0, epsilon); -// EXPECT_NEAR(twist.velocity.linear.y, 0.0, epsilon); -// EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); -// EXPECT_NEAR(twist.velocity.angular.x, 0.0, epsilon); -// EXPECT_NEAR(twist.velocity.angular.y, -1.0, epsilon); -// EXPECT_NEAR(twist.velocity.angular.z, 0.0, epsilon); - -// buffer_->lookupVelocity("bar", "stationary_offset_parent", ros::Time(4.5), tf2::durationFromSec(0.1)); -// EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); -// EXPECT_NEAR(twist.velocity.linear.y, 0.0, epsilon); -// EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); -// EXPECT_NEAR(twist.velocity.angular.x, 0.0, epsilon); -// EXPECT_NEAR(twist.velocity.angular.y, 0.0, epsilon); -// EXPECT_NEAR(twist.velocity.angular.z, 1.0, epsilon); - -// buffer_->lookupVelocity("bar", "stationary_offset_parent", ros::Time(5.5), tf2::durationFromSec(0.1)); -// EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); -// EXPECT_NEAR(twist.velocity.linear.y, 0.0, epsilon); -// EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); -// EXPECT_NEAR(twist.velocity.angular.x, 0.0, epsilon); -// EXPECT_NEAR(twist.velocity.angular.y, 0.0, epsilon); -// EXPECT_NEAR(twist.velocity.angular.z, -1.0, epsilon); - } - catch(tf2::TransformException &ex) - { + auto twist = buffer_->lookupVelocity( + "bar", "stationary_offset_parent", check_time, tf2::durationFromSec( + 0.1)); + EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.y, -1.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.x, 1.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.y, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.z, 0.0, epsilon); + + check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 1.5); + twist = buffer_->lookupVelocity( + "bar", "stationary_offset_parent", check_time, tf2::durationFromSec( + 0.1)); + EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.y, 1.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.x, -1.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.y, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.z, 0.0, epsilon); + + check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 2.5); + twist = buffer_->lookupVelocity( + "bar", "stationary_offset_parent", check_time, tf2::durationFromSec( + 0.1)); + EXPECT_NEAR(twist.velocity.linear.x, 1.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.y, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.y, 1.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.z, 0.0, epsilon); + + check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 3.5); + twist = buffer_->lookupVelocity( + "bar", "stationary_offset_parent", check_time, tf2::durationFromSec( + 0.1)); + EXPECT_NEAR(twist.velocity.linear.x, -1.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.y, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.y, -1.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.z, 0.0, epsilon); + + check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 4.5); + twist = buffer_->lookupVelocity( + "bar", "stationary_offset_parent", check_time, tf2::durationFromSec( + 0.1)); + EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.y, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.y, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.z, 1.0, epsilon); + + check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 5.5); + twist = buffer_->lookupVelocity( + "bar", "stationary_offset_parent", check_time, tf2::durationFromSec( + 0.1)); + EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.y, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.y, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.z, -1.0, epsilon); + } catch (tf2::TransformException & ex) { EXPECT_STREQ("", ex.what()); } } From 0f45173b43d1f6b4975999db840b713bb5fa55f8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 9 Feb 2024 14:46:07 +0100 Subject: [PATCH 04/12] Added python interfaces MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .../tf2_geometry_msgs/tf2_geometry_msgs.hpp | 65 ++++++++------ tf2_py/src/tf2_py.cpp | 90 +++++++++++-------- tf2_py/test/test_buffer_core.py | 26 ++++++ tf2_ros/include/tf2_ros/buffer_interface.h | 2 +- 4 files changed, 122 insertions(+), 61 deletions(-) diff --git a/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp b/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp index 4bf34cba2..59c34eff2 100644 --- a/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp +++ b/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp @@ -1332,33 +1332,48 @@ void doTransform( const geometry_msgs::msg::TransformStamped & transform, double duration) { + tf2::Vector3 twist_rot(t_in.velocity.angular.x, + t_in.velocity.angular.y, + t_in.velocity.angular.z); + tf2::Vector3 twist_vel(t_in.velocity.linear.x, + t_in.velocity.linear.y, + t_in.velocity.linear.z); + tf2::Transform transform_temp; + + transform_temp.setOrigin(tf2::Vector3( + transform.transform.translation.x, + transform.transform.translation.y, + transform.transform.translation.z)); + transform_temp.setRotation(tf2::Quaternion( + transform.transform.rotation.x, + transform.transform.rotation.y, + transform.transform.rotation.z, + transform.transform.rotation.w)); + + // tf2::Transform start, end; + // TimePoint time_out; + // lookupTransformImpl( + // observation_frame, tracking_frame, tf2::timeFromSec( + // start_time), start, time_out); + + // tf::StampedTransform transform; + // lookupTransform(target_frame,msg_in.header.frame_id, msg_in.header.stamp, transform); + + tf2::Vector3 out_rot = transform_temp.getBasis() * twist_rot; + tf2::Vector3 out_vel = transform_temp.getBasis() * twist_vel + transform_temp.getOrigin().cross(out_rot); + + // geometry_msgs::TwistStamped interframe_twist; + // lookupVelocity(target_frame, msg_in.header.frame_id, msg_in.header.stamp, ros::Duration(0.1), interframe_twist); //\todo get rid of hard coded number + + t_out.header = t_in.header; + t_out.velocity.linear.x = out_vel.x() + t_in.velocity.linear.x; + t_out.velocity.linear.y = out_vel.y() + t_in.velocity.linear.y; + t_out.velocity.linear.z = out_vel.z() + t_in.velocity.linear.z; + t_out.velocity.angular.x = out_rot.x() + t_in.velocity.angular.x; + t_out.velocity.angular.y = out_rot.y() + t_in.velocity.angular.y; + t_out.velocity.angular.z = out_rot.z() + t_in.velocity.angular.z; } -// /** \brief Convert a tf2 Vector3 type to its equivalent geometry_msgs representation. -// * This function is a specialization of the toMsg template defined in tf2/convert.h. -// * \param in A tf2 Vector3 object. -// * \return The Vector3 converted to a geometry_msgs message type. -// */ -// inline -// geometry_msgs::msg::Point32 & toMsg(const tf2::Vector3 & in, geometry_msgs::msg::Point32 & out) -// { -// out.x = static_cast(in.getX()); -// out.y = static_cast(in.getY()); -// out.z = static_cast(in.getZ()); -// return out; -// } - -// /** \brief Convert a Vector3 message to its equivalent tf2 representation. -// * This function is a specialization of the fromMsg template defined in tf2/convert.h. -// * \param in A Vector3 message type. -// * \param out The Vector3 converted to a tf2 type. -// */ -// inline -// void fromMsg(const geometry_msgs::msg::Point32 & in, tf2::Vector3 & out) -// { -// out = tf2::Vector3(in.x, in.y, in.z); -// } - /**********************/ /*** WrenchStamped ****/ /**********************/ diff --git a/tf2_py/src/tf2_py.cpp b/tf2_py/src/tf2_py.cpp index 7696f7051..ab6c2b3ce 100644 --- a/tf2_py/src/tf2_py.cpp +++ b/tf2_py/src/tf2_py.cpp @@ -627,51 +627,71 @@ static PyObject * lookupTransformFullCore(PyObject * self, PyObject * args, PyOb // TODO(anyone): Create a converter that will actually return a python message return Py_BuildValue("O&", transform_converter, &transform); } -/* -static PyObject *lookupTwistCore(PyObject *self, PyObject *args, PyObject *kw) + +static PyObject * lookupVelocityCore(PyObject * self, PyObject * args, PyObject * kw) { - tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; - char *tracking_frame, *observation_frame; - builtin_interfaces::msg::Time time; + tf2::BufferCore * bc = ((buffer_core_t *)self)->bc; + char * tracking_frame, * observation_frame; + tf2::TimePoint time; tf2::Duration averaging_interval; - static const char *keywords[] = { "tracking_frame", "observation_frame", "time", "averaging_interval", nullptr }; + static const char * keywords[] = + {"tracking_frame", "observation_frame", "time", "averaging_interval", nullptr}; - if (!PyArg_ParseTupleAndKeywords(args, kw, "ssO&O&", (char**)keywords, &tracking_frame, &observation_frame, rostime_converter, &time, rosduration_converter, &averaging_interval)) + if (!PyArg_ParseTupleAndKeywords( + args, kw, "ssO&O&", (char **)keywords, &tracking_frame, + &observation_frame, rostime_converter, &time, rosduration_converter, &averaging_interval)) + { return nullptr; - geometry_msgs::msg::Twist twist; - WRAP(twist = bc->lookupTwist(tracking_frame, observation_frame, time, averaging_interval)); - - return Py_BuildValue("(ddd)(ddd)", - twist.linear.x, twist.linear.y, twist.linear.z, - twist.angular.x, twist.angular.y, twist.angular.z); + } + geometry_msgs::msg::VelocityStamped velocity_stamped; + WRAP( + velocity_stamped = + bc->lookupVelocity(tracking_frame, observation_frame, time, averaging_interval)); + + return Py_BuildValue( + "(ddd)(ddd)", + velocity_stamped.velocity.linear.x, velocity_stamped.velocity.linear.y, + velocity_stamped.velocity.linear.z, + velocity_stamped.velocity.angular.x, velocity_stamped.velocity.angular.y, + velocity_stamped.velocity.angular.z); } -static PyObject *lookupTwistFullCore(PyObject *self, PyObject *args) +static PyObject * lookupVelocityFullCore(PyObject * self, PyObject * args) { - tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; - char *tracking_frame, *observation_frame, *reference_frame, *reference_point_frame; - builtin_interfaces::msg::Time time; + tf2::BufferCore * bc = ((buffer_core_t *)self)->bc; + char * tracking_frame, * observation_frame, * reference_frame, * reference_point_frame; + tf2::TimePoint time; tf2::Duration averaging_interval; double px, py, pz; - if (!PyArg_ParseTuple(args, "sss(ddd)sO&O&", - &tracking_frame, - &observation_frame, - &reference_frame, - &px, &py, &pz, - &reference_point_frame, - rostime_converter, &time, - rosduration_converter, &averaging_interval)) + if (!PyArg_ParseTuple( + args, "sss(ddd)sO&O&", + &tracking_frame, + &observation_frame, + &reference_frame, + &px, &py, &pz, + &reference_point_frame, + rostime_converter, &time, + rosduration_converter, &averaging_interval)) + { return nullptr; - geometry_msgs::msg::Twist twist; - tf::Point pt(px, py, pz); - WRAP(twist = bc->lookupTwist(tracking_frame, observation_frame, reference_frame, pt, reference_point_frame, time, averaging_interval)); - - return Py_BuildValue("(ddd)(ddd)", - twist.linear.x, twist.linear.y, twist.linear.z, - twist.angular.x, twist.angular.y, twist.angular.z); + } + geometry_msgs::msg::VelocityStamped velocity_stamped; + tf2::Vector3 pt(px, py, pz); + WRAP( + velocity_stamped = + bc->lookupVelocity( + tracking_frame, observation_frame, reference_frame, pt, + reference_point_frame, time, averaging_interval)); + + return Py_BuildValue( + "(ddd)(ddd)", + velocity_stamped.velocity.linear.x, velocity_stamped.velocity.linear.y, + velocity_stamped.velocity.linear.z, + velocity_stamped.velocity.angular.x, velocity_stamped.velocity.angular.y, + velocity_stamped.velocity.angular.z); } -*/ + static inline int checkTranslationType(PyObject * o) { PyTypeObject * translation_type = @@ -1058,8 +1078,8 @@ static struct PyMethodDef buffer_core_methods[] = nullptr}, {"lookup_transform_full_core", (PyCFunction)lookupTransformFullCore, METH_VARARGS | METH_KEYWORDS, nullptr}, - // {"lookupTwistCore", (PyCFunction)lookupTwistCore, METH_VARARGS | METH_KEYWORDS}, - // {"lookupTwistFullCore", lookupTwistFullCore, METH_VARARGS}, + {"lookupVelocityCore", (PyCFunction)lookupVelocityCore, METH_VARARGS | METH_KEYWORDS, nullptr}, + {"lookupVelocityFullCore", lookupVelocityFullCore, METH_VARARGS, nullptr}, {nullptr, nullptr, 0, nullptr} }; diff --git a/tf2_py/test/test_buffer_core.py b/tf2_py/test/test_buffer_core.py index 45721dbb7..c862f22c6 100644 --- a/tf2_py/test/test_buffer_core.py +++ b/tf2_py/test/test_buffer_core.py @@ -32,6 +32,7 @@ from geometry_msgs.msg import TransformStamped import rclpy +from rclpy.duration import Duration from rpyutils import add_dll_directories_from_env # Since Python 3.8, on Windows we should ensure DLL directories are explicitly added @@ -259,6 +260,31 @@ def test_lookup_transform_core_fail(self): self.assertEqual(LookupException, type(ex.exception)) + def test_twist(self): + buffer_core = BufferCore() + + vel = 3 + for ti in range(5): + m = TransformStamped() + m.header.frame_id = 'PARENT' + m.header.stamp = rclpy.time.Time(seconds=ti) + m.child_frame_id = 'THISFRAME' + m.transform.translation.x = ti * vel + m.transform.rotation.x = 0.0 + m.transform.rotation.y = 0.0 + m.transform.rotation.z = 0.0 + m.transform.rotation.w = 1.0 + buffer_core.set_transform(m, 'unittest') + + tw0 = buffer_core.lookupVelocityCore( + 'THISFRAME', 'PARENT', rclpy.time.Time(seconds=0.0), + Duration(seconds=4, nanoseconds=1000000)) + self.assertAlmostEqual(tw0[0][0], vel, 2) + tw1 = buffer_core.lookupVelocityFullCore( + 'THISFRAME', 'PARENT', 'PARENT', (0, 0, 0), 'THISFRAME', + rclpy.time.Time(seconds=0.0), Duration(seconds=4, nanoseconds=1000000)) + self.assertEqual(tw0, tw1) + if __name__ == '__main__': unittest.main() diff --git a/tf2_ros/include/tf2_ros/buffer_interface.h b/tf2_ros/include/tf2_ros/buffer_interface.h index 5ae160fd2..f2092b7e1 100644 --- a/tf2_ros/include/tf2_ros/buffer_interface.h +++ b/tf2_ros/include/tf2_ros/buffer_interface.h @@ -226,7 +226,7 @@ class BufferInterface // do the transform tf2::doTransform( in, out, - lookupVelocity(target_frame, tf2::getFrameId(in), tf2::getTimestamp(in), timeout), + lookupTransform(target_frame, tf2::getFrameId(in), tf2::getTimestamp(in), timeout), duration); return out; } From 789df200eaa7be46a7ad846865a27a84e99f6561 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 13 Feb 2024 11:47:07 +0100 Subject: [PATCH 05/12] add part of the feedback MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- tf2/include/tf2/convert.h | 6 ------ .../include/tf2_geometry_msgs/tf2_geometry_msgs.hpp | 3 +-- tf2_py/test/test_buffer_core.py | 2 +- 3 files changed, 2 insertions(+), 9 deletions(-) diff --git a/tf2/include/tf2/convert.h b/tf2/include/tf2/convert.h index 85ffab5d7..f81a3c65d 100644 --- a/tf2/include/tf2/convert.h +++ b/tf2/include/tf2/convert.h @@ -59,12 +59,6 @@ void doTransform( const T & data_in, T & data_out, const geometry_msgs::msg::TransformStamped & transform); -template -void doTransform( - const T & data_in, T & data_out, - const geometry_msgs::msg::TransformStamped & transform, - double duration); - /**\brief Get the timestamp from data * \param[in] t The data input. * \return The timestamp associated with the data. diff --git a/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp b/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp index 59c34eff2..0807c0bd2 100644 --- a/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp +++ b/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp @@ -1329,8 +1329,7 @@ inline void doTransform( const geometry_msgs::msg::VelocityStamped & t_in, geometry_msgs::msg::VelocityStamped & t_out, - const geometry_msgs::msg::TransformStamped & transform, - double duration) + const geometry_msgs::msg::TransformStamped & transform) { tf2::Vector3 twist_rot(t_in.velocity.angular.x, t_in.velocity.angular.y, diff --git a/tf2_py/test/test_buffer_core.py b/tf2_py/test/test_buffer_core.py index c862f22c6..2b88134d6 100644 --- a/tf2_py/test/test_buffer_core.py +++ b/tf2_py/test/test_buffer_core.py @@ -260,7 +260,7 @@ def test_lookup_transform_core_fail(self): self.assertEqual(LookupException, type(ex.exception)) - def test_twist(self): + def test_velocity(self): buffer_core = BufferCore() vel = 3 From a6ee0e4fcca7ee1e6e2b35e1582ccd8abc0a4e9f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 2 Apr 2024 11:13:06 +0200 Subject: [PATCH 06/12] fix compilation MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp index 3f6d520cc..d1dea7622 100644 --- a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp +++ b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp @@ -636,7 +636,7 @@ TEST(TfGeometry, Velocity) geometry_msgs::msg::TransformStamped trafo; - tf2::doTransform(v1, res, trafo, 0.1); + tf2::doTransform(v1, res, trafo); } int main(int argc, char ** argv) From 52cccf8d993393b3483904a4ad59d6f1b9887160 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 9 Apr 2024 14:20:34 +0200 Subject: [PATCH 07/12] Added feedback MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .../test/test_tf2_geometry_msgs.cpp | 1 - tf2_ros/include/tf2_ros/buffer_interface.h | 15 ------------ tf2_ros/test/test_buffer.cpp | 24 ++++++++++++------- 3 files changed, 16 insertions(+), 24 deletions(-) diff --git a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp index d1dea7622..069965a0c 100644 --- a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp +++ b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp @@ -633,7 +633,6 @@ TEST(TfGeometry, Velocity) v1.header.frame_id = "world"; v1.body_frame_id = "base_link"; - geometry_msgs::msg::TransformStamped trafo; tf2::doTransform(v1, res, trafo); diff --git a/tf2_ros/include/tf2_ros/buffer_interface.h b/tf2_ros/include/tf2_ros/buffer_interface.h index f2092b7e1..4f5e4c31f 100644 --- a/tf2_ros/include/tf2_ros/buffer_interface.h +++ b/tf2_ros/include/tf2_ros/buffer_interface.h @@ -216,21 +216,6 @@ class BufferInterface return out; } - template - T & transform( - const T & in, T & out, - const std::string & target_frame, - double duration, - tf2::Duration timeout = tf2::durationFromSec(0.0)) const - { - // do the transform - tf2::doTransform( - in, out, - lookupTransform(target_frame, tf2::getFrameId(in), tf2::getTimestamp(in), timeout), - duration); - return out; - } - /** \brief Transform an input into the target frame. * This function is templated and can take as input any valid mathematical object that tf knows * how to apply a transform to, by way of the templated math conversions interface. diff --git a/tf2_ros/test/test_buffer.cpp b/tf2_ros/test/test_buffer.cpp index f25101ee7..0362b2091 100644 --- a/tf2_ros/test/test_buffer.cpp +++ b/tf2_ros/test/test_buffer.cpp @@ -232,6 +232,14 @@ TEST(test_buffer, velocity_transform) "bar", "foo", "bar", {0, 0, 0}, "bar", tf2_time, tf2::durationFromSec(0.1)); + + double epsilon = 1e-6; + EXPECT_NEAR(output.velocity.linear.x, 1.0, epsilon); + EXPECT_NEAR(output.velocity.linear.y, 0.0, epsilon); + EXPECT_NEAR(output.velocity.linear.z, 0.0, epsilon); + EXPECT_NEAR(output.velocity.angular.x, 0.0, epsilon); + EXPECT_NEAR(output.velocity.angular.y, 0.0, epsilon); + EXPECT_NEAR(output.velocity.angular.z, 0.0, epsilon); } @@ -245,8 +253,6 @@ TEST(test_buffer, test_twist) rclcpp::Time rclcpp_time = clock->now(); tf2::TimePoint tf2_time(std::chrono::nanoseconds(rclcpp_time.nanoseconds())); - std::cout << "seconds " << rclcpp_time.seconds() << std::endl; - float vel = 0.3; for (int i = -10; i < 5; ++i) { geometry_msgs::msg::TransformStamped transform; @@ -254,12 +260,8 @@ TEST(test_buffer, test_twist) if (i < 0) { transform.header.stamp = builtin_interfaces::msg::Time(rclcpp_time - rclcpp::Duration(std::fabs(i), 0)); - std::cout << "seconds " << - (rclcpp_time - rclcpp::Duration(std::fabs(i), 0)).seconds() << std::endl; - } else { transform.header.stamp = builtin_interfaces::msg::Time(rclcpp_time + rclcpp::Duration(i, 0)); - std::cout << "seconds " << (rclcpp_time + rclcpp::Duration(i, 0)).seconds() << std::endl; } transform.child_frame_id = "THISFRAME"; transform.transform.translation.x = i * vel; @@ -273,12 +275,18 @@ TEST(test_buffer, test_twist) } auto tw0 = buffer.lookupVelocity("THISFRAME", "PARENT", tf2_time, tf2::durationFromSec(4.001)); - std::cout << "tw0 : " << tw0.velocity.linear.x << std::endl; auto tw1 = buffer.lookupVelocity( "THISFRAME", "PARENT", "PARENT", {0, 0, 0}, "THISFRAME", tf2_time, tf2::durationFromSec(4.001)); - std::cout << "tw1 : " << tw1.velocity.linear.x << std::endl; + + double epsilon = 1e-6; + EXPECT_NEAR(tw1.velocity.linear.x, 0.3, epsilon); + EXPECT_NEAR(tw1.velocity.linear.y, 0.0, epsilon); + EXPECT_NEAR(tw1.velocity.linear.z, 0.0, epsilon); + EXPECT_NEAR(tw1.velocity.angular.x, 0.0, epsilon); + EXPECT_NEAR(tw1.velocity.angular.y, 0.0, epsilon); + EXPECT_NEAR(tw1.velocity.angular.z, 0.0, epsilon); } TEST(test_buffer, can_transform_without_dedicated_thread) From cf53a6295c865d058bd2ee9bab8d5d635a1f9bfe Mon Sep 17 00:00:00 2001 From: Tully Foote Date: Tue, 9 Apr 2024 13:50:51 -0700 Subject: [PATCH 08/12] fix line wrap --- .../include/tf2_geometry_msgs/tf2_geometry_msgs.hpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp b/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp index 0807c0bd2..33095fcb0 100644 --- a/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp +++ b/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp @@ -1359,10 +1359,13 @@ void doTransform( // lookupTransform(target_frame,msg_in.header.frame_id, msg_in.header.stamp, transform); tf2::Vector3 out_rot = transform_temp.getBasis() * twist_rot; - tf2::Vector3 out_vel = transform_temp.getBasis() * twist_vel + transform_temp.getOrigin().cross(out_rot); + tf2::Vector3 out_vel = transform_temp.getBasis() * twist_vel + \ + transform_temp.getOrigin().cross(out_rot); // geometry_msgs::TwistStamped interframe_twist; - // lookupVelocity(target_frame, msg_in.header.frame_id, msg_in.header.stamp, ros::Duration(0.1), interframe_twist); //\todo get rid of hard coded number + // lookupVelocity(target_frame, msg_in.header.frame_id, msg_in.header.stamp, + // ros::Duration(0.1), interframe_twist); + // \todo get rid of hard coded number t_out.header = t_in.header; t_out.velocity.linear.x = out_vel.x() + t_in.velocity.linear.x; From 005b90bcbe6ad4a984d68a0ba6db8e0144034813 Mon Sep 17 00:00:00 2001 From: Tully Foote Date: Tue, 9 Apr 2024 13:51:10 -0700 Subject: [PATCH 09/12] fix indention --- .../include/tf2_geometry_msgs/tf2_geometry_msgs.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp b/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp index 33095fcb0..8339b3f86 100644 --- a/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp +++ b/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp @@ -1332,11 +1332,11 @@ void doTransform( const geometry_msgs::msg::TransformStamped & transform) { tf2::Vector3 twist_rot(t_in.velocity.angular.x, - t_in.velocity.angular.y, - t_in.velocity.angular.z); + t_in.velocity.angular.y, + t_in.velocity.angular.z); tf2::Vector3 twist_vel(t_in.velocity.linear.x, - t_in.velocity.linear.y, - t_in.velocity.linear.z); + t_in.velocity.linear.y, + t_in.velocity.linear.z); tf2::Transform transform_temp; transform_temp.setOrigin(tf2::Vector3( From 10dc05c53406455619dd5f2dfd668dcab5cac881 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 9 Apr 2024 23:32:57 +0200 Subject: [PATCH 10/12] Visibility issue MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- tf2/include/tf2/buffer_core.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/tf2/include/tf2/buffer_core.h b/tf2/include/tf2/buffer_core.h index be6b7e98d..0c949e944 100644 --- a/tf2/include/tf2/buffer_core.h +++ b/tf2/include/tf2/buffer_core.h @@ -158,6 +158,7 @@ class BufferCore : public BufferCoreInterface const std::string & source_frame, const TimePoint & source_time, const std::string & fixed_frame) const override; + TF2_PUBLIC geometry_msgs::msg::VelocityStamped lookupVelocity( const std::string & tracking_frame, const std::string & observation_frame, const TimePoint & time, const tf2::Duration & averaging_interval) const; @@ -172,6 +173,7 @@ class BufferCore : public BufferCoreInterface * Possible exceptions TransformReference::LookupException, TransformReference::ConnectivityException, * TransformReference::MaxDepthException */ + TF2_PUBLIC geometry_msgs::msg::VelocityStamped lookupVelocity( const std::string & tracking_frame, const std::string & observation_frame, const std::string & reference_frame, const tf2::Vector3 & reference_point, From 130d29d2af00efc7e64b779ccd9ea2297e673b71 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 10 Apr 2024 00:18:48 +0200 Subject: [PATCH 11/12] make linters happy MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .../include/tf2_geometry_msgs/tf2_geometry_msgs.hpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp b/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp index 8339b3f86..2a60fa3ae 100644 --- a/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp +++ b/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp @@ -1368,12 +1368,12 @@ void doTransform( // \todo get rid of hard coded number t_out.header = t_in.header; - t_out.velocity.linear.x = out_vel.x() + t_in.velocity.linear.x; - t_out.velocity.linear.y = out_vel.y() + t_in.velocity.linear.y; - t_out.velocity.linear.z = out_vel.z() + t_in.velocity.linear.z; - t_out.velocity.angular.x = out_rot.x() + t_in.velocity.angular.x; - t_out.velocity.angular.y = out_rot.y() + t_in.velocity.angular.y; - t_out.velocity.angular.z = out_rot.z() + t_in.velocity.angular.z; + t_out.velocity.linear.x = out_vel.x() + t_in.velocity.linear.x; + t_out.velocity.linear.y = out_vel.y() + t_in.velocity.linear.y; + t_out.velocity.linear.z = out_vel.z() + t_in.velocity.linear.z; + t_out.velocity.angular.x = out_rot.x() + t_in.velocity.angular.x; + t_out.velocity.angular.y = out_rot.y() + t_in.velocity.angular.y; + t_out.velocity.angular.z = out_rot.z() + t_in.velocity.angular.z; } /**********************/ From ee27fb89658831cc84a44f0a888975201c105955 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 10 Apr 2024 10:17:41 +0200 Subject: [PATCH 12/12] Windows warnings MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- tf2_ros/test/test_buffer.cpp | 11 +++-- tf2_ros/test/velocity_test.cpp | 73 +++++++++++++++++----------------- 2 files changed, 44 insertions(+), 40 deletions(-) diff --git a/tf2_ros/test/test_buffer.cpp b/tf2_ros/test/test_buffer.cpp index 0362b2091..69a10d8d9 100644 --- a/tf2_ros/test/test_buffer.cpp +++ b/tf2_ros/test/test_buffer.cpp @@ -196,7 +196,8 @@ TEST(test_buffer, velocity_transform) geometry_msgs::msg::TransformStamped transform; transform.header.frame_id = "foo"; - transform.header.stamp = builtin_interfaces::msg::Time(rclcpp_time - rclcpp::Duration(0, 1e+9)); + transform.header.stamp = builtin_interfaces::msg::Time( + rclcpp_time - rclcpp::Duration(0, static_cast(1e+9))); transform.child_frame_id = "bar"; transform.transform.translation.x = 0; transform.transform.translation.y = 0; @@ -209,7 +210,8 @@ TEST(test_buffer, velocity_transform) EXPECT_TRUE(buffer.setTransform(transform, "unittest")); transform.header.frame_id = "foo"; - transform.header.stamp = builtin_interfaces::msg::Time(rclcpp_time + rclcpp::Duration(0, 1e+9)); + transform.header.stamp = builtin_interfaces::msg::Time( + rclcpp_time + rclcpp::Duration(0, static_cast(1e+9))); transform.child_frame_id = "bar"; transform.transform.translation.x = 2.0; transform.transform.translation.y = 0; @@ -253,13 +255,14 @@ TEST(test_buffer, test_twist) rclcpp::Time rclcpp_time = clock->now(); tf2::TimePoint tf2_time(std::chrono::nanoseconds(rclcpp_time.nanoseconds())); - float vel = 0.3; + float vel = 0.3f; for (int i = -10; i < 5; ++i) { geometry_msgs::msg::TransformStamped transform; transform.header.frame_id = "PARENT"; if (i < 0) { transform.header.stamp = - builtin_interfaces::msg::Time(rclcpp_time - rclcpp::Duration(std::fabs(i), 0)); + builtin_interfaces::msg::Time(rclcpp_time - rclcpp::Duration( + static_cast(std::fabs(i)), 0)); } else { transform.header.stamp = builtin_interfaces::msg::Time(rclcpp_time + rclcpp::Duration(i, 0)); } diff --git a/tf2_ros/test/velocity_test.cpp b/tf2_ros/test/velocity_test.cpp index 92aeecafd..fdd61285a 100644 --- a/tf2_ros/test/velocity_test.cpp +++ b/tf2_ros/test/velocity_test.cpp @@ -260,6 +260,7 @@ TEST_F(LinearVelocitySquareTest, LinearVelocityToThreeFrames) offset_frames.push_back("foo"); offset_frames.push_back("stationary_offset_child"); offset_frames.push_back("stationary_offset_parent"); + double epsilon = 1e-6; for (std::vector::iterator it = offset_frames.begin(); it != offset_frames.end(); ++it) @@ -269,57 +270,57 @@ TEST_F(LinearVelocitySquareTest, LinearVelocityToThreeFrames) auto twist = buffer_->lookupVelocity( "bar", *it, check_time, tf2::durationFromSec(0.1)); - EXPECT_FLOAT_EQ(twist.velocity.linear.x, 1.0); - EXPECT_FLOAT_EQ(twist.velocity.linear.y, 0.0); - EXPECT_FLOAT_EQ(twist.velocity.linear.z, 0.0); - EXPECT_FLOAT_EQ(twist.velocity.angular.x, 0.0); - EXPECT_FLOAT_EQ(twist.velocity.angular.y, 0.0); - EXPECT_FLOAT_EQ(twist.velocity.angular.z, 0.0); + EXPECT_NEAR(twist.velocity.linear.x, 1.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.y, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.y, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.z, 0.0, epsilon); check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 1.5); twist = buffer_->lookupVelocity("bar", *it, check_time, tf2::durationFromSec(0.1)); - EXPECT_FLOAT_EQ(twist.velocity.linear.x, 0.0); - EXPECT_FLOAT_EQ(twist.velocity.linear.y, 1.0); - EXPECT_FLOAT_EQ(twist.velocity.linear.z, 0.0); - EXPECT_FLOAT_EQ(twist.velocity.angular.x, 0.0); - EXPECT_FLOAT_EQ(twist.velocity.angular.y, 0.0); - EXPECT_FLOAT_EQ(twist.velocity.angular.z, 0.0); + EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.y, 1.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.y, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.z, 0.0, epsilon); check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 2.5); twist = buffer_->lookupVelocity("bar", *it, check_time, tf2::durationFromSec(0.1)); - EXPECT_FLOAT_EQ(twist.velocity.linear.x, -1.0); - EXPECT_FLOAT_EQ(twist.velocity.linear.y, 0.0); - EXPECT_FLOAT_EQ(twist.velocity.linear.z, 0.0); - EXPECT_FLOAT_EQ(twist.velocity.angular.x, 0.0); - EXPECT_FLOAT_EQ(twist.velocity.angular.y, 0.0); - EXPECT_FLOAT_EQ(twist.velocity.angular.z, 0.0); + EXPECT_NEAR(twist.velocity.linear.x, -1.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.y, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.y, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.z, 0.0, epsilon); check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 3.5); twist = buffer_->lookupVelocity("bar", *it, check_time, tf2::durationFromSec(0.1)); - EXPECT_FLOAT_EQ(twist.velocity.linear.x, 0.0); - EXPECT_FLOAT_EQ(twist.velocity.linear.y, -1.0); - EXPECT_FLOAT_EQ(twist.velocity.linear.z, 0.0); - EXPECT_FLOAT_EQ(twist.velocity.angular.x, 0.0); - EXPECT_FLOAT_EQ(twist.velocity.angular.y, 0.0); - EXPECT_FLOAT_EQ(twist.velocity.angular.z, 0.0); + EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.y, -1.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.z, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.y, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.z, 0.0, epsilon); check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 4.5); twist = buffer_->lookupVelocity("bar", *it, check_time, tf2::durationFromSec(0.1)); - EXPECT_FLOAT_EQ(twist.velocity.linear.x, 0.0); - EXPECT_FLOAT_EQ(twist.velocity.linear.y, 0.0); - EXPECT_FLOAT_EQ(twist.velocity.linear.z, 1.0); - EXPECT_FLOAT_EQ(twist.velocity.angular.x, 0.0); - EXPECT_FLOAT_EQ(twist.velocity.angular.y, 0.0); - EXPECT_FLOAT_EQ(twist.velocity.angular.z, 0.0); + EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.y, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.z, 1.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.y, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.z, 0.0, epsilon); check_time = tf2::timeFromSec(tf2::timeToSec(tf2_time_) + 5.5); twist = buffer_->lookupVelocity("bar", *it, check_time, tf2::durationFromSec(0.1)); - EXPECT_FLOAT_EQ(twist.velocity.linear.x, 0.0); - EXPECT_FLOAT_EQ(twist.velocity.linear.y, 0.0); - EXPECT_FLOAT_EQ(twist.velocity.linear.z, -1.0); - EXPECT_FLOAT_EQ(twist.velocity.angular.x, 0.0); - EXPECT_FLOAT_EQ(twist.velocity.angular.y, 0.0); - EXPECT_FLOAT_EQ(twist.velocity.angular.z, 0.0); + EXPECT_NEAR(twist.velocity.linear.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.y, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.linear.z, -1.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.x, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.y, 0.0, epsilon); + EXPECT_NEAR(twist.velocity.angular.z, 0.0, epsilon); } catch (tf2::TransformException & ex) { EXPECT_STREQ("", ex.what()); }