4.5.9 导航功能集成
1.准备工作
请先调用如下指令在工作空间的src目录下创建一个功能包：
ros2 pkg create mycar_navigation2 --dependencies navigation2 nav2_common
2.编写launch文件与参数文件
在功能包下，新建launch目录、params目录和bts目录。
launch目录下新建nav2.launch.py文件并输入如下内容：
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
current_pkg = get_package_share_directory("mycar_navigation2")
bt_params = os.path.join(get_package_share_directory("mycar_navigation2"),"params","bt.yaml")
planner_params = os.path.join(get_package_share_directory("mycar_navigation2"),"params","planner.yaml")
controller_params = os.path.join(get_package_share_directory("mycar_navigation2"),"params","controller.yaml")
behavior_params = os.path.join(get_package_share_directory("mycar_navigation2"),"params","behavior.yaml")
waypoint_params = os.path.join(get_package_share_directory("mycar_navigation2"),"params","waypoint.yaml")
velocity_params = os.path.join(get_package_share_directory("mycar_navigation2"),"params","velocity.yaml")
smoother_params = os.path.join(get_package_share_directory("mycar_navigation2"),"params","smoother.yaml")
planner_server_node = Node(
package='nav2_planner',
executable='planner_server',
name='planner_server',
output='screen',
parameters=[planner_params],
)
controller_server_node = Node(
package='nav2_controller',
executable='controller_server',
output='screen',
parameters=[controller_params],
remappings=[('cmd_vel', 'cmd_vel_nav')]
)
behavior_server_node = Node(
package='nav2_behaviors',
executable='behavior_server',
name='behavior_server',
output='screen',
parameters=[behavior_params]
)
waypoint_node = Node(
package='nav2_waypoint_follower',
executable='waypoint_follower',
name='waypoint_follower',
output='screen',
parameters=[waypoint_params]
)
velocity_smoother_node = Node(
package='nav2_velocity_smoother',
executable='velocity_smoother',
name='velocity_smoother',
output='screen',
respawn_delay=2.0,
parameters=[velocity_params],
remappings=
[('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]
)
smoother_server_node = Node(
package='nav2_smoother',
executable='smoother_server',
name='smoother_server',
output='screen',
parameters=[smoother_params],
)
bt_navigator_node = Node(
package='nav2_bt_navigator',
executable='bt_navigator',
name='bt_navigator',
output='screen',
parameters=[
bt_params,
{"default_nav_to_pose_bt_xml": os.path.join(current_pkg,"bts","bt_planner_controller_behavior.xml")},
{"default_nav_through_poses_bt_xml": os.path.join(current_pkg,"bts","bt_planner_controller_behavior_poses.xml")}
],
)
lifecycle_manager_node = Node(
package='nav2_lifecycle_manager',
executable='lifecycle_manager',
name='lifecycle_manager_navigation',
output='screen',
parameters=[{'use_sim_time': True},
{'autostart': True},
{'node_names': [
'bt_navigator',
'planner_server',
'controller_server',
'behavior_server',
'waypoint_follower',
'velocity_smoother',
'smoother_server'
]}])
return LaunchDescription([
lifecycle_manager_node,
bt_navigator_node,
planner_server_node,
controller_server_node,
behavior_server_node,
waypoint_node,
velocity_smoother_node,
smoother_server_node
])
该launch文件主要是加载了生命周期管理器、行为树、规划器、控制器、恢复器、航点跟踪、路径平滑以及速度平滑等节点。并且除了生命周期管理器节点外，每个节点都还会加载一个配置文件，接下来需要编辑这些配置文。
（1）bt_navigator相关配置文件
bt_navigator相关配置文件有两个，分别是描述行为树的xml文件，以及yaml格式的参数文件，前者存储在bts目录下，后者存储在params目录下。
请在bts目录下，新建一个名为bt_planner_controller_behavior.xml的文件并输入如下内容：
<!--
This Behavior Tree replans the global path periodically at 1 Hz and it also has
recovery actions specific to planning / control as well as general system issues.
This will be continuous if a kinematically valid planner is selected.
-->
<root main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
<PipelineSequence name="NavigateWithReplanning">
<RateController hz="1.0">
<RecoveryNode number_of_retries="1" name="ComputePathToPose">
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
</RecoveryNode>
</RateController>
<RecoveryNode number_of_retries="1" name="FollowPath">
<FollowPath path="{path}" controller_id="FollowPath"/>
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
</RecoveryNode>
</PipelineSequence>
<ReactiveFallback name="RecoveryFallback">
<GoalUpdated/>
<RoundRobin name="RecoveryActions">
<Sequence name="ClearingActions">
<ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
</Sequence>
<Spin spin_dist="1.57"/>
<Wait wait_duration="5"/>
<BackUp backup_dist="0.30" backup_speed="0.05"/>
</RoundRobin>
</ReactiveFallback>
</RecoveryNode>
</BehaviorTree>
</root>
继续在bts目录下新建一个名为bt_planner_controller_behavior_poses.xml的文件，并输入如下内容：
<!--
This Behavior Tree replans the global path periodically at 1 Hz through an array of poses continuously
and it also has recovery actions specific to planning / control as well as general system issues.
-->
<root main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
<PipelineSequence name="NavigateWithReplanning">
<RateController hz="0.333">
<RecoveryNode number_of_retries="1" name="ComputePathThroughPoses">
<ReactiveSequence>
<RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7"/>
<ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="GridBased"/>
</ReactiveSequence>
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
</RecoveryNode>
</RateController>
<RecoveryNode number_of_retries="1" name="FollowPath">
<FollowPath path="{path}" controller_id="FollowPath"/>
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
</RecoveryNode>
</PipelineSequence>
<ReactiveFallback name="RecoveryFallback">
<GoalUpdated/>
<RoundRobin name="RecoveryActions">
<Sequence name="ClearingActions">
<ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
</Sequence>
<Spin spin_dist="1.57"/>
<Wait wait_duration="5"/>
<BackUp backup_dist="0.30" backup_speed="0.05"/>
</RoundRobin>
</ReactiveFallback>
</RecoveryNode>
</BehaviorTree>
</root>
在params目录下新建一个名为bt.yaml的文件，并输入如下内容：
/**:
ros__parameters:
use_sim_time: True
global_frame: map
robot_base_frame: base_link
odom_topic: /odom
default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
bt_loop_duration: 10
default_server_timeout: 20
enable_groot_monitoring: True
groot_zmq_publisher_port: 1666
groot_zmq_server_port: 1667
plugin_lib_names:
- nav2_compute_path_to_pose_action_bt_node
- nav2_compute_path_through_poses_action_bt_node
- nav2_follow_path_action_bt_node
- nav2_back_up_action_bt_node
- nav2_spin_action_bt_node
- nav2_wait_action_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_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_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_distance_traveled_condition_bt_node
- nav2_single_trigger_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
（2）planner_server相关配置文件
在params目录下新建一个名为planner.yaml的文件，并输入如下内容：
/**:
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
/**:
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.2
footprint: "[[0.19, 0.13], [0.19, -0.13], [-0.19, -0.13], [-0.19, 0.13]]"
resolution: 0.05
track_unknown_space: true
plugins: ["static_layer", "obstacle_layer", "voxel_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
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
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 4.0
inflation_radius: 0.55
always_send_full_costmap: False
（3）controller_server相关配置文件
在params目录下新建一个名为controller.yaml的文件，并输入如下内容：
/**:
ros__parameters:
use_sim_time: True
controller_frequency: 10.0
min_x_velocity_threshold: 0.001
min_y_velocity_threshold: 0.5
min_theta_velocity_threshold: 0.001
# failure_tolerance: 0.3
failure_tolerance: 1.0
progress_checker_plugin: "progress_checker"
goal_checker_plugins: ["general_goal_checker"]
controller_plugins: ["FollowPath"]
# Progress checker parameters
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.5
movement_time_allowance: 10.0
general_goal_checker:
stateful: True
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.25
yaw_goal_tolerance: 0.25
# DWB parameters
FollowPath:
plugin: "dwb_core::DWBLocalPlanner"
debug_trajectory_details: True
min_vel_x: 0.0
min_vel_y: 0.0
# max_vel_x: 0.15
max_vel_x: 0.2
max_vel_y: 0.0
# max_vel_theta: 1.0
max_vel_theta: 1.0
min_speed_xy: 0.0
max_speed_xy: 0.2
min_speed_theta: 0.0
# Add high threshold velocity for turtlebot 3 issue.
# https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
acc_lim_x: 1.0
acc_lim_y: 0.0
acc_lim_theta: 3.2
decel_lim_x: -1.0
decel_lim_y: 0.0
decel_lim_theta: -3.2
# vx_samples: 20
vx_samples: 20
vy_samples: 5
vtheta_samples: 20
sim_time: 1.7
linear_granularity: 0.05
angular_granularity: 0.025
# transform_tolerance: 0.2
transform_tolerance: 1.0
xy_goal_tolerance: 0.15
trans_stopped_velocity: 0.25
short_circuit_trajectory_evaluation: True
stateful: True
critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
BaseObstacle.scale: 0.02
PathAlign.scale: 32.0
PathAlign.forward_point_distance: 0.1
GoalAlign.scale: 24.0
GoalAlign.forward_point_distance: 0.1
PathDist.scale: 32.0
GoalDist.scale: 24.0
RotateToGoal.scale: 32.0
RotateToGoal.slowing_factor: 5.0
RotateToGoal.lookahead_time: -1.0
/**:
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: 2
height: 2
resolution: 0.05
# robot_radius: 0.20
footprint: "[[0.19, 0.13], [0.19, -0.13], [-0.19, -0.13], [-0.19, 0.13]]"
plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"]
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
inflation_radius: 0.5
cost_scaling_factor: 4.0
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"
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:
map_subscribe_transient_local: True
always_send_full_costmap: True
（4）behavior_server相关配置文件
在params目录下新建一个名为behavior.yaml的文件，并输入如下内容：
/**:
ros__parameters:
costmap_topic: local_costmap/costmap_raw
footprint_topic: local_costmap/published_footprint
cycle_frequency: 5.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
（5）waypoint_follower相关配置文件
在params目录下新建一个名为waypoint.yaml的文件，并输入如下内容：
/**:
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: 5000
（6）velocity_smoother相关配置文件
在params目录下新建一个名为velocity.yaml的文件，并输入如下内容：
/**:
ros__parameters:
use_sim_time: True
smoothing_frequency: 20.0
scale_velocities: False
feedback: "OPEN_LOOP"
max_velocity: [0.1, 0.0, 1.0]
min_velocity: [-0.1, 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
（7）smoother_server相关配置文件
在params目录下新建一个名为smootherr.yaml的文件，并输入如下内容：
/**:
ros__parameters:
costmap_topic: global_costmap/costmap_raw
footprint_topic: global_costmap/published_footprint
robot_base_frame: base_link
transform_timeout: 0.1
smoother_plugins: ["simple_smoother"]
simple_smoother:
plugin: "nav2_smoother::SimpleSmoother"
tolerance: 1.0e-10
do_refinement: True
3.launch集成
导航实现时，需要依赖于地图与定位功能，我们可以在一个launch文件中集成之前的定位launch以及当前编写的导航核心模块的launch，以简化导航功能的启动，在launch目录下新建一个名为bringup.launch.py的launch文件，并输入如下内容：
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
def generate_launch_description():
amcl_pkg = get_package_share_directory("mycar_localization")
nav2_pkg = get_package_share_directory("mycar_navigation2")
amcl_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(amcl_pkg,'launch',
'mycar_loca.launch.py'))
)
nav2_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(nav2_pkg,'launch',
'nav2.launch.py'))
)
ld = LaunchDescription()
ld.add_action(amcl_launch)
ld.add_action(nav2_launch)
return ld
4.编辑配置文件
打开CMakeLists.txt 并输入如下内容：
install(DIRECTORY launch params bts
DESTINATION share/${PROJECT_NAME}
)
5.编译
终端中进入当前工作空间，编译功能包：
colcon build --packages-select mycar_navigation2
6.执行
（1）请先调用如下指令启动仿真环境：
. install/setup.bash
ros2 launch stage_ros2 my_house.launch.py
（2）然后在终端下进入当前工作空间，输入如下指令启动导航功能：
. install/setup.bash
ros2 launch mycar_navigation2 bringup.launch.py
（3）启动rviz2，加载/opt/ros/humble/share/nav2_bringup/rviz下的nav2_default_view.rviz文件，为机器人设置初始位姿后，再通过菜单栏的Nav2 Goal设置目标点，机器人就可以自动导航至目标点了。
