From a0f1f0008ee9670b6dad5d41d753b38f2149f3d5 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 11 Jun 2024 10:25:07 -0700 Subject: [PATCH 1/4] upgrading simple commander to new API --- nav2_bringup/launch/tb4_simulation_launch.py | 1 + .../launch/assisted_teleop_example_launch.py | 82 ++- .../launch/follow_path_example_launch.py | 78 +- .../launch/inspection_demo_launch.py | 78 +- .../nav_through_poses_example_launch.py | 78 +- .../launch/nav_to_pose_example_launch.py | 78 +- .../launch/picking_demo_launch.py | 78 +- .../launch/recoveries_example_launch.py | 78 +- .../launch/security_demo_launch.py | 78 +- nav2_simple_commander/launch/warehouse.world | 683 ------------------ .../waypoint_follower_example_launch.py | 78 +- .../nav2_simple_commander/demo_inspection.py | 38 +- .../nav2_simple_commander/demo_picking.py | 38 +- .../nav2_simple_commander/demo_recoveries.py | 14 +- .../nav2_simple_commander/demo_security.py | 20 +- .../example_assisted_teleop.py | 8 +- .../example_follow_path.py | 12 +- .../example_nav_through_poses.py | 40 +- .../example_nav_to_pose.py | 16 +- .../example_waypoint_follower.py | 40 +- 20 files changed, 671 insertions(+), 945 deletions(-) delete mode 100644 nav2_simple_commander/launch/warehouse.world diff --git a/nav2_bringup/launch/tb4_simulation_launch.py b/nav2_bringup/launch/tb4_simulation_launch.py index 73ae1ac36c..df98cd930b 100644 --- a/nav2_bringup/launch/tb4_simulation_launch.py +++ b/nav2_bringup/launch/tb4_simulation_launch.py @@ -207,6 +207,7 @@ def generate_launch_description(): 'use_respawn': use_respawn, }.items(), ) + # The SDF file for the world is a xacro file because we wanted to # conditionally load the SceneBroadcaster plugin based on wheter we're # running in headless mode. But currently, the Gazebo command line doesn't diff --git a/nav2_simple_commander/launch/assisted_teleop_example_launch.py b/nav2_simple_commander/launch/assisted_teleop_example_launch.py index 3324b0ee4e..4c2443a6c4 100644 --- a/nav2_simple_commander/launch/assisted_teleop_example_launch.py +++ b/nav2_simple_commander/launch/assisted_teleop_example_launch.py @@ -14,28 +14,35 @@ # limitations under the License. import os +import tempfile +from pathlib import Path from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import ( + AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, + OpaqueFunction, + RegisterEventHandler, ) from launch.conditions import IfCondition +from launch.event_handlers import OnShutdown from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PythonExpression +from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node 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') + sim_dir = get_package_share_directory('nav2_minimal_tb4_sim') + desc_dir = get_package_share_directory('nav2_minimal_tb4_description') - map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') - world = os.path.join(python_commander_dir, 'warehouse.world') + robot_sdf = os.path.join(desc_dir, 'urdf', 'standard', 'turtlebot4.urdf.xacro') + world = os.path.join(sim_dir, 'worlds', 'depot.sdf') + map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'depot.yaml') # Launch configuration variables use_rviz = LaunchConfiguration('use_rviz') @@ -51,26 +58,54 @@ def generate_launch_description(): ) # start the simulation - start_gazebo_server_cmd = ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], - cwd=[warehouse_dir], - output='screen', + world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf') + world_sdf_xacro = ExecuteProcess( + cmd=['xacro', '-o', world_sdf, ['headless:=', headless], world]) + start_gazebo_server_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', + 'gz_sim.launch.py')), + launch_arguments={'gz_args': ['-r -s ', world_sdf]}.items()) + + remove_temp_sdf_file = RegisterEventHandler(event_handler=OnShutdown( + on_shutdown=[ + OpaqueFunction(function=lambda _: os.remove(world_sdf)) + ])) + + set_env_vars_resources = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + os.path.join(sim_dir, 'worlds')) + start_gazebo_client_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py') + ), + condition=IfCondition(PythonExpression( + ['not ', headless])), + launch_arguments={'gz_args': ['-v4 -g ']}.items(), ) - start_gazebo_client_cmd = ExecuteProcess( - condition=IfCondition(PythonExpression(['not ', headless])), - cmd=['gzclient'], - cwd=[warehouse_dir], - output='screen', - ) + spawn_robot_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(sim_dir, 'launch', 'spawn_tb4.launch.py')), + launch_arguments={'use_sim_time': 'True', + 'robot_sdf': robot_sdf, + 'x_pose': '-8.0', + 'y_pose': '0.0', + 'z_pose': '0.0', + 'roll': '0.0', + 'pitch': '0.0', + 'yaw': '0.0'}.items()) - urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') start_robot_state_publisher_cmd = Node( package='robot_state_publisher', executable='robot_state_publisher', name='robot_state_publisher', output='screen', - arguments=[urdf], + parameters=[ + {'use_sim_time': True, 'robot_description': Command(['xacro', ' ', robot_sdf])} + ] ) # start the visualization @@ -87,7 +122,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource( os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py') ), - launch_arguments={'map': map_yaml_file}.items(), + launch_arguments={'map': map_yaml_file, 'use_sim_time': 'True'}.items(), ) # start the demo autonomy task @@ -98,11 +133,20 @@ def generate_launch_description(): output='screen', ) + set_env_vars_resources2 = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + str(Path(os.path.join(desc_dir)).parent.resolve())) + ld = LaunchDescription() - ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_use_rviz_cmd) ld.add_action(declare_simulator_cmd) + ld.add_action(world_sdf_xacro) + ld.add_action(remove_temp_sdf_file) + ld.add_action(set_env_vars_resources) + ld.add_action(set_env_vars_resources2) ld.add_action(start_gazebo_server_cmd) ld.add_action(start_gazebo_client_cmd) + ld.add_action(spawn_robot_cmd) ld.add_action(start_robot_state_publisher_cmd) ld.add_action(rviz_cmd) ld.add_action(bringup_cmd) diff --git a/nav2_simple_commander/launch/follow_path_example_launch.py b/nav2_simple_commander/launch/follow_path_example_launch.py index b4fe9b586f..248a9869dd 100644 --- a/nav2_simple_commander/launch/follow_path_example_launch.py +++ b/nav2_simple_commander/launch/follow_path_example_launch.py @@ -13,28 +13,35 @@ # limitations under the License. import os +import tempfile +from pathlib import Path from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import ( + AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, + OpaqueFunction, + RegisterEventHandler, ) from launch.conditions import IfCondition +from launch.event_handlers import OnShutdown from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PythonExpression +from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node 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') + sim_dir = get_package_share_directory('nav2_minimal_tb4_sim') + desc_dir = get_package_share_directory('nav2_minimal_tb4_description') - map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') - world = os.path.join(python_commander_dir, 'warehouse.world') + robot_sdf = os.path.join(desc_dir, 'urdf', 'standard', 'turtlebot4.urdf.xacro') + world = os.path.join(sim_dir, 'worlds', 'depot.sdf') + map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'depot.yaml') # Launch configuration variables use_rviz = LaunchConfiguration('use_rviz') @@ -50,26 +57,54 @@ def generate_launch_description(): ) # start the simulation - start_gazebo_server_cmd = ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], - cwd=[warehouse_dir], - output='screen', + world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf') + world_sdf_xacro = ExecuteProcess( + cmd=['xacro', '-o', world_sdf, ['headless:=', headless], world]) + start_gazebo_server_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', + 'gz_sim.launch.py')), + launch_arguments={'gz_args': ['-r -s ', world_sdf]}.items()) + + remove_temp_sdf_file = RegisterEventHandler(event_handler=OnShutdown( + on_shutdown=[ + OpaqueFunction(function=lambda _: os.remove(world_sdf)) + ])) + + set_env_vars_resources = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + os.path.join(sim_dir, 'worlds')) + start_gazebo_client_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py') + ), + condition=IfCondition(PythonExpression( + ['not ', headless])), + launch_arguments={'gz_args': ['-v4 -g ']}.items(), ) - start_gazebo_client_cmd = ExecuteProcess( - condition=IfCondition(PythonExpression(['not ', headless])), - cmd=['gzclient'], - cwd=[warehouse_dir], - output='screen', - ) + spawn_robot_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(sim_dir, 'launch', 'spawn_tb4.launch.py')), + launch_arguments={'use_sim_time': 'True', + 'robot_sdf': robot_sdf, + 'x_pose': '-8.0', + 'y_pose': '0.0', + 'z_pose': '0.0', + 'roll': '0.0', + 'pitch': '0.0', + 'yaw': '0.0'}.items()) - urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') start_robot_state_publisher_cmd = Node( package='robot_state_publisher', executable='robot_state_publisher', name='robot_state_publisher', output='screen', - arguments=[urdf], + parameters=[ + {'use_sim_time': True, 'robot_description': Command(['xacro', ' ', robot_sdf])} + ] ) # start the visualization @@ -97,11 +132,20 @@ def generate_launch_description(): output='screen', ) + set_env_vars_resources2 = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + str(Path(os.path.join(desc_dir)).parent.resolve())) + ld = LaunchDescription() ld.add_action(declare_use_rviz_cmd) ld.add_action(declare_simulator_cmd) + ld.add_action(world_sdf_xacro) + ld.add_action(remove_temp_sdf_file) + ld.add_action(set_env_vars_resources) + ld.add_action(set_env_vars_resources2) ld.add_action(start_gazebo_server_cmd) ld.add_action(start_gazebo_client_cmd) + ld.add_action(spawn_robot_cmd) ld.add_action(start_robot_state_publisher_cmd) ld.add_action(rviz_cmd) ld.add_action(bringup_cmd) diff --git a/nav2_simple_commander/launch/inspection_demo_launch.py b/nav2_simple_commander/launch/inspection_demo_launch.py index 285f26099e..f01e96c7f2 100644 --- a/nav2_simple_commander/launch/inspection_demo_launch.py +++ b/nav2_simple_commander/launch/inspection_demo_launch.py @@ -13,28 +13,35 @@ # limitations under the License. import os +import tempfile +from pathlib import Path from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import ( + AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, + OpaqueFunction, + RegisterEventHandler, ) from launch.conditions import IfCondition +from launch.event_handlers import OnShutdown from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PythonExpression +from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node 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') + sim_dir = get_package_share_directory('nav2_minimal_tb4_sim') + desc_dir = get_package_share_directory('nav2_minimal_tb4_description') - map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') - world = os.path.join(python_commander_dir, 'warehouse.world') + robot_sdf = os.path.join(desc_dir, 'urdf', 'standard', 'turtlebot4.urdf.xacro') + world = os.path.join(sim_dir, 'worlds', 'depot.sdf') + map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'depot.yaml') # Launch configuration variables use_rviz = LaunchConfiguration('use_rviz') @@ -50,26 +57,54 @@ def generate_launch_description(): ) # start the simulation - start_gazebo_server_cmd = ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], - cwd=[warehouse_dir], - output='screen', + world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf') + world_sdf_xacro = ExecuteProcess( + cmd=['xacro', '-o', world_sdf, ['headless:=', headless], world]) + start_gazebo_server_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', + 'gz_sim.launch.py')), + launch_arguments={'gz_args': ['-r -s ', world_sdf]}.items()) + + remove_temp_sdf_file = RegisterEventHandler(event_handler=OnShutdown( + on_shutdown=[ + OpaqueFunction(function=lambda _: os.remove(world_sdf)) + ])) + + set_env_vars_resources = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + os.path.join(sim_dir, 'worlds')) + start_gazebo_client_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py') + ), + condition=IfCondition(PythonExpression( + ['not ', headless])), + launch_arguments={'gz_args': ['-v4 -g ']}.items(), ) - start_gazebo_client_cmd = ExecuteProcess( - condition=IfCondition(PythonExpression(['not ', headless])), - cmd=['gzclient'], - cwd=[warehouse_dir], - output='screen', - ) + spawn_robot_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(sim_dir, 'launch', 'spawn_tb4.launch.py')), + launch_arguments={'use_sim_time': 'True', + 'robot_sdf': robot_sdf, + 'x_pose': '-8.0', + 'y_pose': '0.0', + 'z_pose': '0.0', + 'roll': '0.0', + 'pitch': '0.0', + 'yaw': '0.0'}.items()) - urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') start_robot_state_publisher_cmd = Node( package='robot_state_publisher', executable='robot_state_publisher', name='robot_state_publisher', output='screen', - arguments=[urdf], + parameters=[ + {'use_sim_time': True, 'robot_description': Command(['xacro', ' ', robot_sdf])} + ] ) # start the visualization @@ -97,11 +132,20 @@ def generate_launch_description(): output='screen', ) + set_env_vars_resources2 = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + str(Path(os.path.join(desc_dir)).parent.resolve())) + ld = LaunchDescription() ld.add_action(declare_use_rviz_cmd) ld.add_action(declare_simulator_cmd) + ld.add_action(world_sdf_xacro) + ld.add_action(remove_temp_sdf_file) + ld.add_action(set_env_vars_resources) + ld.add_action(set_env_vars_resources2) ld.add_action(start_gazebo_server_cmd) ld.add_action(start_gazebo_client_cmd) + ld.add_action(spawn_robot_cmd) ld.add_action(start_robot_state_publisher_cmd) ld.add_action(rviz_cmd) ld.add_action(bringup_cmd) diff --git a/nav2_simple_commander/launch/nav_through_poses_example_launch.py b/nav2_simple_commander/launch/nav_through_poses_example_launch.py index e8b0fefba0..6f7035c736 100644 --- a/nav2_simple_commander/launch/nav_through_poses_example_launch.py +++ b/nav2_simple_commander/launch/nav_through_poses_example_launch.py @@ -13,28 +13,35 @@ # limitations under the License. import os +import tempfile +from pathlib import Path from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import ( + AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, + OpaqueFunction, + RegisterEventHandler, ) from launch.conditions import IfCondition +from launch.event_handlers import OnShutdown from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PythonExpression +from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node 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') + sim_dir = get_package_share_directory('nav2_minimal_tb4_sim') + desc_dir = get_package_share_directory('nav2_minimal_tb4_description') - map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') - world = os.path.join(python_commander_dir, 'warehouse.world') + robot_sdf = os.path.join(desc_dir, 'urdf', 'standard', 'turtlebot4.urdf.xacro') + world = os.path.join(sim_dir, 'worlds', 'depot.sdf') + map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'depot.yaml') # Launch configuration variables use_rviz = LaunchConfiguration('use_rviz') @@ -50,26 +57,54 @@ def generate_launch_description(): ) # start the simulation - start_gazebo_server_cmd = ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], - cwd=[warehouse_dir], - output='screen', + world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf') + world_sdf_xacro = ExecuteProcess( + cmd=['xacro', '-o', world_sdf, ['headless:=', headless], world]) + start_gazebo_server_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', + 'gz_sim.launch.py')), + launch_arguments={'gz_args': ['-r -s ', world_sdf]}.items()) + + remove_temp_sdf_file = RegisterEventHandler(event_handler=OnShutdown( + on_shutdown=[ + OpaqueFunction(function=lambda _: os.remove(world_sdf)) + ])) + + set_env_vars_resources = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + os.path.join(sim_dir, 'worlds')) + start_gazebo_client_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py') + ), + condition=IfCondition(PythonExpression( + ['not ', headless])), + launch_arguments={'gz_args': ['-v4 -g ']}.items(), ) - start_gazebo_client_cmd = ExecuteProcess( - condition=IfCondition(PythonExpression(['not ', headless])), - cmd=['gzclient'], - cwd=[warehouse_dir], - output='screen', - ) + spawn_robot_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(sim_dir, 'launch', 'spawn_tb4.launch.py')), + launch_arguments={'use_sim_time': 'True', + 'robot_sdf': robot_sdf, + 'x_pose': '-8.0', + 'y_pose': '0.0', + 'z_pose': '0.0', + 'roll': '0.0', + 'pitch': '0.0', + 'yaw': '0.0'}.items()) - urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') start_robot_state_publisher_cmd = Node( package='robot_state_publisher', executable='robot_state_publisher', name='robot_state_publisher', output='screen', - arguments=[urdf], + parameters=[ + {'use_sim_time': True, 'robot_description': Command(['xacro', ' ', robot_sdf])} + ] ) # start the visualization @@ -97,11 +132,20 @@ def generate_launch_description(): output='screen', ) + set_env_vars_resources2 = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + str(Path(os.path.join(desc_dir)).parent.resolve())) + ld = LaunchDescription() ld.add_action(declare_use_rviz_cmd) ld.add_action(declare_simulator_cmd) + ld.add_action(world_sdf_xacro) + ld.add_action(remove_temp_sdf_file) + ld.add_action(set_env_vars_resources) + ld.add_action(set_env_vars_resources2) ld.add_action(start_gazebo_server_cmd) ld.add_action(start_gazebo_client_cmd) + ld.add_action(spawn_robot_cmd) ld.add_action(start_robot_state_publisher_cmd) ld.add_action(rviz_cmd) ld.add_action(bringup_cmd) diff --git a/nav2_simple_commander/launch/nav_to_pose_example_launch.py b/nav2_simple_commander/launch/nav_to_pose_example_launch.py index 29aeddebd1..b0a5dbfcac 100644 --- a/nav2_simple_commander/launch/nav_to_pose_example_launch.py +++ b/nav2_simple_commander/launch/nav_to_pose_example_launch.py @@ -13,28 +13,35 @@ # limitations under the License. import os +import tempfile +from pathlib import Path from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import ( + AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, + OpaqueFunction, + RegisterEventHandler, ) from launch.conditions import IfCondition +from launch.event_handlers import OnShutdown from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PythonExpression +from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node 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') + sim_dir = get_package_share_directory('nav2_minimal_tb4_sim') + desc_dir = get_package_share_directory('nav2_minimal_tb4_description') - map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') - world = os.path.join(python_commander_dir, 'warehouse.world') + robot_sdf = os.path.join(desc_dir, 'urdf', 'standard', 'turtlebot4.urdf.xacro') + world = os.path.join(sim_dir, 'worlds', 'depot.sdf') + map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'depot.yaml') # Launch configuration variables use_rviz = LaunchConfiguration('use_rviz') @@ -50,26 +57,54 @@ def generate_launch_description(): ) # start the simulation - start_gazebo_server_cmd = ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], - cwd=[warehouse_dir], - output='screen', + world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf') + world_sdf_xacro = ExecuteProcess( + cmd=['xacro', '-o', world_sdf, ['headless:=', headless], world]) + start_gazebo_server_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', + 'gz_sim.launch.py')), + launch_arguments={'gz_args': ['-r -s ', world_sdf]}.items()) + + remove_temp_sdf_file = RegisterEventHandler(event_handler=OnShutdown( + on_shutdown=[ + OpaqueFunction(function=lambda _: os.remove(world_sdf)) + ])) + + set_env_vars_resources = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + os.path.join(sim_dir, 'worlds')) + start_gazebo_client_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py') + ), + condition=IfCondition(PythonExpression( + ['not ', headless])), + launch_arguments={'gz_args': ['-v4 -g ']}.items(), ) - start_gazebo_client_cmd = ExecuteProcess( - condition=IfCondition(PythonExpression(['not ', headless])), - cmd=['gzclient'], - cwd=[warehouse_dir], - output='screen', - ) + spawn_robot_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(sim_dir, 'launch', 'spawn_tb4.launch.py')), + launch_arguments={'use_sim_time': 'True', + 'robot_sdf': robot_sdf, + 'x_pose': '-8.0', + 'y_pose': '0.0', + 'z_pose': '0.0', + 'roll': '0.0', + 'pitch': '0.0', + 'yaw': '0.0'}.items()) - urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') start_robot_state_publisher_cmd = Node( package='robot_state_publisher', executable='robot_state_publisher', name='robot_state_publisher', output='screen', - arguments=[urdf], + parameters=[ + {'use_sim_time': True, 'robot_description': Command(['xacro', ' ', robot_sdf])} + ] ) # start the visualization @@ -97,11 +132,20 @@ def generate_launch_description(): output='screen', ) + set_env_vars_resources2 = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + str(Path(os.path.join(desc_dir)).parent.resolve())) + ld = LaunchDescription() ld.add_action(declare_use_rviz_cmd) ld.add_action(declare_simulator_cmd) + ld.add_action(world_sdf_xacro) + ld.add_action(remove_temp_sdf_file) + ld.add_action(set_env_vars_resources) + ld.add_action(set_env_vars_resources2) ld.add_action(start_gazebo_server_cmd) ld.add_action(start_gazebo_client_cmd) + ld.add_action(spawn_robot_cmd) ld.add_action(start_robot_state_publisher_cmd) ld.add_action(rviz_cmd) ld.add_action(bringup_cmd) diff --git a/nav2_simple_commander/launch/picking_demo_launch.py b/nav2_simple_commander/launch/picking_demo_launch.py index 8505b6bbc3..08b5ff8169 100644 --- a/nav2_simple_commander/launch/picking_demo_launch.py +++ b/nav2_simple_commander/launch/picking_demo_launch.py @@ -13,28 +13,35 @@ # limitations under the License. import os +import tempfile +from pathlib import Path from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import ( + AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, + OpaqueFunction, + RegisterEventHandler, ) from launch.conditions import IfCondition +from launch.event_handlers import OnShutdown from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PythonExpression +from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node 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') + sim_dir = get_package_share_directory('nav2_minimal_tb4_sim') + desc_dir = get_package_share_directory('nav2_minimal_tb4_description') - map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') - world = os.path.join(python_commander_dir, 'warehouse.world') + robot_sdf = os.path.join(desc_dir, 'urdf', 'standard', 'turtlebot4.urdf.xacro') + world = os.path.join(sim_dir, 'worlds', 'depot.sdf') + map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'depot.yaml') # Launch configuration variables use_rviz = LaunchConfiguration('use_rviz') @@ -50,26 +57,54 @@ def generate_launch_description(): ) # start the simulation - start_gazebo_server_cmd = ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], - cwd=[warehouse_dir], - output='screen', + world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf') + world_sdf_xacro = ExecuteProcess( + cmd=['xacro', '-o', world_sdf, ['headless:=', headless], world]) + start_gazebo_server_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', + 'gz_sim.launch.py')), + launch_arguments={'gz_args': ['-r -s ', world_sdf]}.items()) + + remove_temp_sdf_file = RegisterEventHandler(event_handler=OnShutdown( + on_shutdown=[ + OpaqueFunction(function=lambda _: os.remove(world_sdf)) + ])) + + set_env_vars_resources = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + os.path.join(sim_dir, 'worlds')) + start_gazebo_client_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py') + ), + condition=IfCondition(PythonExpression( + ['not ', headless])), + launch_arguments={'gz_args': ['-v4 -g ']}.items(), ) - start_gazebo_client_cmd = ExecuteProcess( - condition=IfCondition(PythonExpression(['not ', headless])), - cmd=['gzclient'], - cwd=[warehouse_dir], - output='screen', - ) + spawn_robot_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(sim_dir, 'launch', 'spawn_tb4.launch.py')), + launch_arguments={'use_sim_time': 'True', + 'robot_sdf': robot_sdf, + 'x_pose': '-8.0', + 'y_pose': '0.0', + 'z_pose': '0.0', + 'roll': '0.0', + 'pitch': '0.0', + 'yaw': '0.0'}.items()) - urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') start_robot_state_publisher_cmd = Node( package='robot_state_publisher', executable='robot_state_publisher', name='robot_state_publisher', output='screen', - arguments=[urdf], + parameters=[ + {'use_sim_time': True, 'robot_description': Command(['xacro', ' ', robot_sdf])} + ] ) # start the visualization @@ -97,11 +132,20 @@ def generate_launch_description(): output='screen', ) + set_env_vars_resources2 = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + str(Path(os.path.join(desc_dir)).parent.resolve())) + ld = LaunchDescription() ld.add_action(declare_use_rviz_cmd) ld.add_action(declare_simulator_cmd) + ld.add_action(world_sdf_xacro) + ld.add_action(remove_temp_sdf_file) + ld.add_action(set_env_vars_resources) + ld.add_action(set_env_vars_resources2) ld.add_action(start_gazebo_server_cmd) ld.add_action(start_gazebo_client_cmd) + ld.add_action(spawn_robot_cmd) ld.add_action(start_robot_state_publisher_cmd) ld.add_action(rviz_cmd) ld.add_action(bringup_cmd) diff --git a/nav2_simple_commander/launch/recoveries_example_launch.py b/nav2_simple_commander/launch/recoveries_example_launch.py index 638550c44e..0d054eb144 100644 --- a/nav2_simple_commander/launch/recoveries_example_launch.py +++ b/nav2_simple_commander/launch/recoveries_example_launch.py @@ -13,28 +13,35 @@ # limitations under the License. import os +import tempfile +from pathlib import Path from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import ( + AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, + OpaqueFunction, + RegisterEventHandler, ) from launch.conditions import IfCondition +from launch.event_handlers import OnShutdown from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PythonExpression +from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node 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') + sim_dir = get_package_share_directory('nav2_minimal_tb4_sim') + desc_dir = get_package_share_directory('nav2_minimal_tb4_description') - map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') - world = os.path.join(python_commander_dir, 'warehouse.world') + robot_sdf = os.path.join(desc_dir, 'urdf', 'standard', 'turtlebot4.urdf.xacro') + world = os.path.join(sim_dir, 'worlds', 'depot.sdf') + map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'depot.yaml') # Launch configuration variables use_rviz = LaunchConfiguration('use_rviz') @@ -50,26 +57,54 @@ def generate_launch_description(): ) # start the simulation - start_gazebo_server_cmd = ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], - cwd=[warehouse_dir], - output='screen', + world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf') + world_sdf_xacro = ExecuteProcess( + cmd=['xacro', '-o', world_sdf, ['headless:=', headless], world]) + start_gazebo_server_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', + 'gz_sim.launch.py')), + launch_arguments={'gz_args': ['-r -s ', world_sdf]}.items()) + + remove_temp_sdf_file = RegisterEventHandler(event_handler=OnShutdown( + on_shutdown=[ + OpaqueFunction(function=lambda _: os.remove(world_sdf)) + ])) + + set_env_vars_resources = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + os.path.join(sim_dir, 'worlds')) + start_gazebo_client_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py') + ), + condition=IfCondition(PythonExpression( + ['not ', headless])), + launch_arguments={'gz_args': ['-v4 -g ']}.items(), ) - start_gazebo_client_cmd = ExecuteProcess( - condition=IfCondition(PythonExpression(['not ', headless])), - cmd=['gzclient'], - cwd=[warehouse_dir], - output='screen', - ) + spawn_robot_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(sim_dir, 'launch', 'spawn_tb4.launch.py')), + launch_arguments={'use_sim_time': 'True', + 'robot_sdf': robot_sdf, + 'x_pose': '-8.0', + 'y_pose': '0.0', + 'z_pose': '0.0', + 'roll': '0.0', + 'pitch': '0.0', + 'yaw': '0.0'}.items()) - urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') start_robot_state_publisher_cmd = Node( package='robot_state_publisher', executable='robot_state_publisher', name='robot_state_publisher', output='screen', - arguments=[urdf], + parameters=[ + {'use_sim_time': True, 'robot_description': Command(['xacro', ' ', robot_sdf])} + ] ) # start the visualization @@ -97,11 +132,20 @@ def generate_launch_description(): output='screen', ) + set_env_vars_resources2 = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + str(Path(os.path.join(desc_dir)).parent.resolve())) + ld = LaunchDescription() ld.add_action(declare_use_rviz_cmd) ld.add_action(declare_simulator_cmd) + ld.add_action(world_sdf_xacro) + ld.add_action(remove_temp_sdf_file) + ld.add_action(set_env_vars_resources) + ld.add_action(set_env_vars_resources2) ld.add_action(start_gazebo_server_cmd) ld.add_action(start_gazebo_client_cmd) + ld.add_action(spawn_robot_cmd) ld.add_action(start_robot_state_publisher_cmd) ld.add_action(rviz_cmd) ld.add_action(bringup_cmd) diff --git a/nav2_simple_commander/launch/security_demo_launch.py b/nav2_simple_commander/launch/security_demo_launch.py index a958a75f73..513e054ed5 100644 --- a/nav2_simple_commander/launch/security_demo_launch.py +++ b/nav2_simple_commander/launch/security_demo_launch.py @@ -13,28 +13,35 @@ # limitations under the License. import os +import tempfile +from pathlib import Path from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import ( + AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, + OpaqueFunction, + RegisterEventHandler, ) from launch.conditions import IfCondition +from launch.event_handlers import OnShutdown from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PythonExpression +from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node 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') + sim_dir = get_package_share_directory('nav2_minimal_tb4_sim') + desc_dir = get_package_share_directory('nav2_minimal_tb4_description') - map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') - world = os.path.join(python_commander_dir, 'warehouse.world') + robot_sdf = os.path.join(desc_dir, 'urdf', 'standard', 'turtlebot4.urdf.xacro') + world = os.path.join(sim_dir, 'worlds', 'depot.sdf') + map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'depot.yaml') # Launch configuration variables use_rviz = LaunchConfiguration('use_rviz') @@ -50,26 +57,54 @@ def generate_launch_description(): ) # start the simulation - start_gazebo_server_cmd = ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], - cwd=[warehouse_dir], - output='screen', + world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf') + world_sdf_xacro = ExecuteProcess( + cmd=['xacro', '-o', world_sdf, ['headless:=', headless], world]) + start_gazebo_server_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', + 'gz_sim.launch.py')), + launch_arguments={'gz_args': ['-r -s ', world_sdf]}.items()) + + remove_temp_sdf_file = RegisterEventHandler(event_handler=OnShutdown( + on_shutdown=[ + OpaqueFunction(function=lambda _: os.remove(world_sdf)) + ])) + + set_env_vars_resources = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + os.path.join(sim_dir, 'worlds')) + start_gazebo_client_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py') + ), + condition=IfCondition(PythonExpression( + ['not ', headless])), + launch_arguments={'gz_args': ['-v4 -g ']}.items(), ) - start_gazebo_client_cmd = ExecuteProcess( - condition=IfCondition(PythonExpression(['not ', headless])), - cmd=['gzclient'], - cwd=[warehouse_dir], - output='screen', - ) + spawn_robot_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(sim_dir, 'launch', 'spawn_tb4.launch.py')), + launch_arguments={'use_sim_time': 'True', + 'robot_sdf': robot_sdf, + 'x_pose': '-8.0', + 'y_pose': '0.0', + 'z_pose': '0.0', + 'roll': '0.0', + 'pitch': '0.0', + 'yaw': '0.0'}.items()) - urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') start_robot_state_publisher_cmd = Node( package='robot_state_publisher', executable='robot_state_publisher', name='robot_state_publisher', output='screen', - arguments=[urdf], + parameters=[ + {'use_sim_time': True, 'robot_description': Command(['xacro', ' ', robot_sdf])} + ] ) # start the visualization @@ -97,11 +132,20 @@ def generate_launch_description(): output='screen', ) + set_env_vars_resources2 = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + str(Path(os.path.join(desc_dir)).parent.resolve())) + ld = LaunchDescription() ld.add_action(declare_use_rviz_cmd) ld.add_action(declare_simulator_cmd) + ld.add_action(world_sdf_xacro) + ld.add_action(remove_temp_sdf_file) + ld.add_action(set_env_vars_resources) + ld.add_action(set_env_vars_resources2) ld.add_action(start_gazebo_server_cmd) ld.add_action(start_gazebo_client_cmd) + ld.add_action(spawn_robot_cmd) ld.add_action(start_robot_state_publisher_cmd) ld.add_action(rviz_cmd) ld.add_action(bringup_cmd) diff --git a/nav2_simple_commander/launch/warehouse.world b/nav2_simple_commander/launch/warehouse.world deleted file mode 100644 index de6d635d0f..0000000000 --- a/nav2_simple_commander/launch/warehouse.world +++ /dev/null @@ -1,683 +0,0 @@ - - - - - 0 0 -9.8 - - 0.001 - 1 - 1000 - - - - - - - - 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 9 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 - - - - - -4.70385 10.895 16.2659 -0 0.921795 -1.12701 - orbit - perspective - - - - - 3.45 2.15 0.01 0.0 0.0 3.14 - - - - - - - -0.064 0 0.048 0 0 0 - - 0.001 - 0.000 - 0.000 - 0.001 - 0.000 - 0.001 - - 1.0 - - - - -0.064 0 0.048 0 0 0 - - - 0.265 0.265 0.089 - - - - - - -0.064 0 0 0 0 0 - - - model://turtlebot3_common/meshes/waffle_base.dae - 0.001 0.001 0.001 - - - - - - - - true - 200 - - - - - 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 - - - - - - false - - - ~/out:=imu - - - - - - - - -0.052 0 0.111 0 0 0 - - 0.001 - 0.000 - 0.000 - 0.001 - 0.000 - 0.001 - - 0.125 - - - - -0.052 0 0.111 0 0 0 - - - 0.0508 - 0.055 - - - - - - -0.064 0 0.121 0 0 0 - - - model://turtlebot3_common/meshes/lds.dae - 0.001 0.001 0.001 - - - - - - true - true - -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 - - - - - - ~/out:=scan - - sensor_msgs/LaserScan - base_scan - - - - - - - - 0.0 0.144 0.023 -1.57 0 0 - - 0.001 - 0.000 - 0.000 - 0.001 - 0.000 - 0.001 - - 0.1 - - - - 0.0 0.144 0.023 -1.57 0 0 - - - 0.033 - 0.018 - - - - - - - 100000.0 - 100000.0 - 0 0 0 - 0.0 - 0.0 - - - - - 0 - 0.2 - 1e+5 - 1 - 0.01 - 0.001 - - - - - - - 0.0 0.144 0.023 0 0 0 - - - model://turtlebot3_common/meshes/tire.dae - 0.001 0.001 0.001 - - - - - - - - - 0.0 -0.144 0.023 -1.57 0 0 - - 0.001 - 0.000 - 0.000 - 0.001 - 0.000 - 0.001 - - 0.1 - - - - 0.0 -0.144 0.023 -1.57 0 0 - - - 0.033 - 0.018 - - - - - - - 100000.0 - 100000.0 - 0 0 0 - 0.0 - 0.0 - - - - - 0 - 0.2 - 1e+5 - 1 - 0.01 - 0.001 - - - - - - - 0.0 -0.144 0.023 0 0 0 - - - model://turtlebot3_common/meshes/tire.dae - 0.001 0.001 0.001 - - - - - - - -0.177 -0.064 -0.004 0 0 0 - - 0.001 - - 0.00001 - 0.000 - 0.000 - 0.00001 - 0.000 - 0.00001 - - - - - - 0.005000 - - - - - - 0 - 0.2 - 1e+5 - 1 - 0.01 - 0.001 - - - - - - - - -0.177 0.064 -0.004 0 0 0 - - 0.001 - - 0.00001 - 0.000 - 0.000 - 0.00001 - 0.000 - 0.00001 - - - - - - 0.005000 - - - - - - 0 - 0.2 - 1e+5 - 1 - 0.01 - 0.001 - - - - - - - - base_footprint - base_link - 0.0 0.0 0.010 0 0 0 - - - - base_link - wheel_left_link - 0.0 0.144 0.023 -1.57 0 0 - - 0 0 1 - - - - - base_link - wheel_right_link - 0.0 -0.144 0.023 -1.57 0 0 - - 0 0 1 - - - - - base_link - caster_back_right_link - - - - base_link - caster_back_left_link - - - - base_link - base_scan - -0.064 0 0.121 0 0 0 - - 0 0 1 - - - - - - - - /tf:=tf - - - 30 - - - wheel_left_joint - wheel_right_joint - - - 0.287 - 0.066 - - - 20 - 1.0 - - cmd_vel - - - true - true - false - - odom - odom - base_footprint - - - - - - - ~/out:=joint_states - - 30 - wheel_left_joint - wheel_right_joint - - - - - - diff --git a/nav2_simple_commander/launch/waypoint_follower_example_launch.py b/nav2_simple_commander/launch/waypoint_follower_example_launch.py index f164035e3b..5de0a5f04c 100644 --- a/nav2_simple_commander/launch/waypoint_follower_example_launch.py +++ b/nav2_simple_commander/launch/waypoint_follower_example_launch.py @@ -13,28 +13,35 @@ # limitations under the License. import os +import tempfile +from pathlib import Path from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import ( + AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, + OpaqueFunction, + RegisterEventHandler, ) from launch.conditions import IfCondition +from launch.event_handlers import OnShutdown from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PythonExpression +from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node 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') + sim_dir = get_package_share_directory('nav2_minimal_tb4_sim') + desc_dir = get_package_share_directory('nav2_minimal_tb4_description') - map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') - world = os.path.join(python_commander_dir, 'warehouse.world') + robot_sdf = os.path.join(desc_dir, 'urdf', 'standard', 'turtlebot4.urdf.xacro') + world = os.path.join(sim_dir, 'worlds', 'depot.sdf') + map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'depot.yaml') # Launch configuration variables use_rviz = LaunchConfiguration('use_rviz') @@ -50,26 +57,54 @@ def generate_launch_description(): ) # start the simulation - start_gazebo_server_cmd = ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], - cwd=[warehouse_dir], - output='screen', + world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf') + world_sdf_xacro = ExecuteProcess( + cmd=['xacro', '-o', world_sdf, ['headless:=', headless], world]) + start_gazebo_server_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', + 'gz_sim.launch.py')), + launch_arguments={'gz_args': ['-r -s ', world_sdf]}.items()) + + remove_temp_sdf_file = RegisterEventHandler(event_handler=OnShutdown( + on_shutdown=[ + OpaqueFunction(function=lambda _: os.remove(world_sdf)) + ])) + + set_env_vars_resources = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + os.path.join(sim_dir, 'worlds')) + start_gazebo_client_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py') + ), + condition=IfCondition(PythonExpression( + ['not ', headless])), + launch_arguments={'gz_args': ['-v4 -g ']}.items(), ) - start_gazebo_client_cmd = ExecuteProcess( - condition=IfCondition(PythonExpression(['not ', headless])), - cmd=['gzclient'], - cwd=[warehouse_dir], - output='screen', - ) + spawn_robot_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(sim_dir, 'launch', 'spawn_tb4.launch.py')), + launch_arguments={'use_sim_time': 'True', + 'robot_sdf': robot_sdf, + 'x_pose': '-8.0', + 'y_pose': '0.0', + 'z_pose': '0.0', + 'roll': '0.0', + 'pitch': '0.0', + 'yaw': '0.0'}.items()) - urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') start_robot_state_publisher_cmd = Node( package='robot_state_publisher', executable='robot_state_publisher', name='robot_state_publisher', output='screen', - arguments=[urdf], + parameters=[ + {'use_sim_time': True, 'robot_description': Command(['xacro', ' ', robot_sdf])} + ] ) # start the visualization @@ -97,11 +132,20 @@ def generate_launch_description(): output='screen', ) + set_env_vars_resources2 = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + str(Path(os.path.join(desc_dir)).parent.resolve())) + ld = LaunchDescription() ld.add_action(declare_use_rviz_cmd) ld.add_action(declare_simulator_cmd) + ld.add_action(world_sdf_xacro) + ld.add_action(remove_temp_sdf_file) + ld.add_action(set_env_vars_resources) + ld.add_action(set_env_vars_resources2) ld.add_action(start_gazebo_server_cmd) ld.add_action(start_gazebo_client_cmd) + ld.add_action(spawn_robot_cmd) ld.add_action(start_robot_state_publisher_cmd) ld.add_action(rviz_cmd) ld.add_action(bringup_cmd) diff --git a/nav2_simple_commander/nav2_simple_commander/demo_inspection.py b/nav2_simple_commander/nav2_simple_commander/demo_inspection.py index 5f6d04769a..8e5ff07394 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_inspection.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_inspection.py @@ -32,26 +32,28 @@ def main(): navigator = BasicNavigator() # Inspection route, probably read in from a file for a real application - # from either a map or drive and repeat. + # from either a map or drive and repeat. (x, y, yaw) inspection_route = [ - [3.461, -0.450], - [5.531, -0.450], - [3.461, -2.200], - [5.531, -2.200], - [3.661, -4.121], - [5.431, -4.121], - [3.661, -5.850], - [5.431, -5.800], + [8.21, 1.3, 1.57], + [8.7, 4.0, 1.57], + [11.6, 4.0, -1.57], + [11.6, 1.3, -1.57], + [12.6, 1.3, 1.57], + [12.6, 4.0, -1.57], + [15.27, 1.3, -1.57], + [16.4, 1.3, 1.57], + [19.0, 1.3, 1.57], + [19.0, 4.0, 1.57], ] # Set our demo's initial pose initial_pose = PoseStamped() initial_pose.header.frame_id = 'map' initial_pose.header.stamp = navigator.get_clock().now().to_msg() - initial_pose.pose.position.x = 3.45 - initial_pose.pose.position.y = 2.15 - initial_pose.pose.orientation.z = 1.0 - initial_pose.pose.orientation.w = 0.0 + initial_pose.pose.position.x = 0.0 + initial_pose.pose.position.y = 0.0 + initial_pose.pose.orientation.z = 0.0 + initial_pose.pose.orientation.w = 1.0 navigator.setInitialPose(initial_pose) # Wait for navigation to fully activate @@ -62,12 +64,18 @@ def main(): inspection_pose = PoseStamped() inspection_pose.header.frame_id = 'map' inspection_pose.header.stamp = navigator.get_clock().now().to_msg() - inspection_pose.pose.orientation.z = 1.0 - inspection_pose.pose.orientation.w = 0.0 for pt in inspection_route: inspection_pose.pose.position.x = pt[0] inspection_pose.pose.position.y = pt[1] + # Simplification of angle handling for demonstration purposes + if pt[2] > 0: + inspection_pose.pose.orientation.z = 0.707 + inspection_pose.pose.orientation.w = 0.707 + else: + inspection_pose.pose.orientation.z = -0.707 + inspection_pose.pose.orientation.w = 0.707 inspection_points.append(deepcopy(inspection_pose)) + navigator.followWaypoints(inspection_points) # Do something during our route (e.x. AI to analyze stock information or upload to the cloud) diff --git a/nav2_simple_commander/nav2_simple_commander/demo_picking.py b/nav2_simple_commander/nav2_simple_commander/demo_picking.py index 48c0551c2c..7a6a6010c5 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_picking.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_picking.py @@ -22,18 +22,17 @@ # Shelf positions for picking shelf_positions = { - 'shelf_A': [-3.829, -7.604], - 'shelf_B': [-3.791, -3.287], - 'shelf_C': [-3.791, 1.254], - 'shelf_D': [-3.24, 5.861], + 'shelf_A': [8.21, 1.3, 1.57], + 'shelf_B': [11.6, 1.3, -1.57], + 'shelf_C': [12.6, 4.0, -1.57], + 'shelf_D': [19.0, 4.0, 1.57], } # Shipping destination for picked products shipping_destinations = { - 'recycling': [-0.205, 7.403], - 'pallet_jack7': [-0.073, -8.497], - 'conveyer_432': [6.217, 2.153], - 'frieght_bay_3': [-6.349, 9.147], + 'frieght_bay_1': [-6.0, 5.0, 3.14], + 'frieght_bay_2': [-6.0, 0.0, 3.14], + 'frieght_bay_3': [-6.0, -5.0, 3.14], } """ @@ -47,10 +46,10 @@ def main(): # Recieved virtual request for picking item at Shelf A and bring to # worker at the pallet jack 7 for shipping. This request would - # contain the shelf ID ('shelf_A') and shipping destination ('pallet_jack7') + # contain the shelf ID ('shelf_A') and shipping destination ('frieght_bay_3') #################### - request_item_location = 'shelf_C' - request_destination = 'pallet_jack7' + request_item_location = 'shelf_A' + request_destination = 'frieght_bay_3' #################### rclpy.init() @@ -61,10 +60,10 @@ def main(): initial_pose = PoseStamped() initial_pose.header.frame_id = 'map' initial_pose.header.stamp = navigator.get_clock().now().to_msg() - initial_pose.pose.position.x = 3.45 - initial_pose.pose.position.y = 2.15 - initial_pose.pose.orientation.z = 1.0 - initial_pose.pose.orientation.w = 0.0 + initial_pose.pose.position.x = 0.0 + initial_pose.pose.position.y = 0.0 + initial_pose.pose.orientation.z = 0.0 + initial_pose.pose.orientation.w = 1.0 navigator.setInitialPose(initial_pose) # Wait for navigation to fully activate @@ -75,8 +74,13 @@ def main(): shelf_item_pose.header.stamp = navigator.get_clock().now().to_msg() shelf_item_pose.pose.position.x = shelf_positions[request_item_location][0] shelf_item_pose.pose.position.y = shelf_positions[request_item_location][1] - shelf_item_pose.pose.orientation.z = 1.0 - shelf_item_pose.pose.orientation.w = 0.0 + # Simplification of angle handling for demonstration purposes + if shelf_positions[request_item_location][2] > 0: + shelf_item_pose.pose.orientation.z = 0.707 + shelf_item_pose.pose.orientation.w = 0.707 + else: + shelf_item_pose.pose.orientation.z = -0.707 + shelf_item_pose.pose.orientation.w = 0.707 print(f'Received request for item picking at {request_item_location}.') navigator.goToPose(shelf_item_pose) diff --git a/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py b/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py index 0313f956ff..5d6aea783b 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py @@ -35,10 +35,10 @@ def main(): initial_pose = PoseStamped() initial_pose.header.frame_id = 'map' initial_pose.header.stamp = navigator.get_clock().now().to_msg() - initial_pose.pose.position.x = 3.45 - initial_pose.pose.position.y = 2.15 - initial_pose.pose.orientation.z = 1.0 - initial_pose.pose.orientation.w = 0.0 + initial_pose.pose.position.x = 0.0 + initial_pose.pose.position.y = 0.0 + initial_pose.pose.orientation.z = 0.0 + initial_pose.pose.orientation.w = 1.0 navigator.setInitialPose(initial_pose) # Wait for navigation to fully activate @@ -47,8 +47,8 @@ def main(): goal_pose = PoseStamped() goal_pose.header.frame_id = 'map' goal_pose.header.stamp = navigator.get_clock().now().to_msg() - goal_pose.pose.position.x = 6.13 - goal_pose.pose.position.y = 1.90 + goal_pose.pose.position.x = 8.21 + goal_pose.pose.position.y = 1.3 goal_pose.pose.orientation.w = 1.0 navigator.goToPose(goal_pose) @@ -64,7 +64,7 @@ def main(): ) # Robot hit a dead end, back it up - print('Robot hit a dead end, backing up...') + print('Robot hit a dead end (lets pretend), backing up...') navigator.backup(backup_dist=0.5, backup_speed=0.1) i = 0 diff --git a/nav2_simple_commander/nav2_simple_commander/demo_security.py b/nav2_simple_commander/nav2_simple_commander/demo_security.py index 24cc05feff..38cc2653c8 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_security.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_security.py @@ -37,23 +37,21 @@ def main(): # Security route, probably read in from a file for a real application # from either a map or drive and repeat. security_route = [ - [1.792, 2.144], - [1.792, -5.44], - [1.792, -9.427], - [-3.665, -9.427], - [-3.665, -4.303], - [-3.665, 2.330], - [-3.665, 9.283], + [10.15, -0.77], + [17.86, -0.77], + [21.58, -3.5], + [19.4, -6.4], + [-6.0, 5.0], ] # Set our demo's initial pose initial_pose = PoseStamped() initial_pose.header.frame_id = 'map' initial_pose.header.stamp = navigator.get_clock().now().to_msg() - initial_pose.pose.position.x = 3.45 - initial_pose.pose.position.y = 2.15 - initial_pose.pose.orientation.z = 1.0 - initial_pose.pose.orientation.w = 0.0 + initial_pose.pose.position.x = 0.0 + initial_pose.pose.position.y = 0.0 + initial_pose.pose.orientation.z = 0.0 + initial_pose.pose.orientation.w = 1.0 navigator.setInitialPose(initial_pose) # Wait for navigation to fully activate diff --git a/nav2_simple_commander/nav2_simple_commander/example_assisted_teleop.py b/nav2_simple_commander/nav2_simple_commander/example_assisted_teleop.py index 564342e541..0c6a0f58ca 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_assisted_teleop.py +++ b/nav2_simple_commander/nav2_simple_commander/example_assisted_teleop.py @@ -33,10 +33,10 @@ def main(): initial_pose = PoseStamped() initial_pose.header.frame_id = 'map' initial_pose.header.stamp = navigator.get_clock().now().to_msg() - initial_pose.pose.position.x = 3.45 - initial_pose.pose.position.y = 2.15 - initial_pose.pose.orientation.z = 1.0 - initial_pose.pose.orientation.w = 0.0 + initial_pose.pose.position.x = 0.0 + initial_pose.pose.position.y = 0.0 + initial_pose.pose.orientation.z = 0.0 + initial_pose.pose.orientation.w = 1.0 navigator.setInitialPose(initial_pose) # Wait for navigation to fully activate, since autostarting nav2 diff --git a/nav2_simple_commander/nav2_simple_commander/example_follow_path.py b/nav2_simple_commander/nav2_simple_commander/example_follow_path.py index 104083f34a..713857eed5 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_follow_path.py +++ b/nav2_simple_commander/nav2_simple_commander/example_follow_path.py @@ -32,10 +32,10 @@ def main(): initial_pose = PoseStamped() initial_pose.header.frame_id = 'map' initial_pose.header.stamp = navigator.get_clock().now().to_msg() - initial_pose.pose.position.x = 3.45 - initial_pose.pose.position.y = 2.15 - initial_pose.pose.orientation.z = 1.0 - initial_pose.pose.orientation.w = 0.0 + initial_pose.pose.position.x = 0.0 + initial_pose.pose.position.y = 0.0 + initial_pose.pose.orientation.z = 0.0 + initial_pose.pose.orientation.w = 1.0 navigator.setInitialPose(initial_pose) # Wait for navigation to fully activate, since autostarting nav2 @@ -45,8 +45,8 @@ def main(): goal_pose = PoseStamped() goal_pose.header.frame_id = 'map' goal_pose.header.stamp = navigator.get_clock().now().to_msg() - goal_pose.pose.position.x = -3.0 - goal_pose.pose.position.y = -2.0 + goal_pose.pose.position.x = 7.0 + goal_pose.pose.position.y = 0.5 goal_pose.pose.orientation.w = 1.0 # Get the path, smooth it diff --git a/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py b/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py index 66023ec11a..0388522eb8 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py +++ b/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py @@ -32,10 +32,10 @@ def main(): initial_pose = PoseStamped() initial_pose.header.frame_id = 'map' initial_pose.header.stamp = navigator.get_clock().now().to_msg() - initial_pose.pose.position.x = 3.45 - initial_pose.pose.position.y = 2.15 - initial_pose.pose.orientation.z = 1.0 - initial_pose.pose.orientation.w = 0.0 + initial_pose.pose.position.x = 0.0 + initial_pose.pose.position.y = 0.0 + initial_pose.pose.orientation.z = 0.0 + initial_pose.pose.orientation.w = 1.0 navigator.setInitialPose(initial_pose) # Activate navigation, if not autostarted. This should be called after setInitialPose() @@ -59,28 +59,28 @@ def main(): goal_pose1 = PoseStamped() goal_pose1.header.frame_id = 'map' goal_pose1.header.stamp = navigator.get_clock().now().to_msg() - goal_pose1.pose.position.x = 1.5 - goal_pose1.pose.position.y = 0.55 - goal_pose1.pose.orientation.w = 0.707 - goal_pose1.pose.orientation.z = 0.707 + goal_pose1.pose.position.x = 10.15 + goal_pose1.pose.position.y = -0.77 + goal_pose1.pose.orientation.w = 1.0 + goal_pose1.pose.orientation.z = 0.0 goal_poses.append(goal_pose1) # additional goals can be appended goal_pose2 = PoseStamped() goal_pose2.header.frame_id = 'map' goal_pose2.header.stamp = navigator.get_clock().now().to_msg() - goal_pose2.pose.position.x = 1.5 - goal_pose2.pose.position.y = -3.75 - goal_pose2.pose.orientation.w = 0.707 - goal_pose2.pose.orientation.z = 0.707 + goal_pose2.pose.position.x = 17.86 + goal_pose2.pose.position.y = -0.77 + goal_pose2.pose.orientation.w = 1.0 + goal_pose2.pose.orientation.z = 0.0 goal_poses.append(goal_pose2) goal_pose3 = PoseStamped() goal_pose3.header.frame_id = 'map' goal_pose3.header.stamp = navigator.get_clock().now().to_msg() - goal_pose3.pose.position.x = -3.6 - goal_pose3.pose.position.y = -4.75 - goal_pose3.pose.orientation.w = 0.707 - goal_pose3.pose.orientation.z = 0.707 + goal_pose3.pose.position.x = 21.58 + goal_pose3.pose.position.y = -3.5 + goal_pose3.pose.orientation.w = 1.0 + goal_pose3.pose.orientation.z = 0.0 goal_poses.append(goal_pose3) # sanity check a valid path exists @@ -118,10 +118,10 @@ def main(): goal_pose4 = PoseStamped() goal_pose4.header.frame_id = 'map' goal_pose4.header.stamp = navigator.get_clock().now().to_msg() - goal_pose4.pose.position.x = -5.0 - goal_pose4.pose.position.y = -4.75 - goal_pose4.pose.orientation.w = 0.707 - goal_pose4.pose.orientation.z = 0.707 + goal_pose4.pose.position.x = 0.0 + goal_pose4.pose.position.y = 0.0 + goal_pose4.pose.orientation.w = 1.0 + goal_pose4.pose.orientation.z = 0.0 navigator.goThroughPoses([goal_pose4]) # Do something depending on the return code diff --git a/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py b/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py index d7fb850d12..d923a834b8 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py +++ b/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py @@ -32,10 +32,10 @@ def main(): initial_pose = PoseStamped() initial_pose.header.frame_id = 'map' initial_pose.header.stamp = navigator.get_clock().now().to_msg() - initial_pose.pose.position.x = 3.45 - initial_pose.pose.position.y = 2.15 - initial_pose.pose.orientation.z = 1.0 - initial_pose.pose.orientation.w = 0.0 + initial_pose.pose.position.x = 0.0 + initial_pose.pose.position.y = 0.0 + initial_pose.pose.orientation.z = 0.0 + initial_pose.pose.orientation.w = 1.0 navigator.setInitialPose(initial_pose) # Activate navigation, if not autostarted. This should be called after setInitialPose() @@ -58,9 +58,10 @@ def main(): goal_pose = PoseStamped() goal_pose.header.frame_id = 'map' goal_pose.header.stamp = navigator.get_clock().now().to_msg() - goal_pose.pose.position.x = -2.0 - goal_pose.pose.position.y = -0.5 + goal_pose.pose.position.x = 17.86 + goal_pose.pose.position.y = -0.77 goal_pose.pose.orientation.w = 1.0 + goal_pose.pose.orientation.z = 0.0 # sanity check a valid path exists # path = navigator.getPath(initial_pose, goal_pose) @@ -94,7 +95,8 @@ def main(): # Some navigation request change to demo preemption if Duration.from_msg(feedback.navigation_time) > Duration(seconds=18.0): - goal_pose.pose.position.x = -3.0 + goal_pose.pose.position.x = 0.0 + goal_pose.pose.position.y = 0.0 navigator.goToPose(goal_pose) # Do something depending on the return code diff --git a/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py b/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py index 3044465851..0d0ea4c77f 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py +++ b/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py @@ -32,10 +32,10 @@ def main(): initial_pose = PoseStamped() initial_pose.header.frame_id = 'map' initial_pose.header.stamp = navigator.get_clock().now().to_msg() - initial_pose.pose.position.x = 3.45 - initial_pose.pose.position.y = 2.15 - initial_pose.pose.orientation.z = 1.0 - initial_pose.pose.orientation.w = 0.0 + initial_pose.pose.position.x = 0.0 + initial_pose.pose.position.y = 0.0 + initial_pose.pose.orientation.z = 0.0 + initial_pose.pose.orientation.w = 1.0 navigator.setInitialPose(initial_pose) # Activate navigation, if not autostarted. This should be called after setInitialPose() @@ -59,28 +59,28 @@ def main(): goal_pose1 = PoseStamped() goal_pose1.header.frame_id = 'map' goal_pose1.header.stamp = navigator.get_clock().now().to_msg() - goal_pose1.pose.position.x = 1.5 - goal_pose1.pose.position.y = 0.55 - goal_pose1.pose.orientation.w = 0.707 - goal_pose1.pose.orientation.z = 0.707 + goal_pose1.pose.position.x = 10.15 + goal_pose1.pose.position.y = -0.77 + goal_pose1.pose.orientation.w = 1.0 + goal_pose1.pose.orientation.z = 0.0 goal_poses.append(goal_pose1) # additional goals can be appended goal_pose2 = PoseStamped() goal_pose2.header.frame_id = 'map' goal_pose2.header.stamp = navigator.get_clock().now().to_msg() - goal_pose2.pose.position.x = 1.5 - goal_pose2.pose.position.y = -3.75 - goal_pose2.pose.orientation.w = 0.707 - goal_pose2.pose.orientation.z = 0.707 + goal_pose2.pose.position.x = 17.86 + goal_pose2.pose.position.y = -0.77 + goal_pose2.pose.orientation.w = 1.0 + goal_pose2.pose.orientation.z = 0.0 goal_poses.append(goal_pose2) goal_pose3 = PoseStamped() goal_pose3.header.frame_id = 'map' goal_pose3.header.stamp = navigator.get_clock().now().to_msg() - goal_pose3.pose.position.x = -3.6 - goal_pose3.pose.position.y = -4.75 - goal_pose3.pose.orientation.w = 0.707 - goal_pose3.pose.orientation.z = 0.707 + goal_pose3.pose.position.x = 21.58 + goal_pose3.pose.position.y = -3.5 + goal_pose3.pose.orientation.w = 1.0 + goal_pose3.pose.orientation.z = 0.0 goal_poses.append(goal_pose3) # sanity check a valid path exists @@ -118,10 +118,10 @@ def main(): goal_pose4 = PoseStamped() goal_pose4.header.frame_id = 'map' goal_pose4.header.stamp = now.to_msg() - goal_pose4.pose.position.x = -5.0 - goal_pose4.pose.position.y = -4.75 - goal_pose4.pose.orientation.w = 0.707 - goal_pose4.pose.orientation.z = 0.707 + goal_pose4.pose.position.x = 0.0 + goal_pose4.pose.position.y = 0.0 + goal_pose4.pose.orientation.w = 1.0 + goal_pose4.pose.orientation.z = 0.0 goal_poses = [goal_pose4] nav_start = now navigator.followWaypoints(goal_poses) From 16f5cca8121708922d5cbf469be366d37eccea36 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 11 Jun 2024 10:28:58 -0700 Subject: [PATCH 2/4] lint-o-mint --- nav2_simple_commander/launch/assisted_teleop_example_launch.py | 2 +- nav2_simple_commander/launch/follow_path_example_launch.py | 2 +- nav2_simple_commander/launch/inspection_demo_launch.py | 2 +- .../launch/nav_through_poses_example_launch.py | 2 +- nav2_simple_commander/launch/nav_to_pose_example_launch.py | 2 +- nav2_simple_commander/launch/picking_demo_launch.py | 2 +- nav2_simple_commander/launch/recoveries_example_launch.py | 2 +- nav2_simple_commander/launch/security_demo_launch.py | 2 +- .../launch/waypoint_follower_example_launch.py | 2 +- 9 files changed, 9 insertions(+), 9 deletions(-) diff --git a/nav2_simple_commander/launch/assisted_teleop_example_launch.py b/nav2_simple_commander/launch/assisted_teleop_example_launch.py index 4c2443a6c4..9ea12bedf4 100644 --- a/nav2_simple_commander/launch/assisted_teleop_example_launch.py +++ b/nav2_simple_commander/launch/assisted_teleop_example_launch.py @@ -14,8 +14,8 @@ # limitations under the License. import os -import tempfile from pathlib import Path +import tempfile from ament_index_python.packages import get_package_share_directory diff --git a/nav2_simple_commander/launch/follow_path_example_launch.py b/nav2_simple_commander/launch/follow_path_example_launch.py index 248a9869dd..84de9257a7 100644 --- a/nav2_simple_commander/launch/follow_path_example_launch.py +++ b/nav2_simple_commander/launch/follow_path_example_launch.py @@ -13,8 +13,8 @@ # limitations under the License. import os -import tempfile from pathlib import Path +import tempfile from ament_index_python.packages import get_package_share_directory diff --git a/nav2_simple_commander/launch/inspection_demo_launch.py b/nav2_simple_commander/launch/inspection_demo_launch.py index f01e96c7f2..7394f49433 100644 --- a/nav2_simple_commander/launch/inspection_demo_launch.py +++ b/nav2_simple_commander/launch/inspection_demo_launch.py @@ -13,8 +13,8 @@ # limitations under the License. import os -import tempfile from pathlib import Path +import tempfile from ament_index_python.packages import get_package_share_directory diff --git a/nav2_simple_commander/launch/nav_through_poses_example_launch.py b/nav2_simple_commander/launch/nav_through_poses_example_launch.py index 6f7035c736..8dc297d010 100644 --- a/nav2_simple_commander/launch/nav_through_poses_example_launch.py +++ b/nav2_simple_commander/launch/nav_through_poses_example_launch.py @@ -13,8 +13,8 @@ # limitations under the License. import os -import tempfile from pathlib import Path +import tempfile from ament_index_python.packages import get_package_share_directory diff --git a/nav2_simple_commander/launch/nav_to_pose_example_launch.py b/nav2_simple_commander/launch/nav_to_pose_example_launch.py index b0a5dbfcac..52192c0a2d 100644 --- a/nav2_simple_commander/launch/nav_to_pose_example_launch.py +++ b/nav2_simple_commander/launch/nav_to_pose_example_launch.py @@ -13,8 +13,8 @@ # limitations under the License. import os -import tempfile from pathlib import Path +import tempfile from ament_index_python.packages import get_package_share_directory diff --git a/nav2_simple_commander/launch/picking_demo_launch.py b/nav2_simple_commander/launch/picking_demo_launch.py index 08b5ff8169..65c49af0b5 100644 --- a/nav2_simple_commander/launch/picking_demo_launch.py +++ b/nav2_simple_commander/launch/picking_demo_launch.py @@ -13,8 +13,8 @@ # limitations under the License. import os -import tempfile from pathlib import Path +import tempfile from ament_index_python.packages import get_package_share_directory diff --git a/nav2_simple_commander/launch/recoveries_example_launch.py b/nav2_simple_commander/launch/recoveries_example_launch.py index 0d054eb144..429d5d3bd3 100644 --- a/nav2_simple_commander/launch/recoveries_example_launch.py +++ b/nav2_simple_commander/launch/recoveries_example_launch.py @@ -13,8 +13,8 @@ # limitations under the License. import os -import tempfile from pathlib import Path +import tempfile from ament_index_python.packages import get_package_share_directory diff --git a/nav2_simple_commander/launch/security_demo_launch.py b/nav2_simple_commander/launch/security_demo_launch.py index 513e054ed5..4872c031c2 100644 --- a/nav2_simple_commander/launch/security_demo_launch.py +++ b/nav2_simple_commander/launch/security_demo_launch.py @@ -13,8 +13,8 @@ # limitations under the License. import os -import tempfile from pathlib import Path +import tempfile from ament_index_python.packages import get_package_share_directory diff --git a/nav2_simple_commander/launch/waypoint_follower_example_launch.py b/nav2_simple_commander/launch/waypoint_follower_example_launch.py index 5de0a5f04c..d87fd8ea11 100644 --- a/nav2_simple_commander/launch/waypoint_follower_example_launch.py +++ b/nav2_simple_commander/launch/waypoint_follower_example_launch.py @@ -13,8 +13,8 @@ # limitations under the License. import os -import tempfile from pathlib import Path +import tempfile from ament_index_python.packages import get_package_share_directory From 0b964317853fe9e83f205a159a59f5dcf06829dd Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 11 Jun 2024 10:31:54 -0700 Subject: [PATCH 3/4] lint-o-mint --- nav2_simple_commander/launch/assisted_teleop_example_launch.py | 2 +- nav2_simple_commander/nav2_simple_commander/demo_picking.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_simple_commander/launch/assisted_teleop_example_launch.py b/nav2_simple_commander/launch/assisted_teleop_example_launch.py index 9ea12bedf4..90abfcad8a 100644 --- a/nav2_simple_commander/launch/assisted_teleop_example_launch.py +++ b/nav2_simple_commander/launch/assisted_teleop_example_launch.py @@ -138,7 +138,7 @@ def generate_launch_description(): str(Path(os.path.join(desc_dir)).parent.resolve())) ld = LaunchDescription() - ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_use_rviz_cmd) ld.add_action(declare_simulator_cmd) ld.add_action(world_sdf_xacro) ld.add_action(remove_temp_sdf_file) diff --git a/nav2_simple_commander/nav2_simple_commander/demo_picking.py b/nav2_simple_commander/nav2_simple_commander/demo_picking.py index 7a6a6010c5..d0975d1446 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_picking.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_picking.py @@ -60,7 +60,7 @@ def main(): initial_pose = PoseStamped() initial_pose.header.frame_id = 'map' initial_pose.header.stamp = navigator.get_clock().now().to_msg() - initial_pose.pose.position.x = 0.0 + initial_pose.pose.position.x = 0.0 initial_pose.pose.position.y = 0.0 initial_pose.pose.orientation.z = 0.0 initial_pose.pose.orientation.w = 1.0 From a4b0d8d631e3b5d1cb72749df0218c308d5f867d Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 11 Jun 2024 10:35:42 -0700 Subject: [PATCH 4/4] bump cache for deleted files --- .circleci/config.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.circleci/config.yml b/.circleci/config.yml index 776154ed54..d84ada2f52 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -33,12 +33,12 @@ _commands: - restore_cache: name: Restore Cache << parameters.key >> keys: - - "<< parameters.key >>-v24\ + - "<< parameters.key >>-v25\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" - - "<< parameters.key >>-v24\ + - "<< parameters.key >>-v25\ -{{ arch }}\ -main\ -\ @@ -58,7 +58,7 @@ _commands: steps: - save_cache: name: Save Cache << parameters.key >> - key: "<< parameters.key >>-v24\ + key: "<< parameters.key >>-v25\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\