Skip to content

copy of the default nav2 trees and infrastructure to modify them and … #13

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 1 commit into
base: fortress
Choose a base branch
from
Open

copy of the default nav2 trees and infrastructure to modify them and … #13

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
52 changes: 44 additions & 8 deletions launch/robot_navigation.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@

from launch_ros.actions import Node
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, ExecuteProcess, TimerAction
from launch.actions import IncludeLaunchDescription, TimerAction
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
Expand All @@ -13,6 +13,8 @@
from launch.actions import DeclareLaunchArgument
from launch.actions import GroupAction
from launch.conditions import LaunchConfigurationEquals
from nav2_common.launch import ReplaceString


def generate_launch_description():
use_sim_time = LaunchConfiguration('use_sim_time')
Expand All @@ -28,8 +30,19 @@ def generate_launch_description():
default_value="True",
description="Run using a premade map, no SLAM (True/False)",
)



nav_to_pose_tree_arg = DeclareLaunchArgument(
"nav_to_pose_tree",
default_value="navigate_to_pose_w_replanning_and_recovery.xml",
description="Which Nav2 navigate_to_pose BT to use",
)

nav_through_poses_tree_arg = DeclareLaunchArgument(
"nav_through_poses_tree",
default_value="navigate_through_poses_w_replanning_and_recovery.xml",
description="Which Nav2 navigate_through_poses BT to use",
)

# Get the package share directory
pkg_mirte_navigation = get_package_share_directory('mirte_navigation')

Expand All @@ -38,12 +51,33 @@ def generate_launch_description():
pkg_mirte_navigation,
'maps',
'robocup_map.yaml')
params_file = os.path.join(

nav2_params_file = os.path.join(
pkg_mirte_navigation,
'params',
'mirte_nav2_params.yaml')

trees_path = os.path.join(
get_package_share_directory("mirte_navigation"),
"trees"
)

rewritten_nav2_params_file = ReplaceString(
source_file=nav2_params_file,
replacements={
"<nav_to_pose_tree_path>": (
trees_path,
os.path.sep,
LaunchConfiguration("nav_to_pose_tree"),
),
"<nav_through_poses_tree_path>": (
trees_path,
os.path.sep,
LaunchConfiguration("nav_through_poses_tree"),
),
},
)

slam_params_file = os.path.join(
pkg_mirte_navigation,
'params',
Expand All @@ -64,7 +98,7 @@ def generate_launch_description():
])),
launch_arguments={
"map": map_file,
"params_file": params_file,
"params_file": rewritten_nav2_params_file,
'use_sim_time': use_sim_time,
}.items()
)
Expand All @@ -77,7 +111,7 @@ def generate_launch_description():
navigation_launch,
IncludeLaunchDescription(
AnyLaunchDescriptionSource(slam_tb_launch_path),
launch_arguments = {
launch_arguments={
'slam_params_file': slam_params_file,
'use_sim_time': use_sim_time,
}.items()
Expand All @@ -86,7 +120,6 @@ def generate_launch_description():
condition=LaunchConfigurationEquals("use_map", "False"),
)


nav_and_localization = GroupAction(
actions=[
localization_launch,
Expand All @@ -98,6 +131,7 @@ def generate_launch_description():
rviz_file = PathJoinSubstitution([
FindPackageShare("nav2_bringup"), "rviz", "nav2_default_view.rviz"
])

start_rviz_cmd = Node(
package="rviz2",
executable="rviz2",
Expand All @@ -120,6 +154,8 @@ def generate_launch_description():
return LaunchDescription([
use_map_arg,
use_sim_time_arg,
nav_to_pose_tree_arg,
nav_through_poses_tree_arg,
initial_pose_node,
nav_and_localization,
nav2_with_slam,
Expand Down
6 changes: 2 additions & 4 deletions params/mirte_nav2_params.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,10 +52,8 @@ bt_navigator:
plugin: "nav2_bt_navigator::NavigateToPoseNavigator"
navigate_through_poses:
plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator"
# '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.
default_nav_to_pose_bt_xml: "<nav_to_pose_tree_path>"
default_nav_through_poses_bt_xml: "<nav_through_poses_tree_path>"

# plugin_lib_names is used to add custom BT plugins to the executor (vector of strings).
# Built-in plugins are added automatically
Expand Down
7 changes: 2 additions & 5 deletions setup.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,8 @@
(os.path.join('share', package_name, 'launch'), glob('launch/*launch.py')),
(os.path.join('share', package_name, 'params'), glob('params/*')),
(os.path.join('share', package_name, 'maps'), glob('maps/*')),
(os.path.join('share', package_name, 'trees'), glob('trees/*')),

],
install_requires=['setuptools'],
zip_safe=True,
Expand All @@ -28,10 +30,5 @@
'console_scripts': [
'set_initial_pose = mirte_navigation.set_initial_pose: main',
],

},
)




38 changes: 38 additions & 0 deletions trees/navigate_through_poses_w_replanning_and_recovery.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,38 @@

<!--
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>
36 changes: 36 additions & 0 deletions trees/navigate_to_pose_w_replanning_and_recovery.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,36 @@

<!--
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>