Mapping and Navigation Simultaneously Tutorial
From Waveshare Wiki
Function Pack Introduction
In the previous tutorial, the map building and navigation of the robot are separated.
Preceding mapping before starting navigation is inconvenient and limits the ability to explore unknown areas since it can only navigate within pre-built fixed-size maps.
Is there a way to perform mapping and navigation simultaneously?
Next, we will follow the tutorial to implement this feature.
Demo
Create new slam_and_nav.yaml file in /home/waveshare/test/ugv02/src/ugv02_bringup/params/.
bt_navigator:
ros__parameters:
use_sim_time: True
global_frame: map
robot_base_frame: base_link
odom_topic: /odom
bt_loop_duration: 10
default_server_timeout: 20
# 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
# nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
# nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
# They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
plugin_lib_names:
- nav2_compute_path_to_pose_action_bt_node
- nav2_compute_path_through_poses_action_bt_node
- nav2_smooth_path_action_bt_node
- nav2_follow_path_action_bt_node
- nav2_spin_action_bt_node
- nav2_wait_action_bt_node
- nav2_assisted_teleop_action_bt_node
- nav2_back_up_action_bt_node
- nav2_drive_on_heading_bt_node
- nav2_clear_costmap_service_bt_node
- nav2_is_stuck_condition_bt_node
- nav2_goal_reached_condition_bt_node
- nav2_goal_updated_condition_bt_node
- nav2_globally_updated_goal_condition_bt_node
- nav2_is_path_valid_condition_bt_node
- nav2_initial_pose_received_condition_bt_node
- nav2_reinitialize_global_localization_service_bt_node
- nav2_rate_controller_bt_node
- nav2_distance_controller_bt_node
- nav2_speed_controller_bt_node
- nav2_truncate_path_action_bt_node
- nav2_truncate_path_local_action_bt_node
- nav2_goal_updater_node_bt_node
- nav2_recovery_node_bt_node
- nav2_pipeline_sequence_bt_node
- nav2_round_robin_node_bt_node
- nav2_transform_available_condition_bt_node
- nav2_time_expired_condition_bt_node
- nav2_path_expiring_timer_condition
- nav2_distance_traveled_condition_bt_node
- nav2_single_trigger_bt_node
- nav2_goal_updated_controller_bt_node
- nav2_is_battery_low_condition_bt_node
- nav2_navigate_through_poses_action_bt_node
- nav2_navigate_to_pose_action_bt_node
- nav2_remove_passed_goals_action_bt_node
- nav2_planner_selector_bt_node
- nav2_controller_selector_bt_node
- nav2_goal_checker_selector_bt_node
- nav2_controller_cancel_bt_node
- nav2_path_longer_on_approach_bt_node
- nav2_wait_cancel_bt_node
- nav2_spin_cancel_bt_node
- nav2_back_up_cancel_bt_node
- nav2_assisted_teleop_cancel_bt_node
- nav2_drive_on_heading_cancel_bt_node
- nav2_is_battery_charging_condition_bt_node
bt_navigator_navigate_through_poses_rclcpp_node:
ros__parameters:
use_sim_time: True
bt_navigator_navigate_to_pose_rclcpp_node:
ros__parameters:
use_sim_time: True
controller_server:
ros__parameters:
use_sim_time: True
controller_frequency: 20.0
min_x_velocity_threshold: 0.001
min_y_velocity_threshold: 0.5
min_theta_velocity_threshold: 0.001
failure_tolerance: 0.3
progress_checker_plugin: "progress_checker"
goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
controller_plugins: ["FollowPath"]
# Progress checker parameters
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.5
movement_time_allowance: 10.0
# Goal checker parameters
general_goal_checker:
stateful: True
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.05
yaw_goal_tolerance: 0.1
# DWB parameters
FollowPath:
plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
desired_linear_vel: 0.3
lookahead_dist: 0.6
min_lookahead_dist: 0.4
max_lookahead_dist: 0.6
lookahead_time: 1.5
rotate_to_heading_angular_vel: 0.6
transform_tolerance: 0.1
use_velocity_scaled_lookahead_dist: false
min_approach_linear_velocity: 0.05
approach_velocity_scaling_dist: 0.6
use_collision_detection: true
max_allowed_time_to_collision_up_to_carrot: 1.0
use_regulated_linear_velocity_scaling: true
use_fixed_curvature_lookahead: false
curvature_lookahead_dist: 0.15
use_cost_regulated_linear_velocity_scaling: false
regulated_linear_scaling_min_radius: 0.6
regulated_linear_scaling_min_speed: 0.15
use_rotate_to_heading: true
allow_reversing: false
rotate_to_heading_min_angle: 0.785
max_angular_accel: 1.2
max_robot_pose_search_dist: 10.0
use_interpolation: false
local_costmap:
local_costmap:
ros__parameters:
update_frequency: 5.0
publish_frequency: 2.0
global_frame: odom
robot_base_frame: base_link
use_sim_time: True
rolling_window: true
width: 3
height: 3
resolution: 0.05
robot_radius: 0.12
plugins: ["voxel_layer", "inflation_layer"]
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
inflation_radius: 0.35
voxel_layer:
plugin: "nav2_costmap_2d::VoxelLayer"
enabled: True
publish_voxel_map: True
origin_z: 0.0
z_resolution: 0.05
z_voxels: 16
max_obstacle_height: 2.0
mark_threshold: 0
observation_sources: scan
scan:
topic: /scan
max_obstacle_height: 2.0
clearing: True
marking: True
data_type: "LaserScan"
raytrace_max_range: 3.0
raytrace_min_range: 0.0
obstacle_max_range: 2.5
obstacle_min_range: 0.0
static_layer:
plugin: "nav2_costmap_2d::StaticLayer"
map_subscribe_transient_local: True
always_send_full_costmap: True
global_costmap:
global_costmap:
ros__parameters:
update_frequency: 1.0
publish_frequency: 1.0
global_frame: map
robot_base_frame: base_link
use_sim_time: True
robot_radius: 0.12
resolution: 0.05
track_unknown_space: true
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: True
observation_sources: scan
scan:
topic: /scan
max_obstacle_height: 2.0
clearing: True
marking: True
data_type: "LaserScan"
raytrace_max_range: 3.0
raytrace_min_range: 0.0
obstacle_max_range: 2.5
obstacle_min_range: 0.0
static_layer:
plugin: "nav2_costmap_2d::StaticLayer"
map_subscribe_transient_local: True
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
inflation_radius: 0.35
always_send_full_costmap: True
map_saver:
ros__parameters:
use_sim_time: True
save_map_timeout: 5.0
free_thresh_default: 0.25
occupied_thresh_default: 0.65
map_subscribe_transient_local: True
planner_server:
ros__parameters:
expected_planner_frequency: 20.0
use_sim_time: True
planner_plugins: ["GridBased"]
GridBased:
plugin: "nav2_navfn_planner/NavfnPlanner"
tolerance: 0.5
use_astar: false
allow_unknown: true
smoother_server:
ros__parameters:
use_sim_time: True
smoother_plugins: ["simple_smoother"]
simple_smoother:
plugin: "nav2_smoother::SimpleSmoother"
tolerance: 1.0e-10
max_its: 1000
do_refinement: True
behavior_server:
ros__parameters:
costmap_topic: local_costmap/costmap_raw
footprint_topic: local_costmap/published_footprint
cycle_frequency: 10.0
behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
spin:
plugin: "nav2_behaviors/Spin"
backup:
plugin: "nav2_behaviors/BackUp"
drive_on_heading:
plugin: "nav2_behaviors/DriveOnHeading"
wait:
plugin: "nav2_behaviors/Wait"
assisted_teleop:
plugin: "nav2_behaviors/AssistedTeleop"
global_frame: odom
robot_base_frame: base_link
transform_tolerance: 0.1
use_sim_time: true
simulate_ahead_time: 2.0
max_rotational_vel: 1.0
min_rotational_vel: 0.4
rotational_acc_lim: 3.2
robot_state_publisher:
ros__parameters:
use_sim_time: True
waypoint_follower:
ros__parameters:
use_sim_time: True
loop_rate: 20
stop_on_failure: false
waypoint_task_executor_plugin: "wait_at_waypoint"
wait_at_waypoint:
plugin: "nav2_waypoint_follower::WaitAtWaypoint"
enabled: True
waypoint_pause_duration: 200
velocity_smoother:
ros__parameters:
use_sim_time: True
smoothing_frequency: 20.0
scale_velocities: False
feedback: "OPEN_LOOP"
max_velocity: [0.26, 0.0, 1.0]
min_velocity: [-0.26, 0.0, -1.0]
max_accel: [2.5, 0.0, 3.2]
max_decel: [-2.5, 0.0, -3.2]
odom_topic: "odom"
odom_duration: 0.1
deadband_velocity: [0.0, 0.0, 0.0]
velocity_timeout: 1.0
The main difference in configuration from the previous section is the absence of the amcl and map_server components.
amcl plays a crucial role in localization, requiring odometer cooperation with the radar for robot position estimation.
map_server acts as the map server, primarily providing static grid map data for amcl to perform position estimation.
Configure launch File
Create slam_and_nav_bringup_launch.py in ugv02/src/ugv02_bringup/launch/.
Edit the file:
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import (DeclareLaunchArgument, GroupAction,
IncludeLaunchDescription, SetEnvironmentVariable)
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.actions import PushRosNamespace
from nav2_common.launch import RewrittenYaml
def generate_launch_description():
# Get the launch directory
bringup_dir = get_package_share_directory('ugv02_bringup')
nav2_dir = get_package_share_directory('nav2_bringup')
launch_dir = os.path.join(nav2_dir, 'launch')
# Create the launch configuration variables
namespace = LaunchConfiguration('namespace')
use_namespace = LaunchConfiguration('use_namespace')
slam = LaunchConfiguration('slam')
use_sim_time = LaunchConfiguration('use_sim_time')
params_file = LaunchConfiguration('params_file')
autostart = LaunchConfiguration('autostart')
use_composition = LaunchConfiguration('use_composition')
use_respawn = LaunchConfiguration('use_respawn')
log_level = LaunchConfiguration('log_level')
remappings = [('/tf', 'tf'),
('/tf_static', 'tf_static')]
# Create our own temporary YAML files that include substitutions
param_substitutions = {
'use_sim_time': use_sim_time}
configured_params = RewrittenYaml(
source_file=params_file,
root_key=namespace,
param_rewrites=param_substitutions,
convert_types=True)
stdout_linebuf_envvar = SetEnvironmentVariable(
'RCUTILS_LOGGING_BUFFERED_STREAM', '1')
declare_namespace_cmd = DeclareLaunchArgument(
'namespace',
default_value='',
description='Top-level namespace')
declare_use_namespace_cmd = DeclareLaunchArgument(
'use_namespace',
default_value='false',
description='Whether to apply a namespace to the navigation stack')
declare_slam_cmd = DeclareLaunchArgument(
'slam',
default_value='False',
description='Whether run a SLAM')
declare_use_sim_time_cmd = DeclareLaunchArgument(
'use_sim_time',
default_value='false',
description='Use simulation (Gazebo) clock if true')
declare_params_file_cmd = DeclareLaunchArgument(
'params_file',
default_value=os.path.join(bringup_dir, 'params', 'slam_and_nav.yaml'),
description='Full path to the ROS2 parameters file to use for all launched nodes')
declare_autostart_cmd = DeclareLaunchArgument(
'autostart', default_value='true',
description='Automatically startup the nav2 stack')
declare_use_composition_cmd = DeclareLaunchArgument(
'use_composition', default_value='True',
description='Whether to use composed bringup')
declare_use_respawn_cmd = DeclareLaunchArgument(
'use_respawn', default_value='False',
description='Whether to respawn if a node crashes. Applied when composition is disabled.')
declare_log_level_cmd = DeclareLaunchArgument(
'log_level', default_value='info',
description='log level')
# Specify the actions
bringup_cmd_group = GroupAction([
PushRosNamespace(
condition=IfCondition(use_namespace),
namespace=namespace),
Node(
condition=IfCondition(use_composition),
name='nav2_container',
package='rclcpp_components',
executable='component_container_isolated',
parameters=[configured_params, {'autostart': autostart}],
arguments=['--ros-args', '--log-level', log_level],
remappings=remappings,
output='screen'),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(launch_dir, 'slam_launch.py')),
condition=IfCondition(slam),
launch_arguments={'namespace': namespace,
'use_sim_time': use_sim_time,
'autostart': autostart,
'use_respawn': use_respawn,
'params_file': params_file}.items()),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(launch_dir, 'navigation_launch.py')),
launch_arguments={'namespace': namespace,
'use_sim_time': use_sim_time,
'autostart': autostart,
'params_file': params_file,
'use_composition': use_composition,
'use_respawn': use_respawn,
'container_name': 'nav2_container'}.items()),
])
# Create the launch description and populate
ld = LaunchDescription()
# Set environment variables
ld.add_action(stdout_linebuf_envvar)
# Declare the launch options
ld.add_action(declare_namespace_cmd)
ld.add_action(declare_use_namespace_cmd)
ld.add_action(declare_slam_cmd)
ld.add_action(declare_use_sim_time_cmd)
ld.add_action(declare_params_file_cmd)
ld.add_action(declare_autostart_cmd)
ld.add_action(declare_use_composition_cmd)
ld.add_action(declare_use_respawn_cmd)
ld.add_action(declare_log_level_cmd)
# Add the actions to launch all of the navigation nodes
ld.add_action(bringup_cmd_group)
return ld
The configuration inside also lacks the map and localization components.
The missing map and localization parts will be complemented by your SLAM algorithm implementation.
Compile
Return the workspace and compile:
cd ~/ugv02 colcon build
Running Test
Create a new terminal, enter the workspace, source it, and launch the LiDAR in Ugv02 Using LiDAR Tutorial.
cd ~/ugv02 source ./install/setup.bash ros2 launch ldlidar_stl_ros2 ld19.launch.py
Create a new terminal, enter the workspace, source it, and launch the mapping in cartographer Map Building.
cd ~/ugv02 source ./install/setup.bash ros2 launch ugv02_cartographer cartographer.launch.py
Create a new terminal, enter the workspace, source it, and launch the new "launch" file in this tutorial.
cd ~/ugv02 source ./install/setup.bash ros2 launch ugv02_bringup slam_and_nav_bringup_launch.py
Create a new terminal, enter the workspace, source it, and launch the chassis control in the Chassis Control Tutorial.
cd ~/ugv02 source ./install/setup.bash ros2 run communication_module ugv02_control
In a computer (virtual machine) with Ubuntu and ros2, make sure that the computer is connected to the same network or wifi as the Raspberry Pi, and open a new terminal.
rviz2