嗨，又和大家见面了！上次我们在《Webots建模指南3 -ROS控制篇（上）》这篇文章中梳理了Webots与ROS的通信过程，今天我就来带大家一起实操一下，看看Webots怎样实现ROS联合仿真，话不多说，下面进入正题~
1.准备工作
环境说明：
- Ubuntu 18.04 LTS
- Webots R2020a-rev1
- ROS Melodic
相信大家是老ROS了，新建工作空间这种事儿当然不能再一遍又一遍的重复。
那我们要做的是什么呢？既然要跟ROS联合仿真，当然要把官方提供的webots_ros功能包给clone到我们的工作空间啦！ Clone完成以后，我们先来编译一下工作空间，一个原因是我们可以先来看看这些demo是如何运行的，另一个原因就是待会儿我们要新建一个功能包，这个功能包我们需要依赖webots_ros功能包，这也是为了防止编译过程中出现一些由于编译顺序导致的编译不通过的问题。 功能包的结构，在上一篇文章《Webots建模指南3 – webots-ROS联合仿真（上）》中我们已经详细说明了，这里就不再赘述！ 接下来我们新建一个功能包webots_controller，并且依赖于message_generation、roscpp、rospy、sensor_msgs、std_msgs、tf、webots_ros这几个功能包。 完成以后，再新建一个launch文件夹和worlds文件夹，前者大家再熟悉不过，后者我们用来存放webots的仿真环境文件，然后将我们之前建立的new_world_2.wbt文件copy到webots_controller/worlds下。
我们先来看看已经整理好的功能包，它的文件层级如下：
. ├── CMakeLists.txt ├── include ├── launch │ └── webots_ros_simulation.launch ├── package.xml ├── src │ └── first_webots_ros_simulation.cpp └── worlds └── new_world_2.wbt
4 directories, 5 files 对于launch文件及cpp文件的编写，就是我们下一步要进行的工作。
2.联合编程说明
2.1 环境配置
我们先来配置编译环境，对于CMakeList.txt文件，它的内容如下：
cmake_minimum_required(VERSION 3.0.2)
project(webots_controller)
find_package(catkin REQUIRED COMPONENTS
message_generation
roscpp
rospy
sensor_msgs
std_msgs
tf
webots_ros
)
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES webots_controller
CATKIN_DEPENDS message_runtime message_generation roscpp rospy sensor_msgs std_msgs tf webots_ros
# DEPENDS system_lib
)
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
add_executable(first_webots_ros_simulation src/first_webots_ros_simulation.cpp)
target_link_libraries(first_webots_ros_simulation
${catkin_LIBRARIES}
)
对于package.xml的内容，配置如下：
<?xml version="1.0"?>
<package>
<name>webots_controller</name>
<version>0.0.0</version>
<description>The webots_controller package</description>
<maintainer email="robert.h.x.s@foxmail.com">robert</maintainer>
<license>TODO</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>rospy</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>tf</build_depend>
<build_depend>webots_ros</build_depend>
<run_depend>webots_ros</run_depend>
<run_depend>rospy</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>message_runtime</run_depend>
<run_depend>tf</run_depend>
<run_depend>message_generation</run_depend>
</package>
我们再来看下launch文件的内容：
<?xml version="1.0"?>
<launch>
<!-- start 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_controller)/worlds/new_world_2.wbt"/>
</include>
<arg name="auto-close" default="false" doc="Startup mode"/>
<node name="first_webots_ros_simulation" pkg="webots_controller" type="first_webots_ros_simulation" required="$(arg auto-close)"/>
</launch>
对于launch文件，我们仿照着/webots_ros/launch中的文件写即可，文件的第一部分首先要启动webots软件，这是通过/webots_ros/launch/webots.launch来启动的，而webots.launch启动的则是/webots_ros/src/webots_launcher.py脚本，它的启动参数定义了软件的启动模式，这里我们默认就可以了，然后启动的world文件，我们要修改成new_world_2.wbt存放的路径。 这里需要再强调一下，重要的事情说三遍： webots_launcher.py脚本要将权限设置为允许作为程序执行文件！ webots_launcher.py脚本要将权限设置为允许作为程序执行文件！ webots_launcher.py脚本要将权限设置为允许作为程序执行文件！
2.2 具体实现
万事俱备，只欠东风！接下来我们就要在新建的first_webots_ros_simulation.cpp下对机器人的控制器进行编码了！ 在开始前，我们再来回顾一下上次讲过的标准ROS控制器在ROS网络上的服务或者话题的命名格式：
robot_unique_name：是为了避免同一机器人在不同实例之间产生冲突，同一种机器人的多机协作时显得至关重要！device_name：对应设备的名称；service/topic_name：对应已经定义好的service/topic名称，即各种操作所对应的名称； 这里先了解一下即可，因为我们可以直接list出来，所以不需要记~
那么接下来就要搞事情了！！！ 既然我们用的是标准ROS控制器，也就是说，webots端的控制器程序已经写好，那么我们就不用担心设备绑定的事情了，因为我们只需要往对应的话题/服务上发数据就可以了。 首先打开webots，设置一下控制器参数。
选择控制器为ros，点击运行，发现终端显示： Robot’s unique name is robot_128770_robert_vm 其中robot_128770_robert_vm开头的robot是我们机器人模型树中name字段的内容，中间的数字是进程的ID号，最后的robert_vm是我的计算机名称。
我们在终端中查看此时标准ros控制器发布的服务如下：
由此可见，Robot’s unique name定义了服务的名称空间，这会直接影响我们的程序编写，而进程ID每次启动都是随机的，所以我们应该修改这个名称空间。 返回webots，将controllerArgs字段的值编辑为--name=my_robot
保存并重启，再次查看发布到ros网络的服务名称：
这时发现名称空间已经修改成了我们自定义的my_robot 程序放在文末，此处我们假设已经编译完成！接下来启动仿真：
roslaunch webots_controller webots_ros_simulation.launch
打开rqt，将图像显示插件加载进来，选择webots发布的图像话题，最终仿真结果如下：
至此，我们完成了小车的移动和相机数据采集功能，但仅仅是简单的直线移动，另外小车上还有一个激光雷达我们没有启动，这个就留给大家当做小作业来折腾了，祝好运~
3.完整例程
#include <signal.h>
#include <locale.h>
#include "ros/ros.h"
#include <std_msgs/String.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/NavSatFix.h>
#include <tf/transform_broadcaster.h>
#include <webots_ros/set_float.h>
#include <webots_ros/set_int.h>
#define TIME_STEP 32
#define NMOTORS 4
#define MAX_SPEED 4
ros::NodeHandle *n;
static int controllerCount;
static std::vector<std::string> controllerList;
ros::ServiceClient timeStepClient;
webots_ros::set_int timeStepSrv;
static const char *motorNames[NMOTORS] = {"wheel1", "wheel2", "wheel3", "wheel4"};
// 更新速度
void updateSpeed() {
double speeds[NMOTORS];
speeds[0] = MAX_SPEED;
speeds[1] = MAX_SPEED;
speeds[2] = MAX_SPEED;
speeds[3] = MAX_SPEED;
for (int i = 0; i < NMOTORS; ++i) {
ros::ServiceClient set_velocity_client;
webots_ros::set_float set_velocity_srv;
set_velocity_client = n->serviceClient<webots_ros::set_float>(std::string("my_robot/") + std::string(motorNames[i]) +
std::string("/set_velocity"));
set_velocity_srv.request.value = speeds[i];
set_velocity_client.call(set_velocity_srv);
}
}
// 获取可用控制器的名称
void controllerNameCallback(const std_msgs::String::ConstPtr &name) {
controllerCount++;
controllerList.push_back(name->data);
ROS_INFO("Controller #%d: %s.", controllerCount, controllerList.back().c_str());
}
//退出函数
void quit(int sig) {
ROS_INFO("终止节点运行.");
timeStepSrv.request.value = 0;
timeStepClient.call(timeStepSrv);
ros::shutdown();
exit(0);
}
int main(int argc, char **argv) {
setlocale(LC_CTYPE,"zh_CN.utf8");
ros::init(argc, argv, "my_robot", ros::init_options::AnonymousName);
n = new ros::NodeHandle;
signal(SIGINT, quit);
std::string controllerName;
/********************************************** 当作模板即可 ****************************************************/
// 订阅主题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();
// 设置基本仿真步长
timeStepClient = n->serviceClient<webots_ros::set_int>("my_robot/robot/time_step");
timeStepSrv.request.value = TIME_STEP;
// 多控制器时可选
if (controllerCount == 1){
controllerName = controllerList[0];
}
else {
int wantedController = 0;
std::cout << "选择要使用的控制器的编号 :\n";
std::cin >> wantedController;
if (1 <= wantedController && wantedController <= controllerCount)
controllerName = controllerList[wantedController - 1];
else {
ROS_ERROR("无效的控制器编号.");
return 1;
}
}
ROS_INFO("Using controller: '%s'", controllerName.c_str());
nameSub.shutdown();
/********************************************** 电机模式设置 ****************************************************/
for (int i = 0; i < NMOTORS; ++i) {
ros::ServiceClient set_position_client;
webots_ros::set_float set_position_srv;
set_position_client = n->serviceClient<webots_ros::set_float>(std::string("my_robot/") + std::string(motorNames[i]) +
std::string("/set_position"));
//设置速度模式
set_position_srv.request.value = INFINITY;
if (set_position_client.call(set_position_srv) && set_position_srv.response.success)
ROS_INFO("motor %s 工作模式为速度模式.", motorNames[i]);
else
ROS_ERROR("无法调用motor %s的set_position服务 .", motorNames[i]);
// 创建速度client
ros::ServiceClient set_velocity_client;
set_velocity_client = n->serviceClient<webots_ros::set_float>(std::string("my_robot/") + std::string(motorNames[i]) +
std::string("/set_velocity"));
// 创建服务
webots_ros::set_float set_velocity_srv;
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]);
}
/********************************************** 传感器使能 ****************************************************/
// 使能 camera
ros::ServiceClient set_camera_client;
webots_ros::set_int camera_srv;
ros::Subscriber sub_camera;
set_camera_client = n->serviceClient<webots_ros::set_int>("my_robot/kinect_color/enable");
camera_srv.request.value = 64;
set_camera_client.call(camera_srv);
/******************************************** 主循环 ****************************************************** */
while (ros::ok()) {
updateSpeed(); // 更新小车速度
if (!timeStepClient.call(timeStepSrv) || !timeStepSrv.response.success) {
ROS_ERROR("Failed to call service time_step for next step.");
break;
}
ros::spinOnce();
}
timeStepSrv.request.value = 0;
timeStepClient.call(timeStepSrv);
ros::shutdown();
return 0;
}
官方版本更新还是很快的，截止投稿时间（2020-08-08），Webots 发行版本已经更至webots-2021a版本，本文仿真环境基于Webots R2020a-rev1，后续我将更至Webots R2020b，R2020b支持的3D模型格式更丰富。 读万卷书，也要行万里路，我是罗伯特祥，下次见！
参考文献：
