ROS代码经验系列-- tf进行位置查询变换
2015-11-26 19:22
537 查看
include文件:
某时刻机器人在地图上的位置:
当机器人在移动过程中,tf会不断接收 base_link->odom 的位置关系信息,这些信息是根据时间不断变化并被记录下来的。当其它节点需要获取某个时间点上的 base_link的位置时就可以通过下面的方法查询:
x, y, yaw 就是base_link 在t 时刻在地图上的位置:
机器人某个位置相对map的位置关系:
机器人是矩形,四个角儿相对中心的位置已知,获取四个角相对map的位置
随机器人移动点在t+1时刻的位置
已知 t 时刻的位置是 pose_old,求t+1 时刻的位置 pose_new
相对角度的转换Quaternion
当base_link代表机器人时,激光扫描仪laser_scan安装的角度与base_link不平行,即激光数据的零度不对应机器人的正前方零度。已知 laser_scan->angle_min 和 laser_scan->angle_increment 为激光数据信息,转换角度到base_link的位置代码如下,该算法可以考虑到激光器上下颠倒安装的情况导致angle_increment为负:
</pre><pre name="code" class="cpp">// For transform support #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));
相对角度的转换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; //考虑到了激光器上下颠倒安装的情况导致为负数
相关文章推荐
- java如何设置JFrame背景图片
- 以前用C写的2048,那时候写代码还有点傻逼
- Spring源码追踪4——SpringMVC View解析
- python学习 对python的认识
- PHP出现Cannot modify header information问题的解决方法
- 快速排序
- JAVA中int、String的类型转换
- struts中的校验框架
- C++矩阵库 Eigen 快速入门
- Java学习笔记(多线程_1)
- python学习笔记(13)
- 【php wamp的配置】
- c++ ip地址的操作 c版
- C++11学习笔记(二)
- Git的基本使用和上传到github的远程仓库
- python学习笔记(13)
- php Socket通信
- Struts2的国际化
- C++知识点梳理(1)
- java环境变量的配置