tutorial on lookupTwist function

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})

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
     }

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 

tf/include/tf/message_filter.h

@@ -111,9 +111,6 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi
   typedef boost::function<void(const MConstPtr&, FilterFailureReason)> FailureCallback;
   typedef boost::signals2::signal<void(const MConstPtr&, FilterFailureReason)> 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<M>::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<M>::value(*front.getMessage()).c_str(),
+              ros::message_traits::TimeStamp<M>::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<M>::value(*evt.getMessage()).c_str(),
+          ros::message_traits::TimeStamp<M>::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<M>::value(*message).c_str(),
+                ros::message_traits::TimeStamp<M>::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;

tf/include/tf/tf.h

@@ -156,7 +156,14 @@ 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, 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
    * 

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 */

tf/index.rst

@@ -5,6 +5,7 @@ tf
     :maxdepth: 2
 
     tf_python
+    transformations
 
 Indices and tables
 ==================

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:
@@ -58,21 +59,31 @@ int main(int argc, char ** argv)
   //Initialize ROS
   ros::init(argc, argv, "tf_echo", ros::init_options::AnonymousName);
 
-  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;
   }
 
   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
@@ -104,7 +115,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
       }

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,

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);

tf/transformations.rst

@@ -0,0 +1,4 @@
+transformations
+==============
+.. automodule:: tf.transformations
+  :members:

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