ROS与Qt5人机交互界面开发-实现定点返航功能

ROS与Qt5人机交互界面开发-实现定点返航功能

说明：

• 介绍如何实现定点返航功能

步骤：

• 这个功能实现起来也不难，主要就是先监听amcl_pose话题，获取小车在地图上的实时位置
• 如果点击设置返航点了则记下此位置，点击返航时则把该位置作为返航目标话题发送出去
• 第一步：配置package.xml
• 由于获得机器人位置的话题类型是geometry_msgs所以需要添加功能包对此话题的支持
• 在package.xml文件添加如下两行：

 <build_depend>geometry_msgs</build_depend>
  <run_depend>geometry_msgs</run_depend>

• 第二步：创建机器人位置话题订阅者
• 创建机器人位置订阅者及回调函数：

 ros::Subscriber pos_sub;
 void poseCallback(const geometry_msgs::PoseWithCovarianceStamped& pos);

• 订阅机器人位置：

//机器人位置话题
pos_sub=n.subscribe("amcl_pose",1000,&QNode::poseCallback,this);

• 回调函数：
• 我这里是在线程中订阅话题，通过信号把消息发送到主线程

//机器人位置话题的回调函数
void QNode::poseCallback(const geometry_msgs::PoseWithCovarianceStamped& pos)
{
    emit position(pos.pose.pose.position.x,pos.pose.pose.position.y,pos.pose.pose.orientation.z,pos.pose.pose.orientation.w);
//    qDebug()<<<<" "<<pos.pose.pose.position.y;
}

• 主线程绑定坐标改变信号：

//机器人位置信号  connect(&qnode,SIGNAL(position(QString,double,double,double,double)),this,SLOT(slot_position_change(QString,double,double,double,double)));

• 信号的槽函数
• 主要就是更新ui显示当前坐标

//刷新当前坐标
void MainWindow::slot_position_change(QString frame,double x,double y,double z,double w)
{
    //更新ui显示
    ui.label_frame->setText(frame);
    ui.label_x->setText(QString::number(x));
    ui.label_y->setText(QString::number(y));
    ui.label_z->setText(QString::number(z));
    ui.label_w->setText(QString::number(w));
}

• 设置返航点
• 主要就是记录下当前的坐标作为返航点
• 按钮点击后的槽函数:

//刷新返航地点
void MainWindow::slot_set_return_point()
{
    //更新ui返航点显示
    ui.label_return_x->setText(ui.label_x->text());
    ui.label_return_y->setText(ui.label_y->text());
    ui.label_return_z->setText(ui.label_z->text());
    ui.label_return_w->setText(ui.label_w->text());
    //写入setting
    QSettings settings("return-position", "cyrobot_monitor");
    settings.setValue("x",ui.label_x->text());
    settings.setValue("y",ui.label_y->text());
    settings.setValue("z",ui.label_z->text());
    settings.setValue("w",ui.label_w->text());
    //发出声音提醒
    if(media_player!=NULL)
    {
        delete media_player;
        media_player=NULL;
    }
    media_player=new QSoundEffect;
    media_player->setSource(QUrl::fromLocalFile("://media/refresh_return.wav"));
    media_player->play();

}

• 返航
• 按钮点击后的槽函数：

//返航
void MainWindow::slot_return_point()
{
    qnode.set_goal(ui.label_frame->text(),ui.label_return_x->text().toDouble(),ui.label_return_y->text().toDouble(),ui.label_return_z->text().toDouble(),ui.label_return_w->text().toDouble());
    //声音提醒
    if(media_player!=NULL)
    {
        delete media_player;
        media_player=NULL;
    }
    media_player=new QSoundEffect;
    media_player->setSource(QUrl::fromLocalFile("://media/start_return.wav"));
    media_player->play();
}

• qnode.set_goal()函数实现：

//发布导航目标点信息
void QNode::set_goal(QString frame,double x,double y,double z,double w)
{
    geometry_msgs::PoseStamped goal;
    //设置frame
    goal.header.frame_id=frame.toStdString();
    //设置时刻
    goal.header.stamp=ros::Time::now();
    goal.pose.position.x=x;
    goal.pose.position.y=y;
    goal.pose.position.z=0;
    goal.pose.orientation.z=z;
    goal.pose.orientation.w=w;
    goal_pub.publish(goal);
    ros::spinOnce();
}

• 最终效果：

• 完整源码点击查看