在ros下使用tf
tf真是一个好东西,把坐标变换都简化了
首先tf需要有一个broadcaster
#include <ros/ros.h> #include <tf/transform_broadcaster.h> #include <nav_msgs/Odometry.h> void poseCallback(const nav_msgs::OdometryConstPtr& msg){ static tf::TransformBroadcaster br; ROS_INFO("TF BROADCASTER"); tf::Transform transform; transform.setOrigin( tf::Vector3(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z) ); tf::Quaternion q; q.setRPY(0, 0, msg->pose.pose.orientation.z); transform.setRotation(q);
//在这里确定了world frame br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "pioneer_base")); } int main(int argc, char** argv){ ros::init(argc, argv, "tf_broadcaster"); ros::NodeHandle node; ros::Subscriber sub = node.subscribe("/RosAria/pose", 10, &poseCallback); ros::spin(); return 0; }
还要有一个listner
#include <ros/ros.h> #include <tf/transform_listener.h> #include <geometry_msgs/Twist.h> int main(int argc, char** argv){ ros::init(argc, argv, "tf_listener"); ros::NodeHandle node; tf::TransformListener listener; ros::Rate rate(10.0); while (node.ok()){ tf::StampedTransform transform; try{ listener.lookupTransform("/pioneer_base", "/world", ros::Time(0), transform); } catch (tf::TransformException &ex) { ROS_ERROR("%s",ex.what()); ros::Duration(1.0).sleep(); continue; } geometry_msgs::Twist vel_msg; vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(), transform.getOrigin().x()); vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) + pow(transform.getOrigin().y(), 2)); turtle_vel.publish(vel_msg); rate.sleep(); } return 0; }
lookupTransform查找到的是target到source坐标变换
void Transformer::lookupTransform(
const std::string & target_frame, const std::string & source_frame, const ros::Time & time, StampedTransform & transform ) const
http://docs.ros.org/kinetic/api/tf/html/c++/classtf_1_1Transformer.html#ac01a9f8709a828c427f1a5faa0ced42b
而tf_echo
http://wiki.ros.org/tf#tf_remap
格式是
tf_echo <source_frame> <target_frame>
是查找从source_frame 到 target_frame坐标系变换