赞
踩
参考文献:Navigation using ROS 2 Mapping
机器人仿真或实际运动环境的最简描述。
至少目前没有很好的统一的解决方案,相关算法都在研发和改进中。
地图类型
栅格地图 | 图形地图 | 特征地图 |
目前,ROS2 的 SLAM 还没有可靠唯一标准。 一些有力竞争者是:
占用栅格图
用于三维占用地图的八叉树
效果引用(github.com/rsasaki0109/li_slam_ros2)
此处简单用turtlebot3+cartographer+ros2foxy+webots2021b在win10下,跑一个效果:
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-
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。