小乌龟运动控制-2 小乌龟走方形

news2024/11/16 0:01:03

一、简陋版-乌龟行走方形

常见的简陋的控制乌龟行走方形的方式很简单，例如下面，可以测试下：

我们需要创建一个名为draw_square.py 的 Python 文件，并将其保存在 ~/catkin_ws/src/turtlebot3_draw_circle/scripts/ 目录下。

#! /usr/bin/env python3

from pickle import TRUE
import rospy
from turtlesim.msg import Pose
from geometry_msgs.msg import Twist
from rospy.timer import Timer

PI = 3.141592653
turn = True
twist = Twist()

def subCallback(pose):
    rospy.loginfo("come into subCallback!%f", pose.x)


def timeCallback(event):
    pub = rospy.Publisher("turtle1/cmd_vel", Twist, queue_size=1)
    global turn
    if turn:
        twist.linear.x = 1
        twist.angular.z = 0.0
        turn = False
    else:
        twist.linear.x = 0
        twist.angular.z = 1 * PI / 2
        turn = TRUE
    pub.publish(twist)


if __name__ == "__main__":
    rospy.init_node("go_square")

    sub = rospy.Subscriber("turtle1/pose", Pose, subCallback, queue_size=10)

    rospy.Timer(rospy.Duration(1), timeCallback, oneshot=False)

    rospy.spin()

    pass

用rospy.Timer每隔一段固定时间改变乌龟策略，在行进和转弯中进行改变。这样的代码可能受硬件和线程影响较大，画出的方形比较粗糙。如下图：
在这里插入图片描述

二、强化版-乌龟行走方形

下边是模仿ROS自带的乌龟走方形的框架，利用Python写的，这样对乌龟的控制比较强，效果比较好。代码如下：

我们需要创建一个名为draw_square_1.py 的 Python 文件，并将其保存在 ~/catkin_ws/src/turtlebot3_draw_circle/scripts/ 目录下。

 #! /usr/bin/env python3

from genpy.message import fill_message_args
import rospy
from turtlesim.msg import Pose
from geometry_msgs.msg import Twist
from rospy.timer import Timer
from math import cos, fabs, sin
from enum import Enum

State = Enum('State', ('MOVE', 'STOP_MOVE', 'TURN', 'STOP_TURN'))

now_pose = Pose()
goal_pose = Pose()
g_state = State.MOVE
PI = 3.141592653
twist = Twist()
first_set_goal = True

def subCallback(pose):
    global now_pose
    now_pose = pose

def control(pub, linear, angular):
    global twist
    twist.linear.x = linear
    twist.angular.z = angular
    pub.publish(twist)

# whether the turtle has reached the goal
def hasReachedGoal():
    global now_pose, goal_pose

    return (fabs(now_pose.theta - goal_pose.theta) < 0.001) & (fabs(now_pose.x - goal_pose.x) < 0.001) & (
                fabs(now_pose.y - goal_pose.y) < 0.001)

# wheather the turtle has stopped
def hasStopped():
    global now_pose
    return ((now_pose.linear_velocity < 0.001) & (now_pose.angular_velocity < 0.001))

def stopMove(pub):
    global goal_pose, g_state, now_pose

    if hasStopped():
        g_state = State.TURN
        goal_pose.x = now_pose.x
        goal_pose.y = now_pose.y
        goal_pose.theta = now_pose.theta + PI / 2
        if goal_pose.theta > PI:
            goal_pose.theta = goal_pose.theta - 2 * PI
    else:
        control(pub, 0, 0)
    rospy.loginfo('停止行走')

def stopTurn(pub):
    global goal_pose, g_state, now_pose

    if hasStopped():
        g_state = State.MOVE
        goal_pose.x = now_pose.x + cos(now_pose.theta) * 2
        goal_pose.y = now_pose.y + sin(now_pose.theta) * 2
        goal_pose.theta = now_pose.theta
    else:
        control(pub, 0, 0)
    rospy.loginfo('停止转弯')

def move(pub):
    global g_state
    if hasReachedGoal():
        g_state = State.STOP_MOVE
        control(pub, 0, 0)
    else:
        control(pub, 1, 0)
    rospy.loginfo('直线行走move')

def turn(pub):
    global g_state
    if hasReachedGoal():
        g_state = State.STOP_TURN
        control(pub, 0, 0)
    else:
        control(pub, 0, PI / 4)
    rospy.loginfo('转弯')

def timeCallback(event):
    rospy.loginfo("come into timeCallback.....")

    pub = rospy.Publisher("turtle1/cmd_vel", Twist, queue_size=1)

    global goal_pose, now_pose, first_set_goal, g_state
    # if this is the first time to call this function
    if first_set_goal:
        goal_pose.x = now_pose.x + cos(now_pose.theta) * 2
        goal_pose.y = now_pose.y + sin(now_pose.theta) * 2
        goal_pose.theta = now_pose.theta

        first_set_goal = False

    if g_state == State.STOP_MOVE:
        rospy.loginfo('状态为：' + str(g_state))
        stopMove(pub)
    elif g_state == State.MOVE:
        rospy.loginfo('状态为：' + str(g_state))
        move(pub)
    elif g_state == State.TURN:
        turn(pub)
    else:
        stopTurn(pub)

if __name__ == "__main__":
    rospy.init_node("go_square")
    sub = rospy.Subscriber("turtle1/pose", Pose, subCallback, queue_size=10)
    rospy.Timer(rospy.Duration(0.016), timeCallback, oneshot=False)
    rospy.spin()
    pass