实时接受乌龟坐标系相对于33558www.Sina.com/Word坐标系的位置关系
将乌龟坐标系的点转换为世界坐标系的点
1.实现目标
# include ' ROS/ROS.h ' # include ' turtles im/pose.h ' /龟姿消息类型# include ' tf2 _ ROS/transform _ broadcastes include ' tf2/linear math/quaternion.h '//将欧拉角转换为四元数/*以固定乌龟的位置和姿态信息，然后转换为相对四元数所需的头文件以及发布准备工作： pose消息： Turtlesim/pose float 32 x float 32 y float 32 theta float 32 linear _ velocity float 32 angular _ velocity float 32 angular _ velocity voiddopose (cont ) onstptrpose )//获取位姿势信息，变换为坐标系的相对关系，发行static tf2 _ ROS 3360: transform borm//生成分发对象的geometry _ msgs 33603360 tretration //组织公开的数据ts.header.frame_id='world '； ts.header.stamp=ROS : time : now (； ts.child_frame_id='turtlesim1 '； ts.transform.translation.x=pose-x； ts.transform.translation.y=pose-y； ts.transform.translation.z=0； //将欧拉角转换为四元数tf2:Quaternion qtn的设置； //将此对象设置为欧拉角qtn.se trpy (0，0，pose-theta )； //此处的参数为xyz轴上的旋转量，单位为弧度//四元数ts.transform.rotation.x=qtn.getx (； ts.transform.rotation.y=qtn.gety (； ts.transform.rotation.z=qtn.getz (； ts.transform.rotation.w=qtn.getw (； //发布数据pub.sendtransform(ts ) }intmain(intargv，char *argv[] () ) ROS:3360init ) argc，argv，' dy _ pu puput ros:NodeHandle nh； ROS : subscriber sub=NH.subscribe ('/turtle1/pose '，100，dopose )； //阅读乌龟相对于世界坐标系的关系ros:spin (； 返回0； (3)实现订阅者
# include ' ROS/ROS.h ' # include ' tf2 _ ROS/transform _ listener.h ' /创建订阅所需的头文件# include ' tf2 _ ROS/include ' tf2 _ geometry _ msgs/tf2 _ geometry _ msgs.h ' /坐标转换所需的头文件/*定位符实现定位符所发布的坐标系的相对关系的其父坐标系下的坐标计算流基于nodehandle的扫描对象创建、扫描坐标系相对关系的整理坐标点数据转换算法的实现、tf内置的最终输出旋转的坐标系*/intmain(intargc，char *argv[] () ROS33603360iin tf2_ros:Buffer buffer； tf2 _ ROS : transformlistenerlistener (buffer )； //要为其创建订阅的geometry _ msgs 3360: pointstampedps； //坐标点ps.header.frame_id='turtlesim1 '； PS.header.stamp=ROS :3360 time (0.0 )； //ros将具有此时的时间与定时读完的时间进行对照。 使用ros:TIME:now时，ros认为buffer的消息收到得很快，没有不转换的意义。 ps.point.x=2.0； ps.point.y=3.0； ps.point.z=4.0； //ROS:3360duration(2).sleep )； //为了确认订阅的数据已保存在缓存器中，提供缓冲ROS:3360rater(10 )的时间。 //转换频率while(ROS:ok () ) try )/* code */geometry _ msgs 33603360 pointstampedpsout； PSout=buffer.transform(PS，' world ' )； //buffer有两个参数：要转换的坐标点和要转换为哪个坐标系。 ROS_info(thepointfram=%sx=%.2f，y=%.2f，z=%.2f )、psout.header.frame_id.c_str )、PS out r.PS ros:spinOnce (； } catch (const STD :3360 exception e ) ROS_info('%s”，e.what )； //转换代码实现，ros内部已经封装) } return 0； }