旋转变换的坐标变换公式,坐标系的旋转变换公式

　　ROS 5.1.3动态坐标变换《ROS入门-理论与实践》视频教程城镇建筑动态坐标变换是指两个坐标系之间相对位置的变化。

需求描述：

　　启动turtlesim_node。此节点的形状包含世界坐标系。底部是坐标系的原点。龟是另一个坐标系。键盘控制乌龟的移动，并动态发布两个坐标系的相对位置。

结果演示：

实现分析：

　　龟本身不仅可以看作一个坐标系，也可以看作世界坐标系中的一个坐标点。订阅turtle1/pose，获取乌龟在世界坐标系中的X坐标，Y坐标，偏移量，线速度，角速度，将姿态信息转换成坐标系的相对信息，http://www。新浪网

　　做一个新的功能包，增加一个依赖于坐标相对关系的发布源(同时需要订阅乌龟的姿态信息)。坐标相对关系的制造源委托方案A: C的安装

　　1.创建功能包创建项目功能包依赖于tf2，tf2_ros，tf2_geometry_msgs，roscpprospystd _ msgs geometry _ msgs，turtlesim。

　　2.发布者/*动态坐标系的相对姿态发布(一个坐标系的相对姿态是随着另一个坐标系不断变化的)。启动要求：Turtlesim_node。这个节点的形状有一个世界坐标系(左下是坐标系的原点)，乌龟是另一个坐标系。键盘控制乌龟的移动。动态释放两个坐标系的相对位置：1。Turtle本身不仅可以看作一个坐标系，还订阅了世界坐标系中的一个坐标点turtle1/pose。可以获得乌龟在世界坐标系中的x坐标、y坐标、偏移量、线速度、角速度的位姿信息，转换成坐标系的相对信息，提交给实现过程：1 .包括头文件2。正在初始化ROS节点3。正在创建ROS句柄4。正在创建订阅对象5。回调函数处理订阅的数据(实现TF广播))。5-1.TF创建广播5-2。创建广播数据(以姿势设置)5-3。广播数据6。Spin *//1。头文件# include ROS/ROS . h # include # inclue include geometry _ msgs/Transform stamped . h # include tf2/linear math/四元数. h Voiddopose(常量广播)5-1 . TF create broadcaster static tf2 _ ROS 336033333332//5-2 .创建广播数据(在pose中设置)。geometry _ msgs 33603:TransformStampApp/-耳机TFS . header . frame _ id= world ；世界；TFS . header . stamp=ROS:3360 time:3360 now(；//-坐标系id TFS . child _ frame _ id= turtle 1 ；//-坐标系相对信息设置TFS . transform . translation . x=pose-x；TFS . transform . translation . y=pose-y；TFS . transform . translation . z=0.0；//2D实现，pose中没有Z，Z为0// -四元数设置tf2:四元数qtn；qtn.se trpy (0，0，pose-theta)；TFS . transform . rotation . x=qtn . getx(；TFS . transform . rotation . y=qtn . gety(；TFS . transform . rotation . z=qtn . getz(；TFS . transform . rotation . w=qtn . getw(；//5-3.广播站发布数据broadcaster . send transform(TFS)；}intmain(intargc，char *argv[] ) setlocale ) LC_all，；//2.初始化ROS节点ROS:3360init(argc，argv， dynamic _ TF _ pub )；//3.ROS handle创建ROS:node handle NH；//4.创建订阅对象ROS:3360 subscriber sub=NH . subscribe URT。

　　lesim:Pose(/turtle1/pose ，1000，doPose)；//5.回调函数处理订阅的数据(实现TF广播)/////6 . spin ROS:spin()；返回0；}此处省略配置文件。

　　3.订阅方//1.包含头文件#包含 ro/ro。h # include tf2 _ ROS/transform _ listener。h # include tf2 _ ROS/buffer。h # include 几何图形_消息/点戳。h # include tf2 _ geometry _ msgs/tf2 _ geometry _ msgs。h //注意：调用改变必须包含该头文件int main(int argc，char *argv[]){ setlocale(LC_ALL， )；//2.初始化活性氧簇(活性氧物种的缩写)节点ros:init(argc，argv， dynamic _ TF _ sub )；ROS:节点句柄NH；//3.创建法国南部(法国南部领地的缩写)订阅节点tf2_ros:缓冲区缓冲区；tf2 _ ROS:转换监听器监听器(缓冲区);ROS:Rate r(1)；while (ros:ok()) { //4 .生成一个坐标点（相对于子级坐标系)几何_msgs:点戳点_激光；点激光。标题。frame _ id=海龟1 ；点激光。标题。stamp=ROS:Time()；点激光。点。x=1；点激光。点。y=1；点激光。点。z=0；//5.转换坐标点（相对于父级坐标系) //新建一个坐标点，用于接收转换结果//- 使用尝试语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败-try { geometry _ msgs:point stamped point _ base；point _ base=缓冲区。transform(点_激光，世界)；ROS_INFO(坐标点相对于世界的坐标为：(%.2f，%.2f，%.2f)，point_base.point.x，point_base.point.y，point_base。点。z)；} catch(const STD:exception e){//STD:cerr e . what() n ；ROS_INFO(程序异常：%s ，e . what())；} r . sleep()；ROS:spin once()；}返回0;} 配置文件此处略。

　　4.执行可以使用命令行或发动文件的方式分别启动发布节点与订阅节点，如果程序无异常，与演示结果类似。

　　可以使用rviz查看坐标系相对关系。

　　方案乙：Python实现

　　1.创建功能包创建项目功能包依赖于tf2、tf2_ros、tf2_geometry_msgs、ROS CPP rospy STD _ msgs geometry _ msgs、turtlesim

　　2.发布者#！/usr/blue coat /env python 动态坐标系相对姿态释放(一个坐标系相对于另一个坐标系的相对姿态是不断变化的)要求：启动turtlesim_node，其中窗体有一个世界坐标系(左下角是坐标系原点)，乌龟是另一个坐标系，键盘控制乌龟移动。动态发布和分析两个坐标系的相对位置：1 .乌龟本身不仅可以看作是坐标系，也可以看作是世界坐标系中的一个坐标点。2.订阅turtle1/pose，可以获取乌龟在世界坐标系中的x坐标，y坐标，偏移量，线速度，角速度。3.将姿态信息转换成坐标系的相对信息并公布实现过程：1 .引导包裹。2.初始化ROS节点。3.订阅/turtle1/Pose话题消息4。回调函数处理4-1。创建TF广播4-2。创建广播数据(按姿势设置)4-3。广播公司发布数据5。旋转 # 1。import rospyimport tf2 _ rosimport TF from turtles im . msg import pose from geometry _ msgs . msg import transform stamped # 4。回调函数处理def doPose(pose): # 4-1。创建TF广播电台=TF2 _罗斯。TransformBroadcaster () # 4-2。创建广播数据(由姿势设置)TFS=transform stamped()TFS . header . frame _ id= world TFS . header . stamp=rospy。time . now()TFS . child _ frame _ id= turtle 1 TFS . translation . x=pose . x TFS . transform . y=pose . y TFS . transform . translation . z=0.0 qtn=TF . tr transformations .四元数_from_euler(0，0，pose)TFS . transform . rotation . x=qtn[0]TFS . transform . rotation . y=qtn[1]TFS . transform . rotation . z=qtn[2]TFS . transform . rotation广播员。send transform(TFS)if _ _ name _= _ main _ _ :# 2。初始化ROS节点rospy . init _ node( dynamic _ TF _ pub _ p )# 3。Subscribe /turtle1/Pose话题消息sub=rospy . subscriber(/turtle 1/Pose ，Pose，do pose) # 4。回调函数处理# 4-1。正在创建TF广播# 4-2。创建广播数据(按姿势设置)# 4-3。广播数据# 5。Spinrospy.spin

　　3.订阅者#！/usr/blue coat /env python 动态坐标系相对姿态释放(一个坐标系相对于另一个坐标系的相对姿态是不断变化的)要求：启动turtlesim_node，其中窗体有一个世界坐标系(左下角是坐标系原点)，乌龟是另一个坐标系，键盘控制乌龟移动。动态发布和分析两个坐标系的相对位置：1 .乌龟本身不仅可以看作是坐标系，也可以看作是世界坐标系中的一个坐标点。2.订阅turtle1/pose，可以获取乌龟在世界坐标系中的x坐标，y坐标，偏移量，线速度，角速度。3.将姿态信息转换为坐标系相对信息并发布实现过程：1 .指南包2。初始化ROS节点3。创建TF订阅对象4。处理订阅的数据 # 1。引导包导入rospyimport tf2_ros#不使用geometry_msgs，消息类型from F2 _ geometry _ msgs importpointstamped # from geometry _ msgs . msgimportpointstamped if _ _ name _= _ _ main _ _ :# 2 .初始化ROS节点rospy . init _ node( static _ sub _ TF _ p )# 3。创建TF订阅对象buffer=tf2 _ ROS . buffer()listener=tf2 _ ROS . transform listener(buffer)Rate=rospy . Rate(1)而not rospy.is _ shutdown (): # 4。在雷达坐标系中创建一个坐标点point _ source=point stamped()point _ source . header . frame _ id= turtle 1 point _ source . header . stamp=rospy。time . now()point _ source . point . x=10 point _ source . point . y=2 point _ source . point . z=3 try:# 5。研究订阅对象的API将4中的点坐标转换为相对于世界的坐标point _ target=buffer . transform(point _ source， world ，rospy . duration(1))rospy . log info(转换结果：x=%.2f，Y=% .2f，z=% .2f ，point _ target.point.x，point _ target.point.y，point _ target.point.z)除异常为e: rospy.logerr(异常：%s ，e) # 6.spin rate.sleep

　　4.执行可以使用命令行或启动文件分别启动发布节点和订阅节点。如果程序中没有异常，则类似于演示结果。

　　您可以使用rviz查看坐标系相对关系。