3.5实现机器人定点返航

首先在qnode.hpp需要导入amclPose的头文件，和发布导航目标点的头文件

#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/PoseStamped.h>

可以查看数据结构

rosmsg show geometry_msgs/PoseWithCovarianceStamped

qnode.hpp声明回调函数

void amcl_pose_callback(const geometry_msgs::PoseWithCovarianceStamped &msg);

声明一个订阅者：

ros::Subscriber amcl_pose_sub;

ui设计：

qnode.cpp中年发布自定义信号

//发布自定义信号，然后去main_window中更新坐标
void QNode::amcl_pose_callback(const geometry_msgs::PoseWithCovarianceStamped &msg)
{

    emit position(msg.pose.pose.position.x,msg.pose.pose.position.y,msg.pose.pose.orientation.z);
}

只有x,y速度，以及z轴航向角
在main_window中去链接这个信号

connect(&qnode,SIGNAL(position(double,double,double)),this,SLOT(slot_update_pos(double,double,double)));

在槽函数中根新ui显示

void MainWindow::slot_update_pos(double x,double y,double z)
{
    ui.pos_x->setText(QString::number(x));
    ui.pos_y->setText(QString::number(y));
    ui.pos_z->setText(QString::number(z));
}

运行看看效果：
可以用

rostopic echo amcl_pose

看到x,y z坐标变化了

进行设置返航，记录返航点，实现返航 ui设计

然后在main_window中进行槽函数的定义，声明，链接

    connect(ui.set_return_pos_btn,SIGNAL(clicked()),this,SLOT(slot_set_return_pos()));
    connect(ui.return_btn,SIGNAL(clicked()),this,SLOT(slot_return()));



    //设置记录返航点
void MainWindow::slot_set_return_pos()
{
  ui.return_x->setText(ui.pos_x->text());
   ui.return_y->setText(ui.pos_y->text());
    ui.return_z->setText(ui.pos_z->text());
}

重点：//我们已经将返航点记录下来了，如何实现返航？
//需要我们在qnode.hpp中声明一个发布者。以及一个接口函数

ros::Publisher goal_pub;    //返航的发布者

返航接口函数

void set_goal(double x,double y,double z);  //返航接口函数

注意在.cpp文件中定义发布者，发布导航目标点的一个话题方法move_base_simple/goal

goal_pub=n.advertise<geometry_msgs::PoseStamped>("move_base_simple/goal",1000);

实现接口函数

//返航接口函数
void QNode::set_goal(double x,double y,double z)
{
    geometry_msgs::PoseStamped goal;
    //设置frame
    goal.header.frame_id="map";
    //设置时刻
    goal.header.stamp=ros::Time::now();
    goal.pose.position.x=x;
    goal.pose.position.y=y;
    goal.pose.orientation.z=z;
    goal_pub.publish(goal);
}

接下来就在main_window中调用接口函数

//我们已经将返航点记录下来了，如何实现返航？
//我们在qnode.hpp中声明一个发布者。以及一个接口函数qnode.set_goal，通过这个接口函数去发布返航
void MainWindow::slot_return()
{
  qnode.set_goal(ui.return_x->text().toDouble(),ui.return_y->text().toDouble(),ui.return_z->text().toDouble());
}