using "tf2" to transform coordinates
>Dependency:
1 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
2 #include <tf2_ros/transform_listener.h>
>To install the dependencies, do:
1 [molmc-chy:~/SLAM-proj] 4s $ sudo apt-get install ros-indigo-tf2-geometry-msgs 2 3 [molmc-chy:~/SLAM-proj] 4s $ sudo apt-get install ros-indigo-orocos-kdl //tf2-geometry-msgs.h refers to "kdl" library
>In CMakeLists.txt, add:
1 find_package(Orocos-KDL REQUIRED) 2 add_executable(${PROJECT_NAME} src/prueba.cpp) 3 target_link_libraries(${PROJECT_NAME} orocos-kdl)
>After above, then in code, for example:
In this example, "inputPose" represents a point in frame "base_link", then I can transform this point into frame "leap_motion" when I recieve the transformation from "base_link" to "leap_motion".
1 #include <tf2_geometry_msgs/tf2_geometry_msgs.h> 2 #include <tf2_ros/transform_listener.h> 3 ...... 4 5 tf2_ros::Buffer tfBuffer; 6 tf2_ros::TransformListener tf2_listener(tfBuffer); 7 geometry_msgs::TransformStamped base_link_to_leap_motion; // My frames are named "base_link" and "leap_motion" 8 9 base_link_to_leap_motion = tfBuffer.lookupTransform("leap_motion", "base_link", ros::Time(0), ros::Duration(1.0) ); 10 11 tf2::doTransform(inputPose, outputPose, base_link_to_leap_motion); // inputPose and outputPose is the PointStamped I want to transform
>Refer to the document of tf2:
http://docs.ros.org/diamondback/api/tf2_geometry_msgs/html/namespacetf2.html We can find that in function: "void doTransform(const T& data_in, T& data_out, const geometry_msgs::TransformStamped& transform);" the template T can be geometry_msgs::PoseStamped/PointStamped/Vector3Stamped, that means tf2::doTransform() can transform both point and coordinate.
Detailed realization of function can be found in "tf2_geometry_msgs.h", where the "KDL::Frame" has been used.
PS. "(gmTransformToKDL(transform) * KDL::Frame(r, v)" : The reason that two variables of type KDL::Frame can be multipled directly is "operator overloading(运算符重载)".
PPS. Another reference: http://wiki.ros.org/tf2/Tutorials/Using%20stamped%20datatypes%20with%20tf2%3A%3AMessageFilter
