设定目标点进行导航

注意：

• 再学习本系列教程时，应该已经安装过ROS了并且需要有一些ROS的基本知识
• 此教程以webots_demo为基础
  ubuntu版本：20.04
  webots版本：2021a
  ros版本：noetic
  为了能和读者进一步讨论问题，建立了一个微信群，方便给大家解答问题，也可以一起讨论问题。
  加群链接

前言

平时，我们都是在Rviz中选中2D Nav Goal来设置目标点的，这很麻烦。
如果我们想象我们的环境是一个100平的房子，里面有着厨房、卧室、客厅等等，想让机器人自动到达厨房或者卧室，就必须使用程序来解决，如果我们人为在Rviz中设置目标点就不叫自动了。

1. 想法

其实第一时间我是上百度查解决方案，网上教程如出一辙：ROS之用程序设定导航目标点

但是不知为何，笔者在修改参数运行后，move_base会直接崩溃。（未解决）

如出一辙的教程无果后，只能找解决方案。

1.1 另辟蹊径

2D Nav Goal发布到哪里了，那我直接publish那个topic应该就能达到同样的效果吧。

1.2 测试环境

测试环境如下所示：

2. 测试方法是否可行

启动顺序如下：

1. roslaunch webots.launch 启动webots
2. slam_base_gmapping.launch 启动机器人始能检测元件程序、RVIZ、Gmapping和move_base

启动完成后，确保机器人能正常建图。使用rostopic list查找2D Nav Goalpublish的topic。

最终我找到/move_base_simple/goal这个topic是2D Nav Goalpublish的topic。
可以使用如下命令查看数据类型。

$ rostopic info /move_base_simple/goal

Type: geometry_msgs/PoseStamped

Publishers: 
 * /rviz (http://mckros-GL553VD:42121/)

Subscribers: 
 * /move_base (http://mckros-GL553VD:41431/)

可以看到使用的数据类型为geometry_msgs/PoseStamped
使用如下命令查看数据geometry_msgs/PoseStamped格式。

$ rosmsg show geometry_msgs/PoseStamped

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/Pose pose
  geometry_msgs/Point position
    float64 x
    float64 y
    float64 z
  geometry_msgs/Quaternion orientation
    float64 x
    float64 y
    float64 z
    float64 w

基本了解过了一些数据类型，和我们应该publish的目标topic，接下来可以在webots中获取坐标点。

3.在webots中获取坐标

我们可以在webots中拖动机器人至想去的地方.
例如，我将机器人拖至主卧室门口，如下图所示：

有两种方法可以查看当前位姿(注意坐标轴)：

1. 从节点树窗口中找到机器人，查看他的坐标值和旋转轴，如下图所示
   

2. 使用命令rostopic echo /robot/gps/values查看机器人的坐标值,如下图所示
   

使用命令rostopic echo /robot/inertial_unit/quaternion查看机器人的IMU数据获取旋转角度,如下图所示

从上面获取的数据可以看到当前机器人所处的位姿数据：

pose.position.x = -1.3;
pose.position.y = -2.5; //这里webots中的z轴为rviz中的y轴
pose.orientation.z = 0.0016;
pose.orientation.w = -0.6538;

其他位置可以使用这种方法获取，这里就以这一个位姿进行编程

4.编程

1. 创建一个msg名为Goalname.msg, 创建一个字符串变量，用于接收用户发送的房间名。

string goal_name  

1. 配置CMakeList.txt

 add_message_files(
    FILES
    Goalname.msg
  )

1. catkin_make编译
2. 创建一个名为robot_set_goals.cpp的代码文件。将在里面编程。源代码如下（含有注释）

#include "string.h"
#include "webots_demo/Goalname.h"           // 自己新建的msg头文件
#include "geometry_msgs/PoseStamped.h"
#include "ros/ros.h"
 
using namespace std;
 
ros::NodeHandle *n;
ros::Publisher pub_goal;
void goalCallback(const webots_demo::Goalname::ConstPtr &value);

int main(int argc, char **argv) {
   
    // create a node named 'robot' on ROS network
    ros::init(argc, argv, "robot_set_goal");
    n = new ros::NodeHandle;
    ros::Subscriber sub_goal;
    sub_goal = n->subscribe("/robot/goal",1,goalCallback); //订阅用户发布的topic
    
    pub_goal = n->advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal",2);
    ROS_INFO("Started success ");
    ros::spin();
}
void goalCallback(const webots_demo::Goalname::ConstPtr &value){
    int isture=0;
    string goal_name = value->goal_name;
    geometry_msgs::PoseStamped target_pose;
    target_pose.header.seq = 1;
    target_pose.header.frame_id = "map";
    
    if (goal_name == "bedroom")         // 如果用户发送了bedroom
    {
        target_pose.pose.position.x = -1.3;
        target_pose.pose.position.y = -2.5;
        target_pose.pose.orientation.z = 0.0016;
        target_pose.pose.orientation.w = -0.6538;
        isture = 1;
    }

    if (isture)
    {
        target_pose.header.stamp = ros::Time::now();
        pub_goal.publish(target_pose);  // 目标点设置为主卧并且发布目标点
        ROS_INFO("Ready to go to the goal %s",goal_name.c_str());
    }
    else
    {
        ROS_ERROR("Can't compare the goal");
    }
}

1. 配置CMakeLists.txt,添加如下程序

add_executable(robot_set_goals src/robot_set_goals.cpp)
add_dependencies(robot_set_goals webots_ros_generate_messages_cpp)
target_link_libraries(robot_set_goals	${catkin_LIBRARIES})

1. catkin_make编译

5.测试

1. 运行webots
2. 运行slam
3. 运行robot_set_goals.cpp
4. 向/robot/goal 发布信息如下

$ rostopic pub /robot/goal webots_demo/Goalname "goal_name: 'bedroom'" 

效果：

可能是角度没搞好，转的方向还需要调整
经过测试，良好的导航算法可以减少误差并且更加平稳。

大家可以发挥想象更好的去利用自动导航程序，比如语音控制。

结语

本文也是基于笔者的学习和使用经验总结的，主观性较强，如果有哪些不对的地方或者不明白的地方，欢迎评论区留言交流~
为了能和读者进一步讨论问题，建立了一个微信群，方便给大家解答问题，也可以一起讨论问题。
加群链接
✌Bye

此项目github地址：https://github.com/JackyMao1999/webots_demo