常见的简陋的控制乌龟行走方形的方式很简单,例如:
代码有些地方是测试用的,可以不要。
#! /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每隔一段固定时间改变乌龟策略,在行进和转弯中进行改变。这样的代码可能受硬件和线程影响较大,画出的方形比较粗糙。如下图: