From 62322b8fa74c90d144dfe32294f6670bf6cfe786 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 10 Apr 2024 14:06:00 +0200 Subject: [PATCH] Enable Twist interpolator (#646) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero Co-authored-by: Tully Foote --- tf2/include/tf2/buffer_core.h | 23 + tf2/src/buffer_core.cpp | 119 ++++ .../tf2_geometry_msgs/tf2_geometry_msgs.hpp | 63 ++ .../test/test_tf2_geometry_msgs.cpp | 13 + tf2_py/src/tf2_py.cpp | 90 +-- tf2_py/test/test_buffer_core.py | 26 + tf2_ros/CMakeLists.txt | 8 + tf2_ros/test/test_buffer.cpp | 108 ++++ tf2_ros/test/velocity_test.cpp | 545 ++++++++++++++++++ 9 files changed, 960 insertions(+), 35 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 cb6f78c44..0c949e944 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 "rcutils/logging_macros.h" #include "tf2/buffer_core_interface.h" #include "tf2/exceptions.h" @@ -157,6 +158,28 @@ 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; + + /** \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 + */ + 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, + const std::string & reference_point_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/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index 3da2b2d48..97fa96bca 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" @@ -574,6 +576,123 @@ struct TransformAccum tf2::Vector3 result_vec; }; +geometry_msgs::msg::VelocityStamped BufferCore::lookupVelocity( + const std::string & tracking_frame, const std::string & observation_frame, + const TimePoint & time, const tf2::Duration & averaging_interval) const +{ + // 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(observation_frame), + lookupFrameNumber(tracking_frame), + latest_time, + 0); + + 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)); + + 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(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(); + + 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::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::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( + 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 = tracking_frame; + + velocity.velocity.linear.x = out_vel.x(); + velocity.velocity.linear.y = out_vel.y(); + velocity.velocity.linear.z = out_vel.z(); + velocity.velocity.angular.x = out_rot.x(); + velocity.velocity.angular.y = out_rot.y(); + velocity.velocity.angular.z = out_rot.z(); + + return velocity; +} + 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..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 @@ -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,68 @@ 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) +{ + 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; +} + /**********************/ /*** 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..069965a0c 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,17 @@ 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); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); 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..2b88134d6 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_velocity(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/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..69a10d8d9 100644 --- a/tf2_ros/test/test_buffer.cpp +++ b/tf2_ros/test/test_buffer.cpp @@ -184,6 +184,114 @@ 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, static_cast(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, static_cast(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)); + + 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); +} + + +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())); + + 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( + static_cast(std::fabs(i)), 0)); + } else { + transform.header.stamp = builtin_interfaces::msg::Time(rclcpp_time + rclcpp::Duration(i, 0)); + } + 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)); + + auto tw1 = buffer.lookupVelocity( + "THISFRAME", "PARENT", "PARENT", {0, 0, 0}, "THISFRAME", + tf2_time, tf2::durationFromSec(4.001)); + + 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) { 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..fdd61285a --- /dev/null +++ b/tf2_ros/test/velocity_test.cpp @@ -0,0 +1,545 @@ +/* + * 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); + quat = quat.normalize(); + + { + 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 = 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; + 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.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"); + double epsilon = 1e-6; + + 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_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_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_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_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_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_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()); + } + } +} + +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); + + 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()); + } +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}