diff --git a/.circleci/config.yml b/.circleci/config.yml index 87cf7be47e..5f6c56adfc 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -33,12 +33,12 @@ _commands: - restore_cache: name: Restore Cache << parameters.key >> keys: - - "<< parameters.key >>-v14\ + - "<< parameters.key >>-v15\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" - - "<< parameters.key >>-v14\ + - "<< parameters.key >>-v15\ -{{ arch }}\ -main\ -\ @@ -58,7 +58,7 @@ _commands: steps: - save_cache: name: Save Cache << parameters.key >> - key: "<< parameters.key >>-v14\ + key: "<< parameters.key >>-v15\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ diff --git a/nav2_bringup/launch/rviz_launch.py b/nav2_bringup/launch/rviz_launch.py index c87ae01b41..f6dd99cc97 100644 --- a/nav2_bringup/launch/rviz_launch.py +++ b/nav2_bringup/launch/rviz_launch.py @@ -34,6 +34,7 @@ def generate_launch_description(): namespace = LaunchConfiguration('namespace') use_namespace = LaunchConfiguration('use_namespace') rviz_config_file = LaunchConfiguration('rviz_config') + use_sim_time = LaunchConfiguration('use_sim_time') # Declare the launch arguments declare_namespace_cmd = DeclareLaunchArgument( @@ -52,9 +53,17 @@ def generate_launch_description(): default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'), description='Full path to the RVIZ config file to use') + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='true', + description='Use simulation (Gazebo) clock if true') + # Launch rviz start_rviz_cmd = Node( condition=UnlessCondition(use_namespace), + parameters=[{ + 'use_sim_time': use_sim_time, + }], package='rviz2', executable='rviz2', arguments=['-d', rviz_config_file], @@ -66,6 +75,9 @@ def generate_launch_description(): start_namespaced_rviz_cmd = Node( condition=IfCondition(use_namespace), + parameters=[{ + 'use_sim_time': use_sim_time, + }], package='rviz2', executable='rviz2', namespace=namespace, @@ -97,6 +109,7 @@ def generate_launch_description(): ld.add_action(declare_namespace_cmd) ld.add_action(declare_use_namespace_cmd) ld.add_action(declare_rviz_config_file_cmd) + ld.add_action(declare_use_sim_time_cmd) # Add any conditioned actions ld.add_action(start_rviz_cmd) diff --git a/nav2_simple_commander/launch/gz_assisted_teleop_example_launch.py b/nav2_simple_commander/launch/gz_assisted_teleop_example_launch.py new file mode 100644 index 0000000000..d815536fcf --- /dev/null +++ b/nav2_simple_commander/launch/gz_assisted_teleop_example_launch.py @@ -0,0 +1,207 @@ +# Copyright (c) 2021 Samsung Research America +# Copyright (c) 2022 Joshua Wallace +# +# 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 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +import xacro + + +def generate_launch_description(): + warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + python_commander_dir = get_package_share_directory('nav2_simple_commander') + + map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') + world = os.path.join(python_commander_dir, 'gz_warehouse.world') + + # Launch configuration variables + use_rviz = LaunchConfiguration('use_rviz') + headless = LaunchConfiguration('headless') + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # Declare the launch arguments + declare_use_rviz_cmd = DeclareLaunchArgument( + 'use_rviz', + default_value='True', + description='Whether to start RVIZ') + + declare_simulator_cmd = DeclareLaunchArgument( + 'headless', + default_value='False', + description='Whether to execute gzclient)') + + # Launch gazebo environment + start_gazebo_server_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-r -s ' + world]}.items(), + ) + + start_gazebo_client_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-g ' + world]}.items(), + condition=IfCondition(PythonExpression(['not ', headless])), + ) + + urdf = os.path.join(python_commander_dir, 'gz_tb3_waffle.urdf') + + doc = xacro.parse(open(urdf)) + xacro.process_doc(doc) + + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[{ + 'use_sim_time': True, + 'robot_description': doc.toxml() + }], + remappings=remappings + ) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + condition=IfCondition(use_rviz), + launch_arguments={'namespace': '', + 'use_namespace': 'False', + 'use_sim_time': 'true'}.items()) + + os.environ['IGN_GAZEBO_RESOURCE_PATH'] = os.path.join( + get_package_share_directory('turtlebot3_gazebo'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( + get_package_share_directory('aws_robomaker_small_warehouse_world'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( + get_package_share_directory('turtlebot3_gazebo'), '..') + + # Clock bridge + clock_bridge = Node( + package='ros_gz_bridge', executable='parameter_bridge', + name='clock_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[ + '/clock' + '@rosgraph_msgs/msg/Clock' + '[ignition.msgs.Clock'] + ) + + # lidar bridge + lidar_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='lidar_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[ + ['/scan' + '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan'] + ] + ) + + imu_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='imu_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[['/imu' + '@sensor_msgs/msg/Imu[ignition.msgs.IMU']], + ) + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file}.items()) + + start_gazebo_spawner_cmd = Node( + package='ros_gz_sim', + executable='create', + output='screen', + arguments=[ + '-entity', 'turtlebot3_waffle', + '-string', doc.toxml(), + '-x', '3.45', '-y', '2.15', '-z', '0.1', + '-R', '0', '-P', '0', '-Y', '3.14' + ] + ) + + load_joint_state_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'joint_state_broadcaster'], + output='screen' + ) + load_diffdrive_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'diffdrive_controller'], + output='screen' + ) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='example_assisted_teleop', + emulate_tty=True, + output='screen') + + ld = LaunchDescription() + ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_simulator_cmd) + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_gazebo_spawner_cmd) + ld.add_action(clock_bridge) + ld.add_action(lidar_bridge) + ld.add_action(imu_bridge) + ld.add_action(load_joint_state_controller) + ld.add_action(load_diffdrive_controller) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + return ld diff --git a/nav2_simple_commander/launch/gz_follow_path_example_launch.py b/nav2_simple_commander/launch/gz_follow_path_example_launch.py new file mode 100644 index 0000000000..0501a8e517 --- /dev/null +++ b/nav2_simple_commander/launch/gz_follow_path_example_launch.py @@ -0,0 +1,205 @@ +# Copyright (c) 2021 Samsung Research America +# +# 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 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +import xacro + + +def generate_launch_description(): + warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + python_commander_dir = get_package_share_directory('nav2_simple_commander') + + map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') + world = os.path.join(python_commander_dir, 'gz_warehouse.world') + + # Launch configuration variables + use_rviz = LaunchConfiguration('use_rviz') + headless = LaunchConfiguration('headless') + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # Declare the launch arguments + declare_use_rviz_cmd = DeclareLaunchArgument( + 'use_rviz', + default_value='True', + description='Whether to start RVIZ') + + declare_simulator_cmd = DeclareLaunchArgument( + 'headless', + default_value='False', + description='Whether to execute gzclient)') + + # Launch gazebo environment + start_gazebo_server_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-r -s ' + world]}.items(), + ) + + start_gazebo_client_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-g ' + world]}.items(), + condition=IfCondition(PythonExpression(['not ', headless])), + ) + + urdf = os.path.join(python_commander_dir, 'gz_tb3_waffle.urdf') + + doc = xacro.parse(open(urdf)) + xacro.process_doc(doc) + + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[{ + 'use_sim_time': True, + 'robot_description': doc.toxml() + }], + remappings=remappings + ) + + os.environ['IGN_GAZEBO_RESOURCE_PATH'] = os.path.join( + get_package_share_directory('turtlebot3_gazebo'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( + get_package_share_directory('aws_robomaker_small_warehouse_world'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( + get_package_share_directory('turtlebot3_gazebo'), '..') + + # Clock bridge + clock_bridge = Node( + package='ros_gz_bridge', executable='parameter_bridge', + name='clock_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[ + '/clock' + '@rosgraph_msgs/msg/Clock' + '[ignition.msgs.Clock'] + ) + + # lidar bridge + lidar_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='lidar_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[ + ['/scan' + '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan'] + ] + ) + + imu_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='imu_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[['/imu' + '@sensor_msgs/msg/Imu[ignition.msgs.IMU']], + ) + + start_gazebo_spawner_cmd = Node( + package='ros_gz_sim', + executable='create', + output='screen', + arguments=[ + '-entity', 'turtlebot3_waffle', + '-string', doc.toxml(), + '-x', '3.45', '-y', '2.15', '-z', '0.1', + '-R', '0', '-P', '0', '-Y', '3.14' + ] + ) + + load_joint_state_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'joint_state_broadcaster'], + output='screen' + ) + load_diffdrive_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'diffdrive_controller'], + output='screen' + ) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + condition=IfCondition(use_rviz), + launch_arguments={'namespace': '', + 'use_namespace': 'False'}.items()) + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file}.items()) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='example_follow_path', + emulate_tty=True, + output='screen') + + ld = LaunchDescription() + ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_simulator_cmd) + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_gazebo_spawner_cmd) + ld.add_action(clock_bridge) + ld.add_action(lidar_bridge) + ld.add_action(imu_bridge) + ld.add_action(load_joint_state_controller) + ld.add_action(load_diffdrive_controller) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + return ld diff --git a/nav2_simple_commander/launch/gz_inspection_demo_launch.py b/nav2_simple_commander/launch/gz_inspection_demo_launch.py new file mode 100644 index 0000000000..9be518d3b3 --- /dev/null +++ b/nav2_simple_commander/launch/gz_inspection_demo_launch.py @@ -0,0 +1,206 @@ +# Copyright (c) 2021 Samsung Research America +# +# 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 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +import xacro + + +def generate_launch_description(): + warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + python_commander_dir = get_package_share_directory('nav2_simple_commander') + + map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') + world = os.path.join(python_commander_dir, 'gz_warehouse.world') + + # Launch configuration variables + use_rviz = LaunchConfiguration('use_rviz') + headless = LaunchConfiguration('headless') + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # Declare the launch arguments + declare_use_rviz_cmd = DeclareLaunchArgument( + 'use_rviz', + default_value='True', + description='Whether to start RVIZ') + + declare_simulator_cmd = DeclareLaunchArgument( + 'headless', + default_value='False', + description='Whether to execute gzclient)') + + # Launch gazebo environment + start_gazebo_server_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-r -s ' + world]}.items(), + ) + + start_gazebo_client_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-g ' + world]}.items(), + condition=IfCondition(PythonExpression(['not ', headless])), + ) + + urdf = os.path.join(python_commander_dir, 'gz_tb3_waffle.urdf') + + doc = xacro.parse(open(urdf)) + xacro.process_doc(doc) + + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[{ + 'use_sim_time': True, + 'robot_description': doc.toxml() + }], + remappings=remappings + ) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + condition=IfCondition(use_rviz), + launch_arguments={'namespace': '', + 'use_namespace': 'False', + 'use_sim_time': 'true'}.items()) + + os.environ['IGN_GAZEBO_RESOURCE_PATH'] = os.path.join( + get_package_share_directory('turtlebot3_gazebo'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( + get_package_share_directory('aws_robomaker_small_warehouse_world'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( + get_package_share_directory('turtlebot3_gazebo'), '..') + + # Clock bridge + clock_bridge = Node( + package='ros_gz_bridge', executable='parameter_bridge', + name='clock_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[ + '/clock' + '@rosgraph_msgs/msg/Clock' + '[ignition.msgs.Clock'] + ) + + # lidar bridge + lidar_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='lidar_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[ + ['/scan' + '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan'] + ] + ) + + imu_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='imu_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[['/imu' + '@sensor_msgs/msg/Imu[ignition.msgs.IMU']], + ) + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file, 'use_sim_time': 'true'}.items()) + + start_gazebo_spawner_cmd = Node( + package='ros_gz_sim', + executable='create', + output='screen', + arguments=[ + '-entity', 'turtlebot3_waffle', + '-string', doc.toxml(), + '-x', '3.45', '-y', '2.15', '-z', '0.1', + '-R', '0', '-P', '0', '-Y', '3.14' + ] + ) + + load_joint_state_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'joint_state_broadcaster'], + output='screen' + ) + load_diffdrive_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'diffdrive_controller'], + output='screen' + ) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='demo_inspection', + emulate_tty=True, + output='screen') + + ld = LaunchDescription() + ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_simulator_cmd) + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_gazebo_spawner_cmd) + ld.add_action(clock_bridge) + ld.add_action(lidar_bridge) + ld.add_action(imu_bridge) + ld.add_action(load_joint_state_controller) + ld.add_action(load_diffdrive_controller) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + return ld diff --git a/nav2_simple_commander/launch/gz_nav_through_poses_example_launch.py b/nav2_simple_commander/launch/gz_nav_through_poses_example_launch.py new file mode 100644 index 0000000000..a880a2c053 --- /dev/null +++ b/nav2_simple_commander/launch/gz_nav_through_poses_example_launch.py @@ -0,0 +1,210 @@ +# Copyright 2023 Open Source Robotics Foundation, Inc. +# +# 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 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +import xacro + + +def generate_launch_description(): + warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + python_commander_dir = get_package_share_directory('nav2_simple_commander') + + map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') + world = os.path.join(python_commander_dir, 'gz_warehouse.world') + + # Launch configuration variables + use_rviz = LaunchConfiguration('use_rviz') + headless = LaunchConfiguration('headless') + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # Declare the launch arguments + declare_use_rviz_cmd = DeclareLaunchArgument( + 'use_rviz', + default_value='True', + description='Whether to start RVIZ') + + declare_simulator_cmd = DeclareLaunchArgument( + 'headless', + default_value='False', + description='Whether to execute gzclient)') + + # Launch gazebo environment + start_gazebo_server_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-r -s ' + world]}.items(), + ) + + start_gazebo_client_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-g ' + world]}.items(), + condition=IfCondition(PythonExpression(['not ', headless])), + ) + + urdf = os.path.join(python_commander_dir, 'gz_tb3_waffle.urdf') + + doc = xacro.parse(open(urdf)) + xacro.process_doc(doc) + + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[{ + 'use_sim_time': True, + 'robot_description': doc.toxml() + }], + remappings=remappings + ) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + condition=IfCondition(use_rviz), + launch_arguments={'namespace': '', + 'use_namespace': 'False', + 'use_sim_time': 'true'}.items()) + + os.environ['IGN_GAZEBO_RESOURCE_PATH'] = os.path.join( + get_package_share_directory('turtlebot3_gazebo'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( + get_package_share_directory('aws_robomaker_small_warehouse_world'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( + get_package_share_directory('turtlebot3_gazebo'), '..') + + # Clock bridge + clock_bridge = Node( + package='ros_gz_bridge', executable='parameter_bridge', + name='clock_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[ + '/clock' + '@rosgraph_msgs/msg/Clock' + '[ignition.msgs.Clock'] + ) + + # lidar bridge + lidar_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='lidar_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[ + ['/scan' + '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan'] + ] + ) + + imu_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='imu_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[['/imu' + '@sensor_msgs/msg/Imu[ignition.msgs.IMU']], + ) + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file, 'use_sim_time': 'true'}.items()) + + start_gazebo_spawner_cmd = Node( + package='ros_gz_sim', + executable='create', + output='screen', + arguments=[ + '-entity', 'turtlebot3_waffle', + '-string', doc.toxml(), + '-x', '3.45', '-y', '2.15', '-z', '0.1', + '-R', '0', '-P', '0', '-Y', '3.14' + ] + ) + + load_joint_state_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'joint_state_broadcaster'], + output='screen' + ) + load_diffdrive_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'diffdrive_controller'], + output='screen' + ) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='example_nav_through_poses', + emulate_tty=True, + parameters=[{ + 'use_sim_time': True + }], + output='screen') + + ld = LaunchDescription() + ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_simulator_cmd) + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_gazebo_spawner_cmd) + ld.add_action(clock_bridge) + ld.add_action(lidar_bridge) + ld.add_action(imu_bridge) + ld.add_action(load_joint_state_controller) + ld.add_action(load_diffdrive_controller) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + + return ld diff --git a/nav2_simple_commander/launch/gz_nav_to_pose_example_launch.py b/nav2_simple_commander/launch/gz_nav_to_pose_example_launch.py new file mode 100644 index 0000000000..bd0a547a4a --- /dev/null +++ b/nav2_simple_commander/launch/gz_nav_to_pose_example_launch.py @@ -0,0 +1,210 @@ +# Copyright 2023 Open Source Robotics Foundation, Inc. +# +# 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 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +import xacro + + +def generate_launch_description(): + warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + python_commander_dir = get_package_share_directory('nav2_simple_commander') + + map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') + world = os.path.join(python_commander_dir, 'gz_warehouse.world') + + # Launch configuration variables + use_rviz = LaunchConfiguration('use_rviz') + headless = LaunchConfiguration('headless') + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # Declare the launch arguments + declare_use_rviz_cmd = DeclareLaunchArgument( + 'use_rviz', + default_value='True', + description='Whether to start RVIZ') + + declare_simulator_cmd = DeclareLaunchArgument( + 'headless', + default_value='False', + description='Whether to execute gzclient)') + + # Launch gazebo environment + start_gazebo_server_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-r -s ' + world]}.items(), + ) + + start_gazebo_client_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-g ' + world]}.items(), + condition=IfCondition(PythonExpression(['not ', headless])), + ) + + urdf = os.path.join(python_commander_dir, 'gz_tb3_waffle.urdf') + + doc = xacro.parse(open(urdf)) + xacro.process_doc(doc) + + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[{ + 'use_sim_time': True, + 'robot_description': doc.toxml() + }], + remappings=remappings + ) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + condition=IfCondition(use_rviz), + launch_arguments={'namespace': '', + 'use_namespace': 'False', + 'use_sim_time': 'true'}.items()) + + os.environ['IGN_GAZEBO_RESOURCE_PATH'] = os.path.join( + get_package_share_directory('turtlebot3_gazebo'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( + get_package_share_directory('aws_robomaker_small_warehouse_world'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( + get_package_share_directory('turtlebot3_gazebo'), '..') + + # Clock bridge + clock_bridge = Node( + package='ros_gz_bridge', executable='parameter_bridge', + name='clock_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[ + '/clock' + '@rosgraph_msgs/msg/Clock' + '[ignition.msgs.Clock'] + ) + + # lidar bridge + lidar_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='lidar_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[ + ['/scan' + '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan'] + ] + ) + + imu_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='imu_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[['/imu' + '@sensor_msgs/msg/Imu[ignition.msgs.IMU']], + ) + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file, 'use_sim_time': 'true'}.items()) + + start_gazebo_spawner_cmd = Node( + package='ros_gz_sim', + executable='create', + output='screen', + arguments=[ + '-entity', 'turtlebot3_waffle', + '-string', doc.toxml(), + '-x', '3.45', '-y', '2.15', '-z', '0.1', + '-R', '0', '-P', '0', '-Y', '3.14' + ] + ) + + load_joint_state_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'joint_state_broadcaster'], + output='screen' + ) + load_diffdrive_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'diffdrive_controller'], + output='screen' + ) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='example_nav_to_pose', + emulate_tty=True, + parameters=[{ + 'use_sim_time': True + }], + output='screen') + + ld = LaunchDescription() + ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_simulator_cmd) + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_gazebo_spawner_cmd) + ld.add_action(clock_bridge) + ld.add_action(lidar_bridge) + ld.add_action(imu_bridge) + ld.add_action(load_joint_state_controller) + ld.add_action(load_diffdrive_controller) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + + return ld diff --git a/nav2_simple_commander/launch/gz_picking_demo_launch.py b/nav2_simple_commander/launch/gz_picking_demo_launch.py new file mode 100644 index 0000000000..fcb4611424 --- /dev/null +++ b/nav2_simple_commander/launch/gz_picking_demo_launch.py @@ -0,0 +1,206 @@ +# Copyright (c) 2021 Samsung Research America +# +# 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 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +import xacro + + +def generate_launch_description(): + warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + python_commander_dir = get_package_share_directory('nav2_simple_commander') + + map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') + world = os.path.join(python_commander_dir, 'gz_warehouse.world') + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # Launch configuration variables + use_rviz = LaunchConfiguration('use_rviz') + headless = LaunchConfiguration('headless') + + # Declare the launch arguments + declare_use_rviz_cmd = DeclareLaunchArgument( + 'use_rviz', + default_value='True', + description='Whether to start RVIZ') + + declare_simulator_cmd = DeclareLaunchArgument( + 'headless', + default_value='False', + description='Whether to execute gzclient)') + + # Launch gazebo environment + start_gazebo_server_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-r -s ' + world]}.items(), + ) + + start_gazebo_client_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-g ' + world]}.items(), + condition=IfCondition(PythonExpression(['not ', headless])), + ) + + urdf = os.path.join(python_commander_dir, 'gz_tb3_waffle.urdf') + + doc = xacro.parse(open(urdf)) + xacro.process_doc(doc) + + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[{ + 'use_sim_time': True, + 'robot_description': doc.toxml() + }], + remappings=remappings + ) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + condition=IfCondition(use_rviz), + launch_arguments={'namespace': '', + 'use_namespace': 'False', + 'use_sim_time': 'true'}.items()) + + os.environ['IGN_GAZEBO_RESOURCE_PATH'] = os.path.join( + get_package_share_directory('turtlebot3_gazebo'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( + get_package_share_directory('aws_robomaker_small_warehouse_world'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( + get_package_share_directory('turtlebot3_gazebo'), '..') + + # Clock bridge + clock_bridge = Node( + package='ros_gz_bridge', executable='parameter_bridge', + name='clock_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[ + '/clock' + '@rosgraph_msgs/msg/Clock' + '[ignition.msgs.Clock'] + ) + + # lidar bridge + lidar_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='lidar_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[ + ['/scan' + '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan'] + ] + ) + + imu_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='imu_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[['/imu' + '@sensor_msgs/msg/Imu[ignition.msgs.IMU']], + ) + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file}.items()) + + start_gazebo_spawner_cmd = Node( + package='ros_gz_sim', + executable='create', + output='screen', + arguments=[ + '-entity', 'turtlebot3_waffle', + '-string', doc.toxml(), + '-x', '3.45', '-y', '2.15', '-z', '0.1', + '-R', '0', '-P', '0', '-Y', '3.14' + ] + ) + + load_joint_state_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'joint_state_broadcaster'], + output='screen' + ) + load_diffdrive_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'diffdrive_controller'], + output='screen' + ) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='demo_picking', + emulate_tty=True, + output='screen') + + ld = LaunchDescription() + ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_simulator_cmd) + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_gazebo_spawner_cmd) + ld.add_action(clock_bridge) + ld.add_action(lidar_bridge) + ld.add_action(imu_bridge) + ld.add_action(load_joint_state_controller) + ld.add_action(load_diffdrive_controller) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + return ld diff --git a/nav2_simple_commander/launch/gz_recoveries_example_launch.py b/nav2_simple_commander/launch/gz_recoveries_example_launch.py new file mode 100644 index 0000000000..23c85849c0 --- /dev/null +++ b/nav2_simple_commander/launch/gz_recoveries_example_launch.py @@ -0,0 +1,205 @@ +# Copyright (c) 2021 Samsung Research America +# +# 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 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +import xacro + + +def generate_launch_description(): + warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + python_commander_dir = get_package_share_directory('nav2_simple_commander') + + map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') + world = os.path.join(python_commander_dir, 'gz_warehouse.world') + + # Launch configuration variables + use_rviz = LaunchConfiguration('use_rviz') + headless = LaunchConfiguration('headless') + + # Declare the launch arguments + declare_use_rviz_cmd = DeclareLaunchArgument( + 'use_rviz', + default_value='True', + description='Whether to start RVIZ') + + declare_simulator_cmd = DeclareLaunchArgument( + 'headless', + default_value='False', + description='Whether to execute gzclient)') + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # Launch gazebo environment + start_gazebo_server_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-r -s ' + world]}.items(), + ) + + start_gazebo_client_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-g ' + world]}.items(), + condition=IfCondition(PythonExpression(['not ', headless])), + ) + + urdf = os.path.join(python_commander_dir, 'gz_tb3_waffle.urdf') + + doc = xacro.parse(open(urdf)) + xacro.process_doc(doc) + + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[{ + 'use_sim_time': True, + 'robot_description': doc.toxml() + }], + remappings=remappings) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + condition=IfCondition(use_rviz), + launch_arguments={'namespace': '', + 'use_namespace': 'False', + 'use_sim_time': 'true'}.items()) + + os.environ['IGN_GAZEBO_RESOURCE_PATH'] = os.path.join( + get_package_share_directory('turtlebot3_gazebo'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( + get_package_share_directory('aws_robomaker_small_warehouse_world'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( + get_package_share_directory('turtlebot3_gazebo'), '..') + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file}.items()) + + # Clock bridge + clock_bridge = Node( + package='ros_gz_bridge', executable='parameter_bridge', + name='clock_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[ + '/clock' + '@rosgraph_msgs/msg/Clock' + '[ignition.msgs.Clock'] + ) + + # lidar bridge + lidar_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='lidar_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[ + ['/scan' + '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan'] + ] + ) + + imu_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='imu_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[['/imu' + '@sensor_msgs/msg/Imu[ignition.msgs.IMU']], + ) + + start_gazebo_spawner_cmd = Node( + package='ros_gz_sim', + executable='create', + output='screen', + arguments=[ + '-entity', 'turtlebot3_waffle', + '-string', doc.toxml(), + '-x', '3.45', '-y', '2.15', '-z', '0.1', + '-R', '0', '-P', '0', '-Y', '3.14' + ] + ) + + load_joint_state_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'joint_state_broadcaster'], + output='screen' + ) + load_diffdrive_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'diffdrive_controller'], + output='screen' + ) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='demo_recoveries', + emulate_tty=True, + output='screen') + + ld = LaunchDescription() + ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_simulator_cmd) + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_gazebo_spawner_cmd) + ld.add_action(clock_bridge) + ld.add_action(lidar_bridge) + ld.add_action(imu_bridge) + ld.add_action(load_joint_state_controller) + ld.add_action(load_diffdrive_controller) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + return ld diff --git a/nav2_simple_commander/launch/gz_security_demo_launch.py b/nav2_simple_commander/launch/gz_security_demo_launch.py new file mode 100644 index 0000000000..00c1c5661a --- /dev/null +++ b/nav2_simple_commander/launch/gz_security_demo_launch.py @@ -0,0 +1,206 @@ +# Copyright (c) 2021 Samsung Research America +# +# 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 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +import xacro + + +def generate_launch_description(): + warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + python_commander_dir = get_package_share_directory('nav2_simple_commander') + + map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') + world = os.path.join(python_commander_dir, 'gz_warehouse.world') + + # Launch configuration variables + use_rviz = LaunchConfiguration('use_rviz') + headless = LaunchConfiguration('headless') + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # Declare the launch arguments + declare_use_rviz_cmd = DeclareLaunchArgument( + 'use_rviz', + default_value='True', + description='Whether to start RVIZ') + + declare_simulator_cmd = DeclareLaunchArgument( + 'headless', + default_value='False', + description='Whether to execute gzclient)') + + # Launch gazebo environment + start_gazebo_server_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-r -s ' + world]}.items(), + ) + + start_gazebo_client_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-g ' + world]}.items(), + condition=IfCondition(PythonExpression(['not ', headless])), + ) + + urdf = os.path.join(python_commander_dir, 'gz_tb3_waffle.urdf') + + doc = xacro.parse(open(urdf)) + xacro.process_doc(doc) + + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[{ + 'use_sim_time': True, + 'robot_description': doc.toxml() + }], + remappings=remappings + ) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + condition=IfCondition(use_rviz), + launch_arguments={'namespace': '', + 'use_namespace': 'False', + 'use_sim_time': 'true'}.items()) + + os.environ['IGN_GAZEBO_RESOURCE_PATH'] = os.path.join( + get_package_share_directory('turtlebot3_gazebo'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( + get_package_share_directory('aws_robomaker_small_warehouse_world'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( + get_package_share_directory('turtlebot3_gazebo'), '..') + + # Clock bridge + clock_bridge = Node( + package='ros_gz_bridge', executable='parameter_bridge', + name='clock_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[ + '/clock' + '@rosgraph_msgs/msg/Clock' + '[ignition.msgs.Clock'] + ) + + # lidar bridge + lidar_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='lidar_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[ + ['/scan' + '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan'] + ] + ) + + imu_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='imu_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[['/imu' + '@sensor_msgs/msg/Imu[ignition.msgs.IMU']], + ) + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file, 'use_sim_time': 'true'}.items()) + + start_gazebo_spawner_cmd = Node( + package='ros_gz_sim', + executable='create', + output='screen', + arguments=[ + '-entity', 'turtlebot3_waffle', + '-string', doc.toxml(), + '-x', '3.45', '-y', '2.15', '-z', '0.1', + '-R', '0', '-P', '0', '-Y', '3.14' + ] + ) + + load_joint_state_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'joint_state_broadcaster'], + output='screen' + ) + load_diffdrive_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'diffdrive_controller'], + output='screen' + ) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='demo_security', + emulate_tty=True, + output='screen') + + ld = LaunchDescription() + ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_simulator_cmd) + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_gazebo_spawner_cmd) + ld.add_action(clock_bridge) + ld.add_action(lidar_bridge) + ld.add_action(imu_bridge) + ld.add_action(load_joint_state_controller) + ld.add_action(load_diffdrive_controller) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + return ld diff --git a/nav2_simple_commander/launch/gz_tb3_waffle.urdf b/nav2_simple_commander/launch/gz_tb3_waffle.urdf new file mode 100644 index 0000000000..2be185c222 --- /dev/null +++ b/nav2_simple_commander/launch/gz_tb3_waffle.urdf @@ -0,0 +1,486 @@ + + + + + + + + + + + + + + + 0.4 0.4 0.4 1 + 0.4 0.4 0.4 1 + 0.0 0.0 1.0 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.3 0.3 0.3 1 + 0.3 0.3 0.3 1 + 0.0 0.0 1.0 1 + + + + + + + + + + + + + + + + + + + + 100000.0 + 100000.0 + 0 0 0 + 0.0 + 0.0 + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + + + + + + + + + + + + + + + + + 0.3 0.3 0.3 1 + 0.3 0.3 0.3 1 + 0.0 0.0 1.0 1 + + + + + + + + + + + + + + + + + + + + + + 100000.0 + 100000.0 + 0 0 0 + 0.0 + 0.0 + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.3 0.3 0.3 1 + 0.3 0.3 0.3 1 + 0.0 0.0 1.0 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + " + scan + base_scan + -0.064 0 0.121 0 0 0 + 5 + + + + 360 + 1.000000 + 0.000000 + 6.280000 + + + + 0.120000 + 3.5 + 0.015000 + + + gaussian + 0.0 + 0.01 + + + 1 + true + + + + + + true + 200 + imu + imu_link + + + + + 0.0 + 2e-4 + + + + + 0.0 + 2e-4 + + + + + 0.0 + 2e-4 + + + + + + + 0.0 + 1.7e-2 + + + + + 0.0 + 1.7e-2 + + + + + 0.0 + 1.7e-2 + + + + + + + + + + ign_ros2_control/IgnitionSystem + + + + + + + + + + + + + + + + $(find nav2_bringup)/params/control.yaml + + ~/odom:=odom + /tf:=tf + /tf_static:=tf_static + /diagnostics:=diagnostics + /diffdrive_controller/cmd_vel_unstamped:=cmd_vel + + + + + true + true + false + false + true + true + true + + + + diff --git a/nav2_simple_commander/launch/gz_warehouse.world b/nav2_simple_commander/launch/gz_warehouse.world new file mode 100644 index 0000000000..7f50bd819d --- /dev/null +++ b/nav2_simple_commander/launch/gz_warehouse.world @@ -0,0 +1,206 @@ + + + + + + + + + ogre2 + + + + 0 0 -9.8 + + 0.001 + 1 + 1000.0 + + + quick + 150 + 0 + 1.400000 + 1 + + + + + + + model://aws_robomaker_warehouse_ShelfF_01 + + -5.795143 -0.956635 0 0 0 0 + + + + model://aws_robomaker_warehouse_WallB_01 + + 0.0 0.0 0 0 0 0 + + + + model://aws_robomaker_warehouse_ShelfE_01 + + 4.73156 0.57943 0 0 0 0 + + + + model://aws_robomaker_warehouse_ShelfE_01 + + 4.73156 -4.827049 0 0 0 0 + + + + model://aws_robomaker_warehouse_ShelfE_01 + + 4.73156 -8.6651 0 0 0 0 + + + + model://aws_robomaker_warehouse_ShelfD_01 + + 4.73156 -1.242668 0 0 0 0 + + + + model://aws_robomaker_warehouse_ShelfD_01 + + 4.73156 -3.038551 0 0 0 0 + + + + model://aws_robomaker_warehouse_ShelfD_01 + + 4.73156 -6.750542 0 0 0 0 + + + + model://aws_robomaker_warehouse_GroundB_01 + + 0.0 0.0 -0.090092 0 0 0 + + + + model://aws_robomaker_warehouse_Lamp_01 + + 0 0 -4 0 0 0 + + + + model://aws_robomaker_warehouse_Bucket_01 + + 0.433449 9.631706 0 0 0 -1.563161 + + + + model://aws_robomaker_warehouse_Bucket_01 + + -1.8321 -6.3752 0 0 0 -1.563161 + + + + model://aws_robomaker_warehouse_Bucket_01 + + 0.433449 8.59 0 0 0 -1.563161 + + + + model://aws_robomaker_warehouse_ClutteringA_01 + + 5.708138 8.616844 -0.017477 0 0 0 + + + + model://aws_robomaker_warehouse_ClutteringA_01 + + 3.408638 8.616844 -0.017477 0 0 0 + + + + model://aws_robomaker_warehouse_ClutteringA_01 + + -1.491287 5.222435 -0.017477 0 0 -1.583185 + + + + model://aws_robomaker_warehouse_ClutteringC_01 + + 3.324959 3.822449 -0.012064 0 0 1.563871 + + + + model://aws_robomaker_warehouse_ClutteringC_01 + + 5.54171 3.816475 -0.015663 0 0 -1.583191 + + + + model://aws_robomaker_warehouse_ClutteringC_01 + + 5.384239 6.137154 0 0 0 3.150000 + + + + model://aws_robomaker_warehouse_ClutteringC_01 + + 3.236 6.137154 0 0 0 3.150000 + + + + model://aws_robomaker_warehouse_ClutteringC_01 + + -1.573677 2.301994 -0.015663 0 0 -3.133191 + + + + model://aws_robomaker_warehouse_ClutteringC_01 + + -1.2196 9.407 -0.015663 0 0 1.563871 + + + + model://aws_robomaker_warehouse_ClutteringD_01 + + -1.634682 -7.811813 -0.319559 0 0 0 + + + + model://aws_robomaker_warehouse_TrashCanC_01 + + -1.592441 7.715420 0 0 0 0 + + + + model://aws_robomaker_warehouse_PalletJackB_01 + + -0.276098 -9.481944 0.023266 0 0 0 + + + + 0 0 8.5 0 0 0 + 0.5 0.5 0.5 1 + 0.2 0.2 0.2 1 + + 80 + 0.3 + 0.01 + 0.001 + + 1 + 0.1 0.1 -1 + + + diff --git a/nav2_simple_commander/launch/gz_waypoint_follower_example_launch.py b/nav2_simple_commander/launch/gz_waypoint_follower_example_launch.py new file mode 100644 index 0000000000..e83a4d4718 --- /dev/null +++ b/nav2_simple_commander/launch/gz_waypoint_follower_example_launch.py @@ -0,0 +1,210 @@ +# Copyright 2023 Open Source Robotics Foundation, Inc. +# +# 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 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +import xacro + + +def generate_launch_description(): + warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + python_commander_dir = get_package_share_directory('nav2_simple_commander') + + map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') + world = os.path.join(python_commander_dir, 'gz_warehouse.world') + + # Launch configuration variables + use_rviz = LaunchConfiguration('use_rviz') + headless = LaunchConfiguration('headless') + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # Declare the launch arguments + declare_use_rviz_cmd = DeclareLaunchArgument( + 'use_rviz', + default_value='True', + description='Whether to start RVIZ') + + declare_simulator_cmd = DeclareLaunchArgument( + 'headless', + default_value='False', + description='Whether to execute gzclient)') + + # Launch gazebo environment + start_gazebo_server_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-r -s ' + world]}.items(), + ) + + start_gazebo_client_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-g ' + world]}.items(), + condition=IfCondition(PythonExpression(['not ', headless])), + ) + + urdf = os.path.join(python_commander_dir, 'gz_tb3_waffle.urdf') + + doc = xacro.parse(open(urdf)) + xacro.process_doc(doc) + + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[{ + 'use_sim_time': True, + 'robot_description': doc.toxml() + }], + remappings=remappings + ) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + condition=IfCondition(use_rviz), + launch_arguments={'namespace': '', + 'use_namespace': 'False', + 'use_sim_time': 'true'}.items()) + + os.environ['IGN_GAZEBO_RESOURCE_PATH'] = os.path.join( + get_package_share_directory('turtlebot3_gazebo'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( + get_package_share_directory('aws_robomaker_small_warehouse_world'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( + get_package_share_directory('turtlebot3_gazebo'), '..') + + # Clock bridge + clock_bridge = Node( + package='ros_gz_bridge', executable='parameter_bridge', + name='clock_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[ + '/clock' + '@rosgraph_msgs/msg/Clock' + '[ignition.msgs.Clock'] + ) + + # lidar bridge + lidar_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='lidar_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[ + ['/scan' + '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan'] + ] + ) + + imu_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='imu_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[['/imu' + '@sensor_msgs/msg/Imu[ignition.msgs.IMU']], + ) + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file, 'use_sim_time': 'true'}.items()) + + start_gazebo_spawner_cmd = Node( + package='ros_gz_sim', + executable='create', + output='screen', + arguments=[ + '-entity', 'turtlebot3_waffle', + '-string', doc.toxml(), + '-x', '3.45', '-y', '2.15', '-z', '0.1', + '-R', '0', '-P', '0', '-Y', '3.14' + ] + ) + + load_joint_state_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'joint_state_broadcaster'], + output='screen' + ) + load_diffdrive_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'diffdrive_controller'], + output='screen' + ) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='example_waypoint_follower', + emulate_tty=True, + parameters=[{ + 'use_sim_time': True + }], + output='screen') + + ld = LaunchDescription() + ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_simulator_cmd) + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_gazebo_spawner_cmd) + ld.add_action(clock_bridge) + ld.add_action(lidar_bridge) + ld.add_action(imu_bridge) + ld.add_action(load_joint_state_controller) + ld.add_action(load_diffdrive_controller) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + + return ld