ROS（Python）—小车运动的基本实现以及小乌龟位姿优化实现

声明

本文主要介绍小车运动的相关代码书写，包括小车的速度设置与旋转角度的设置。并且将以小乌龟为实例，讲述如何找到要用到的相关节点和话题，并用小乌龟来进行演示。在实现的优化方面我将实现小乌龟运动位姿的输出。

小车运动相关基础知识

1.寻找小车运动话题

当你拿到这台小车你一定是不知道这个小车的运动话题的这时候就需要我们调用rostopic list来进行话题查询，话题与运动相关一般会带有cmd_vel的字样。例如当你运行出来了小乌龟后，我们输入 rostopic list，就会显示如下：

我们得到了话题：/turtle1/cmd_vel

2.代码编写的相关知识

还有就是在编写速度的时候我们需要清楚linear中x表示小车的前进与后退，y表示左右偏移的速度（麦克纳姆轮），z表示上下的速度（这是无人机才需要用到的我们不需要一般）单位都是m/s。

在angular中 z表示左右偏移（偏航），就是围绕着z轴转动的，我们对于转动这是最常见的命令。

下面我将演示一份代码可以使小乌龟做圆周运动和方形运动：

#! /usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from geometry_msgs.msg import Twist
"""
     发布方实现 发布速度消息
          话题 /turtlel/cmd_vel
          消息 geometry_msgs/Twist
    1.导包
    2,初始化ros节点
    3,创建发布者对象
    4,组织数据并发布数据
"""
if __name__ == "__main__":
    # 初始化ros节点
    rospy.init_node("yubanwangluo88")
    # 创建发布者对象
    pub = rospy.Publisher("/turtle1/cmd_vel",Twist,queue_size=10)
    rate = rospy.Rate(10)
    twist = Twist()
    
    twist.linear.x = 1
    twist.linear.y = 0.0
    twist.linear.z = 0.0

    twist.angular.x = 0.0
    twist.angular.y = 0.0
    twist.angular.z = 1
    rospy.loginfo("已启动")
    # side_length = 1
    # turn_angle = 1.5708
    # # 循环发布
    while not rospy.is_shutdown():
    #      # 使小车沿正方形的四个边分别前进和转弯
    #     for _ in range(4):
    #         # 前进
    #         twist.linear.x = side_length
    #         twist.angular.z = 0.1
    #         for _ in range(10):  # 持续1秒
    #             pub.publish(twist)
    #             rate.sleep()

    #         # 转弯
    #         twist.linear.x = 0.1
    #         twist.angular.z = turn_angle
    #         for _ in range(10):  # 持续1秒
    #             pub.publish(twist)
    #             rate.sleep()
        pub.publish(twist)
        rate.sleep()

大家注意看我们是只用到了linear x与angular 的z，一个是前进与后退，一个是旋转速度，如果你的小车是麦克纳姆轮不要忘记设计linear y的速度。其次就是记得修改话题，即在/turtle1/cmd_vel的位置修改你的小车对应的话题。

下面是代码结果演示

这是运行圆形运动的结果：

方形运动类似这里就不在赘述了。

小乌龟位姿输出的优化实现

1.相关文件的配置

在编写获取小乌龟运动位姿的文件之前，我们需要添加相关依赖。建立以来分为两种:

一种是在建立一个新的功能包时候，要添加如下依赖：

roscpp rospy std_msgs turtlesim

二是你已经建立好了功能包，我们需要修改相关的package文件和cmakelist文件

在package文件中，我们直接拉到文件的最后，添加如下依赖：

在cmk文件中，我们找到大概代码第十五行的位置，添加如下依赖：

接下来编写输出小乌龟运动位姿的py文件：

#! /usr/bin/env python
# -*-coding:utf8 -*-
from turtlesim.msg import Pose
import rospy
"""
   需求：订阅并输出乌龟的位姿消息
   1.导包
   2.初始化ros节点
   3.创建订阅对象
   4.使用回调函数处理订阅信息
   5.spin()
"""
def dopose(pose):
   rospy.loginfo("->乌龟消息为`：坐标（%.2f,%.2f)\n朝向:%.2f\nlinear:%.2f\nangular:%.2f",
                 pose.x,pose.y,pose.theta,pose.linear_velocity,pose.angular_velocity)
if __name__ == "__main__":
   # 初始化ros节点
   rospy.init_node("yubanwangluo7")
   # 创建订阅对象
   sub = rospy.Subscriber("/turtle1/pose",Pose,dopose,queue_size=100)
   # 使用回调函数处理订阅的消息
   # spin（）
   rospy.spin()

其中"/turtle1/pose"可以使用rostopic list这个命令获取。

以下时运行优化后的结果演示：