参考文献：Navigation using ROS 2 Mapping
SLAM - Simultaneous Localization And Mapping
同步定位和建图
机器人仿真或实际运动环境的最简描述。
建图 - SLAM
- SLAM：同时估计机器人的位置姿态和环境的地图
- 定位：给定地图推断位置
- 建图：推断给定位置的地图
- SLAM：同时学习地图和定位机器人
- SLAM 的目标是创建或增强环境地图。
- 地图表示 2D 或 3D 环境。
- 实际上是一个很难解决的问题！
至少目前没有很好的统一的解决方案，相关算法都在研发和改进中。
地图类型
| 栅格地图 | 图形地图 | 特征地图 |
 |  |  |
ROS2 SLAM工具箱
目前，ROS2 的 SLAM 还没有可靠唯一标准。 一些有力竞争者是：
- LaMa (2D) - IRIS Labs - 新的，可以说是更好的，强有力的竞争者
- Cartographer (2D/3D) - Google - 从 ROS1 移植，经常使用，但没有维护
- SLAM 工具箱 (2D) - Steve Macenski - 当前随 Navigation2 一起提供，需要支持。 如果可以，请参与帮助！
二维SLAM
占用栅格图
- 为每个网格单元计算概率（贝叶斯过滤器）
三维SLAM
用于三维占用地图的八叉树
- 基于树的数据结构
 |  |  |
效果引用（github.com/rsasaki0109/li_slam_ros2）?
此处简单用turtlebot3+cartographer+ros2foxy+webots2021b在win10下，跑一个效果：
- set TURTLEBOT3_MODEL=burger?
- ros2 launch webots_ros2_turtlebot robot_launch.py
- ros2 launch turtlebot3_cartographer cartographer.launch.py use_sim_time:=true
- ros2 run turtlebot3_teleop teleop_keyboard
ros2 launch webots_ros2_turtlebot robot_launch.py
启动webots仿真
"""Launch Webots TurtleBot3 Burger driver."""
import os
import pathlib
from launch.substitutions import LaunchConfiguration
from launch.actions import DeclareLaunchArgument
from launch.substitutions.path_join_substitution import PathJoinSubstitution
from launch import LaunchDescription
from launch_ros.actions import Node
import launch
from ament_index_python.packages import get_package_share_directory
from webots_ros2_core.webots_launcher import WebotsLauncher
def generate_launch_description():
package_dir = get_package_share_directory('webots_ros2_turtlebot')
world = LaunchConfiguration('world')
robot_description = pathlib.Path(os.path.join(package_dir, 'resource', 'turtlebot_webots.urdf')).read_text()
ros2_control_params = os.path.join(package_dir, 'resource', 'ros2control.yml')
webots = WebotsLauncher(
world=PathJoinSubstitution([package_dir, 'worlds', world])
)
# TODO: Revert once the https://github.com/ros-controls/ros2_control/pull/444 PR gets into the release
controller_manager_timeout = ['--controller-manager-timeout', '50'] if os.name == 'nt' else []
controller_manager_prefix = 'python.exe' if os.name == 'nt' else "bash -c 'sleep 10; $0 $@' "
diffdrive_controller_spawner = Node(
package='controller_manager',
executable='spawner.py',
output='screen',
prefix=controller_manager_prefix,
arguments=['diffdrive_controller'] + controller_manager_timeout,
)
joint_state_broadcaster_spawner = Node(
package='controller_manager',
executable='spawner.py',
output='screen',
prefix=controller_manager_prefix,
arguments=['joint_state_broadcaster'] + controller_manager_timeout,
)
turtlebot_driver = Node(
package='webots_ros2_driver',
executable='driver',
output='screen',
parameters=[
{'robot_description': robot_description},
ros2_control_params
],
remappings=[
('/diffdrive_controller/cmd_vel_unstamped', '/cmd_vel')
]
)
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[{
'robot_description': '<robot name=""><link name=""/></robot>'
}],
)
footprint_publisher = Node(
package='tf2_ros',
executable='static_transform_publisher',
output='screen',
arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_footprint'],
)
return LaunchDescription([
DeclareLaunchArgument(
'world',
default_value='turtlebot3_burger_example.wbt',
description='Choose one of the world files from `/webots_ros2_turtlebot/world` directory'
),
joint_state_broadcaster_spawner,
diffdrive_controller_spawner,
webots,
robot_state_publisher,
turtlebot_driver,
footprint_publisher,
launch.actions.RegisterEventHandler(
event_handler=launch.event_handlers.OnProcessExit(
target_action=webots,
on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())],
)
)
])
ros2 launch turtlebot3_cartographer cartographer.launch.py use_sim_time:=true
启动SLAM建图
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import ThisLaunchFileDir
def generate_launch_description():
use_sim_time = LaunchConfiguration('use_sim_time', default='false')
turtlebot3_cartographer_prefix = get_package_share_directory('turtlebot3_cartographer')
cartographer_config_dir = LaunchConfiguration('cartographer_config_dir', default=os.path.join(
turtlebot3_cartographer_prefix, 'config'))
configuration_basename = LaunchConfiguration('configuration_basename',
default='turtlebot3_lds_2d.lua')
resolution = LaunchConfiguration('resolution', default='0.05')
publish_period_sec = LaunchConfiguration('publish_period_sec', default='1.0')
rviz_config_dir = os.path.join(get_package_share_directory('turtlebot3_cartographer'),
'rviz', 'tb3_cartographer.rviz')
return LaunchDescription([
DeclareLaunchArgument(
'cartographer_config_dir',
default_value=cartographer_config_dir,
description='Full path to config file to load'),
DeclareLaunchArgument(
'configuration_basename',
default_value=configuration_basename,
description='Name of lua file for cartographer'),
DeclareLaunchArgument(
'use_sim_time',
default_value='false',
description='Use simulation (Gazebo) clock if true'),
Node(
package='cartographer_ros',
executable='cartographer_node',
name='cartographer_node',
output='screen',
parameters=[{'use_sim_time': use_sim_time}],
arguments=['-configuration_directory', cartographer_config_dir,
'-configuration_basename', configuration_basename]),
DeclareLaunchArgument(
'resolution',
default_value=resolution,
description='Resolution of a grid cell in the published occupancy grid'),
DeclareLaunchArgument(
'publish_period_sec',
default_value=publish_period_sec,
description='OccupancyGrid publishing period'),
IncludeLaunchDescription(
PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/occupancy_grid.launch.py']),
launch_arguments={'use_sim_time': use_sim_time, 'resolution': resolution,
'publish_period_sec': publish_period_sec}.items(),
),
Node(
package='rviz2',
executable='rviz2',
name='rviz2',
arguments=['-d', rviz_config_dir],
parameters=[{'use_sim_time': use_sim_time}],
output='screen'),
])
ros2 run turtlebot3_teleop teleop_keyboard
启动键盘遥控机器人
import os
import select
import sys
import rclpy
from geometry_msgs.msg import Twist
from rclpy.qos import QoSProfile
if os.name == 'nt':
import msvcrt
else:
import termios
import tty
BURGER_MAX_LIN_VEL = 0.22
BURGER_MAX_ANG_VEL = 2.84
WAFFLE_MAX_LIN_VEL = 0.26
WAFFLE_MAX_ANG_VEL = 1.82
LIN_VEL_STEP_SIZE = 0.01
ANG_VEL_STEP_SIZE = 0.1
TURTLEBOT3_MODEL = os.environ['TURTLEBOT3_MODEL']
msg = """
Control Your TurtleBot3!
---------------------------
Moving around:
w
a s d
x
w/x : increase/decrease linear velocity (Burger : ~ 0.22, Waffle and Waffle Pi : ~ 0.26)
a/d : increase/decrease angular velocity (Burger : ~ 2.84, Waffle and Waffle Pi : ~ 1.82)
space key, s : force stop
CTRL-C to quit
"""
e = """
Communications Failed
"""
def get_key(settings):
if os.name == 'nt':
return msvcrt.getch().decode('utf-8')
tty.setraw(sys.stdin.fileno())
rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
if rlist:
key = sys.stdin.read(1)
else:
key = ''
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
return key
def print_vels(target_linear_velocity, target_angular_velocity):
print('currently:\tlinear velocity {0}\t angular velocity {1} '.format(
target_linear_velocity,
target_angular_velocity))
def make_simple_profile(output, input, slop):
if input > output:
output = min(input, output + slop)
elif input < output:
output = max(input, output - slop)
else:
output = input
return output
def constrain(input_vel, low_bound, high_bound):
if input_vel < low_bound:
input_vel = low_bound
elif input_vel > high_bound:
input_vel = high_bound
else:
input_vel = input_vel
return input_vel
def check_linear_limit_velocity(velocity):
if TURTLEBOT3_MODEL == 'burger':
return constrain(velocity, -BURGER_MAX_LIN_VEL, BURGER_MAX_LIN_VEL)
else:
return constrain(velocity, -WAFFLE_MAX_LIN_VEL, WAFFLE_MAX_LIN_VEL)
def check_angular_limit_velocity(velocity):
if TURTLEBOT3_MODEL == 'burger':
return constrain(velocity, -BURGER_MAX_ANG_VEL, BURGER_MAX_ANG_VEL)
else:
return constrain(velocity, -WAFFLE_MAX_ANG_VEL, WAFFLE_MAX_ANG_VEL)
def main():
settings = None
if os.name != 'nt':
settings = termios.tcgetattr(sys.stdin)
rclpy.init()
qos = QoSProfile(depth=10)
node = rclpy.create_node('teleop_keyboard')
pub = node.create_publisher(Twist, 'cmd_vel', qos)
status = 0
target_linear_velocity = 0.0
target_angular_velocity = 0.0
control_linear_velocity = 0.0
control_angular_velocity = 0.0
try:
print(msg)
while(1):
key = get_key(settings)
if key == 'w':
target_linear_velocity =\
check_linear_limit_velocity(target_linear_velocity + LIN_VEL_STEP_SIZE)
status = status + 1
print_vels(target_linear_velocity, target_angular_velocity)
elif key == 'x':
target_linear_velocity =\
check_linear_limit_velocity(target_linear_velocity - LIN_VEL_STEP_SIZE)
status = status + 1
print_vels(target_linear_velocity, target_angular_velocity)
elif key == 'a':
target_angular_velocity =\
check_angular_limit_velocity(target_angular_velocity + ANG_VEL_STEP_SIZE)
status = status + 1
print_vels(target_linear_velocity, target_angular_velocity)
elif key == 'd':
target_angular_velocity =\
check_angular_limit_velocity(target_angular_velocity - ANG_VEL_STEP_SIZE)
status = status + 1
print_vels(target_linear_velocity, target_angular_velocity)
elif key == ' ' or key == 's':
target_linear_velocity = 0.0
control_linear_velocity = 0.0
target_angular_velocity = 0.0
control_angular_velocity = 0.0
print_vels(target_linear_velocity, target_angular_velocity)
else:
if (key == '\x03'):
break
if status == 20:
print(msg)
status = 0
twist = Twist()
control_linear_velocity = make_simple_profile(
control_linear_velocity,
target_linear_velocity,
(LIN_VEL_STEP_SIZE / 2.0))
twist.linear.x = control_linear_velocity
twist.linear.y = 0.0
twist.linear.z = 0.0
control_angular_velocity = make_simple_profile(
control_angular_velocity,
target_angular_velocity,
(ANG_VEL_STEP_SIZE / 2.0))
twist.angular.x = 0.0
twist.angular.y = 0.0
twist.angular.z = control_angular_velocity
pub.publish(twist)
except Exception as e:
print(e)
finally:
twist = Twist()
twist.linear.x = 0.0
twist.linear.y = 0.0
twist.linear.z = 0.0
twist.angular.x = 0.0
twist.angular.y = 0.0
twist.angular.z = 0.0
pub.publish(twist)
if os.name != 'nt':
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
if __name__ == '__main__':
main()
-End-