ROS+python控制小乌龟走方形的实现

常见的简陋的控制乌龟行走方形的方式很简单，例如：

代码有些地方是测试用的，可以不要。

#! /usr/bin/env python

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每隔一段固定时间改变乌龟策略，在行进和转弯中进行改变。这样的代码可能受硬件和线程影响较大，画出的方形比较粗糙。如下图：

当然，不用rospy.Timer也可以实现，但总感觉画的不踏实，比较有偶然性。

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

#! /usr/bin/env python

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

 运行图如下：

绘制的方形相对比较整齐。但根据计算机当前分配内存和CPU影响，有时可能仍会出现在拐弯处出现过度判断。当然，如果把：

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

中0.016改为更小数字，效果应该会更好。个人感觉应该就向微分那样，分的约细小就能判断更好。不过还未 验证。