#include <ros/ros.h>
#include <robot_audio/robot_iat.h> //语音听写
#include <robot_audio/Collect.h> //语音采集
#include <robot_audio/robot_tts.h> //语音合成
#include <face_rec/recognition_results.h>//人脸识别
#include <actionlib/client/simple_action_client.h>//导航
#include "move_base_msgs/MoveBaseAction.h"
#include <iostream>
#include <string>
#include <relative_move/SetRelativeMove.h>
#include <ar_pose/Track.h>
using namespace std;
struct Point //定义一个名为Student的结构体
{
float x; //x坐标
float y; //y坐标
float z; //姿态z
float w; //姿态w
string name; //地点名字
string present; //介绍语
};
struct Point m_point[9] = {
{2.547,2.110,0.039,0.999,"吉林"," 吉林省,简称 “吉”,地处东北中部,与俄、朝接壤。是重要商品粮基地与老工业基地,有长白山等美景,人文风情浓郁。"},
{1.075,2.110,0.306,0.952,"上海","上海,简称 “沪” 或 “申”,是中国直辖市,位于长江入海口,是国际经济、金融、贸易、航运、科技创新中心,有独特海派文化。"},
{1.065,1.113,0.374,0.928,"深圳"," 深圳,是广东副省级市、经济特区。毗邻香港,经济发达,创新力强,有众多世界500 强企业,是粤港澳大湾区中心城市。"},
{2.569,0.105,0.028,0.999,"北京"," 北京,中国首都,千年古都与现代都市交融,尽显独特魅力。这里有宏伟的故宫、绵延的长城等历史古迹,见证着岁月的沧桑变迁"},
{2.553,1.115,0.028,1.000,"广州"," 广州,别称羊城、花城,广东省会。历史悠久,美食诱人,经济发达,是充满魅力与活力的国家中心城市和粤港澳大湾区核心。"},
{0.030, -0.053, 0.497, 0.867},
{0.460, 1.736, -0.709, 0.705},
{2.0, 0.105, 0.028, 0.99},
{0.045, -0.056, 0.497, 0.867}
};
struct Point pre_point[5] = {
{2.063, 1.638, 0.481, 0.877},//吉林
{0.506, 1.675, 0.543, 1.840},//上海
{0.520, 0.577, -0.68, 998},//深圳
{1.917, 0.084, 0.020, 1.000},//北京
{2.15, 1.145,0.028, 1.00}//广州
};
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> AC;//定义用到的action客户端的别名
class interaction{
public:
interaction();
string voice_collect(); //语音采集
string voice_dictation(const char* filename); //语音听写
string voice_tts(const char* text); //语音合成
string face_recognition(); // 人脸识别
void goto_nav(struct Point* point); //导航到目标位置
int auto_charge(); // 自动充电
private:
ros::NodeHandle n; //创建一个节点句柄
actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>* ac; //创建action客户端对象指针
ros::ServiceClient collect_client,dictation_client,tts_client, face_rec_client,relative_move_client,ar_track_client; //创建客户端
};
interaction::interaction(){
collect_client = n.serviceClient<robot_audio::Collect>("voice_collect"); //定义语音采集客户端
dictation_client = n.serviceClient<robot_audio::robot_iat>("voice_iat"); //定义语音听写客户端
tts_client = n.serviceClient<robot_audio::robot_tts>("voice_tts"); //定义语音合成客户端
face_rec_client = n.serviceClient<face_rec::recognition_results>("face_recognition_results"); // 定义人脸识别客户端
relative_move_client = n.serviceClient<relative_move::SetRelativeMove>("relative_move"); // 定义相对移动客户端
ar_track_client = n.serviceClient<ar_pose::Track>("track"); // 定义AR码识别客户端
}
string interaction::voice_collect(){
//请求"voice_collect"服务,返回音频保存位置
ros::service::waitForService("voice_collect");
robot_audio::Collect srv;
srv.request.collect_flag = 1;
collect_client.call(srv);
return srv.response.voice_filename;
}
string interaction::voice_dictation(const char* filename){
//请求"voice_dictation"服务,返回听写出的文本
ros::service::waitForService("voice_iat");
robot_audio::robot_iat srv;
srv.request.audiopath = filename;
dictation_client.call(srv);
return srv.response.text;
}
string interaction::voice_tts(const char* text){
//请求"voice_tts"服务,返回合成的文件目录
ros::service::waitForService("voice_tts");
robot_audio::robot_tts srv;
srv.request.text = text;
tts_client.call(srv);
string cmd= "play "+srv.response.audiopath;
system(cmd.c_str());
sleep(1);
return srv.response.audiopath;
}
string interaction::face_recognition() {
face_rec::recognition_results face_rec_data;
ros::service::waitForService("face_recognition_results");
face_rec_data.request.mode = 1;
face_rec_client.call(face_rec_data);
//if(face_rec_data.response.success) {
return "您是" + face_rec_data.response.result.face_data[0].name;
// } else {
// return "未识别到人脸";
//}
}
int interaction::auto_charge() { //导航到充电站
relative_move::SetRelativeMove RelativeMove_data;
ar_pose::Track Track_data;
ros::service::waitForService("relative_move");
ros::service::waitForService("track");
Track_data.request.ar_id = 0;
Track_data.request.goal_dist = 0.3;
ar_track_client.call(Track_data);
RelativeMove_data.request.goal.x = -0.10;
RelativeMove_data.request.global_frame = "odom";
relative_move_client.call(RelativeMove_data);
return 1;
}
void interaction::goto_nav(struct Point* point){ //导航到目标
ac = new AC("move_base",true);
ROS_INFO("Waiting for action server to start.");
ac->waitForServer();//一直等待move_base Action服务开启
ROS_INFO("Action server started, sending goal.");
//定义一个导航目标
move_base_msgs::MoveBaseGoal goal;
goal.target_pose.header.frame_id = "map";
goal.target_pose.header.stamp = ros::Time::now(); //设置时间戳
//导航点位置信息
goal.target_pose.pose.position.x = point->x;
goal.target_pose.pose.position.y = point->y;
goal.target_pose.pose.orientation.z = point->z;
goal.target_pose.pose.orientation.w = point->w;
ac->sendGoal(goal); //发送导航目标
ac->waitForResult(); //等待导航结果
if(ac->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) //判断导航状态
ROS_INFO("Goal succeeded!");
ac->cancelGoal(); //取消动作
delete ac;
}
int main(int argc,char **argv){
ros::init(argc,argv,"interaction");
interaction audio; //创建一个交互实例
string dir,text,path; //创建两个字符串变量
//要改以下!!!!?????????????????????????????????????????????????????
while(ros::ok()){
cout << "等待语音指令" << endl;
dir = audio.voice_collect(); //采集语音
text = audio.voice_dictation(dir.c_str()).c_str(); //语音听写
// text = "带我去上海";
cout << "听到的内容为:" << text << endl;
if (text.find("元宝元宝") != string::npos) {
audio.voice_tts("你好,欢迎您的到来!有什么需要帮助的吗?"); //合成应答语音
// dir = audio.voice_collect(); //采集语音
// text = audio.voice_dictation(dir.c_str()).c_str(); //语音听写
}
//加了以下!!!!!!!
else if(text.find("参观一下") != string::npos){ //识别到“带我去”关键词
for(int i=0;i<5;i++){ //遍历所有参数
if(text.find(m_point[i].name.c_str()) != string::npos){ //查找所有导航点是否有匹配的导航点
audio.voice_tts("好的,请跟我来。"); //合成应答语音
audio.goto_nav(&m_point[5]);
audio.goto_nav(&pre_point[i]); //导航到匹配的导航点
audio.goto_nav(&m_point[i]); //导航到匹配的导航点
audio.voice_tts(m_point[i].present.c_str()); //介绍导航语
string voice_char = "这里就是" + m_point[i].name + "馆"+"啦,"+"我要继续回去工作啦!";
audio.voice_tts(voice_char.c_str()); //合成应答语音
audio.goto_nav(&m_point[5]);
break;
}
}
}
else if(text.find("执行巡检任务。") != string::npos) {
audio.voice_tts("好的,进入巡检模式。"); //合成应答语音
for(int i = 0; i < 5; i ++) {
audio.goto_nav(&pre_point[i]); //导航到匹配的导航点
audio.goto_nav(&m_point[i]);
audio.goto_nav(&m_point[6]);
audio.auto_charge();
audio.voice_tts("充电成功"); //合成应答语音
sleep(2);
audio.goto_nav(&m_point[5]);
}
}
//加了以上!!!!!!!!!!
else if(text.find("我要到") != string::npos){ //识别到“带我去”关键词
for(int i=0;i<5;i++){ //遍历所有参数
if(text.find(m_point[i].name.c_str()) != string::npos){ //查找所有导航点是否有匹配的导航点
string voice_char = "好的,这就带您去" + m_point[i].name + "馆";
audio.voice_tts(voice_char.c_str()); //合成应答语音
audio.goto_nav(&m_point[5]);
audio.goto_nav(&pre_point[i]); //导航到匹配的导航点
audio.goto_nav(&m_point[i]); //导航到匹配的导航点
audio.voice_tts(m_point[i].present.c_str()); //介绍导航语
audio.goto_nav(&m_point[6]);
audio.auto_charge();
audio.voice_tts("充电成功"); //合成应答语音
sleep(2);
audio.goto_nav(&m_point[5]);
break;
}
}
}
else if(text.find("参观一圈") != string::npos) {
audio.voice_tts("好的,这就带您参观一圈"); //合成应答语音
for(int i = 0; i < 5; i ++) {
audio.goto_nav(&pre_point[i]); //导航到匹配的导航点
audio.goto_nav(&m_point[i]);
audio.voice_tts(m_point[i].present.c_str());
}
audio.goto_nav(&m_point[5]);
}
else if(text.find("我是谁") != string::npos) {
audio.voice_tts("好的,让我看一看"); //合成应答语音
string result = audio.face_recognition();
cout << result << " ";
audio.voice_tts(result.c_str());
}
}
return 0;
}哪里错了?为什么不能回答”我是谁”的问题