实现cartographer建图与导航[1]（2021.01.25）

  注意：  

• 再学习本系列教程时，应该已经安装过ROS了并且需要有一些ROS的基本知识

webots版本：2020b rev1 ros版本：melodic  

1.前言

  花了将近3-4天时间完成了Cartographer在ROS1和Webots之间的移植。baidu+google了很多资料，发现各大仿真软件中都有前人移植过的例子，但是在ROS1和Webots之间没有先例，所以只能笔者自己移植。   在这几天也花了点时间看了2016年谷歌发表的Cartographer论文《Real-Time Loop Closure in 2D LIDAR SLAM》，下面简单介绍一下这个算法。  

2.cartographer 算法

  2016年谷歌推出了Cartographer。当时，谷歌制作了一个背包，将传感器安装在背包上，使用Cartographer算法由实验人员在室内行走，便可以同时绘测室内的二维地图或三维地图。一开始并不是在ROS系统中使用，经过开发者移植后创建了一个名为cartographer_ros 可在 ROS 系统中使用的功能包。cartographer 包是该理论的底层使用，负责通过整合多种传感器的数据来搭建地图，cartographer_ros是上层应用，负责获取符合ROS通信机制的传感器数据转换成底层使用的格式来处理。   有关Cartographer算法的一些细节还是去看谷歌发表的论文比较好，也有大佬翻译好的：Google Cartographer 《Real-Time Loop Closure in 2D LIDAR SLAM》翻译 (中英对照）  

2.1 Cartographer 算法原理

  该算法可以通过激光测距仪等传感器测量的数据生成分辨率为５cm的实时栅格地图。Cartographer采用基于图优化方法的SLAM理论框架，分为前端和后端两部分：前端主要负责Scan-to-Submap和回环检测，将处理后的激光雷达数据与子图进行匹配，同时当生成一个子图且没有新的数据帧插入的时候会进行局部回环检测。子图创建完成后，如果找到了与当前估计位姿最佳的匹配结果，则将其添加到回环约束中。后端主要负责对位姿估计进行优化，采用分支定界和预先计算的网格来实现全局闭环检测。  

2.2 什么是图优化SLAM算法

  基于图优化SLAM算法与基于滤波方法的思路完全不同，它不再只修正机器人当前时刻的位姿，使当前时刻的位姿尽可能的准确，还通过回环检测等来优化之前时刻的机器人位姿。图优化SLAM算法的基本思路是利用保存的所有传感器测量信息以及它们之间的空间约束关系，通过各个位姿间的约束关系来对移动机器人的运动轨迹及地图进行估计。这种方法用节点来代表移动机器人的位姿，而图中节点之间的边代表位姿间的空间约束关系，所得到的图被称为位姿图。在完成位姿图的构造后，通过对位姿序列进行调整，使其能够最优地满足边所表示的约束关系，优化后的结果即为机器人的运动轨迹及地图。  

2.安装Cartographer

  好了，理论的东西不讲太多，直接开始安装。 在之前我就写过一篇有关安装Cartographer，并且成功测试过的博客，主要安装测试过程都在这篇博客上面：Ubuntu 18.04 安装cartographer  

3.在自己的机器人上使用Cartographer

  我想大家在第二节已经能够成功安装cartographer包了，仿真数据跑成功后，就要用自己的机器人跑算法了。一般来说，跑仿真数据或者跑demo很容易，只要照着教程一步一步来，基本都能成功建图，但是跑自己的机器人就会因为各种问题导致崩溃啦～～～不过大家不用怕，跟着笔者一定能在webots下跑通Cartographer。  

3.1 修改launch文件

  因为我们的机器人是两轮差动式的，所以看了一下只有cartographer_ros/launch/demo_revo_lds.launch这个启动文件符合。   官方的demo_revo_lds.launch文件如下：  

<launch>
  <param name="/use_sim_time" value="true" />
  <!-- 因为cartographer使用的是lua作为配置文件，而且我们是以双轮差速型小车作为基础的，所以使用revo_lds.lua -->
  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename revo_lds.lua"
      output="screen">

    <!-- 将from=“scan” to后面的内容换成自己激光雷达发布的激光topic -->
    <remap from="scan" to="/volcano/Sick_LMS_291/laser_scan/layer0" />
  </node>
  <!-- 这个包是用来建图的 -->
  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />

  <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
  <!-- 为了跑自己的机器人才注释的 -->
  <!-- <node name="playbag" pkg="rosbag" type="play"
      args="- -clock $(arg bag_filename)" /> -->
</launch>

  在这里我们需要修改两个地方：  

1.注释到最后一个playbag node 2.将from=“scan” to后面的内容换成自己激光雷达发布的激光topic  

3.1.1由于use_sim_time需要的topic

  一般来说当我们跑.bag文件时需要将/use_sim_time改为true，则不跑时应该为false，但是当我们仿真时只需要给它一个/clock话题来同步时间即可解决问题。 方法很简单，我们可以在robot_broadcaster.cpp中更改：  

#include <rosgraph_msgs/Clock.h>
ros::Publisher time = n->advertise<rosgraph_msgs::Clock>("/clock",10);//订阅/clock话题
while (ros::ok()) {
    if (!timeStepClient.call(timeStepSrv) || !timeStepSrv.response.success) {
    ROS_ERROR("Failed to call service time_step for next step.");
    break;
    }
    rosgraph_msgs::Clock clock;
    clock.clock = ros::Time::now();
    time.publish(clock);//实时发布rosgraph_msgs::Clock信息
    ros::spinOnce();
}

修改好，编译一下即可。 不过不要着急，我们还有很多前期步骤没有弄好。  

3.2修改lua配置文件

  launch文件配置好了，那接下来看看所需要的lua配置文件,文件地址cartographer_ros/cartographer_ros/configuration_files/revo_lds.lua： 这些参数的解释可以看官方发布的教程：Lua configuration reference documentation  

include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "horizontal_laser_link",
  published_frame = "horizontal_laser_link",
  odom_frame = "odom",
  provide_odom_frame = true,
  publish_frame_projected_to_2d = false,
  use_pose_extrapolator = true,
  use_odometry = false,
  use_nav_sat = false,
  use_landmarks = false,
  num_laser_scans = 1,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  num_point_clouds = 0,
  lookup_transform_timeout_sec = 0.2,
  submap_publish_period_sec = 0.3,
  pose_publish_period_sec = 5e-3,
  trajectory_publish_period_sec = 30e-3,
  rangefinder_sampling_ratio = 1.,
  odometry_sampling_ratio = 1.,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1.,
  landmarks_sampling_ratio = 1.,
}

MAP_BUILDER.use_trajectory_builder_2d = true

TRAJECTORY_BUILDER_2D.submaps.num_range_data = 35
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 8.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10.
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1

POSE_GRAPH.optimization_problem.huber_scale = 1e2
POSE_GRAPH.optimize_every_n_nodes = 35
POSE_GRAPH.constraint_builder.min_score = 0.65

return options

  因为下面要修改的东西还是比较多的，为了不耽误大家阅读的时间，这次我就只提供以下这两种方式吧，其实还有更多优化的方式，  

3.2.1 只使用激光雷达数据

  当我们只使用激光雷达时主要修改：  

tracking_frame = "horizontal_laser_link", 
published_frame = "horizontal_laser_link",
odom_frame = "odom",

  我们需要查一下我们的tf_tree才能知道我们要怎么改，执行操作如下：  

$ roslaunch webots_demo webots.launch
$ rosrun webots_demo robot_broadcaster
$ rosrun rqt_tf_tree rqt_tf_tree

  可以看到:
  从上图中我们可以看到更改方案如下：  

tracking_frame = "base_link", 
published_frame = "base_link",
odom_frame = "odom",

3.2.2 使用激光雷达数据+里程计数据

  改变以下数据：  

tracking_frame = "base_link",
published_frame = "odom",
odom_frame = "odom",
provide_odom_frame = false, --闭环
use_odometry = true,

3.3 测试

3.3.1 只使用激光雷达数据

  当我们修改万3.2.1所示的内容后，我们可以尝试测试一下效果如何。  

$ roslaunch webots_demo webots.launch
$ rosrun webots_demo robot_broadcaster
$ rosrun webots_demo velocity_keyboard
$ roslaunch cartographer_ros demo_revo_lds.launch

  效果所示：  

  可以看到，效果不是很理想，base_link位姿非常不稳定，甚至还会闪现！！ 不过大家不要怕，经过几天的摸索，也找到一些解决方法：  

1.将激光雷达的位置移动到机器人中心位置。 2.我们需要更换机器人的移动方式，目前的移动方式不适合利用cartographer建图，在odom中会产生很大的累计误差。

  说干就干： 我们直接将本来直接控制机器人的移动方式改掉，利用线速度和角速度来控制机器人的移动。 可以在原来的velocity_keyboard上直接改，也可以重新建一个cpp文件，这个随便大家。 直接附上代码：  

#include <signal.h>
#include <std_msgs/String.h>
#include "ros/ros.h"
#include <locale.h> 
#include <nav_msgs/Odometry.h>
#include <webots_ros/set_float.h>
#include <webots_ros/set_int.h>
#include <webots_ros/Int32Stamped.h>

using namespace std;

#define TIME_STEP 32    //时钟
#define NMOTORS 2       //电机数量
#define MAX_SPEED 2.0   //电机最大速度

ros::NodeHandle *n;

static int controllerCount;
static std::vector<std::string> controllerList; 

ros::ServiceClient timeStepClient;          //时钟通讯客户端
webots_ros::set_int timeStepSrv;            //时钟服务数据

ros::ServiceClient set_velocity_client;
webots_ros::set_float set_velocity_srv;

ros::ServiceClient set_position_client;   
webots_ros::set_float set_position_srv;   

ros::Publisher pub_speed;

double speeds[NMOTORS]={0.0,0.0};// 四电机速度值 0～100
float linear_temp=0, angular_temp=0;    //暂存的线速度和角速度
// 匹配电机名
static const char *motorNames[NMOTORS] ={"left_motor", "right_motor"};

/*******************************************************
* Function name ：updateSpeed
* Description   ：将速度请求以set_float的形式发送给set_velocity_srv
* Parameter     ：无
* Return        ：无
**********************************************************/
void updateSpeed() {   
    nav_msgs::Odometry speed_data;
    //两轮之间的距离
    float L = 0.6;
    speeds[0]  = 10.0*(2.0*linear_temp - L*angular_temp)/2.0;
    speeds[1]  = 10.0*(2.0*linear_temp + L*angular_temp)/2.0;
    for (int i = 0; i < NMOTORS; ++i) {
        // 更新速度
        set_velocity_client = n->serviceClient<webots_ros::set_float>(string("/volcano/") + string(motorNames[i]) + string("/set_velocity"));   
        set_velocity_srv.request.value = speeds[i];
        set_velocity_client.call(set_velocity_srv);
    }
    speed_data.header.stamp = ros::Time::now();
    speed_data.twist.twist.linear.x = linear_temp;
    speed_data.twist.twist.angular.z = angular_temp;
    pub_speed.publish(speed_data);
    speeds[0]=0;
    speeds[1]=0;
}

/*******************************************************
* Function name ：controllerNameCallback
* Description   ：控制器名回调函数，获取当前ROS存在的机器人控制器
* Parameter     ：
        @name   控制器名
* Return        ：无
**********************************************************/
// catch names of the controllers availables on ROS network
void controllerNameCallback(const std_msgs::String::ConstPtr &name) { 
    controllerCount++; 
    controllerList.push_back(name->data);//将控制器名加入到列表中
    ROS_INFO("Controller #%d: %s.", controllerCount, controllerList.back().c_str());

}

/*******************************************************
* Function name ：quit
* Description   ：退出函数
* Parameter     ：
        @sig   信号
* Return        ：无
**********************************************************/
void quit(int sig) {
    ROS_INFO("User stopped the '/volcano' node.");
    timeStepSrv.request.value = 0; 
    timeStepClient.call(timeStepSrv); 
    ros::shutdown();
    exit(0);
}
/*******************************************************
* Function name ：键盘返回函数
* Description   ：当键盘动作，就会进入此函数内
* Parameter     ：
        @value   返回的值
* Return        ：无
**********************************************************/
void keyboardDataCallback(const webots_ros::Int32Stamped::ConstPtr &value)
{
    switch (value->data)
    {
        case 314:
            angular_temp-=0.1;
            break;
        case 315:
            linear_temp += 0.1;
            break;
        case 316:
            angular_temp+=0.1;
            break;
        case 317:
            linear_temp-=0.1;
            break;
        case 32:
            linear_temp = 0;
            angular_temp = 0;
            break;
        default:
            break;
    }
}

int main(int argc, char **argv) {
    std::string controllerName;
    // 在ROS网络中创建一个名为volcano_init的节点
    ros::init(argc, argv, "volcano_init", ros::init_options::AnonymousName);
    n = new ros::NodeHandle;
    // 截取退出信号
    signal(SIGINT, quit);

    // 订阅webots中所有可用的model_name
    ros::Subscriber nameSub = n->subscribe("model_name", 100, controllerNameCallback);
    while (controllerCount == 0 || controllerCount < nameSub.getNumPublishers()) {
        ros::spinOnce();
        ros::spinOnce();
        ros::spinOnce();
    }
    ros::spinOnce();
    // 服务订阅time_step和webots保持同步
    timeStepClient = n->serviceClient<webots_ros::set_int>("volcano/robot/time_step");
    timeStepSrv.request.value = TIME_STEP;

    // 如果在webots中有多个控制器的话，需要让用户选择一个控制器
    if (controllerCount == 1)
        controllerName = controllerList[0];
    else {
        int wantedController = 0;
        std::cout << "Choose the # of the controller you want to use:\n";
        std::cin >> wantedController;
        if (1 <= wantedController && wantedController <= controllerCount)
        controllerName = controllerList[wantedController - 1];
        else {
        ROS_ERROR("Invalid number for controller choice.");
        return 1;
        }
    }
    ROS_INFO("Using controller: '%s'", controllerName.c_str());
    // 退出主题，因为已经不重要了
    nameSub.shutdown();

    //初始化电机
    for (int i = 0; i < NMOTORS; ++i) {
        // position速度控制时设置为缺省值INFINITY   
        set_position_client = n->serviceClient<webots_ros::set_float>(string("/volcano/") + string(motorNames[i]) + string("/set_position"));   
        set_position_srv.request.value = INFINITY;
        if (set_position_client.call(set_position_srv) && set_position_srv.response.success)     
            ROS_INFO("Position set to INFINITY for motor %s.", motorNames[i]);   
        else     
            ROS_ERROR("Failed to call service set_position on motor %s.", motorNames[i]);
        // velocity初始速度设置为0   
        set_velocity_client = n->serviceClient<webots_ros::set_float>(string("/volcano/") + string(motorNames[i]) + string("/set_velocity"));   
        set_velocity_srv.request.value = 0.0;   
        if (set_velocity_client.call(set_velocity_srv) && set_velocity_srv.response.success == 1)     
            ROS_INFO("Velocity set to 0.0 for motor %s.", motorNames[i]);   
        else     
            ROS_ERROR("Failed to call service set_velocity on motor %s.", motorNames[i]);
    }   

    pub_speed = n->advertise<nav_msgs::Odometry>("/vel",10);
    // 服务订阅键盘
    ros::ServiceClient keyboardEnableClient;
    webots_ros::set_int keyboardEnablesrv;

    keyboardEnableClient = n->serviceClient<webots_ros::set_int>("/volcano/keyboard/enable");
    keyboardEnablesrv.request.value = TIME_STEP;
    if (keyboardEnableClient.call(keyboardEnablesrv) && keyboardEnablesrv.response.success)
    {
        ros::Subscriber keyboardSub;
        keyboardSub = n->subscribe("/volcano/keyboard/key",1,keyboardDataCallback);
        while (keyboardSub.getNumPublishers() == 0) {}
        setlocale(LC_CTYPE,"zh_CN.utf8");//设置中文
        ROS_INFO("Keyboard enabled.");
        ROS_INFO("控制方向：");
        ROS_INFO("  ↑  ");
        ROS_INFO("← ↓ →");
        ROS_INFO("刹车：空格键");
        ROS_INFO("Use the arrows in Webots window to move the robot.");
        ROS_INFO("Press the End key to stop the node.");
        while (ros::ok()) {   
            ros::spinOnce();
            updateSpeed();
            if (!timeStepClient.call(timeStepSrv) || !timeStepSrv.response.success)
            {  
                ROS_ERROR("Failed to call service time_step for next step.");     
                break;   
            }   
            ros::spinOnce();
        } 
    }
    else
    ROS_ERROR("Could not enable keyboard, success = %d.", keyboardEnablesrv.response.success);

    timeStepSrv.request.value = 0;
    timeStepClient.call(timeStepSrv);
    ros::shutdown(); 
    return 0;
}

  在程序中笔者已经将线速度和角速度以nav_msgs/Odometry类型的消息发布到/vel话题上了。下面会用到 算法嘛，也就是之前第5章导航功能2中提到的,同样的图哦～～：  
  接下来不多说，直接编译看效果吧～～:  
  看上去还是有点歪，说明还是有累计误差的存在的，不过已经比刚刚好很多了，继续往下看。  

3.3.2 使用激光雷达数据+里程计数据

  因为需要里程计数据，所以我们需要自己发布一个数据。   在这里笔者是直接在robot_broadcaster.cpp中更改的。 我们可以先看一下odom需要的是什么类型的数据，这是我从tianbot_mini上截下来的数据如下所示：  
 

1. position->因为仿真中用到了GPS，所以这里可以用上GPS数据，也可以自己算。。
2. orientation->我们需要IMU数据
3. linear->线速度，只需要x
4. angular->角速度，只需要z

  直接放上代码，代码下面讲解细节～～：  

#include <signal.h>
#include <std_msgs/String.h>
#include <sensor_msgs/NavSatFix.h>
#include <sensor_msgs/Imu.h>
#include <nav_msgs/Odometry.h>
#include <rosgraph_msgs/Clock.h>
#include <tf/transform_broadcaster.h>
#include "ros/ros.h"

#include <webots_ros/set_float.h>
#include <webots_ros/set_int.h>
#include <webots_ros/Int32Stamped.h>


using namespace std;
#define TIME_STEP 32    //时钟
#define NMOTORS 2       //电机数量
#define MAX_SPEED 2.0   //电机最大速度

ros::NodeHandle *n;

static int controllerCount;
static vector<string> controllerList; 

ros::ServiceClient timeStepClient;          //时钟通讯客户端
webots_ros::set_int timeStepSrv;            //时钟服务数据

ros::Publisher odompub;

double GPSvalues[6];                        // gps数据
int gps_flag=1;
double Inertialvalues[4];                   // inertial_unit数据
double liner_speed=0;
double angular_speed=0;
/*******************************************************
* Function name ：controllerNameCallback
* Description   ：控制器名回调函数，获取当前ROS存在的机器人控制器
* Parameter     ：
        @name   控制器名
* Return        ：无
**********************************************************/
void controllerNameCallback(const std_msgs::String::ConstPtr &name) { 
    controllerCount++; 
    controllerList.push_back(name->data);//将控制器名加入到列表中
    ROS_INFO("Controller #%d: %s.", controllerCount, controllerList.back().c_str());

}

/*******************************************************
* Function name ：quit
* Description   ：退出函数
* Parameter     ：
        @sig   信号
* Return        ：无
**********************************************************/
void quit(int sig) {
    ROS_INFO("User stopped the '/robot' node.");
    timeStepSrv.request.value = 0; 
    timeStepClient.call(timeStepSrv); 
    ros::shutdown();
    exit(0);
}

void broadcastTransform()
{
    static tf::TransformBroadcaster br;
    tf::Transform transform;
    transform.setOrigin(tf::Vector3(-(GPSvalues[2]-GPSvalues[5]),GPSvalues[0]-GPSvalues[3],GPSvalues[1]));// 设置原点
    tf::Quaternion q(Inertialvalues[0],Inertialvalues[1],Inertialvalues[2],Inertialvalues[3]);
    q = q.inverse();
    transform.setRotation(q);
    br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"odom","base_link"));
    transform.setIdentity();
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "base_link", "/robot/Sick_LMS_291"));
}

void send_odom_data()
{
    nav_msgs::Odometry odom;
    odom.header.frame_id = "odom";
    odom.child_frame_id = "base_link";
    odom.header.stamp = ros::Time::now();
    odom.pose.pose.position.x = -(GPSvalues[2]-GPSvalues[5]);
    odom.pose.pose.position.y = GPSvalues[0]-GPSvalues[3];
    odom.pose.pose.position.z = 0;

    odom.pose.pose.orientation.x = 0;
    odom.pose.pose.orientation.y = 0;
    odom.pose.pose.orientation.z = Inertialvalues[2];
    odom.pose.pose.orientation.w = -Inertialvalues[3];

    odom.twist.twist.linear.x = liner_speed;
    odom.twist.twist.angular.z = angular_speed;

    odompub.publish(odom);

}

void gpsCallback(const sensor_msgs::NavSatFix::ConstPtr &values)
{
    GPSvalues[0] = values->latitude;
    GPSvalues[1] = values->altitude;
    GPSvalues[2] = values->longitude;
    if (gps_flag)
    {
        GPSvalues[3] = values->latitude;
        GPSvalues[4] = values->altitude;
        GPSvalues[5] = values->longitude;
        gps_flag=0;
    }
    broadcastTransform();
}

void inertial_unitCallback(const sensor_msgs::Imu::ConstPtr &values)
{
    Inertialvalues[0] = values->orientation.x;
    Inertialvalues[1] = values->orientation.y;
    Inertialvalues[2] = values->orientation.z;
    Inertialvalues[3] = values->orientation.w;
    broadcastTransform();
}
void velCallback(const nav_msgs::Odometry::ConstPtr &value)
{
    liner_speed = value->twist.twist.linear.x;
    angular_speed = value->twist.twist.angular.z;
    send_odom_data();
}

int main(int argc, char **argv) {
    setlocale(LC_ALL, ""); // 用于显示中文字符
    string controllerName;
    // 在ROS网络中创建一个名为robot_init的节点
    ros::init(argc, argv, "robot_init", ros::init_options::AnonymousName);
    n = new ros::NodeHandle;
    // 截取退出信号
    signal(SIGINT, quit);

    // 订阅webots中所有可用的model_name
    ros::Subscriber nameSub = n->subscribe("model_name", 100, controllerNameCallback);
    while (controllerCount == 0 || controllerCount < nameSub.getNumPublishers()) {
        ros::spinOnce();
    }
    ros::spinOnce();
    // 服务订阅time_step和webots保持同步
    timeStepClient = n->serviceClient<webots_ros::set_int>("robot/robot/time_step");
    timeStepSrv.request.value = TIME_STEP;

    // 如果在webots中有多个控制器的话，需要让用户选择一个控制器
    if (controllerCount == 1)
        controllerName = controllerList[0];
    else {
        int wantedController = 0;
        cout << "Choose the # of the controller you want to use:\n";
        cin >> wantedController;
        if (1 <= wantedController && wantedController <= controllerCount)
        controllerName = controllerList[wantedController - 1];
        else {
        ROS_ERROR("Invalid number for controller choice.");
        return 1;
        }
    }
    ROS_INFO("Using controller: '%s'", controllerName.c_str());
    // 退出主题，因为已经不重要了
    nameSub.shutdown();

    // 使能lidar
    ros::ServiceClient lidar_Client;          
    webots_ros::set_int lidar_Srv;            
    lidar_Client = n->serviceClient<webots_ros::set_int>("/robot/Sick_LMS_291/enable");
    lidar_Srv.request.value = TIME_STEP;
    if (lidar_Client.call(lidar_Srv) && lidar_Srv.response.success) {
        ROS_INFO("gps enabled.");
    } else {
        if (!lidar_Srv.response.success)
        ROS_ERROR("Failed to enable lidar.");
        return 1;
    }

    // 订阅gps服务
    ros::ServiceClient gps_Client;          
    webots_ros::set_int gps_Srv;            
    ros::Subscriber gps_sub;
    gps_Client = n->serviceClient<webots_ros::set_int>("/robot/gps/enable");
    gps_Srv.request.value = TIME_STEP;
    if (gps_Client.call(gps_Srv) && gps_Srv.response.success) {
        ROS_INFO("gps enabled.");
        gps_sub = n->subscribe("/robot/gps/values", 1, gpsCallback);
        ROS_INFO("Topic for gps initialized.");
        while (gps_sub.getNumPublishers() == 0) {
        }
        ROS_INFO("Topic for gps connected.");
    } else {
        if (!gps_Srv.response.success)
        ROS_ERROR("Sampling period is not valid.");
        ROS_ERROR("Failed to enable gps.");
        return 1;
    }

    // 订阅inertial_unit服务
    ros::ServiceClient inertial_unit_Client;          
    webots_ros::set_int inertial_unit_Srv;            
    ros::Subscriber inertial_unit_sub;
    inertial_unit_Client = n->serviceClient<webots_ros::set_int>("/robot/inertial_unit/enable");
    inertial_unit_Srv.request.value = TIME_STEP;
    if (inertial_unit_Client.call(inertial_unit_Srv) && inertial_unit_Srv.response.success) {
        ROS_INFO("inertial_unit enabled.");
        inertial_unit_sub = n->subscribe("/robot/inertial_unit/roll_pitch_yaw", 1, inertial_unitCallback);
        ROS_INFO("Topic for inertial_unit initialized.");
        while (inertial_unit_sub.getNumPublishers() == 0) {
        }
        ROS_INFO("Topic for inertial_unit connected.");
    } else {
        if (!inertial_unit_Srv.response.success)
        ROS_ERROR("Sampling period is not valid.");
        ROS_ERROR("Failed to enable inertial_unit.");
        return 1;
    }
    ROS_INFO("You can now start the creation of the map using 'rosrun gmapping slam_gmapping "
            "scan:=/volcano/Sick_LMS_291/laser_scan/layer0 _xmax:=30 _xmin:=-30 _ymax:=30 _ymin:=-30 _delta:=0.2'.");
    ROS_INFO("You can now visualize the sensors output in rqt using 'rqt'.");

    ros::Subscriber sub_speed;
    sub_speed = n->subscribe("/vel", 1, velCallback);
    ros::Publisher time = n->advertise<rosgraph_msgs::Clock>("/clock",10);//订阅/clock话题
    odompub = n->advertise<nav_msgs::Odometry>("/odom",10);

    // main loop
    while (ros::ok()) {
        if (!timeStepClient.call(timeStepSrv) || !timeStepSrv.response.success) {
        ROS_ERROR("Failed to call service time_step for next step.");
        break;
        }
        rosgraph_msgs::Clock clock;
        clock.clock = ros::Time::now();
        time.publish(clock);//实时发布rosgraph_msgs::Clock信息
        ros::spinOnce();
    }

    timeStepSrv.request.value = 0;
    timeStepClient.call(timeStepSrv);
    ros::shutdown(); 
    return 0;
}

  改完记得编译哦～ 还有lua配置文件不要忘记根据上面的教程更改哦。   测试： 先rostopic看一下,这就是所有的话题，发布的/odom、/clock、/vel都有：  
 

$ roslaunch webots_demo webots.launch
$ rosrun webots_demo robot_broadcaster
$ rosrun webots_demo velocity_keyboard_v2  #刚刚改的控制方式的程序，改成自己的
$ roslaunch cartographer_ros demo_revo_lds.launch

效果：  

结语

  本文也是基于笔者的学习和使用经验总结的，主观性较强，如果有哪些不对的地方或者不明白的地方，欢迎评论区留言交流~   Bye 此项目github地址：https://github.com/JackyMao1999/webots_demo Reference： [1]贾浩. 基于Cartographer算法的SLAM与导航机器人设计[D].山东大学,2019.