ROS联合Webots仿真

文章开始之前首先提出以下几点：

• 本文的目的：通过在Webots中搭建仿真环境使读者快速入门Webots，能使用Webots开展ROS下的仿真工作，为实际项目的落地打好基础。
• 笔者的工作内容：整合关于Webots的一些较好的入门资料，通过跑通一些例程：一如建图、导航等，与读者一起学习并且实操Webots这款仿真软件，后续的工作笔者也在学习当中，敬待更新；
• 为什么要学Webots：Webots是一款类似于Gazebo的物理仿真软件，其优点在于集成了大量的场景文件，同时在社区内有许多开源的资源可使用（当然这些优点其实大部分仿真软件都具备，但笔者一直认为：会用工具本身也是一种能力，多掌握一种工具不是一件坏事，这样面对实际的问题也有多种工具可供选择）令我感觉最深刻的是，需要自己编写控制器代码，这点虽然不是新手友好型，但是这点可以当作一种算法验证的工具。

注：本文Ubuntu系统是16.04，ros版本为kinetic，所需的Webots版本为2020a rev1。

---

一、Webots的安装和配置

1. 下载并安装Webots

​由于git和Webots官网下载速度较慢，这里附上百度网盘链接（实际上读者也可以尝试前面两种下载方式，因为网盘下载速度更慢…）：

链接：https://pan.baidu.com/s/1uAafKhKggayauXdWxTbXJA
提取码：zlg7

• 下载完成后进入文件所在目录，打开终端输入以下指令解压安装包：

dpkg -i webots_2020a-rev1_amd64.deb

• 安装webots_ros功能包：

sudo apt install ros-kinetic-webots-ros 

2. 配置Webots

• 安装完成后进入/usr/local/webots/projects/languages/文件夹下，里面有四个文件夹。

• 将其中的webots_ros文件复制到catkin_ws/src下；
• 把controllers、plugings、worlds剩余三个文件夹复制到catkin_ws/src下刚刚复制过来的webots_ros下；
• 进入/usr/local/webots/projects/default/controllers/ros/include/下，将其中的srv和msg文件夹也复制到catkin_ws/src/webots_ros下，完成复制结果如下图:

• 添加WEBOTS_HOME环境变量。打开bashrc文件：

gedit ~/.bashrc

在文件最后一行加入下列代码，点击保存：

export WEBOTS_HOME=/usr/local/webots

• 进入catkin_ws/src打开终端编译：

catkin_make 

• 打开官方例程pioneer3at机器人：

roslaunch webots_ros pioneer3at.launch

可以看到下面提示栏里提示：

[ros] [ INFO] [1634120441.840263312]: The controller is now connected to the ROS master.

至此也完成了Webots的安装与配置。

---

二、搭建仿真环境

上一章节中我们安装并配置了Webots，并且运行了官方的Webots例程。本章将为读者讲解如何搭建一个简单的仿真环境，在Webots中搭建一个属于自己的双轮差速机器人。

1. 创建一个新世界

• step 1：

打开 Webots，设置中文模式：按下Win键，搜索Webots，为了方便起见，建议把图标固定到任务栏；菜单栏选择Tools->Preferences进入如下界面把Language设置为中文。

这里设置为中文后，软件内只有部分位置被翻译过来，不过没关系，后续笔者会对用到的地方做出详细的解释。

• step 2：

菜单栏选择文件->新世界，创建一个新的地图。

• step 3：

左侧窗口我们称之为场景树，在场景树中点击worldinfo，可以看到有个“+”按钮被激活，点击该按钮。

• step 4：

在弹出的窗口中为环境添加节点：找到PROTO nodes(Webots Projects)->objects->backgrounds，将展开后的两个节点均Add进新建的世界。

• step 5：

添加地板：找到PROTO nodes(Webots Projects)->objects->floors，将展开后的RectangleArena(solid)加入世界中。

以上操作过后，显示的世界如下图：

我们可以改变场景树中的RectangleArena->floorsize：双击后改为x=5m、y=5m，方便后面进行小车的仿真。

2. 创建小车模型

• step 1：建立车身：

1. 在场景树中添加Robot节点；
2. 双击Robot->children节点，创建Base_nodes->Shape节点；
3. 在场景树中单击Robot->children->Shape，下面会弹出一个DEF框，添加定义为body；
   
4. 双击appearance添加颜色纹理Base_nodes->Appearance，双击geometry 添加形状Base_nodes->Box；
5. 双击appearance->material添加金属属性，可以修改其节点下的diffusecolor属性更改颜色；
6. 在geometry节点添加几何属性，设置车体大小：x=0.5m、y=0.15m、z=0.5m，在translation节点中将其在y上坐标设置高一些，方便后面加轮子。（注意：translation节点的参数值都是相对于世界坐标系的物体坐标）现在得到的车身如下图所示：
   
7. 在Robot节点下双击boundingObject：因为上面DEF了body，直接use body即可；同理双击Robot节点下重力属性physics（只要创建出physics子节点即可）

• step 2：加入车轮：

1. 在Robot->children下创建一个Base_nodes->HingeJoint节点,DEF为left_wheel，该节点下会出现三个节点，如下图所示：
   
2. 在HingeJoint->jointParameters节点下创建子节点HingeJointParameters；

该节点主要用来设置：

• 铰链轴的方向（axis，电机旋转轴的平行向量）
• 铰链轴的位置（anchor，表示电机转轴相对于父节点的坐标偏移量）
• 弹簧系数（springConstant）
• 阻尼系数系数（dampingConstant）
• 静摩擦系数（staticFriction）

3. 点击查看–>可选显示–>show joint Axes或者同时按下ctrl+F5，可以显示出旋转轴，方便我们调整旋转轴及旋转原点；
4. 在HingeJoint->device节点下添加电机Rotatioal Motor,Rotatioal Motor->name命名为left_motor；

该节点主要用来设置：

• 电机加速度（acceleration）
• 消耗因数（comsuptionFactor）
• PID参数（controlPID，从左到右为P、I、D，位置控制模式）
• 电机最大速度（maxVelocity）
• 电机最大转矩（maxTorque）

5. 在HingeJoint->endPoint节点下，增加Base_nodes->soild节点，该节点主要是设置电机模型以及其他功能参数，之后在soild->children节点下创建一个shape节点，在shape节点下设置轮子的形状为Cylinder；
6. 通过调整translation和rotation，将轮子设置到合适的位置，轮子大小设置为（height=0.08 radius=0.12）如图：
   
7. 接下来通过调整axis和 anchor，将电机旋转轴调整到正确位置，比如我这里电机的位置相对于车体（x=0.31、y=0、z=0）,于是我将旋转轴相对于父节点(body)的位置通过anchor调整至此，并且修改axis的各个值，观察到我的轮子是绕x轴旋转，所以将axis设置为（1 0 0）。可以看到，轮子处有一个黑线（如上图中所示），表示旋转轴在这里，黑线表示的就是电机旋转轴的位置和方向。笔者在这里设置的（axis=2 0 0）m和（anchor=0.31 0 0）m
8. 最后，在HingeJoint->endPoint节点下给轮子增加碰撞属性boundingObject(与轮子大小一致)及重力属性physics（创建一个physics子节点）
9. 接下来复制出另一个轮子，按照上面的方法调整axis和anchor,并且更改电机name为right_motor

以上操作完成后，模型变为如下图所示:

• step 3：添加万向轮

1. 在Robot->children下创建一个Base_nodes->HingeJoint节点；
2. 创建jointParameters节点，axis设置为（0 1 0），anchor设置为（0 -0.09 0.23）；
3. 创建endpoint节点，万向轮是被动轮，不需要电机驱动（HingeJoint->device节点不需要创建子节点）；
4. 在HingeJoint->endPoint节点下，增加Base_nodes->soild节点，soild->children节点下创建一个shape节点，设置轮子的形状为sphere；
5. 在HingeJoint->endPoint节点下给轮子增加碰撞属性boundingObject及重力属性physics；
6. 复制一个万向轮到anchor（0,-0.09,0.23）。
   可以打开ctrl+F5调整axis和anchor，最后模型如下图所示:
   

• step 4：加入激光雷达

1. Robot->children节点下双击添加PROTO nodes—>devices->sick->SickLms291(lidar)；

在sick（Sensor Intelligence）公司官网上可以查到SickLms291这款雷达的各项参数：

工作范围示意图如下图所示：

这里注意到：其扫描范围只有前方一个半圆区域，这点不同于常见的激光雷达（例如Rplidar）；

2. 改变雷达的translation，移动到合适的位置，并且保证：雷达的坐标轴各轴方向与车体各轴方向相同。
   

• step 5：加入GPS和IMU

Robot->children下添GPS"gps"和InertialUnit"inertial unit"

至此，小车的模型我们就建立完成，点击窗口上的运行按钮，便可以看到小车由于自身的重力属性落在了地板上。

最后建议在环境中加入一些障碍物，例如墙体、箱体之类，便于后面仿真，对于此便不再操作和赘述。
此时，我们将世界保存为webots_map.wbt

3. 创建控制器

前面两小节我们已经完成世界模型和小车模型的搭建，本节在此基础上需要创建一个控制器，通过ROS来实现对小车的控制。在实现控制前，我们需要一些前置的工作。

（1）配置功能包

• 命令行创建功能包：

cd ~/.catkin_ws/src
catkin_create_pkg webots_demo std_msgs rospy roscpp sensor_msgs

• 编译：

cd ~/.catkin_ws
catkin_make

• 把/usr/local/webots/projects/default/controllers/ros/include/下的srv和msg文件夹复制到catkin_ws/src/webots_demo下；
• 进入webots_demo的CMakeList.txt文件进行配置，将附录中[1]段代码粘贴进文件并保存；
• 进入同目录下webots_demo的package.xml文件，加入两行：

<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

• 同目录下建立world、launch和controller文件夹，controller下建立my_controller文件夹；
• 将上一小节建立的世界文件webots_map.wbt放入world中；
• 在launch文件夹中创建webots.launch，代码如下：

<?xml version="1.0"?>
<launch>
  <!-- 启动webots -->
  <arg name="no-gui" default="false," doc="Start Webots with minimal GUI"/>
  <include file="$(find webots_ros)/launch/webots.launch">
    <arg name="mode" value="realtime"/>
    <arg name="no-gui" value="$(arg no-gui)"/>
    <arg name="world" value="$(find webots_demo)/worlds/webots_map.wbt"/>
  </include>
</launch>

其实也可以手动连接ROS master，开启终端输入如下指令后，用Webots打开webots_demo下的webots_map.wbt世界文件即可

roscore

• 用Webots打开世界文件，展开Robot节点，双击controller，将控制器从void改为ros，controllerArgs改为--name=robot；
  

（2）编写控制器（键盘控制小车）

• cpp文件代码见附录中[2]；
• 将该文件命名为my_controller.cpp，分别复制到catkin_ws/src/webots_demo/controller/my_controller和catkin_ws/src/webots_demo/src下
• 在catkin_ws/src/webots_demo/CMakeList.txt最后补充：

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

这三行使得我们编译功能包后会生成对应的可执行文件。

三、ROS联合Webots进行仿真

在前面两章的基础之上，我们已经完成了仿真环境的搭建，本章节将动手实操，用我们写好的键盘控制器来控制仿真环境中的机器人，并对其中的一些数据进行分析。

1. 运行仿真环境

• 编译功能包webots_demo：

cd ~/.catkin_ws
catkin_make

• 运行webots.launch文件，启动世界：

roslaunch webots_demo webots.launch

这里如果出现一些问题，例如笔者出现了启动launch文件后Webots闪退，可以打开launch文件检查一下做出相应的修改，也可以用第二章第3小节里提到的手动连接ROS master的方法。此处旨在让仿真环境连接上ROS，才能进行后续操作。

• 世界文件运行起来后，运行控制器，启动键盘控制节点：

rosrun webots_demo my_controller

这个时候鼠标点击仿真环境地图内，按下键盘上的方向键即可操控小车。

• 查看rosservice：

rosservice list

可以看到如下服务：

• 开启雷达服务：

rosservice call /robot/Sick_LMS_291/enable "value: 1"

终端显示：

success: True

• 查看rostopic：

rostopic list

这里可以看到：键盘节点、雷达均有话题发布
键盘话题：/robot/keyboard/key
雷达发布的话题：/robot/Sick_LMS_291/laser_scan/layer0、
/robot/Sick_LMS_291/range_image

• 为了可视化的看到雷达的数据，我们可以打开rviz：

rviz

将左手Displays栏里的Fixed Frame改为robot/Sick_LMS_291
点击Add->By topic订阅话题/robot->/Sick_LMS_291->/laser_scan->/layer0->LaserScan和/robot->/Sick_LMS_291->/range_image->Image
可以看到雷达扫描的数据被可视化：

在Webots中用鼠标点击一下background，然后用键盘上的方向键来控制小车运动，可以看到rviz中LaserScan图像在实时变化：

2. 导航前置

在前文的基础之上，我们可以像以往在Gazebo里仿真一样，实现移动机器人的自主导航。前文加入了三个传感器：

GPS：用于Webots中移动小车的定位；
IMU：利用获取的惯性信息来补偿小车位姿；
Sick_LMS_291：识别障碍信息，用于地图构建和避障。

具体实现小车导航的仿真，可转到Reference[1]中的https://blog.csdn.net/xiaokai1999/article/details/112596613

这里说明一下，笔者也是一步一步跟着这位博主学习操作Webots的，本文中有许多的步骤，都是与这位博主的相同，但是笔者都有自己去动手操作，毕竟这东西光看是学不会的，更别提看着教程操作有的地方会出现不同的状况了。所以，我尽量中和了一些关键的博客，部分文字是自己总结出来的，方便读者理解和上手。

在跟随这篇博文操作后，可以查看到tf_tree：

其中：

• odom：odometry，里程计节点，其基本内容包含了位姿（x、y、theta）和x、y方向的角速度和线速度（有注意到在第二章节建模的时候，机器人运动平面是xoz并非xoy！y轴是垂直地面的）；
• base_link：机器人基坐标系，已经刚性地固连到移动机器人的底座（车体），在本案例中，base_link就是黄色车体的坐标系；
• robot/Sick_LMS_291：激光雷达坐标系。

2021.10.17更新

---

不足与展望

• 后续希望自己搭建出一个地图，用gmapping或者cartographer进行建图；
• 后续可实现Lidar Slam的仿真；
• 后续可实现VSlam的仿真；
• 后续希望搭建机械臂相关仿真环境；
• 整个项目希望利用git进行管理。

---

Reference

[1] CSDN.锡城筱凯.ROS联合webots实战案例目录
[2] CSDN.JameScottX.Webots 舵轮底盘小教程

---

附录

catkin_ws/src/webots_demo/CMakeList.txt

cmake_minimum_required(VERSION 2.8.3)
project(webots_demo)

find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs sensor_msgs message_generation)

#######################################
## Declare ROS messages and services ##
#######################################

## Generate messages in the 'msg' folder
  add_message_files(
    FILES
    BoolStamped.msg
    Float64Stamped.msg
    Int32Stamped.msg
    Int8Stamped.msg
    RadarTarget.msg
    RecognitionObject.msg
    StringStamped.msg
  )

## Generate services in the 'srv' folder
  add_service_files(
    FILES
    camera_get_focus_info.srv
    camera_get_info.srv
    camera_get_zoom_info.srv
    display_draw_line.srv
    display_draw_oval.srv
    display_draw_pixel.srv
    display_draw_polygon.srv
    display_draw_rectangle.srv
    display_draw_text.srv
    display_get_info.srv
    display_image_copy.srv
    display_image_delete.srv
    display_image_load.srv
    display_image_new.srv
    display_image_paste.srv
    display_image_save.srv
    display_set_font.srv
    field_get_bool.srv
    field_get_color.srv
    field_get_count.srv
    field_get_float.srv
    field_get_int32.srv
    field_get_node.srv
    field_get_rotation.srv
    field_get_string.srv
    field_get_type.srv
    field_get_type_name.srv
    field_get_vec2f.srv
    field_get_vec3f.srv
    field_import_node.srv
    field_import_node_from_string.srv
    field_remove_node.srv
    field_remove.srv
    field_set_bool.srv
    field_set_color.srv
    field_set_float.srv
    field_set_int32.srv
    field_set_rotation.srv
    field_set_string.srv
    field_set_vec2f.srv
    field_set_vec3f.srv
    get_bool.srv
    get_float_array.srv
    get_float.srv
    get_int.srv
    get_string.srv
    get_uint64.srv
    get_urdf.srv
    gps_decimal_degrees_to_degrees_minutes_seconds.srv
    lidar_get_frequency_info.srv
    lidar_get_info.srv
    lidar_get_layer_point_cloud.srv
    lidar_get_layer_range_image.srv
    motor_set_control_pid.srv
    mouse_get_state.srv
    node_add_force_or_torque.srv
    node_add_force_with_offset.srv
    node_get_center_of_mass.srv
    node_get_contact_point.srv
    node_get_field.srv
    node_get_id.srv
    node_get_number_of_contact_points.srv
    node_get_name.srv
    node_get_orientation.srv
    node_get_parent_node.srv
    node_get_position.srv
    node_get_static_balance.srv
    node_get_status.srv
    node_get_type.srv
    node_get_velocity.srv
    node_remove.srv
    node_reset_functions.srv
    node_move_viewpoint.srv
    node_set_visibility.srv
    node_set_velocity.srv
    pen_set_ink_color.srv
    range_finder_get_info.srv
    receiver_get_emitter_direction.srv
    robot_get_device_list.srv
    robot_set_mode.srv
    robot_wait_for_user_input_event.srv
    save_image.srv
    set_bool.srv
    set_float.srv
    set_float_array.srv
    set_int.srv
    set_string.srv
    skin_get_bone_name.srv
    skin_get_bone_orientation.srv
    skin_get_bone_position.srv
    skin_set_bone_orientation.srv
    skin_set_bone_position.srv
    speaker_is_sound_playing.srv
    speaker_speak.srv
    speaker_play_sound.srv
    supervisor_get_from_def.srv
    supervisor_get_from_id.srv
    supervisor_movie_start_recording.srv
    supervisor_set_label.srv
    supervisor_virtual_reality_headset_get_orientation.srv
    supervisor_virtual_reality_headset_get_position.srv
  )

## Generate added messages and services with any dependencies listed here
  generate_messages(
    DEPENDENCIES
    std_msgs
    sensor_msgs
  )

###################################
## catkin specific configuration ##
###################################

catkin_package(
   CATKIN_DEPENDS roscpp rospy std_msgs sensor_msgs message_runtime
)

###########
## Build ##
###########

include_directories(
  ${catkin_INCLUDE_DIRS}
)

## Instructions for keyboard_teleop node

#############
## Install ##
#############

catkin_ws/src/webots_demo/controller/my_controller/my_controller.cpp

#include <signal.h>
#include <std_msgs/String.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::ServiceClient set_velocity_client;     //速度设置客户端
webots_ros::set_float set_velocity_srv;     //速度设置数据

ros::ServiceClient set_position_client;     //位置设置客户端
webots_ros::set_float set_position_srv;     //位置设置数据


double speeds[NMOTORS]={0.0,0.0};           //电机速度值 0～100

// 匹配电机名
static const char *motorNames[NMOTORS] ={"left_motor", "right_motor"};
/*******************************************************
* Function name ：updateSpeed
* Description   ：将速度请求以set_float的形式发送给set_velocity_srv
* Parameter     ：无
* Return        ：无
**********************************************************/
void updateSpeed() {   
    for (int i = 0; i < NMOTORS; ++i) {
        // 更新速度
        set_velocity_client = n->serviceClient<webots_ros::set_float>(string("/robot/") + string(motorNames[i]) + string("/set_velocity"));   
        set_velocity_srv.request.value = -speeds[i];
        set_velocity_client.call(set_velocity_srv);
    }
}

/*******************************************************
* 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);
}
/*******************************************************
* Function name ：键盘返回函数
* Description   ：当键盘动作，就会进入此函数内
* Parameter     ：
        @value   返回的值
* Return        ：无
**********************************************************/
void keyboardDataCallback(const webots_ros::Int32Stamped::ConstPtr &value)
{
    // 发送控制变量
    int send =0;
    //ROS_INFO("sub keyboard value = %d",value->data);
    switch (value->data)
    {
        // 左转
        case 314:
            speeds[0] = 5.0;
            speeds[1] = -5.0;
            send=1;
            break;
        // 前进
        case 315:
            speeds[0] = 5.0;
            speeds[1] = 5.0;
            send=1;
            break;
        // 右转
        case 316:
            speeds[0] = -5.0;
            speeds[1] = 5.0;
            send=1;
            break;
        // 后退
        case 317:
            speeds[0] = -5.0;
            speeds[1] = -5.0;
            send=1;
            break;
        // 停止
        case 32:
            speeds[0] = 0;
            speeds[1] = 0;
            send=1;
            break;
        default:
            send=0; 
            break;
    }
    //当接收到信息时才会更新速度值
    if (send)
    {
        updateSpeed();
        send=0;
    } 
}

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();
        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();

    //初始化电机
    for (int i = 0; i < NMOTORS; ++i) {
        // position速度控制时设置为缺省值INFINITY   
        set_position_client = n->serviceClient<webots_ros::set_float>(string("/robot/") + 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("/robot/") + 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]);
    }   


    // 服务订阅键盘
    ros::ServiceClient keyboardEnableClient;
    webots_ros::set_int keyboardEnablesrv;
   
    keyboardEnableClient = n->serviceClient<webots_ros::set_int>("/robot/keyboard/enable");
    keyboardEnablesrv.request.value = TIME_STEP;
    if (keyboardEnableClient.call(keyboardEnablesrv) && keyboardEnablesrv.response.success)
    {
        ros::Subscriber keyboardSub;
        keyboardSub = n->subscribe("/robot/keyboard/key",1,keyboardDataCallback);
        while (keyboardSub.getNumPublishers() == 0) {}
        ROS_INFO("Keyboard enabled.");
        ROS_INFO("control directions：");
        ROS_INFO("  ↑  ");
        ROS_INFO("← ↓ →");
        ROS_INFO("stop：space");
        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();
            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;
}