设定目标点进行导航

注意:

  • 再学习本系列教程时,应该已经安装过ROS了并且需要有一些ROS的基本知识
  • 此教程以webots_demo为基础
    ubuntu版本:20.04
    webots版本:2021a
    ros版本:noetic

0.前言

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

1. 想法

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

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

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

1.1 另辟蹊径

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

1.2 测试环境

测试环境如下所示:
1

2. 测试方法是否可行

启动顺序如下:

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

启动完成后,确保机器人能正常建图。使用rostopic list查找2D Nav Goalpublish的topic。
2
最终我找到/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中拖动机器人至想去的地方.
例如,我将机器人拖至主卧室门口,如下图所示:
3
有两种方法可以查看当前位姿(注意坐标轴):

  1. 从节点树窗口中找到机器人,查看他的坐标值和旋转轴,如下图所示
    4
  2. 使用命令rostopic echo /robot/gps/values查看机器人的坐标值,如下图所示
    5
    使用命令rostopic echo /robot/inertial_unit/quaternion查看机器人的IMU数据获取旋转角度,如下图所示
    6
    从上面获取的数据可以看到当前机器人所处的位姿数据:
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'"

效果:

1

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

结语

本文也是基于笔者的学习和使用经验总结的,主观性较强,如果有哪些不对的地方或者不明白的地方,欢迎评论区留言交流~

✌Bye

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