Webots与ROS的联合仿真使用说明

    几年前第一次接触webots这款仿真软件，这是一款具有强大物理引擎的优秀机器人模拟仿真器，在移动机器人方面可以毫不夸张的说是世界第一。这款软件支持C/C++、python、java、matlab等一系列语言，唯一遗憾的是对ros的支持力度远没有想象中的那么友好。

    近来发现，这么优秀的软件全网竟然没有关于webots与ros联用的教程，包括官网也说的模糊不清，张瑞雷博士在其博客对webots的使用只进行了简单翻译，可以阅读瑞雷博士的博文先了解一番。

    为了填补全网的空白，博主决定装个X，写下此文，也算是为webots在中国的开源做贡献了~

注：

目前官方已经推出ROS及ROS2功能包，参见GitHub，https://github.com/cyberbotics/webots_ros
https://github.com/cyberbotics/webots_ros2
以下教程仍适用，不过过于繁琐，原理相同

参考手册

---

    任何一款软件的使用都离不开其官方提供的参考手册和用户指南，

• webots官方用户指南：
  https://www.cyberbotics.com/doc/guide/index

• webots官方参考手册：
  https://www.cyberbotics.com/doc/reference/index

1. 系统环境说明

博主系统环境：Ubuntu 18.04.2 LTS 64bit
ROS版本：ROS Melodic 1.14.3
webots版本：R2019b

注意：
    本文对于ubuntu 16下的ros kinetic版本同样适用，但webots R2019a及之前版本并未进行测试。

2. 创建仿真环境

---

本文以kuka的youbot移动机械臂进行讲解。

step1：打开webots，点击帮助(H)->Webots引导之旅(G)，找到Webots Guided Tour选项下的youbot.wbt

模型加载完后，界面如下：

step2：删除多余的模型(非必需)
在世界或模型树中选中要删除的模型，按Del删除

step3：新建控制器，单击webots软件工具栏的向导(W)，选择新机器人控制器(C)

点击OK按钮

选择项目目录（默认设置即可），单击复制按钮。

成功后，出现如下提示。

单击下一步，并选择Python（注意，此处应根据实际使用语言来进行选择，自定义的ROS控制器需要使用Python），单击下一步。

填写控制器名称，单击下一步。

单击完成，出现如下界面。

3. 编写ROS控制器

---

    webots对ROS的支持是通过webots官方自行编写ros功能包，通过网络通信向ROS Master进行节点注册的，webots官方集成的ros功能包与ROS官方的写法略有不同。

    roscpp是通过webots_ros的功能包实现的，只能实现标准控制器，官方不允许用户在此基础上进行修改，因此我们通过Python实现rospy控制器的编写。

3.1 rospy控制器

在webots用户界面的右侧程序编写栏中（如下图所示），写入控制器代码。

控制器代码结构，由软件自动生成，在此基础上，我们来编写自己的控制器程序，程序编写完成后注意保存（如下图所示）。

控制器程序代码：

"""my_ros_control controller."""

#!/usr/bin/env python
# -*- coding: UTF-8 -*-

import rospy
import os
import math
from controller import Robot
from geometry_msgs.msg import Twist
import roslib
from std_msgs.msg import Float64

# 定义速度数据变量
vx=0
vy=0
wz=0

# 回调函数，用于接受ROS节点发布的速度数据
def velCallback(data):
    global vx
    global vy
    global wz
    vx = data.linear.x
    vy = data.linear.y
    wz = data.angular.z
    message = 'velocity value:\n vx=' + str(vx)+'\nvy=' + str(vy) + '\nwz=' + str(wz)



# 实例化Robot类，由webots决定
robot = Robot()
# 获取当前世界的时间步长
timestep = int(robot.getBasicTimeStep())
# 获取模型中robot的名称，以便在多机器人仿真时区分机器人
robot_name = robot.getControllerArguments();

# 将电机变量与机器人模型中的电机相关联
motor1 = robot.getMotor('wheel2')
motor2 = robot.getMotor('wheel1')
motor3 = robot.getMotor('wheel4')
motor4 = robot.getMotor('wheel3')

# 基于速度控制，因此要把电机行程设置成无限远
motor1.setPosition(float('inf'))
motor2.setPosition(float('inf'))
motor3.setPosition(float('inf'))
motor4.setPosition(float('inf'))

# 设置初始速度为0
motor1.setVelocity(0)
motor2.setVelocity(0)
motor3.setVelocity(0)
motor4.setVelocity(0)

# 打印ROS Master的URI
message = 'Initializing ROS:connecting to' + os.environ['ROS_MASTER_URI']
print(message)

robot.step(timestep)

# 初始化rospy节点
rospy.init_node( robot_name+'_controller_node', anonymous=True)
print('Subscribing to "keyboard_cmd" topic')
robot.step(timestep)

# 订阅key_cmd节点
rospy.Subscriber('key_cmd',Twist,velCallback)
print('*******test******')


# 控制循环
while robot.step(timestep) != -1 and not rospy.is_shutdown():
    print('----------test---------')
    # youbot的运动学解算方程
    w1 = (-vy + vx - 0.3855 * wz) / 0.05
    w2 = (vy + vx + 0.3855 * wz) / 0.05
    w3 = (vy + vx - 0.3855 * wz) / 0.05
    w4 = (-vy + vx + 0.3855 * wz) / 0.05
    print(str(w1)+'  '+str(w2)+'  '+str(w3)+'  '+str(w4) +'\n')
    
    # 把解算的数值发送给模型电机
    motor1.setVelocity(w1)
    motor2.setVelocity(w2)
    motor3.setVelocity(w3)
    motor4.setVelocity(w4)

注意：
    1. webots提供的rospy包自动集成了回调机制，不需要写rospy.spin()
    2. 上述代码在复制时，需要将中文注释删掉，否则会报错

3.1.1 程序解释

关于模型中电机与车轮的关联，可以先找到webots提供的youbot移动机械臂模型，根据其提供的名称来进行电机与车轮的关联。（https://www.cyberbotics.com/doc/guide/youbot）

具体细节不再叙述，请在以上程序基础上查看API文档，如下图所示，在网页可ctrl + F进行函数或节点的查找，如下图所示：

3.1.2 webots提供的rospy包的使用

step1：获取webots提供的rospy包
该包位于webots安装目录下的projects/languages/ros/controllers/ros_python文件夹下，如下图所示。

注意：直接从官方网站下载的webots，安装目录下可能没有ros_python文件夹，此时可以在github上下载，或者直接安装github上提供的webots R2019b安装包（https://github.com/omichel/webots）

step2：将webots提供rospy包中的kinetic、python、Makefile、runtime.ini文件复制到新建的控制器目录（文档->youbot->controllers->my_ros_control）下，如下图所示。

3.2 roscpp控制器

请参考瑞雷博士的博文:
https://blog.csdn.net/ZhangRelay/article/details/85247284

4. 创建ROS功能包

---

step1：创建ROS工作空间catkin_webots_ws，并初始化

step2：将用于键盘控制的teleop_twist_keyboard包下载到工作空间src目录下。（功能包下载：https://github.com/ros-teleop/teleop_twist_keyboard）

注意要给teleop_twist_keyboard_descartes.py文件添加可执行权限

step3：按如下代码修改teleop_twist_keyboard_descartes.py程序：

#!/usr/bin/env python
# -*- coding: utf-8 -*

from __future__ import print_function
import roslib; roslib.load_manifest('teleop_twist_keyboard')
import rospy
from geometry_msgs.msg import Twist
import sys, select, termios, tty

msg = """
---------------------------
   q    w    e
   a         d
        s     

w : +x		a : +y		q : +w
s : -x		d : -y		e : -w

其他按键 : stop

u/i : 最大速度增加/减少10%
j/k : 仅线性速度增加/减少10%
m/, : 只增加/减少角速度10%

CTRL-C 退出
---------------------------
"""

moveBindings = {
		'w':(1,0,0,0),
		'a':(0,1,0,0),
		'd':(0,-1,0,0),
		's':(-1,0,0,0),

		'W':(1,0,0,0),
		'A':(0,1,0,0),
		'D':(0,-1,0,0),
		'S':(-1,0,0,0),


		'Q':(0,0,0,1),
		'q':(0,0,0,1),
		'E':(0,0,0,-1),
		'e':(0,0,0,-1),
	       }

speedBindings={
		'u':(1.1,1.1),
		'i':(.9,.9),
		'j':(1.1,1),
		'k':(.9,1),
		'm':(1,1.1),
		',':(1,.9),
		'U':(1.1,1.1),
		'I':(.9,.9),
		'J':(1.1,1),
		'K':(.9,1),
		'M':(1,1.1),
		'<':(1,.9),
	      }

def getKey():
	tty.setraw(sys.stdin.fileno())
	select.select([sys.stdin], [], [], 0)
	key = sys.stdin.read(1)
	termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
	return key


def vels(speed,turn):
	return "currently:\tspeed %s\tturn %s " % (speed,turn)

if __name__=="__main__":
    	settings = termios.tcgetattr(sys.stdin)
	
	pub = rospy.Publisher('key_cmd', Twist, queue_size = 1)
	rospy.init_node('teleop_twist_keyboard')

	speed = rospy.get_param("~speed", 0.5)
	turn = rospy.get_param("~turn", 0.5)
	x = 0
	y = 0
	z = 0
	th = 0
	status = 0

	try:
		print(msg)
		print(vels(speed,turn))
		while(1):
			key = getKey()
			if key in moveBindings.keys():
				x = moveBindings[key][0]
				y = moveBindings[key][1]
				z = moveBindings[key][2]
				th = moveBindings[key][3]
			elif key in speedBindings.keys():
				speed = speed * speedBindings[key][0]
				turn = turn * speedBindings[key][1]

				print(vels(speed,turn))
				if (status == 14):
					print(msg)
				status = (status + 1) % 15
			else:
				x = 0
				y = 0
				z = 0
				th = 0
				if (key == '\x03'):
					break

			twist = Twist()
			twist.linear.x = x*speed; twist.linear.y = y*speed; twist.linear.z = z*speed
			twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = th*turn
			pub.publish(twist)


	except Exception as e:
		print(e)

	finally:
		twist = Twist()
		twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
		twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
		pub.publish(twist)


    		termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

step4：编译工作空间

step5：编译工作空间并启动roscore

step6：source工作空间

source devel/setup.bash 

step7：启动刚刚编写的键盘控制节点

rosrun teleop_twist_keyboard teleop_twist_keyboard_descartes.py

5. 开始仿真

---

step1：初始化环境，在用户界面中单击按钮，使世界模型以及时钟复位。

step2：在模型树中选中youbot下的controller，点击下方的选择按钮，选择我们新建的控制器my_ros_control，单击OK

仿真如下：

说明

---

    虽然软件已经开源，但其技术支持服务还属于收费业务。尽管软件的仿真性能非常优秀，但是其建模非常复杂，因此博主个人认为这款软件适合基于官方给出的机器人模型做机器人算法的仿真实验。

    对于建模，官方在github上给出了一个Python工具，能够将urdf文件转成proto，通过这种方式可实现将自定义的机器人模型加载到webots世界当中。

---

可以从以下途径熟悉或学习这款软件

注意注意！！！

不要陷入软件的使用，这只是一款仿真你的算法的软件，是一种工具而已，包括ros也是，我们的目的是使用这些工具达到自己的目的，而不是陷在这些工具里出不来！！！

张瑞雷博士的博文：
https://blog.csdn.net/ZhangRelay/article/details/85247284

webots官方使用说明：
https://cyberbotics.com/doc/guide/using-ros

webots官方用户指南：
https://www.cyberbotics.com/doc/guide/index

webots官方参考手册：
https://www.cyberbotics.com/doc/reference/index

_阿龙clliu的博客
https://www.cnblogs.com/clliu/

webots提供的机器人模型

https://www.cyberbotics.com/doc/guide/robots