最近几天在学习语音识别、语音合成，应用到ROS当中实现一些简单的案例，下面是使用语音来控制turtle小乌龟走设定图形路线的案例。
ROS小乌龟走设定图形路线（键盘控制+Python代码实现）_笨小古的博客-CSDN博客
在上面代码的基础上加了一个语音控制函数 ：
# 接收到语音命令后发布速度指令
def get_voice(data):
voice_text=data.data
rospy.loginfo("语音已接收")
if voice_text == "走圆圈。":
name = 'circle'
rospy.loginfo("I said:: %s",voice_text)
Turtlerun(name)
elif voice_text == "走方形。":
name = 'squ'
rospy.loginfo("I said:: %s",voice_text)
Turtlerun(name)
elif voice_text == "走漏斗形。":
name = 'hourglass'
rospy.loginfo("I said:: %s",voice_text)
Turtlerun(name)
elif voice_text == "走60度三角形。":
name = 'tri60'
rospy.loginfo("I said:: %s",voice_text)
Turtlerun(name)
elif voice_text == "走直角三角形。":
name = 'tri90'
rospy.loginfo("I said:: %s",voice_text)
Turtlerun(name)完整代码：
#!/usr/bin/env python3
# coding: utf-8
import math
import rospy
import sys
from turtlesim.msg import Pose # Pose数据类型包含乌龟的坐标和角度
from geometry_msgs.msg import Twist # Twist数据类型包含线速度和角速度
from std_msgs.msg import String
class Turtle:
def __init__(self, name, graph):
rospy.init_node(name) # 初始化节点
rospy.Subscriber('/turtle1/pose', Pose, self.control) # 实例化订阅者，参数为订阅的话题名，消息类型，回调函数
self.pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10) # 实例化发布者，参数为发布的话题名，消息类型，队列长度
self._graph = graph
self.size = 5 # 图形的大小(3-5)
self.kp1 = 6 # 走直线的比例控制参数
self.kp2 = 4 # 转角度的比例控制参数
self.kd = 0 # 走直线的微分控制参数
self.aim = 0.025 # 直走的误差值
self.aim_rotate = 0.005 # 转弯的误差值
self.vel_cmd = Twist() # 实例化Twist消息类型的消息
self.point = {} # 储存所走路径目标点
self.x = None # 乌龟目前所在x坐标
self.y = None # 乌龟目前所在y坐标
self.theta = None # 乌龟目前角度
self.goal = 0 # 乌龟下一个要到达的目标点
self.error = None # 现在距离目标点的误差值
self.flag = 0 # flag和lock实现走直线和转角度的互锁，执行完一个才能执行另一个
self.lock = 0
self.key = 0
self.theta_list = []
self.speed = 4 # 控制旋转时的方向,顺or逆
def control(self, pose):
"""订阅的回调函数"""
self.choose_graph(self._graph, pose) # 设定要走什么形状
def choose_graph(self, graph, pose):
"""根据传入的图形设定目标点，获取当前位置，控制运动"""
self.set_goal_points(pose, self.size)
rospy.loginfo(self.point['squ'][0][0])
self.get_present_point(pose)
self.go_graph(graph, self.kp1, self.kp2, self.kd, self.aim)
def set_goal_points(self, pose, size=5):
"""设定不同形状的目标点，坐标和角度"""
if self.key == 0: # 只设定一次
self.point = {
'squ': [
[pose.x + size, pose.y, math.pi / 2],
[pose.x + size, pose.y + size, math.pi],
[pose.x, pose.y + size, - math.pi / 2],
[pose.x, pose.y, 0],
],
'squ2': [
[pose.x + size, pose.y, -math.pi / 2],
[pose.x + size, pose.y - size, -math.pi],
[pose.x, pose.y - size, math.pi / 2],
[pose.x, pose.y, 0],
],
'squ3': [
[pose.x, pose.y, math.pi / 2],
[pose.x, pose.y + size, 0],
[pose.x + size, pose.y + size, -math.pi / 2],
[pose.x + size, pose.y, -math.pi]
],
'rec': [
[pose.x + 5, pose.y, math.pi / 2],
[pose.x + 5, pose.y + 3, math.pi],
[pose.x, pose.y + 3, math.pi / 2],
[pose.x, pose.y, 0],
],
'tri_60': [
[pose.x + size, pose.y, math.pi * 2 / 3],
[pose.x + size / 2, pose.y + (size / 2 * math.tan(math.pi / 3)), - math.pi * 2 / 3],
[pose.x, pose.y, 0]
],
'tri_60_2': [
[pose.x + size, pose.y, -math.pi * 2 / 3],
[pose.x + size / 2, pose.y - (size / 2 * math.tan(math.pi / 3)), math.pi * 2 / 3],
[pose.x, pose.y, 0]
],
'tri_60_3': [
[pose.x, pose.y, -math.pi / 3],
[pose.x + size / 2, pose.y - (size / 2 * math.tan(math.pi / 3)), -math.pi],
[pose.x - size / 2, pose.y - (size / 2 * math.tan(math.pi / 3)), math.pi / 3]
],
'hourglass': [
[pose.x, pose.y, -math.pi / 3],
[pose.x + 3 / 2, pose.y - (3 / 2 * math.tan(math.pi / 3)), -math.pi],
[pose.x - 3 / 2, pose.y - (3 / 2 * math.tan(math.pi / 3)), math.pi / 3],
[pose.x + 3 / 2, pose.y + (3 / 2 * math.tan(math.pi / 3)), math.pi],
[pose.x - 3 / 2, pose.y + (3 / 2 * math.tan(math.pi / 3)), -math.pi / 3]
],
'tri_90': [
[pose.x + size, pose.y, math.pi / 2],
[pose.x + size, pose.y + size, -3 * math.pi / 4],
[pose.x, pose.y, 0]
]
}
self.key += 1
def get_present_point(self, pose):
"""获取当前坐标和角度"""
self.x = pose.x
self.y = pose.y
self.theta = pose.theta
def go_graph(self, graph, kp1=4, kp2=4, kd=15, aim=0.025):
"""控制运动"""
self.go_line(graph, kp1, kd, aim)
self.rotate(graph, kp2)
def set_goal(self, graph):
"""设定下一个目标点"""
if self.lock == 1:
print(self.goal)
self.goal = self.goal + 1 if self.goal != len(self.point[graph]) - 1 else 0
def go_line(self, graph, kp=2, kd=15, aim=0.08, ):
"""控制走直线"""
if self.lock == 0: # 和旋转互锁
if self.flag == 0: # 第一次进函数执行一次初始化
self.error = self.size # 初始误差为设定的图形大小
self.flag += 1 # 保证初始化只执行一次
last_error = self.error # 上一次的误差
self.error = math.sqrt(
(self.x - self.point[graph][self.goal][0]) ** 2 + (
self.y - self.point[graph][self.goal][1]) ** 2) # 计算现在的误差
if abs(self.error) > aim: # 误差大于设定精度时前进
self.vel_cmd.linear.x = kp * self.error + kd * (self.error - last_error) # pd控制计算当前速度
rospy.loginfo('mode:going')
rospy.loginfo('goal:{},error:{},speed:{}'.format(self.goal, self.error, self.vel_cmd.linear.x))
else:
self.vel_cmd.linear.x = 0
self.lock = 1 # 解锁旋转
self.pub.publish(self.vel_cmd) # 发布乌龟速度
def rotate(self, graph, kp=2):
"""控制旋转"""
if self.lock == 1: # 和直走互锁
if self.flag == 1: # 第一次进函数执行一次初始化函数
self.set_goal(graph)
theta1 = self.theta + 2 * math.pi if self.theta < 0 else self.theta # 将现在的角度换算成0-2π
theta2 = theta1 + math.pi if theta1 <= math.pi else theta1 - math.pi # 和现在的角度差π的角
target = self.point[graph][self.goal - 1][2] + 2 * math.pi if self.point[graph][self.goal - 1][2] < 0 \
else self.point[graph][self.goal - 1][2] # 将目标角度换算到0-2π
self.speed = kp if theta1 < target < theta2 or theta1 > theta2 and not (theta1 > target > theta2) \
else -kp # 判断顺时针转还是逆时针转距离最短
self.flag += 1 # 保证只执行一次初始化
self.error = abs(self.theta - self.point[graph][self.goal - 1][2]) # 计算当前误差
if self.error > self.aim_rotate: # 误差大于0.01时保持旋转
self.vel_cmd.angular.z = self.speed * (self.error + 0.1) # p控制计算旋转速度
rospy.loginfo('mode:rotating')
rospy.loginfo('goal:{},error:{},speed:{}'.format(self.goal, self.error, self.vel_cmd.angular.z))
else:
self.vel_cmd.angular.z = 0
self.flag = 0
self.lock = 0
self.pub.publish(self.vel_cmd) # 发布乌龟速度
# 接收到语音命令后发布速度指令
def get_voice(data):
voice_text=data.data
rospy.loginfo("语音已接收")
if voice_text == "走方形。" or "squ":
name = 'squ'
rospy.loginfo("I said:: %s",voice_text)
Turtle('voice_teleop',name)
# Turtlerun.goal = Turtlerun.goals['squ']
elif voice_text == "走漏斗形。":
name = 'hourglass'
rospy.loginfo("I said:: %s",voice_text)
Turtle('voice_teleop',name)
# Turtlerun.goal = Turtlerun.goals['hourglass']
elif voice_text == "走60度三角形。":
name = 'tri60'
rospy.loginfo("I said:: %s",voice_text)
Turtle('voice_teleop',name)
# Turtlerun.goal = Turtlerun.goals['tri60']
elif voice_text == "走直角三角形。":
name = 'tri90'
rospy.loginfo("I said:: %s",voice_text)
Turtle('voice_teleop',name)
if __name__ == '__main__':
a = input("选择控制方式:1.命令行;2.语音控制\n")
a = int(a)
if a == 1:
graph_list = ['circle', 'squ', 'tri60', 'tri90', 'hourglass']
# print("input")
# print("-------",sys.argv)
if len(sys.argv) > 1 and sys.argv[1] in graph_list: # 实现rosrun xx yy.py squ直接跑对应图形
name = sys.argv[1]
# print("input finish")
else:
name = input(
'Please input graph name(circle squ tri60 tri90 hourglass): ') # 没在命令输对图形名称时提示输入
try:
name = str(name)
Turtle('voice_graph',name)
rospy.spin()
except rospy.ROSInterruptException:
pass
else:
rospy.init_node('voice_teleop')
while not rospy.is_shutdown():
rospy.loginfo("Starting voice Teleop")
# 订阅语音识别的输出字符
rospy.Subscriber("/voiceWords", String, get_voice)
rospy.spin()
这里的语音功能包使用的是科大讯飞的，需要到科大讯飞网站注册帐号(需要APPID)，讯飞开放平台-以语音交互为核心的人工智能开放平台
2905
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
