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

本文主要是介绍小乌龟运动控制-2 小乌龟走方形，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

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

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

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

#! /usr/bin/env python3from pickle import TRUE
import rospy
from turtlesim.msg import Pose
from geometry_msgs.msg import Twist
from rospy.timer import TimerPI = 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 turnif turn:twist.linear.x = 1twist.angular.z = 0.0turn = Falseelse:twist.linear.x = 0twist.angular.z = 1 * PI / 2turn = TRUEpub.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 python3from 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 EnumState = 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 = Truedef subCallback(pose):global now_posenow_pose = posedef control(pub, linear, angular):global twisttwist.linear.x = lineartwist.angular.z = angularpub.publish(twist)# whether the turtle has reached the goal
def hasReachedGoal():global now_pose, goal_posereturn (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_posereturn ((now_pose.linear_velocity < 0.001) & (now_pose.angular_velocity < 0.001))def stopMove(pub):global goal_pose, g_state, now_poseif hasStopped():g_state = State.TURNgoal_pose.x = now_pose.xgoal_pose.y = now_pose.ygoal_pose.theta = now_pose.theta + PI / 2if goal_pose.theta > PI:goal_pose.theta = goal_pose.theta - 2 * PIelse:control(pub, 0, 0)rospy.loginfo('停止行走')def stopTurn(pub):global goal_pose, g_state, now_poseif hasStopped():g_state = State.MOVEgoal_pose.x = now_pose.x + cos(now_pose.theta) * 2goal_pose.y = now_pose.y + sin(now_pose.theta) * 2goal_pose.theta = now_pose.thetaelse:control(pub, 0, 0)rospy.loginfo('停止转弯')def move(pub):global g_stateif hasReachedGoal():g_state = State.STOP_MOVEcontrol(pub, 0, 0)else:control(pub, 1, 0)rospy.loginfo('直线行走move')def turn(pub):global g_stateif hasReachedGoal():g_state = State.STOP_TURNcontrol(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 functionif first_set_goal:goal_pose.x = now_pose.x + cos(now_pose.theta) * 2goal_pose.y = now_pose.y + sin(now_pose.theta) * 2goal_pose.theta = now_pose.thetafirst_set_goal = Falseif 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