From 47098d6ff84f8941872c9aba0b186ecc058393cb Mon Sep 17 00:00:00 2001 From: Adnan Munawar Date: Tue, 20 May 2014 16:15:06 -0400 Subject: [PATCH 01/11] Added and optional third argument to specify publishing frequency --- tf/src/tf_echo.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/tf/src/tf_echo.cpp b/tf/src/tf_echo.cpp index 6be88d56..aa15db8c 100644 --- a/tf/src/tf_echo.cpp +++ b/tf/src/tf_echo.cpp @@ -57,8 +57,9 @@ int main(int argc, char ** argv) { //Initialize ROS ros::init(argc, argv, "tf_echo", ros::init_options::AnonymousName); + double default_rate = 1; - if (argc != 3) + if (argc < 3) { printf("Usage: tf_echo source_frame target_frame\n\n"); printf("This will echo the transform from the coordinate frame of the source_frame\n"); @@ -66,8 +67,14 @@ int main(int argc, char ** argv) printf("Note: This is the transform to get data from target_frame into the source_frame.\n"); return -1; } + if (argc == 4) + { + default_rate = atof(argv[3]); + } ros::NodeHandle nh; + //Initiate a default rate of 500 Hz + ros::Rate rate(default_rate); //Instantiate a local listener echoListener echoListener; @@ -109,7 +116,7 @@ int main(int argc, char ** argv) std::cout << echoListener.tf.allFramesAsString()< Date: Wed, 11 Feb 2015 11:51:06 -0800 Subject: [PATCH 02/11] using TimeStamp and FrameId in message filter this allows to use tf::MessageFilter with pcl::PointCloud see #55 --- tf/include/tf/message_filter.h | 23 +++++++++++++++++------ 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/tf/include/tf/message_filter.h b/tf/include/tf/message_filter.h index efee719c..1afde9b5 100644 --- a/tf/include/tf/message_filter.h +++ b/tf/include/tf/message_filter.h @@ -111,9 +111,6 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi typedef boost::function FailureCallback; typedef boost::signals2::signal FailureSignal; - // If you hit this assert your message does not have a header, or does not have the HasHeader trait defined for it - ROS_STATIC_ASSERT(ros::message_traits::HasHeader::value); - /** * \brief Constructor * @@ -260,7 +257,11 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi { ++dropped_message_count_; const MEvent& front = messages_.front(); - TF_MESSAGEFILTER_DEBUG("Removed oldest message because buffer is full, count now %d (frame_id=%s, stamp=%f)", message_count_, front.getMessage()->header.frame_id.c_str(), front.getMessage()->header.stamp.toSec()); + TF_MESSAGEFILTER_DEBUG( + "Removed oldest message because buffer is full, count now %d (frame_id=%s, stamp=%f)", + message_count_, + ros::message_traits::FrameId::value(*front.getMessage()).c_str(), + ros::message_traits::TimeStamp::value(*front.getMessage()).toSec()); signalFailure(messages_.front(), filter_failure_reasons::Unknown); messages_.pop_front(); @@ -272,7 +273,11 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi ++message_count_; } - TF_MESSAGEFILTER_DEBUG("Added message in frame %s at time %.3f, count now %d", evt.getMessage()->header.frame_id.c_str(), evt.getMessage()->header.stamp.toSec(), message_count_); + TF_MESSAGEFILTER_DEBUG( + "Added message in frame %s at time %.3f, count now %d", + ros::message_traits::FrameId::value(*evt.getMessage()).c_str(), + ros::message_traits::TimeStamp::value(*evt.getMessage()).toSec(), + message_count_); ++incoming_message_count_; } @@ -369,7 +374,13 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi { ++failed_out_the_back_count_; ++dropped_message_count_; - TF_MESSAGEFILTER_DEBUG("Discarding Message, in frame %s, Out of the back of Cache Time(stamp: %.3f + cache_length: %.3f < latest_transform_time %.3f. Message Count now: %d", message->header.frame_id.c_str(), message->header.stamp.toSec(), tf_.getCacheLength().toSec(), latest_transform_time.toSec(), message_count_); + TF_MESSAGEFILTER_DEBUG( + "Discarding Message, in frame %s, Out of the back of Cache Time " + "(stamp: %.3f + cache_length: %.3f < latest_transform_time %.3f. " + "Message Count now: %d", + ros::message_traits::FrameId::value(*message).c_str(), + ros::message_traits::TimeStamp::value(*message).toSec(), + tf_.getCacheLength().toSec(), latest_transform_time.toSec(), message_count_); last_out_the_back_stamp_ = stamp; last_out_the_back_frame_ = frame_id; From 57730a23d47020a916a6eca555c2be44ac31974e Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Thu, 2 Apr 2015 11:13:51 -0700 Subject: [PATCH 03/11] Fixed command line arguments --- tf/src/tf_echo.cpp | 25 +++++++++++++++---------- 1 file changed, 15 insertions(+), 10 deletions(-) diff --git a/tf/src/tf_echo.cpp b/tf/src/tf_echo.cpp index 688efeae..2f4069e6 100644 --- a/tf/src/tf_echo.cpp +++ b/tf/src/tf_echo.cpp @@ -57,27 +57,32 @@ int main(int argc, char ** argv) { //Initialize ROS ros::init(argc, argv, "tf_echo", ros::init_options::AnonymousName); - double default_rate = 1; - if (argc < 3) + // Allow 2 or 3 command line arguments + if (argc < 3 || argc > 4) { - printf("Usage: tf_echo source_frame target_frame\n\n"); + printf("Usage: tf_echo source_frame target_frame [echo_rate]\n\n"); printf("This will echo the transform from the coordinate frame of the source_frame\n"); printf("to the coordinate frame of the target_frame. \n"); printf("Note: This is the transform to get data from target_frame into the source_frame.\n"); + printf("Default echo rate is 1 if echo_rate is not given.\n"); return -1; } - if (argc == 4) - { - default_rate = atof(argv[3]); - } ros::NodeHandle nh; - // read rate parameter - ros::NodeHandle p_nh("~"); double rate_hz; - p_nh.param("rate", rate_hz, 1.0); + if (argc == 4) + { + // read rate from command line + rate_hz = atof(argv[3]); + } + else + { + // read rate parameter + ros::NodeHandle p_nh("~"); + p_nh.param("rate", rate_hz, 1.0); + } ros::Rate rate(rate_hz); //Instantiate a local listener From 392546dd9931d1c913561f9aab0a8df8b00df9cd Mon Sep 17 00:00:00 2001 From: Ying Lu Date: Thu, 2 Apr 2015 11:23:11 -0700 Subject: [PATCH 04/11] display RPY in both radian and degree --- tf/src/tf_echo.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/tf/src/tf_echo.cpp b/tf/src/tf_echo.cpp index 7ec50af7..b11a4362 100644 --- a/tf/src/tf_echo.cpp +++ b/tf/src/tf_echo.cpp @@ -31,6 +31,7 @@ #include "tf/transform_listener.h" #include "ros/ros.h" +#define _USE_MATH_DEFINES class echoListener { public: @@ -104,7 +105,8 @@ int main(int argc, char ** argv) std::cout << "- Translation: [" << v.getX() << ", " << v.getY() << ", " << v.getZ() << "]" << std::endl; std::cout << "- Rotation: in Quaternion [" << q.getX() << ", " << q.getY() << ", " << q.getZ() << ", " << q.getW() << "]" << std::endl - << " in RPY [" << roll << ", " << pitch << ", " << yaw << "]" << std::endl; + << " in RPY (radian) [" << roll << ", " << pitch << ", " << yaw << "]" << std::endl + << " in RPY (degree) [" << roll*180.0/M_PI << ", " << pitch*180.0/M_PI << ", " << yaw*180.0/M_PI << "]" << std::endl; //print transform } From e4e19ffee46ee3dc55dbbb52c201b1e7d2dce54a Mon Sep 17 00:00:00 2001 From: Ying Lu Date: Thu, 2 Apr 2015 14:36:57 -0700 Subject: [PATCH 05/11] more tutorial added for lookupTwist function --- tf/include/tf/tf.h | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/tf/include/tf/tf.h b/tf/include/tf/tf.h index 39cadd1b..488cc24d 100644 --- a/tf/include/tf/tf.h +++ b/tf/include/tf/tf.h @@ -156,7 +156,12 @@ class Transformer * This will compute the average velocity on the interval * (time - duration/2, time+duration/2). If that is too close to the most * recent reading, in which case it will shift the interval up to - * duration/2 to prevent extrapolation. Possible exceptions + * duration/2 to prevent extrapolation. The observation_frame is the same with + * reference_frame, except when we express the twist with respect to (wrt) a point, which + * is NOT the origin of the tracking_frame. A simple example is: we want to get the wrist + * at the corner of a box, instead of the center, which is the origin of the tracking_frame + * locates. + * Possible exceptions * tf::LookupException, tf::ConnectivityException, * tf::MaxDepthException, tf::ExtrapolationException * From 95b076ea02191d4509b92e0d90acd2fb2e92d2bc Mon Sep 17 00:00:00 2001 From: Ying Lu Date: Thu, 2 Apr 2015 14:43:22 -0700 Subject: [PATCH 06/11] minor change on space issue --- tf/include/tf/tf.h | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/tf/include/tf/tf.h b/tf/include/tf/tf.h index 488cc24d..bd833bd9 100644 --- a/tf/include/tf/tf.h +++ b/tf/include/tf/tf.h @@ -157,10 +157,12 @@ class Transformer * (time - duration/2, time+duration/2). If that is too close to the most * recent reading, in which case it will shift the interval up to * duration/2 to prevent extrapolation. The observation_frame is the same with - * reference_frame, except when we express the twist with respect to (wrt) a point, which - * is NOT the origin of the tracking_frame. A simple example is: we want to get the wrist - * at the corner of a box, instead of the center, which is the origin of the tracking_frame - * locates. + * reference_frame, the observation_frame helps when we express the twist with + * respect to a point, which is NOT the origin of the tracking_frame. A simple + * example is: in order to get the wrist at a corner of a box, instead of the + * center of the box, which is the origin of the tracking_frame locates, we need + * to get the transform from the desired point (a corner of box) to origin of tracking + * frame (center of box), and then from the tracking frame to the reference frame * Possible exceptions * tf::LookupException, tf::ConnectivityException, * tf::MaxDepthException, tf::ExtrapolationException From 67cd21556c5c74a2591b04e7541f9c9149042ce2 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Thu, 2 Apr 2015 19:14:52 -0700 Subject: [PATCH 07/11] Fixed Vector3 documentation --- tf/conf.py | 3 +-- tf/include/tf/LinearMath/Vector3.h | 8 +++++--- tf_conversions/conf.py | 3 +-- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/tf/conf.py b/tf/conf.py index 9b9c307b..b3482136 100644 --- a/tf/conf.py +++ b/tf/conf.py @@ -199,7 +199,6 @@ # Example configuration for intersphinx: refer to the Python standard library. intersphinx_mapping = { 'http://docs.python.org/': None, - 'http://opencv.willowgarage.com/documentation/': None, - 'http://opencv.willowgarage.com/documentation/python/': None, + 'http://docs.opencv.org/3.0-last-rst/': None, 'http://docs.scipy.org/doc/numpy' : None } diff --git a/tf/include/tf/LinearMath/Vector3.h b/tf/include/tf/LinearMath/Vector3.h index 6e0309cc..0ae3bdec 100644 --- a/tf/include/tf/LinearMath/Vector3.h +++ b/tf/include/tf/LinearMath/Vector3.h @@ -29,11 +29,13 @@ namespace tf{ -/**@brief Vector3 can be used to represent 3D points and vectors. +/** + * @class Vector3 + * @brief Vector3 can be used to represent 3D points and vectors. * It has an un-used w component to suit 16-byte alignment when Vector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user * Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers */ -ATTRIBUTE_ALIGNED16(class) Vector3 +class Vector3 { public: @@ -345,7 +347,7 @@ ATTRIBUTE_ALIGNED16(class) Vector3 TFSIMD_FORCE_INLINE void deSerializeDouble(const struct Vector3DoubleData& dataIn); -}; +} __attribute__ ((aligned(16))); /**@brief Return the sum of two vectors (Point symantics)*/ TFSIMD_FORCE_INLINE Vector3 diff --git a/tf_conversions/conf.py b/tf_conversions/conf.py index 6d49f76e..4f6b7609 100644 --- a/tf_conversions/conf.py +++ b/tf_conversions/conf.py @@ -199,8 +199,7 @@ # Example configuration for intersphinx: refer to the Python standard library. intersphinx_mapping = { 'http://docs.python.org/': None, - 'http://opencv.willowgarage.com/documentation/': None, - 'http://opencv.willowgarage.com/documentation/python/': None, + 'http://docs.opencv.org/3.0-last-rst/': None, 'http://www.ros.org/doc/api/tf/html/python/' : None, 'http://docs.scipy.org/doc/numpy' : None, 'http://www.ros.org/doc/api/kdl/html/python/' : None From 6abb79c419ae89e0c4e903c292b6b4e7ae864140 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Thu, 2 Apr 2015 21:52:28 -0700 Subject: [PATCH 08/11] Attempted to add transformations --- tf/index.rst | 1 + tf/src/tf/transformations.rst | 3 +++ 2 files changed, 4 insertions(+) create mode 100644 tf/src/tf/transformations.rst diff --git a/tf/index.rst b/tf/index.rst index f3a69eaa..942936c2 100644 --- a/tf/index.rst +++ b/tf/index.rst @@ -5,6 +5,7 @@ tf :maxdepth: 2 tf_python + transformations Indices and tables ================== diff --git a/tf/src/tf/transformations.rst b/tf/src/tf/transformations.rst new file mode 100644 index 00000000..f883c5ea --- /dev/null +++ b/tf/src/tf/transformations.rst @@ -0,0 +1,3 @@ +transformations +============== +.. autodoc::tf.transformations From f905d92f0ffb29132c752f9b4415975f048ca16c Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Thu, 2 Apr 2015 23:23:34 -0700 Subject: [PATCH 09/11] generated autodoc --- tf/src/tf/transformations.rst | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/tf/src/tf/transformations.rst b/tf/src/tf/transformations.rst index f883c5ea..06182616 100644 --- a/tf/src/tf/transformations.rst +++ b/tf/src/tf/transformations.rst @@ -1,3 +1,4 @@ transformations ============== -.. autodoc::tf.transformations +.. automodule:: tf.transformations + :members: From 6dd45fdab84a0d5cf004316499e4634fd8a6ac7b Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Thu, 2 Apr 2015 23:28:09 -0700 Subject: [PATCH 10/11] Moved to the right dir --- tf/{src/tf => }/transformations.rst | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename tf/{src/tf => }/transformations.rst (100%) diff --git a/tf/src/tf/transformations.rst b/tf/transformations.rst similarity index 100% rename from tf/src/tf/transformations.rst rename to tf/transformations.rst From 1064ecbacc5f3a2dffa4a034c832e758b36e8794 Mon Sep 17 00:00:00 2001 From: Ying Lu Date: Sun, 19 Apr 2015 17:46:54 -0700 Subject: [PATCH 11/11] issue 89 compiles --- tf/CMakeLists.txt | 6 +++--- tf/include/tf/transform_listener.h | 2 +- tf/src/transform_listener.cpp | 10 +++++++--- tf/test/transform_twist_test.cpp | 4 ++-- 4 files changed, 13 insertions(+), 9 deletions(-) diff --git a/tf/CMakeLists.txt b/tf/CMakeLists.txt index 80775619..85c624b8 100644 --- a/tf/CMakeLists.txt +++ b/tf/CMakeLists.txt @@ -70,10 +70,10 @@ add_rostest(test/transform_listener_unittest.launch) catkin_add_gtest(test_velocity test/velocity_test.cpp) target_link_libraries(test_velocity ${PROJECT_NAME}) -#add_executable(test_transform_twist test/transform_twist_test.cpp) -#target_link_libraries(test_transform_twist ${PROJECT_NAME}) +add_executable(test_transform_twist test/transform_twist_test.cpp) +target_link_libraries(test_transform_twist ${PROJECT_NAME} ${GTEST_LIBRARIES}) #catkin_add_gtest_build_flags(test_transform_twist) -#add_rostest(test/transform_twist_test.launch) +add_rostest(test/transform_twist_test.launch) catkin_add_gtest(cache_unittest test/cache_unittest.cpp) target_link_libraries(cache_unittest ${PROJECT_NAME}) diff --git a/tf/include/tf/transform_listener.h b/tf/include/tf/transform_listener.h index a2d30591..8009cc4a 100644 --- a/tf/include/tf/transform_listener.h +++ b/tf/include/tf/transform_listener.h @@ -106,7 +106,7 @@ class TransformListener : public Transformer { //subscribes to message and autom /** \brief Transform a Stamped Twist Message into the target frame * This can throw all that lookupTransform can throw as well as tf::InvalidTransform */ // http://www.ros.org/wiki/tf/Reviews/2010-03-12_API_Review - // void transformTwist(const std::string& target_frame, const geometry_msgs::TwistStamped& stamped_in, geometry_msgs::TwistStamped& stamped_out) const; + void transformTwist(const std::string& target_frame, const geometry_msgs::TwistStamped& stamped_in, geometry_msgs::TwistStamped& stamped_out) const; /** \brief Transform a Stamped Quaternion Message into the target frame * This can throw all that lookupTransform can throw as well as tf::InvalidTransform */ diff --git a/tf/src/transform_listener.cpp b/tf/src/transform_listener.cpp index 75116022..d3b57329 100644 --- a/tf/src/transform_listener.cpp +++ b/tf/src/transform_listener.cpp @@ -108,7 +108,7 @@ void TransformListener::transformPose(const std::string& target_frame, transformPose(target_frame, pin, pout); poseStampedTFToMsg(pout, msg_out); } -/* http://www.ros.org/wiki/tf/Reviews/2010-03-12_API_Review +// http://www.ros.org/wiki/tf/Reviews/2010-03-12_API_Review void TransformListener::transformTwist(const std::string& target_frame, const geometry_msgs::TwistStamped& msg_in, geometry_msgs::TwistStamped& msg_out) const @@ -128,7 +128,11 @@ void TransformListener::transformTwist(const std::string& target_frame, tf::Vector3 out_vel = transform.getBasis()* twist_vel + transform.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 + const std::string observe_frame = msg_in.header.frame_id; + const ros::Time ltime = msg_in.header.stamp; + const ros::Duration lduration = ros::Duration(0.1); + lookupTwist(target_frame, observe_frame, ltime, lduration, interframe_twist.twist); msg_out.header.stamp = msg_in.header.stamp; msg_out.header.frame_id = target_frame; @@ -139,7 +143,7 @@ void TransformListener::transformTwist(const std::string& target_frame, msg_out.twist.angular.y = out_rot.y() + interframe_twist.twist.angular.y; msg_out.twist.angular.z = out_rot.z() + interframe_twist.twist.angular.z; - }*/ +} void TransformListener::transformQuaternion(const std::string& target_frame, const ros::Time& target_time, const geometry_msgs::QuaternionStamped& msg_in, diff --git a/tf/test/transform_twist_test.cpp b/tf/test/transform_twist_test.cpp index e303a2bc..854b9bb8 100644 --- a/tf/test/transform_twist_test.cpp +++ b/tf/test/transform_twist_test.cpp @@ -235,7 +235,7 @@ TEST_F(TransformTwistAngularTest, AngularVelocityAlone) EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon); - // tf_.lookupVelocity("foo", "bar", ros::Time(2.5), ros::Duration(0.1), tw); + //tf_.lookupVelocity("foo", "bar", ros::Time(2.5), ros::Duration(0.1), tw); tw_in =tw_y; tw_in.header.stamp = ros::Time(2.5); tf_.transformTwist("foo", tw_in, tw); @@ -268,7 +268,7 @@ TEST_F(TransformTwistAngularTest, AngularVelocityAlone) EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.z, 2.0, epsilon); - // tf_.lookupVelocity("foo", "bar", ros::Time(5.5), ros::Duration(0.1), tw); + //tf_.lookupVelocity("foo", "bar", ros::Time(5.5), ros::Duration(0.1), tw); tw_in =tw_z; tw_in.header.stamp = ros::Time(5.5); tf_.transformTwist("foo", tw_in, tw);