ROS Qt5 librviz人机交互界面开发十（实现定点返航功能）

这个功能实现起来也不难，主要就是先监听amcl_pose话题，获取小车在地图上的实时位置，如果点击设置返航点了则记下此位置，点击返航时则把该位置作为返航目标话题发送出去

一，配置package.xml

由于获得机器人位置的话题类型是geometry_msgs所以需要添加功能包对此话题的支持
在package.xml文件添加如下两行：

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

二，创建机器人位置话题订阅者

1，创建机器人位置订阅者及回调函数：

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

2，订阅机器人位置：

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

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

//机器人位置话题的回调函数
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;
}

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

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

5，信号的槽函数
主要就是更新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();
}

五，最终效果

六，完整项目地址

在我自己学习的过程中目前发现没有相关类似完整开源项目，为了帮助其他人少走弯路，我决定将自己的完整项目开源：
github
创作不易，如果本教程对你有帮助，关注或点个赞吧，或者github标个星哦～～
您的支持就是我最大的动力～

（转载请注明作者和出处：https://blog.csdn.net/qq_38441692 未经允许请勿用于商业用途）