webots和ros2笔记05-新建

在完成04-入门：https://zhangrelay.blog.csdn.net/article/details/112739671。

如果将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为例：

选择Robot：

找一找controller：

然后保存环境：

使用如下命令：

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

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

这就完成了一个机器人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 WebotsDifferentialDriveNodeOUT_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.035DISTANCE_SENSOR_ANGLE = [-15 * pi / 180,   # ps0-45 * pi / 180,   # ps1-90 * pi / 180,   # ps2-150 * pi / 180,  # ps3150 * pi / 180,   # ps490 * pi / 180,    # ps545 * pi / 180,    # ps615 * 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 topicself.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)] = sensorself.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.0laser_transform.transform.rotation.y = 0.0laser_transform.transform.rotation.z = 0.0laser_transform.transform.rotation.w = 1.0laser_transform.transform.translation.x = 0.0laser_transform.transform.translation.y = 0.0laser_transform.transform.translation.z = 0.033self.static_broadcaster = StaticTransformBroadcaster(self)self.static_broadcaster.sendTransform(laser_transform)# Main loopself.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_SENSORSdist_tof = OUT_OF_RANGE# Calculate distancesfor i, key in enumerate(self.distance_sensors):dists[i] = interpolate_lookup_table(self.distance_sensors[key].getValue(), self.distance_sensors[key].getLookupTable())# Publish range: ToFif 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 rvizlaser_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 = stampmsg.angle_min = - 150 * pi / 180msg.angle_max = 150 * pi / 180msg.angle_increment = 15 * pi / 180msg.range_min = SENSOR_DIST_FROM_CENTER + INFRARED_MIN_RANGEmsg.range_max = SENSOR_DIST_FROM_CENTER + TOF_MAX_RANGEmsg.ranges = [laser_dists[3] + SENSOR_DIST_FROM_CENTER,   # -150OUT_OF_RANGE,                               # -135OUT_OF_RANGE,                               # -120OUT_OF_RANGE,                               # -105laser_dists[2] + SENSOR_DIST_FROM_CENTER,   # -90OUT_OF_RANGE,                               # -75OUT_OF_RANGE,                               # -60laser_dists[1] + SENSOR_DIST_FROM_CENTER,   # -45OUT_OF_RANGE,                               # -30laser_dists[0] + SENSOR_DIST_FROM_CENTER,   # -15dist_tof + SENSOR_DIST_FROM_CENTER,         # 0laser_dists[7] + SENSOR_DIST_FROM_CENTER,   # 15OUT_OF_RANGE,                               # 30laser_dists[6] + SENSOR_DIST_FROM_CENTER,   # 45OUT_OF_RANGE,                               # 60OUT_OF_RANGE,                               # 75laser_dists[5] + SENSOR_DIST_FROM_CENTER,   # 90OUT_OF_RANGE,                               # 105OUT_OF_RANGE,                               # 120OUT_OF_RANGE,                               # 135laser_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数据的特点：

        msg = LaserScan()msg.header.frame_id = 'laser_scanner'msg.header.stamp = stampmsg.angle_min = - 150 * pi / 180msg.angle_max = 150 * pi / 180msg.angle_increment = 15 * pi / 180msg.range_min = SENSOR_DIST_FROM_CENTER + INFRARED_MIN_RANGEmsg.range_max = SENSOR_DIST_FROM_CENTER + TOF_MAX_RANGEmsg.ranges = [laser_dists[3] + SENSOR_DIST_FROM_CENTER,   # -150OUT_OF_RANGE,                               # -135OUT_OF_RANGE,                               # -120OUT_OF_RANGE,                               # -105laser_dists[2] + SENSOR_DIST_FROM_CENTER,   # -90OUT_OF_RANGE,                               # -75OUT_OF_RANGE,                               # -60laser_dists[1] + SENSOR_DIST_FROM_CENTER,   # -45OUT_OF_RANGE,                               # -30laser_dists[0] + SENSOR_DIST_FROM_CENTER,   # -15dist_tof + SENSOR_DIST_FROM_CENTER,         # 0laser_dists[7] + SENSOR_DIST_FROM_CENTER,   # 15OUT_OF_RANGE,                               # 30laser_dists[6] + SENSOR_DIST_FROM_CENTER,   # 45OUT_OF_RANGE,                               # 60OUT_OF_RANGE,                               # 75laser_dists[5] + SENSOR_DIST_FROM_CENTER,   # 90OUT_OF_RANGE,                               # 105OUT_OF_RANGE,                               # 120OUT_OF_RANGE,                               # 135laser_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 WebotsNodeclass 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_directorydef 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.INFRAREDself.sensor_publisher.publish(msg)

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

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

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