潍坊网站设计制作,棋牌源码交易商城,建立网店,lnmp架构部署wordpress目录 一、静态坐标变换#xff08;C实现#xff09;二、静态坐标变换#xff08;Python实现#xff09; 如前文所属#xff0c;ROS通过广播的形式告知各模块的位姿关系#xff0c;接下来详述这一机制的代码实现。
模块间的位置关系有两种类型#xff0c;一种是相对固定… 目录 一、静态坐标变换C实现二、静态坐标变换Python实现 如前文所属ROS通过广播的形式告知各模块的位姿关系接下来详述这一机制的代码实现。
模块间的位置关系有两种类型一种是相对固定的称为静态坐标变换一种是相对不固定变化的称为动态坐标变换。
一、静态坐标变换C实现
所谓静态坐标变换是指两个坐标系之间的相对位置是固定的。比如机器人底盘上安装了一个激光雷达他和底盘组成一个刚体它们的相对位姿不会随机器人的运动而变化他们之间的坐标变换即属于静态坐标变换。
假设激光雷达相对与底盘的欧拉位姿为0.5, 0.0, 0.3; 0.0, 0.0, 0.0
雷达检测到的障碍物位置为2.0, 2.5, 0.3
若要计算障碍物和底盘的相对位置就可以通过雷达到底盘的坐标变换来计算步骤如下
雷达laser发布自己和底盘base_link的相对静态坐标避障模块监听雷达laser和底盘base_link的相对坐标关系并通过tf 计算障碍物位置。
首先创建 tf2_learning 包命令如下这一步不是必须这里只是为了方便清晰的说明也可以使用已有的包在包里新增节点等方法
catkin_creat_pkg tf2_learning roscpp rospy geometry_msgs std_msgs tf2 tf2_geometry_msgs tf2_ros创建后文件结构如下 在创建的 tf2_learning 包路径下有一个 src 目录在这里存储C源码我们创建 static_frame_broadcast.cpp 和 static_frame_listen.cpp 修改 CMakeLists.txt 添加如下内容
add_executable(${PROJECT_NAME}_broadcast src/static_frame_broadcast.cpp)
add_executable(${PROJECT_NAME}_listen src/static_frame_listen.cpp)target_link_libraries(${PROJECT_NAME}_broadcast${catkin_LIBRARIES}
)target_link_libraries(${PROJECT_NAME}_listen${catkin_LIBRARIES}
)static_frame_broadcast.cpp 实现广播子坐标系相对于父坐标系的静态坐标内容如下
#include ros/ros.h
#include tf2_ros/static_transform_broadcaster.h
#include geometry_msgs/TransformStamped.h
#include tf2/LinearMath/Quaternion.hint main(int argc, char **argv)
{// 初始化 ROS 节点ros::init(argc, argv, static_frame_broadcast);// 创建静态坐标转换广播器tf2_ros::StaticTransformBroadcaster broadcaster;// 创建坐标系信息geometry_msgs::TransformStamped ts;// --设置头信息ts.header.seq 100;ts.header.stamp ros::Time::now();ts.header.frame_id base_link;// --设置子级坐标系ts.child_frame_id laser;// --设置子坐标系相对于父坐标系的平移偏移量ts.transform.translation.x 0.5;ts.transform.translation.y 0.0;ts.transform.translation.z 0.3;// --设置子坐标系相对于父坐标系的旋转偏移量// --将欧拉角转换成四元数tf2::Quaternion qtn; // tf2的四元数类qtn.setRPY(0, 0, 0); // 设置欧拉角// 获取旋转的四元数值ts.transform.rotation.x qtn.getX();ts.transform.rotation.y qtn.getY();ts.transform.rotation.z qtn.getZ();ts.transform.rotation.w qtn.getW();// 广播器发布坐标系信息broadcaster.sendTransform(ts);ros::spin();return 0;
}static_frame_listen.cpp 实现订阅静态坐标转换关系并利用该关系将雷达坐标系的点转换到 base_link 坐标系内容如下
#include ros/ros.h
#include tf2_ros/transform_listener.h
#include tf2_ros/buffer.h
#include geometry_msgs/PointStamped.h
#include tf2_geometry_msgs/tf2_geometry_msgs.hint main(int argc, char *argv[])
{// 初始化 ROS 节点ros::init(argc, argv, static_frame_listen);ros::NodeHandle nh;// 创建 TF 订阅节点tf2_ros::Buffer buffer;tf2_ros::TransformListener listener(buffer);ros::Rate rate(1);while (ros::ok()){// 生成一个坐标点, 模拟雷达检测到的障碍物坐标点(雷达坐标系下的坐标)geometry_msgs::PointStamped point_laser;point_laser.header.frame_id laser;point_laser.header.stamp ros::Time::now();point_laser.point.x 2.0;point_laser.point.y 2.5;point_laser.point.z 0.3;// 转换坐标点, 计算障碍物坐标点在 base_link 下的坐标try{geometry_msgs::PointStamped point_base;point_base buffer.transform(point_laser, base_link);ROS_INFO(point_base: (%.2f, %.2f, %.2f), frame: %s,point_base.point.x, point_base.point.y, point_base.point.z,point_base.header.frame_id.c_str());}catch (const std::exception e){ROS_ERROR(%s, e.what());}rate.sleep();ros::spinOnce();}return 0;
}编译后执行 rosrun tf2_learning tf2_learning_broadcast 开始广播坐标此时打开rviz订阅TF看到TF树模型操作与结果如下
输入命令rviz在启动的 rviz 中设置 Fixed Frame 为 base_link点击左下的 Add 按钮在弹出的窗口中选择 TF 组件即可显示坐标关系。 继续执行命令rosrun tf2_learning tf2_learning_listen可以看到转换后的坐标以及所属父坐标系如下 其中ERROR是由于节点刚起来时TF数据还未来得及写入缓存导致base_link不存在可以发现第二次调用就没有报错了实际使用中可以等待要操作的frame存在再做转换如下
tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer);
// _frameExists()返回指定frame是否存在于tf树中
if (!buffer._frameExists(base_link))
{ROS_WARN(base_link frame does not exist.);
}二、静态坐标变换Python实现
在创建的 tf2_learning 包路径下 src 目录的同级创建一个 scripts 目录在这里存储脚本如python脚本我们创建 tf2_learning_broadcast.py 以实现坐标广播编辑内容如下
#! /usr/bin/env pythonimport rospy
import tf
import tf2_ros
from geometry_msgs.msg import TransformStampedif __name__ __main__:# 初始化 ROS 节点rospy.init_node(static_frame_broadcast_py)# 创建静态坐标广播器broadcaster tf2_ros.StaticTransformBroadcaster()# 创建并组织被广播的消息tfs TransformStamped()# -- 头信息tfs.header.frame_id base_link # 父坐标系tfs.header.stamp rospy.Time.now()tfs.header.seq 101# -- 子坐标系tfs.child_frame_id laser# -- 坐标系相对信息# ---- 相对于父坐标系的平移偏移量tfs.transform.translation.x 0.5tfs.transform.translation.y 0.0tfs.transform.translation.z 0.3# ---- 相对于父坐标系的旋转偏移量# ---- 设置欧拉角并将欧拉角转换成四元数qtn tf.transformations.quaternion_from_euler(0, 0, 0)tfs.transform.rotation.x qtn[0]tfs.transform.rotation.y qtn[1]tfs.transform.rotation.z qtn[2]tfs.transform.rotation.w qtn[3]# 广播器发送消息broadcaster.sendTransform(tfs)# spinrospy.spin()创建 tf2_learning_listen.py 以订阅静态坐标转换关系并利用该关系将雷达坐标系的点转换到 base_link 坐标系编辑内容如下
#! /usr/bin/env pythonimport rospy
import tf2_ros
# 不要使用 geometry_msgs,需要使用 tf2 内置的消息类型
from tf2_geometry_msgs import PointStamped
# from geometry_msgs.msg import PointStampedif __name__ __main__:# 初始化 ROS 节点rospy.init_node(static_frame_listen)# 创建 TF 订阅对象buffer tf2_ros.Buffer()listener tf2_ros.TransformListener(buffer)rate rospy.Rate(1)while not rospy.is_shutdown():# 生成一个坐标点, 模拟雷达检测到的障碍物坐标点(雷达坐标系下的坐标)point_laser PointStamped()point_laser.header.frame_id laserpoint_laser.header.stamp rospy.Time.now()point_laser.point.x 2.0point_laser.point.y 2.5point_laser.point.z 0.3try:# 转换坐标点, 计算障碍物坐标点在 base_link 下的坐标point_base buffer.transform(point_laser, base_link)rospy.loginfo(point_base: (%.2f, %.2f, %.2f), frame: %s,point_base.point.x,point_base.point.y,point_base.point.z,point_base.header.frame_id)except Exception as e:rospy.logerr(%s, e)# spinrate.sleep()
