ROS小乌龟走设定图形路线（键盘控制+Python代码实现）

相信在很多人学习ROS的时候都会写一下这个demo，不仅是对代码能力的考察（代码语法、结构都还是相对简单的），还是对ROS话题通信这些基础概念的理解的考量。

首先命令运行

roscore

rosrun turtlesim turtlesim_node

 

如果要通过键盘控制小乌龟的运动，则运行

rosrun turtlesim turtle_teleop_key

 下面展示代码实现的小乌龟案例

1.圆形

#! /usr/bin/env python
# coding:UTF-8

import turtle
import rospy
from geometry_msgs.msg import Twist # Twist数据类型包含线速度和角速度


def turtle_publisher():
    rospy.init_node('turtle_publisher', anonymous=True)
    # 对节点进行初始化，命名一个叫turtle_publisher的节点
    turtle_pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
    # 实例化一个发布者对象，发布的话题名为 /turtle1/cmd_vel ,消息类型为 Twist,队列长度为10
    rate = rospy.Rate(10)
    # 设置循环的频率
    while not rospy.is_shutdown():
        # 装载消息
        cmd_msg = Twist()
        cmd_msg.linear.x = 2.0
        cmd_msg.angular.z = 1.0
        turtle_pub.publish(cmd_msg)
        # 发布话题消息
        rospy.loginfo("message have published x = %d angular z = %0.2f", cmd_msg.linear.x, cmd_msg.angular.z)
        # 在终端打印一份日志
        rate.sleep()
        # 按照设置的速率进行延时


if __name__ == '__main__':
    try:
        turtle_publisher()
    except rospy.ROSInterruptException:
        pass

2.多种图形

#! /usr/bin/env python2
# coding:UTF-8

import math
import rospy
import sys
from turtlesim.msg import Pose  # Pose数据类型包含坐标和角度
from geometry_msgs.msg import Twist  # Twist数据类型包含线速度和角速度




class Turtlerun:
    """ 一个乌龟走图形路线的类 """

    def __init__(self, name):
        """ 初始化 """
        self.pub = rospy.Publisher("/turtle1/cmd_vel", Twist, queue_size=10)  # 实例化发布者
        rospy.Subscriber("/turtle1/pose", Pose, self.turtle_run, queue_size=10)  # 实例化订阅者
        self.twist = Twist()
        self.x = None  # 乌龟目前所在x坐标
        self.y = None  # 乌龟目前所在y坐标
        self.size = 3.0  # 图形边长大小
        self.rate = rospy.Rate(100)  # 循环频率
        self.theta = None  # 乌龟初始角度
        self.goal = []  # 选择的图形目标点
        self.goals = {}  # 储存的图形目标点
        self.error = None  # 距离目标点的差值
        self.lock = 0  # 直线行走与旋转互锁
        self.side = 0  # 乌龟移动目标点的索引值
        self.aim_line = 0.035  # 乌龟转弯前走直线时距离目标距离的精度值
        self.aim_angle = 0.01  # 乌龟直走前转弯时距离目标角度的精度值
        self.i = 1
        self.turns = 0
        self.num = None  # 图形边数索引量

    def turtle_run(self, pose):
        """ 控制乌龟运动 """
        self.pose_get(pose)
        if self.i == 1:
            # 目标点只设定一次
            self.set_goal(pose)
            self.i += 1
        self.go_line()
        self.rorate()

    def pose_get(self, pose):
        """ 获取当前坐标和角度 """
        self.x = pose.x
        self.y = pose.y
        self.theta = pose.theta

    def set_goal(self, pose):
        """ 设定目标点 """
        self.goals = {
            'squ': [[pose.x + self.size, pose.y, math.pi / 2],
                    [pose.x + self.size, pose.y + self.size, math.pi],
                    [pose.x, pose.y + self.size, - math.pi / 2],
                    [pose.x, pose.y, 0]],
            'tri60': [[pose.x + self.size, pose.y, math.pi * 2 / 3],
                      [pose.x + self.size / 2, pose.y + (self.size / 2 * math.tan(math.pi / 3)), - math.pi * 2 / 3],
                      [pose.x, pose.y, 0]],
            'tri90': [[pose.x + self.size, pose.y, math.pi / 2],
                      [pose.x + self.size, pose.y + self.size, -3 * math.pi / 4],
                      [pose.x, pose.y, 0]],
            'hourglass': [[pose.x, pose.y, math.pi / 3],
                          [pose.x + self.size / 2, pose.y + (self.size * math.sin(math.pi / 3)), math.pi],
                          [pose.x - self.size / 2, pose.y + (self.size * math.sin(math.pi / 3)), - math.pi / 3],
                          [pose.x + self.size / 2, pose.y - (self.size * math.sin(math.pi / 3)), math.pi],
                          [pose.x - self.size / 2, pose.y - (self.size * math.sin(math.pi / 3)), math.pi / 3],
                          [pose.x, pose.y, math.pi / 3]],

        }
        # 条件选择目标图形
        if name == 'squ':
            self.goal = self.goals['squ']
        elif name == 'tri60':
            self.goal = self.goals['tri60']
        elif name == 'tri90':
            self.goal = self.goals['tri90']
        elif name == 'hourglass':
            self.goal = self.goals['hourglass']
        elif name == 'circle':
            self.circle()
        self.num = len(self.goal) - 1

    def go_line(self):
        """ 走直线 """
        if self.lock == 0:  # 和旋转互锁
            # 计算现在的误差
            self.error = math.sqrt(
                (self.x - self.goal[self.side][0]) ** 2 + (self.y - self.goal[self.side][1]) ** 2)

            if self.error > self.aim_line:  # 未到设定精度时前进
                self.twist.linear.x = 1.0
            else:
                self.twist.linear.x = 0
                self.lock = 1  # 解锁旋转
            self.pub.publish(self.twist)  # 发布乌龟速度

    def rorate(self):
        """ 转动角度到达另一边 """
        if self.lock == 1:
            self.error = abs(self.theta - self.goal[self.side][2])
            if self.error > self.aim_angle:  # 未到达转动角度精度时继续旋转
                self.twist.angular.z = 1.0
            else:
                self.twist.angular.z = 0
                self.lock = 0
                if self.side < self.num:
                    self.side = self.side + 1
                else:
                    self.side = 0  # 走完一轮目标点后重置目标点
                    self.i = 1
                    self.turns += 1
                    print(self.turns)
            self.pub.publish(self.twist)  # 发布乌龟速度

    def circle(self):
        """ 走圆圈 """
        while not rospy.is_shutdown():
            self.twist.linear.x = 2.0
            self.twist.angular.z = 1.0
            self.pub.publish(self.twist)
            rospy.loginfo("message have published x = %d angular z = %0.2f", self.twist.linear.x, self.twist.angular.z)
            # 在终端打印一份日志
            self.rate.sleep()
            # 按照设置的速率进行延时


if __name__ == '__main__':
    graph_list = ['circle', 'squ', 'tri60', 'tri90', 'hourglass']
    if len(sys.argv) > 1 and sys.argv[1] in graph_list:  # 实现rosrun xx yy.py squ直接跑对应图形
        name = sys.argv[1]
    else:
        name = input(
            'Please input graph name(circle squ tri60 tri90 hourglass): ')  # 没在命令输对图形名称时提示输入
    try:
        rospy.init_node('turtle_run', anonymous=True)  # 对节点进行初始化，命名一个叫turtle_run的节点
        name = str(name)
        tgraph = Turtlerun(name)
        rospy.spin()
    except rospy.ROSInterruptException:
        pass

 下面是走方形的案例实现：