ROS代碼經驗系列-- tf進行位置查詢變換
include文件:
#include "tf/transform_broadcaster.h" #include "tf/transform_listener.h" #include "tf/message_filter.h" #include "tf/tf.h" #include "message_filters/subscriber.h"
某時刻機器人在地圖上的位置:
當機器人在移動過程中,tf會不斷接收 base_link->odom 的位置關系信息,這些信息是根據時間不斷變化並被記錄下來的。當其它節點需要獲取某個時間點上的 base_link的位置時就可以通過下面的方法查詢:
x, y, yaw 就是base_link 在t 時刻在地圖上的位置:
bool getOdomPose(tf::Stamped<tf::Pose>& odom_pose, double& x, double& y, double& yaw, const ros::Time& t, const std::string& base_link) { // Get the robot‘s pose tf::Stamped<tf::Pose> ident (tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(0,0,0)), t, base_link ); try { tf_ = new tf::TransformListener(); tf_->transformPose(odom_frame_id_, ident, odom_pose); } catch(tf::TransformException e) { ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what()); return false; } x = odom_pose.getOrigin().x(); y = odom_pose.getOrigin().y(); double pitch,roll; odom_pose.getBasis().getEulerYPR(yaw, pitch, roll); return true; }
機器人某個位置相對map的位置關系:
機器人是矩形,四個角兒相對中心的位置已知,獲取四個角相對map的位置
tf::Stamped<tf::Pose> corner1( tf::Pose(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.30, -0.45, 0.0)), ros::Time(0), "base_link"); tf::Stamped<tf::Pose> corner2( tf::Pose(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.30, 0.45, 0.0)), ros::Time(0), "base_link"); tf::Stamped<tf::Pose> corner3( tf::Pose(tf::Quaternion(0, 0, 0, 1), tf::Vector3(-0.30, -0.45, 0.0)), ros::Time(0), "base_link"); tf::Stamped<tf::Pose> corner4( tf::Pose(tf::Quaternion(0, 0, 0, 1), tf::Vector3(-0.30, 0.45, 0.0)), ros::Time(0), "base_link"); transform_listener = new tf::TransformListener(); tf::Stamped<tf::Pose> transformed_corner_1; transform_listener.transformPose("map", corner_1, transformed_corner_1); tf::Stamped<tf::Pose> transformed_corner_2; transform_listener.transformPose("map", corner_2, transformed_corner_2); tf::Stamped<tf::Pose> transformed_corner_3; transform_listener.transformPose("map", corner_3, transformed_corner_3); tf::Stamped<tf::Pose> transformed_corner_1; transform_listener.transformPose("map", corner_4, transformed_corner_4);
隨機器人移動點在t+1時刻的位置
已知 t 時刻的位置是 pose_old,求t+1 時刻的位置 pose_new
tf_ = new tf::TransformListener(); tf::StampedTransform tx_odom; try { tf_->lookupTransform(base_frame_id_, ros::Time::now(), base_frame_id_, msg.header.stamp, global_frame_id_, tx_odom); } catch(tf::TransformException e) { ROS_WARN("Failed to transform initial pose in time (%s)", e.what()); tx_odom.setIdentity(); } tf::Pose pose_old, pose_new; tf::poseMsgToTF(msg.pose.pose, pose_old); pose_new = tx_odom.inverse() * pose_old; // Transform into the global frame ROS_INFO("Setting pose (%.6f): %.3f %.3f %.3f", ros::Time::now().toSec(), pose_new.getOrigin().x(), pose_new.getOrigin().y(), getYaw(pose_new));
這裏認為global_frame_id是不動的,pose_old和pose_new都是在global_frame_id坐標系下的坐標。但是pose_old描述的物體是隨著base_frame_id同步移動的
關於fixed frame的解釋:2.3 Transforms in Time
相對角度的轉換Quaternion
當base_link代表機器人時,激光掃描儀laser_scan安裝的角度與base_link不平行,即激光數據的零度不對應機器人的正前方零度。已知 laser_scan->angle_min 和 laser_scan->angle_increment 為激光數據信息,轉換角度到base_link的位置代碼如下,該算法可以考慮到激光器上下顛倒安裝的情況導致angle_increment為負:
tf::Quaternion q; q.setRPY(0.0, 0.0, laser_scan->angle_min); tf::Stamped<tf::Quaternion> min_q(q, laser_scan->header.stamp, laser_scan->header.frame_id); q.setRPY(0.0, 0.0, laser_scan->angle_min + laser_scan->angle_increment); tf::Stamped<tf::Quaternion> inc_q(q, laser_scan->header.stamp, laser_scan->header.frame_id); try { tf_->transformQuaternion(base_frame_id_, min_q, min_q); tf_->transformQuaternion(base_frame_id_, inc_q, inc_q); } catch(tf::TransformException& e) { ROS_WARN("Unable to transform min/max laser angles into base frame: %s", e.what()); return; } double angle_min = tf::getYaw(min_q); double angle_increment = tf::getYaw(inc_q) - angle_min; //考慮到了激光器上下顛倒安裝的情況導致為負數
已知 W->B 和B->A的坐標轉換,求W->A的坐標轉換
ROS 主動蒙特卡羅粒子濾波定位算法 AMCL 解析-- map與odom坐標轉換的方法
有時間差的lookupTransform
ros上的詳細教程
turtle1和turtle2都是 world 的child frame. turtle1->world 和turtle2->world 的tf都不斷發布的,現在需要知道這樣的一個transform轉換關系:
5秒中之前turtle1相對與現在的turtle2的位置關系
try{ ros::Time now = ros::Time::now(); ros::Time past = now - ros::Duration(5.0); listener.waitForTransform("/turtle2", now, "/turtle1", past, "/world", ros::Duration(1.0)); listener.lookupTransform("/turtle2", now, "/turtle1", past, "/world", transform);
得到的轉換結果可以這樣理解, ( transform.getOrigin().x(), transform.getOrigin().y() ) 是以turtle2為原點的XY平面上turtle1的坐標。
轉載:https://blog.csdn.net/crazyquhezheng/article/details/49124115
ROS代碼經驗系列-- tf進行位置查詢變換