球面坐标变换,普通坐标转化为极坐标方程,球面坐标系变换

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

需求描述：

　　在传统的机器人模型中，核心结构包括机身和雷达，分别对应坐标系，坐标系的原点分别位于机身和雷达的物理中心。已知雷达原点与机身原点的位移关系为：x 0.2 y0.0 z0.5，如下图。目前雷达已经探测到障碍物。障碍物在雷达坐标系中的坐标为(2.0)(3.0)(5.0)。障碍物相对于主体的坐标是什么？

结果演示：

实现分析：

　　坐标系的相对关系是发布者、注册者与注册坐标系之间的相对关系。它传递坐标点信息，用tf实现坐标转换，输出结果到

实现流程

:C，可以和Python一致。

　　创建新的功能包，添加并运行依赖于发布者创建实现的阅读器创建实现，从而验证结果方案A: C的实现。

　　1.创建功能包创建项目功能包依赖于tf2，tf2_ros，tf2_geometry_msgs，roscpropspystd _msgs几何_ MSGS。

　　2.publisher/*静态坐标转换的publisher:发布激光坐标系位置信息的安装过程：1。包含标题文件的ROS节点的初始化；3.静态坐标转换广播器的制作：4.产生坐标系统信息；5.广播者是坐标系信息；6.ROS . h # include tf2 _ ROS/Static _ transform _ broadcaster . h # include geometry _ msgs/transform stamped//3。静态坐标转换广播电台TF2 _ ROS:3360 statictransformbroadcaster；//4.创建坐标系信息几何_ MSGS 33603360变换Stampeds//-设置表头信息ts . header . seq=100ts . header . stamp=ROS:time:now(；ts . header . frame _ id= base _ link ；//-子坐标系ts.child _ frame _ id= laser//-ts . transform . translation . x=0.2 ts . transform . translation . y=0.0，设置子体相对于父体的偏移量；ts . transform . translation . z=0.5；//-设置四元数：将欧拉角数据转换为四元数tf2:四元数qtn；qtn.se trpy (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(；//5.ROS:spin(；0;}此处省略配置文件。

　　3.注册注册方/*坐标系信息，生成相对于子坐标系的坐标点数据，坐标点转换到母坐标系的实现过程：1。2.初始化ROS节点，包括头文件3。创建TF订阅节点4。生成相对于子坐标系5的坐标点。相对于父坐标系1的坐标点数据。Include TF2 _罗斯/转换_听众. h # include TF2 _罗斯/缓冲区. h 包含头文件注意：被调用的转换必须包含头文件intmain(intargc，char *argv[] () (setlocale) LC_all， ))。//2.ROS节点ROS :3360 init(初始化argv，argv，

　　TF _ sub’)；ROS:节点句柄NH；//3.创建法国南部(法国南部领地的缩写)订阅节点tf2_ros:缓冲区缓冲区；tf2 _ ROS:转换监听器监听器(缓冲区);ROS:Rate r(1)；while (ros:ok()) { //4 .生成一个坐标点（相对于子级坐标系)几何_msgs:点戳点_激光；点激光。标题。frame _ id= laser点激光。标题。stamp=ROS:Time:now()；点激光。点。x=1；点激光。点。y=2；点激光。点。z=7.3//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.point.z，point_base。标题。帧标识。c _ str())；} catch(const STD:exception e){//STD:cerr e . what() n ；ROS_INFO(程序异常.);} r . sleep()；ROS:spin once()；}返回0;} 配置文件此处略。

　　4.执行可以使用命令行或发动文件的方式分别启动发布节点与订阅节点，如果程序无异常，控制台将输出，坐标转换后的结果。

　　方案乙：Python实现

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

　　2.发布方#! /usr/sxdfn/env python 静态坐标变换发布方：发布关于激光坐标系的位置信息实现流程： 1.导包2.初始化活性氧簇(活性氧物种的缩写)节点3.创建静态坐标广播器4.创建并组织被广播的消息5.广播器发送消息6 .旋转。导包从geometry _ msgs导入rospyimport tf2 _ rosimport TF。msg import TransformStampedif _ _ name _ _= _ _ main _ _ :# 2 .初始化活性氧簇(活性氧物种的缩写)节点罗斯贝。init _ node( static _ TF _ pub _ p )# 3 .创建静态坐标广播器广播员=tf2_ros .StaticTransformBroadcaster() # 4 .创建并组织被广播的消息tfs=TransformStamped() # -头信息TFS。标题。frame _ id=世界 TFS标题。stamp=rospy .Time.now() tfs.header.seq=101 # -子坐标系tfs.child_frame_id=radar # -坐标系相对信息# - 偏移量TFS。转变。翻译。x=0.2 TFS。转变。翻译。y=0.0 TFS。转变。翻译。z=0.5 #-四元数qtn=tf.transformations。四元数_ from _欧拉(0，0，0)TFS。转变。旋转。x=qtn[0]TFS转变。旋转。TFS。转变。旋转。TFS。转变。旋转。w=qtn[3]# 5 .广播器发送消息广播员。发送转换(TFS)# 6。旋转罗斯比。旋转()权限设置以及配置文件此处略。

　　3.订阅者#！/usr/sxdfn/env python 订阅坐标系信息，生成相对于子坐标系的坐标点数据。转换到父坐标系中坐标点的实现过程：1。指南包2。初始化ROS节点3。创建TF订阅对象4。在雷达坐标系5中创建一个坐标点。调查订阅对象的API将4中的点坐标转换为相对于世界6的坐标。旋转 # 1。import 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= radar 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.loginfo(转换结果：x=%.2f，y=%.2f，z=%.2f ，Point _ target.point.x，point _ target.point.y，point _ target.point.z)除了exception为e: rospy.logerr (exception: %s ，e) # 6.spin rate.sleep()权限设置和配置文件此处省略。

PS: 在 tf2 的 python 实现中，tf2 已经封装了一些消息类型，不可以使用 geometry_msgs.msg 中的类型

　　4.执行时可以通过命令行或启动文件分别启动发布节点和订阅节点。如果程序中没有异常，控制台会输出坐标转换后的结果。

　　1:当坐标系之间的相对位置固定时，所需参数也固定：父坐标名称、子坐标名称、X偏移、Y偏移、Z偏移、X滚动角、Y俯仰角和Z偏航角。实现逻辑相同，但参数不同。那么ROS系统已经封装了一个特殊的节点，它的用法如下：

　　rost F2 _ Ross static _ transform _ publisher x offset y offset z offset z yaw angle y pitch angle x roll angle父坐标系和子坐标系示例：rosruntf 2 _ Ross static _ transform _ publisher 0.200 0.5000/baselink/laser

　　还建议用这种方法直接发布静态坐标系的相对信息。

　　2:可以借助rviz显示坐标系关系。具体操作是：

　　新窗口输入命令：rviz在已启动的rviz中将固定框架设置为base _ link点击左下方的添加按钮，在弹出窗口中选择TF component，显示坐标关系。