-
Notifications
You must be signed in to change notification settings - Fork 65
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Fleet navigation ROS 2 package and assets (#450)
Fleet navigation ROS2 package and assets --------- Signed-off-by: Piotr Jaroszek <[email protected]>
- Loading branch information
Showing
27 changed files
with
2,813 additions
and
35 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
1 change: 1 addition & 0 deletions
1
Templates/Ros2FleetRobotTemplate/Template/Examples/ros2_ws/src/o3de_fleet_nav/NOTICE
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1 @@ | ||
| This package contains a modified code from "nav2_bringup" package (https://github.com/ros-planning/navigation2). |
19 changes: 19 additions & 0 deletions
19
...2FleetRobotTemplate/Template/Examples/ros2_ws/src/o3de_fleet_nav/config/fleet_config.yaml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,19 @@ | ||
| fleet: | ||
| - robot_name: proteus | ||
| robot_namespace: robot1 | ||
| position: | ||
| x: -6.0 | ||
| y: 0.5 | ||
| z: 0.2 | ||
| - robot_name: proteus | ||
| robot_namespace: robot2 | ||
| position: | ||
| x: -6.0 | ||
| y: 7.5 | ||
| z: 0.2 | ||
| - robot_name: proteus | ||
| robot_namespace: robot3 | ||
| position: | ||
| x: -6.0 | ||
| y: -6.0 | ||
| z: 0.2 |
153 changes: 153 additions & 0 deletions
153
...obotTemplate/Template/Examples/ros2_ws/src/o3de_fleet_nav/launch/o3de_fleet_nav_launch.py
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,153 @@ | ||
| # Copyright (c) 2018 Intel Corporation | ||
| # Copyright (c) Contributors to the Open 3D Engine Project. | ||
| # | ||
| # Licensed under the Apache License, Version 2.0 (the "License"); | ||
| # you may not use this file except in compliance with the License. | ||
| # You may obtain a copy of the License at | ||
| # | ||
| # http://www.apache.org/licenses/LICENSE-2.0 | ||
| # | ||
| # Unless required by applicable law or agreed to in writing, software | ||
| # distributed under the License is distributed on an "AS IS" BASIS, | ||
| # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| # See the License for the specific language governing permissions and | ||
| # limitations under the License. | ||
|
|
||
| import os | ||
| import yaml | ||
|
|
||
| from ament_index_python.packages import get_package_share_directory | ||
|
|
||
| from launch import LaunchDescription | ||
| from launch.actions import (DeclareLaunchArgument, ExecuteProcess, GroupAction, | ||
| IncludeLaunchDescription, LogInfo) | ||
| from launch.conditions import IfCondition | ||
| from launch.launch_description_sources import PythonLaunchDescriptionSource | ||
| from launch.substitutions import LaunchConfiguration, TextSubstitution | ||
| from nav2_common.launch import ReplaceString | ||
|
|
||
|
|
||
| def generate_launch_description(): | ||
| # Get the launch directory | ||
| o3de_fleet_nav_dir = get_package_share_directory('o3de_fleet_nav') | ||
| o3de_launch_dir = os.path.join(o3de_fleet_nav_dir, 'launch') | ||
|
|
||
| # Load fleet configuration | ||
| fleet_config_file = os.path.join(o3de_fleet_nav_dir, 'config', 'fleet_config.yaml') | ||
| robots = [] | ||
| with open(fleet_config_file, 'r') as f: | ||
| configuration = yaml.safe_load(f) | ||
| for robot in configuration["fleet"]: | ||
| robots.append( | ||
| { | ||
| "name": robot["robot_name"], | ||
| "namespace": robot["robot_namespace"], | ||
| "x_pose": robot["position"]["x"], | ||
| "y_pose": robot["position"]["y"], | ||
| "z_pose": robot["position"]["z"] | ||
| } | ||
| ) | ||
|
|
||
| map_yaml_file = LaunchConfiguration('map') | ||
| autostart = LaunchConfiguration('autostart') | ||
| rviz_config_file = LaunchConfiguration('rviz_config') | ||
| use_rviz = LaunchConfiguration('use_rviz') | ||
| log_settings = LaunchConfiguration('log_settings', default='true') | ||
|
|
||
| distro = os.getenv('ROS_DISTRO') | ||
|
|
||
| # Declare the launch arguments | ||
| declare_map_yaml_cmd = DeclareLaunchArgument( | ||
| 'map', | ||
| default_value=os.path.join(o3de_fleet_nav_dir, 'maps', 'map_warehouse.yaml'), | ||
| description='Full path to map file to load') | ||
|
|
||
| declare_robot_params_file_cmd = DeclareLaunchArgument( | ||
| 'robot_params_file', | ||
| default_value=os.path.join(o3de_fleet_nav_dir, 'params', distro, 'nav2_multirobot_params.yaml'), | ||
| description='Full path to the ROS2 parameters file to use for all robot launched nodes') | ||
|
|
||
| declare_autostart_cmd = DeclareLaunchArgument( | ||
| 'autostart', default_value='True', | ||
| description='Automatically startup the stacks') | ||
|
|
||
| declare_rviz_config_file_cmd = DeclareLaunchArgument( | ||
| 'rviz_config', | ||
| default_value=os.path.join(o3de_fleet_nav_dir, 'rviz', 'nav2_namespaced_view.rviz'), | ||
| description='Full path to the RVIZ config file to use.') | ||
|
|
||
| declare_use_rviz_cmd = DeclareLaunchArgument( | ||
| 'use_rviz', | ||
| default_value='True', | ||
| description='Whether to start RVIZ') | ||
|
|
||
| # Launch navigation for each robot in fleet configuration | ||
| nav_instances_cmds = [] | ||
| for robot in robots: | ||
| params_file = LaunchConfiguration("robot_params_file") | ||
|
|
||
| configured_params = ReplaceString( | ||
| source_file=params_file, | ||
| replacements={ | ||
| '<robot_name>': robot['name'], | ||
| '<robot_namespace>': robot['namespace'], | ||
| '<robot_initial_pose_x>': str(robot['x_pose']), | ||
| '<robot_initial_pose_y>': str(robot['y_pose']), | ||
| '<robot_initial_pose_z>': str(robot['z_pose']) | ||
| }) | ||
|
|
||
|
|
||
| group = GroupAction([ | ||
| IncludeLaunchDescription( | ||
| PythonLaunchDescriptionSource( | ||
| os.path.join(o3de_launch_dir, 'o3de_rviz_launch.py')), | ||
| condition=IfCondition(use_rviz), | ||
| launch_arguments={ | ||
| 'namespace': TextSubstitution(text=robot['namespace']), | ||
| 'use_namespace': 'True', | ||
| 'rviz_config': rviz_config_file}.items()), | ||
|
|
||
| IncludeLaunchDescription( | ||
| PythonLaunchDescriptionSource(os.path.join(o3de_launch_dir, | ||
| 'o3de_nav_launch.py')), | ||
| launch_arguments={'namespace': robot['namespace'], | ||
| 'use_namespace': 'True', | ||
| 'map': map_yaml_file, | ||
| 'use_sim_time': 'True', | ||
| 'params_file': configured_params, | ||
| 'autostart': autostart}.items()), | ||
|
|
||
| LogInfo( | ||
| condition=IfCondition(log_settings), | ||
| msg=['Launching ', robot['namespace']]), | ||
| LogInfo( | ||
| condition=IfCondition(log_settings), | ||
| msg=[robot['namespace'], ' map yaml: ', map_yaml_file]), | ||
| LogInfo( | ||
| condition=IfCondition(log_settings), | ||
| msg=[robot['namespace'], ' params yaml: ', params_file]), | ||
| LogInfo( | ||
| condition=IfCondition(log_settings), | ||
| msg=[robot['namespace'], ' rviz config file: ', rviz_config_file]), | ||
| LogInfo( | ||
| condition=IfCondition(log_settings), | ||
| msg=[robot['namespace'], ' autostart: ', autostart]) | ||
| ]) | ||
|
|
||
| nav_instances_cmds.append(group) | ||
|
|
||
| # Create the launch description and populate | ||
| ld = LaunchDescription() | ||
|
|
||
| # Declare the launch options | ||
| ld.add_action(declare_map_yaml_cmd) | ||
| ld.add_action(declare_robot_params_file_cmd) | ||
| ld.add_action(declare_use_rviz_cmd) | ||
| ld.add_action(declare_autostart_cmd) | ||
| ld.add_action(declare_rviz_config_file_cmd) | ||
|
|
||
| # Launch robots | ||
| for simulation_instance_cmd in nav_instances_cmds: | ||
| ld.add_action(simulation_instance_cmd) | ||
|
|
||
| return ld |
Oops, something went wrong.