ROS 用python打印乌龟的位姿

需求描述: 已知turtlesim中的乌龟显示节点，会发布当前乌龟的位姿(窗体中乌龟的坐标以及朝向)，要求控制乌龟运动，并时时打印当前乌龟的位姿。

实现分析:

1. 首先，需要启动乌龟显示以及运动控制节点并控制乌龟运动。
2. 要通过ROS命令，来获取乌龟位姿发布的话题以及消息。
3. 编写订阅节点，订阅并打印乌龟的位姿。

实现流程:

1. 通过ros命令获取话题与消息信息。
2. 编码实现位姿获取节点。
3. 启动 roscore、turtlesim_node 、控制节点以及位姿订阅节点，控制乌龟运动并输出乌龟的位姿。

话题与消息获取（类型与格式）

 

实现订阅节点

创建功能包需要依赖的功能包: roscpp rospy std_msgs turtlesim

#! /usr/bin/env python
from turtlesim.msg import Pose

import rospy
"""
    需求：订阅并输出乌龟的位姿信息

    1、导包
    2、初始化ROS节点
    3、创建订阅对象
    4、使用回调函数处理订阅到的消息
    5、spin()
"""

def dopose(pose):
    rospy.loginfo("p->乌龟位姿信息：坐标（%.2f,%.2f）,朝向：%.2f,线速度：%.2f,角速度：%.2f",
                  pose.x,pose.y,pose.theta,pose.linear_velocity,pose.angular_velocity)

if __name__=="__main__":
    #2、初始化ROS节点
    rospy.init_node("sub_pose_p")
    #3、创建订阅对象
    sub=rospy.Subscriber("/turtle1/pose",Pose,dopose,queue_size=10)
    #4、使用回调函数处理订阅到的消息
    #5、spin()
    rospy.spin()

修改scripts权限

修改CMakeLists.txt文件

编译运行

先运行launch文件

弹出可视化窗口

再执行roscore,用按键控制乌龟运动

通过下图命令来获取乌龟的位姿信息