157 lines
4.8 KiB
Python
157 lines
4.8 KiB
Python
from launch import LaunchDescription
|
|
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
|
|
from launch.conditions import IfCondition, UnlessCondition
|
|
from launch.substitutions import LaunchConfiguration, AllSubstitution
|
|
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
|
from launch_ros.actions import Node
|
|
from launch_ros.substitutions import FindPackageShare
|
|
import os
|
|
|
|
def generate_launch_description():
|
|
nav_pkg_share = FindPackageShare("").find('toid_navigation')
|
|
control_pkg_share = FindPackageShare("").find('toid_control')
|
|
lidar_pkg_share = FindPackageShare("").find('toid_lidar')
|
|
params = os.path.join(nav_pkg_share, 'params', 'toid_general_params.yaml')
|
|
map = os.path.join(nav_pkg_share, 'maps', 'mapb2_5cm.yaml')
|
|
ctrl_launch_dir = os.path.join(control_pkg_share, 'launch')
|
|
default_rviz_config_path = os.path.join(nav_pkg_share, 'rviz', 'nav2.rviz')
|
|
bt_path = os.path.join(nav_pkg_share, "behaviors", "navigate_to_pose_w_backtracking.xml")
|
|
lattice_path = os.path.join(nav_pkg_share, "params", "output.json")
|
|
|
|
visualize = LaunchConfiguration("visualize")
|
|
visualize_arg = DeclareLaunchArgument(
|
|
'visualize',
|
|
default_value='False',
|
|
description="Whether to launch rviz2"
|
|
)
|
|
|
|
use_mock = LaunchConfiguration("use_mock")
|
|
use_mock_arg = DeclareLaunchArgument(
|
|
'use_mock',
|
|
default_value='True',
|
|
description="Whether to launch rviz2"
|
|
)
|
|
|
|
run_nodes = LaunchConfiguration("run_nodes")
|
|
run_nodes_arg = DeclareLaunchArgument(
|
|
'run_nodes',
|
|
default_value='True',
|
|
description="Whether to launch rviz2"
|
|
)
|
|
|
|
use_lidar = LaunchConfiguration("use_lidar")
|
|
use_lidar_arg = DeclareLaunchArgument(
|
|
'use_lidar',
|
|
default_value='True',
|
|
description="Whether to launch rviz2"
|
|
)
|
|
|
|
is_blue = LaunchConfiguration("is_blue")
|
|
is_blue_arg = DeclareLaunchArgument(
|
|
'is_blue',
|
|
default_value='True',
|
|
description="Whether to launch rviz2"
|
|
)
|
|
|
|
toid_control = IncludeLaunchDescription(
|
|
PythonLaunchDescriptionSource(
|
|
os.path.join(ctrl_launch_dir, 'toid.launch.py')
|
|
),
|
|
launch_arguments={
|
|
'visualize': 'False',
|
|
'use_mock': use_mock
|
|
}.items(),
|
|
condition=IfCondition(run_nodes),
|
|
)
|
|
|
|
toid_lidar = IncludeLaunchDescription(
|
|
PythonLaunchDescriptionSource(
|
|
os.path.join(lidar_pkg_share, 'launch' , 'launch.py')
|
|
),
|
|
launch_arguments={
|
|
'visualize': 'False',
|
|
'lidar_frame': 'lidar_frame',
|
|
}.items(),
|
|
condition=IfCondition(AllSubstitution(run_nodes, use_lidar)),
|
|
)
|
|
|
|
map_server = Node(
|
|
package='nav2_map_server',
|
|
executable='map_server',
|
|
name='map_server',
|
|
output='screen',
|
|
parameters=[{'yaml_filename': map}],
|
|
condition=IfCondition(run_nodes),
|
|
)
|
|
|
|
planner_server = Node(
|
|
package='nav2_planner',
|
|
executable='planner_server',
|
|
name='planner_server',
|
|
output='screen',
|
|
parameters=[{'GridBased':{'lattice_filepath': lattice_path}}, params],
|
|
condition=IfCondition(run_nodes),
|
|
)
|
|
|
|
controller_server = Node(
|
|
package='nav2_controller',
|
|
executable='controller_server',
|
|
name='controller_server',
|
|
output='screen',
|
|
parameters=[params],
|
|
condition=IfCondition(run_nodes),
|
|
)
|
|
|
|
bt_navigator = Node(
|
|
package='nav2_bt_navigator',
|
|
executable='bt_navigator',
|
|
name='bt_navigator',
|
|
output='screen',
|
|
parameters=[{'default_nav_to_pose_bt_xml': bt_path}, params],
|
|
condition=IfCondition(run_nodes),
|
|
)
|
|
|
|
# default_nav_to_pose_bt_xml: "$(find-pkg-share toid_navigation)/behaviors/navigate_to_pose_w_backtracking.xml"
|
|
|
|
behavior_server = Node(
|
|
package='nav2_behaviors',
|
|
executable='behavior_server',
|
|
name='behavior_server',
|
|
output='screen',
|
|
parameters=[params],
|
|
condition=IfCondition(run_nodes),
|
|
)
|
|
|
|
lifecycle_manager_node = Node(
|
|
package='nav2_lifecycle_manager',
|
|
executable='lifecycle_manager',
|
|
name='lifecycle_manager',
|
|
output='screen',
|
|
parameters=[params],
|
|
condition=IfCondition(run_nodes),
|
|
)
|
|
|
|
rviz_node = Node(
|
|
package='rviz2',
|
|
executable='rviz2',
|
|
name='rviz2',
|
|
output='screen',
|
|
arguments=['-d', default_rviz_config_path],
|
|
condition=IfCondition(visualize)
|
|
)
|
|
|
|
return LaunchDescription([
|
|
visualize_arg,
|
|
use_mock_arg,
|
|
run_nodes_arg,
|
|
use_lidar_arg,
|
|
toid_lidar,
|
|
rviz_node,
|
|
map_server,
|
|
bt_navigator,
|
|
behavior_server,
|
|
planner_server,
|
|
controller_server,
|
|
lifecycle_manager_node,
|
|
toid_control
|
|
]) |