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:
pass2.多种图形
#! /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下面是走方形的案例实现：
