语音控制
通过前面的教程讲解,我们已经理解了整个实现语音自定义交互的过程。接下来让我们学习如何通过语音交互功能来控制我们的机器人进行移动,您也可以基于前面讲解的自定义交互模式自行设计语音来控制机器人。
整体效果
运行默认demo后,您可以通过语音下达前进
、后退
、左转
和右转
的指令,并控制机器人执行相应的操作。
我们还提供了可以添加加速、减速等功能的额外接口。
实现过程
仿真使用
首先我们得通过唤醒词小道小道
将机器人唤醒,让他进入唤醒状态。所以需要先启动语音离线命令词检测
和VAD检测
节点。
打开两个新终端并分别运行以下两条指令:
roslaunch handsfree_speech offline_interactive_ros.launch//语音交互功能节点
rosrun handsfree_speech vad_record.py//vad检测节点
接着,我们运行打开一个新终端并运行如下指令:
rosrun handsfree_speech cn_voice_cmd_vel.py//这个就是语音控制节点
根据语音第一节的内容,我们可以先使用仿真环境对机器人先进行测试,先看一下效果,这里我们也提供了相应的仿真环境。
roslaunch handsfree_2dnav navigation.launch
这两个都是仿真环境,在空间不够空旷的情况下或者还没熟练的情况下,可以执行其中任意一个就能进行仿真测试。
您可以通过仿真环境,用我们的语音交互的功能去看执行效果,并进行驱动功能的调试。
实机使用
当您对机器人的语音交互功能逐渐熟悉了之后,您就可以在实际环境中对我们的机器人进行调试。
首先还是先启动 语音交互节点
和 vad检测节点
:
roslaunch handsfree_speech offline_interactive_ros.launch//语音交互功能节点
rosrun handsfree_speech vad_record.py//vad检测节点
接着需要启动 语音控制节点
:
rosrun handsfree_speech cn_voice_cmd_vel.py//这个就是语音控制节点
然后还需要启动驱动节点
:
roslaunch handsfree_hw handsfree_hw.launch//驱动节点
现在您就可以尝试利用语音控制机器人了。
控制核心
您可以利用前面所提到过的的语音交互功能节点
和VAD检测节点
,来获取我们的交互数据,并通过交互数据的提取,将ID号的信息通过topic发送到控制节点处,然后再让控制节点通过/mobile_base/mobile_base_controller/cmd_vel
topic来发送数据。通过发送线速度和角速度的数据,我们可以将其转化成小车的车轮线速度或转速度并进行控制。
我们的控制核心是根据英文语音控制来进行的中文语音适配和优化。
驱动控制
rospy.on_shutdown(self.cleanup)
# Set a number of parameters affecting the robot's speed
self.max_speed = rospy.get_param("~max_speed", 0.4)
self.min_speed = rospy.get_param("~min_speed", 0.1)
self.linear_increment = rospy.get_param("~linear_increment", 0.1)
self.max_angular_speed = rospy.get_param("~max_angular_speed", 0.8)
self.min_angular_speed = rospy.get_param("~min_angular_speed", 0.1)
self.angular_increment = rospy.get_param("~angular_increment", 0.1)
# Initial parameters
self.speed = rospy.get_param("~start_speed", 0.4)
self.angular_speed = rospy.get_param("~start_angular_speed", 0.2)
# We don't have to run the script very fast
self.rate = rospy.get_param("~rate", 5)
r = rospy.Rate(self.rate)
# Time, in seconds, for waiting correct command
self.initial_wait_time = 5
self.wait_time = self.initial_wait_time
# A flag to determine whether or not TIAGo voice control
self.TIAGo = False
# Initialize the Twist message we will publish.
self.cmd_vel = Twist()
# Publish the Twist message to the cmd_vel topic
self.cmd_vel_pub = rospy.Publisher('/mobile_base/mobile_base_controller/cmd_vel', Twist, queue_size=5)
# Subscribe to the /recognizer/output topic to receive voice commands.
rospy.Subscriber('/recognizer/output', String, self.speech_callback)
# A mapping from keywords or phrases to commands
self.keywords_to_command = {'backward': ['backward', 'go backward', 'move backward', 'back', 'move back'],
'forward': ['forward', 'go forward', 'move forward'],
'turn left': ['turn left', 'left'],
'turn right': ['turn right', 'right'],
'stop': ['stop', 'halt'],
'faster': ['faster'],
'slower': ['slower'],
'quarter': ['quarter speed', 'quarter'],
'half': ['half speed', 'half'],
'full': ['full speed', 'full']}
rospy.loginfo("Ready to receive voice commands")
# We have to keep publishing the cmd_vel message if we want the robot to keep moving.
while not rospy.is_shutdown():
self.cmd_vel_pub.publish(self.cmd_vel)
r.sleep()
if command == 'backward':
self.cmd_vel.linear.x = -self.speed
self.cmd_vel.angular.z = 0
rospy.loginfo("Command: " + str(command))
time.sleep(3)
rospy.loginfo("Command: " + self.get_command(msg.data))
self.cmd_vel = Twist()
self.TIAGo = False
self.wait_time = self.initial_wait_time
return
elif command == 'forward':
self.cmd_vel.linear.x = self.speed
print(self.speed)
self.cmd_vel.angular.z = 0
rospy.loginfo("Command: " + str(command))
time.sleep(3)
rospy.loginfo("Command: " + self.get_command(msg.data))
self.cmd_vel = Twist()
self.TIAGo = False
self.wait_time = self.initial_wait_time
return
语音控制反馈部分
if(todo_id==99999)
{
char* text;
int x =random(0, 5);
if(x==1)
{
text =(char*)"有什么事情"; //合成文本
}
else if(x==2)
{
text =(char*)"怎么啦"; //合成文本
}
else if(x==3)
{
text =(char*)" 来了"; //合成文本
}
else
{
text =(char*)" 我在"; //合成文本
}
printf("收到唤醒指令");
ret =text_to_speech(text, filename_move, session_begin_params);
if (MSP_SUCCESS != ret)
{
printf("text_to_speech failed, error code: %d.\n", ret);
}
printf("合成完毕\n");
ifwake=1;
std_msgs::String msg_pub;
msg_pub.data ="stop";
pub.publish(msg_pub);
play_wav((char*)"/home/handsfree/catkin_ws/src/handsfree_speech/res/tts_sample_move.wav");
msg_pub.data ="tiago";
pub.publish(msg_pub);
}
if(todo_id==10001&&ifwake==1)
{
const char* text ="收到指令,请注意避让"; //合成文本
ret =text_to_speech(text, filename_move, session_begin_params);
if (MSP_SUCCESS != ret)
{
printf("text_to_speech failed, error code: %d.\n", ret);
}
printf("合成完毕\n");
printf("收到前进指令");
play_wav((char*)filename_move);
ifwake=0;
std_msgs::String msg_pub;
msg_pub.data ="forward";
pub.publish(msg_pub);
}
if(todo_id==10002&&ifwake==1)
{
const char* text ="收到指令,请注意,倒车"; //合成文本
ret =text_to_speech(text, filename_move, session_begin_params);
if (MSP_SUCCESS != ret)
{
printf("text_to_speech failed, error code: %d.\n", ret);
}
printf("合成完毕\n");
printf("收到倒车指令");
play_wav((char*)filename_move);
ifwake=0;
std_msgs::String msg_pub;
msg_pub.data ="backward";
pub.publish(msg_pub);
}
if(todo_id==10003&&ifwake==1)
{
const char* text ="收到指令,正在转弯"; //合成文本
ret =text_to_speech(text, filename_move, session_begin_params);
if (MSP_SUCCESS != ret)
{
printf("text_to_speech failed, error code: %d.\n", ret);
}
printf("合成完毕\n");
printf("收到左转指令");
play_wav((char*)filename_move);
ifwake=0;
std_msgs::String msg_pub;
msg_pub.data ="left";
pub.publish(msg_pub);
}
if(todo_id==10004&&ifwake==1)
{
const char* text ="收到指令,正在转弯"; //合成文本
ret =text_to_speech(text, filename_move, session_begin_params);
if (MSP_SUCCESS != ret)
{
printf("text_to_speech failed, error code: %d.\n", ret);
}
printf("合成完毕\n");
printf("收到右转指令");
play_wav((char*)filename_move);
ifwake=0;
std_msgs::String msg_pub;
msg_pub.data ="right";
pub.publish(msg_pub);
}
在语音控制程序中,我们设置了一个叫tiago
的标志,只有当机器人接收到这个标志,机器人才会判定接收到的后续控制数据算有效数据。这也相当于是一个状态判定的标志。所以我们在接收到唤醒命令的唤醒反馈中添加了一个tiago
数据,代表后面的发送的才为有效数据。
这里我们设计了一个额外的功能,当您说出唤醒词小道小道
时,机器人会停下正在运行的任务,这样我们在机器人进行移动的时候,我们通过唤醒词小道小道
来停止机器人。
在前进
和后退
操作中,我们还添加了一个延时函数,可以让机器人每次执行一段时间后,就停止当前的前进
和后退
动作。避免机器人移动的距离比较远,导致语音唤醒词没办法正常唤醒机器人,使得机器人没办法停止,一直前进
或者后退
。
下面是一个科大讯飞的错误码信息查询,如果在调用科大讯飞的时候报错了相关的错误码,可以通过这个链接查询。 科大讯飞错误码信息查询