webots和ros2笔记05-新建-CFANZ编程社区

如果将webots丰富的机器人库和ros2结合呢？？？

官网教程非常简洁：

https://github.com/cyberbotics/webots_ros2/wiki/Tutorial-Create-Webots-Robot
https://github.com/cyberbotics/webots_ros2/wiki/Tutorial-Write-ROS2-Driver

掌握如上两篇即可，都是去年的旧文档了。

下面简单说明一下：

创建Webots-ros2机器人

如果需要从零搭建机器人模型和ROS2驱动参考官网“教程6”，此处不涉及。

将机器人的controller字段值更改为<extern>。
保存此环境。
执行ros2 launch webots_ros2_core robot_launch.py world:=/path/to/your/world.wbt

这里以irobot为例：

webots和ros2笔记05-新建_驱动程序

选择Robot：

webots和ros2笔记05-新建_ros2_02

找一找controller：

webots和ros2笔记05-新建_python_03

然后保存环境：

webots和ros2笔记05-新建_官网_04

使用如下命令：

ros2 launch webots_ros2_core robot_launch.py world:=create_ros.wbt

webots和ros2笔记05-新建_python_05

注意有警告，稍后具体说明。

这就完成了一个机器人ros2配置，当然功能有各种bug。

需要参考第二个链接，有关如何改进ROS2接口的更多详细信息，“编写ROS2驱动程序”！

前情回顾：

简单解释一下警告！

[WARNING] [launch_ros.actions.node]: Parameter file path is not a file: nul

个人理解未必准确！

如上方式只是最简单的ros2接口，并非全功能的，即便是编写ros2驱动程序中教程介绍的，也只是基本功能。

此处以笔记04-epuck为例。

其ros2驱动如下：

"""ROS2 e-puck driver."""

from math import pi
import rclpy
from rclpy.time import Time
from tf2_ros import StaticTransformBroadcaster
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import TransformStamped
from webots_ros2_core.math.interpolation import interpolate_lookup_table
from webots_ros2_core.webots_differential_drive_node import WebotsDifferentialDriveNode


OUT_OF_RANGE = 0.0
INFRARED_MAX_RANGE = 0.04
INFRARED_MIN_RANGE = 0.009
TOF_MAX_RANGE = 1.0
DEFAULT_WHEEL_RADIUS = 0.02
DEFAULT_WHEEL_DISTANCE = 0.05685
NB_INFRARED_SENSORS = 8
SENSOR_DIST_FROM_CENTER = 0.035


DISTANCE_SENSOR_ANGLE = [
    -15 * pi / 180,   # ps0
    -45 * pi / 180,   # ps1
    -90 * pi / 180,   # ps2
    -150 * pi / 180,  # ps3
    150 * pi / 180,   # ps4
    90 * pi / 180,    # ps5
    45 * pi / 180,    # ps6
    15 * pi / 180,    # ps7
]


DEVICE_CONFIG = {
    'camera': {'topic_name': ''},
    'robot': {'publish_base_footprint': True},
    'ps0': {'always_publish': True},
    'ps1': {'always_publish': True},
    'ps2': {'always_publish': True},
    'ps3': {'always_publish': True},
    'ps4': {'always_publish': True},
    'ps5': {'always_publish': True},
    'ps6': {'always_publish': True},
    'ps7': {'always_publish': True},
    'tof': {'always_publish': True}
}


class EPuckDriver(WebotsDifferentialDriveNode):
    def __init__(self, args):
        super().__init__(
            'epuck_driver',
            args,
            wheel_distance=DEFAULT_WHEEL_DISTANCE,
            wheel_radius=DEFAULT_WHEEL_RADIUS
        )
        self.start_device_manager(DEVICE_CONFIG)

        # Intialize distance sensors for LaserScan topic
        self.distance_sensors = {}
        for i in range(NB_INFRARED_SENSORS):
            sensor = self.robot.getDistanceSensor('ps{}'.format(i))
            sensor.enable(self.timestep)
            self.distance_sensors['ps{}'.format(i)] = sensor

        self.laser_publisher = self.create_publisher(LaserScan, '/scan', 1)
        self.tof_sensor = self.robot.getDistanceSensor('tof')
        if self.tof_sensor:
            self.tof_sensor.enable(self.timestep)
        else:
            self.get_logger().info('ToF sensor is not present for this e-puck version')

        laser_transform = TransformStamped()
        laser_transform.header.stamp = Time(seconds=self.robot.getTime()).to_msg()
        laser_transform.header.frame_id = 'base_link'
        laser_transform.child_frame_id = 'laser_scanner'
        laser_transform.transform.rotation.x = 0.0
        laser_transform.transform.rotation.y = 0.0
        laser_transform.transform.rotation.z = 0.0
        laser_transform.transform.rotation.w = 1.0
        laser_transform.transform.translation.x = 0.0
        laser_transform.transform.translation.y = 0.0
        laser_transform.transform.translation.z = 0.033

        self.static_broadcaster = StaticTransformBroadcaster(self)
        self.static_broadcaster.sendTransform(laser_transform)

        # Main loop
        self.create_timer(self.timestep / 1000, self.__publish_laserscan_data)

    def __publish_laserscan_data(self):
        stamp = Time(seconds=self.robot.getTime()).to_msg()
        dists = [OUT_OF_RANGE] * NB_INFRARED_SENSORS
        dist_tof = OUT_OF_RANGE

        # Calculate distances
        for i, key in enumerate(self.distance_sensors):
            dists[i] = interpolate_lookup_table(
                self.distance_sensors[key].getValue(), self.distance_sensors[key].getLookupTable()
            )

        # Publish range: ToF
        if self.tof_sensor:
            dist_tof = interpolate_lookup_table(self.tof_sensor.getValue(), self.tof_sensor.getLookupTable())

        # Max range of ToF sensor is 2m so we put it as maximum laser range.
        # Therefore, for all invalid ranges we put 0 so it get deleted by rviz
        laser_dists = [OUT_OF_RANGE if dist > INFRARED_MAX_RANGE else dist for dist in dists]
        msg = LaserScan()
        msg.header.frame_id = 'laser_scanner'
        msg.header.stamp = stamp
        msg.angle_min = - 150 * pi / 180
        msg.angle_max = 150 * pi / 180
        msg.angle_increment = 15 * pi / 180
        msg.range_min = SENSOR_DIST_FROM_CENTER + INFRARED_MIN_RANGE
        msg.range_max = SENSOR_DIST_FROM_CENTER + TOF_MAX_RANGE
        msg.ranges = [
            laser_dists[3] + SENSOR_DIST_FROM_CENTER,   # -150
            OUT_OF_RANGE,                               # -135
            OUT_OF_RANGE,                               # -120
            OUT_OF_RANGE,                               # -105
            laser_dists[2] + SENSOR_DIST_FROM_CENTER,   # -90
            OUT_OF_RANGE,                               # -75
            OUT_OF_RANGE,                               # -60
            laser_dists[1] + SENSOR_DIST_FROM_CENTER,   # -45
            OUT_OF_RANGE,                               # -30
            laser_dists[0] + SENSOR_DIST_FROM_CENTER,   # -15
            dist_tof + SENSOR_DIST_FROM_CENTER,         # 0
            laser_dists[7] + SENSOR_DIST_FROM_CENTER,   # 15
            OUT_OF_RANGE,                               # 30
            laser_dists[6] + SENSOR_DIST_FROM_CENTER,   # 45
            OUT_OF_RANGE,                               # 60
            OUT_OF_RANGE,                               # 75
            laser_dists[5] + SENSOR_DIST_FROM_CENTER,   # 90
            OUT_OF_RANGE,                               # 105
            OUT_OF_RANGE,                               # 120
            OUT_OF_RANGE,                               # 135
            laser_dists[4] + SENSOR_DIST_FROM_CENTER,   # 150
        ]
        self.laser_publisher.publish(msg)


def main(args=None):
    rclpy.init(args=args)

    epuck_controller = EPuckDriver(args=args)

    rclpy.spin(epuck_controller)
    rclpy.shutdown()


if __name__ == '__main__':
    main()

看完这段代码，就能理解04-入门中，laserscan数据的特点：

webots和ros2笔记05-新建_驱动程序_06

msg = LaserScan()
        msg.header.frame_id = 'laser_scanner'
        msg.header.stamp = stamp
        msg.angle_min = - 150 * pi / 180
        msg.angle_max = 150 * pi / 180
        msg.angle_increment = 15 * pi / 180
        msg.range_min = SENSOR_DIST_FROM_CENTER + INFRARED_MIN_RANGE
        msg.range_max = SENSOR_DIST_FROM_CENTER + TOF_MAX_RANGE
        msg.ranges = [
            laser_dists[3] + SENSOR_DIST_FROM_CENTER,   # -150
            OUT_OF_RANGE,                               # -135
            OUT_OF_RANGE,                               # -120
            OUT_OF_RANGE,                               # -105
            laser_dists[2] + SENSOR_DIST_FROM_CENTER,   # -90
            OUT_OF_RANGE,                               # -75
            OUT_OF_RANGE,                               # -60
            laser_dists[1] + SENSOR_DIST_FROM_CENTER,   # -45
            OUT_OF_RANGE,                               # -30
            laser_dists[0] + SENSOR_DIST_FROM_CENTER,   # -15
            dist_tof + SENSOR_DIST_FROM_CENTER,         # 0
            laser_dists[7] + SENSOR_DIST_FROM_CENTER,   # 15
            OUT_OF_RANGE,                               # 30
            laser_dists[6] + SENSOR_DIST_FROM_CENTER,   # 45
            OUT_OF_RANGE,                               # 60
            OUT_OF_RANGE,                               # 75
            laser_dists[5] + SENSOR_DIST_FROM_CENTER,   # 90
            OUT_OF_RANGE,                               # 105
            OUT_OF_RANGE,                               # 120
            OUT_OF_RANGE,                               # 135
            laser_dists[4] + SENSOR_DIST_FROM_CENTER,   # 150
        ]

echo数据中的0.0，其实没有对应传感器啊！！！也就是OUT_OF_RANGE。

重点说明：

由于官网教程比较简洁，如果想学好webots和ros2，请务必大量阅读源码，而不是满足于输入命令看到效果。

精彩继续：

通用启动器

webots-ros2提供了一个通用启动器，可以根据Webots的机器人说明自动创建ROS2服务和主题。只需在其中包含机器人即可提供Webots世界文件的路径，例如：

ros2 launch webots_ros2_core robot_launch.py \
    world:=$(ros2 pkg prefix webots_ros2_universal_robot --share)/worlds/universal_robot_rviz.wbt

自定义ROS2驱动程序和软件包

如果通用启动程序未涵盖Webots设备，或者希望以其他方式创建ROS接口，则可以从头开始构建ROS2驱动程序。

如果创建自定义包为my_webots_driver，编译成功后，可以新建驱动，如/my_webots_driver/my_webots_driver/driver.py

import rclpy
from webots_ros2_core.webots_node import WebotsNode


class MyWebotsDriver(WebotsNode):
    def __init__(self, args):
        super().__init__('my_webots_driver', args=args)


def main(args=None):
    rclpy.init(args=args)
    my_webots_driver = MyWebotsDriver(args=args)
    rclpy.spin(my_webots_driver)
    my_webots_driver.destroy()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

然后，在启动文件中加入如下说明：

import os
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch import LaunchDescription
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    webots = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(get_package_share_directory('webots_ros2_core'), 'launch', 'robot_launch.py')
        ),
        launch_arguments=[
            ('package', 'my_webots_driver'),
            ('executable', 'driver'),
            ('world', path_to_webots_world_file),
        ]
    )

    return LaunchDescription([
        webots
    ])

确保这些添加到setup.py，并使用colcon build成功编译，才可以使用哦！

ros2 launch my_webots_driver robot_launch.py

使用webots的API实现距离传感器示例

当然官网还贴心的给了一个距离传感器示例，或者也可以直接参考epuck案例中对应部分：

class MyWebotsDriver(WebotsNode):
    def __init__(self, args):
        super().__init__('my_webots_driver', args=args)
        self.sensor = self.robot.getDistanceSensor('my_distance_sensor')
        self.sensor.enable(self.timestep)
        self.sensor_publisher = self.create_publisher(Range, '/my_distance_sensor', 1)
        self.create_timer(self.timestep * 1e-3, self.publish_sensor_data)

    def publish_sensor_data(self)
        msg = Range()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = 'my_distance_sensor'
        msg.field_of_view = self.sensor.getAperture()
        msg.min_range = self.sensor.getMinValue()
        msg.max_range = self.sensor.getMaxValue()
        msg.range = self.sensor.getValue()
        msg.radiation_type = Range.INFRARED
        self.sensor_publisher.publish(msg)

当然也可以加入如下扩展：

self.start_device_manager({
            'my_distance_sensor': {
                'disable': True
            }
        })

更多内容推荐阅读文首的官网教程链接。