欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

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

程序员文章站 2022-03-12 17:10:10
...

这个功能实现起来也不难,主要就是先监听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();
}

五,最终效果

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

六,完整项目地址

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

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