在完成04-入门:webots和ros2笔记04-入城 - 古月居 (guyuehome.com)
如果将webots丰富的机器人库和ros2结合呢???
官网教程非常简洁:
掌握如上两篇即可,都是去年的旧文档了。
下面简单说明一下:
创建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 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数据的特点:
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
}
})
更多内容推荐阅读文首的官网教程链接。
————————————————
评论(0)
您还未登录,请登录后发表或查看评论