From be370db6ea74280eff7b2075b9f31cc6eca56d31 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Wed, 8 Nov 2023 05:31:02 +0100 Subject: [PATCH] Nacho/fix flake8 remaining tests (#3942) * Use ament_black as linter * Fix standard flake8 errors for `tools - F401 'ament_index_python.packages.get_package_share_directory' imported but unused * Fix flake8 tests for nav2_common - nav2_common/nav2_common/launch/__init__.py:15:1: F401 '.has_node_params.HasNodeParams' imported but unused - nav2_common/nav2_common/launch/__init__.py:16:1: F401 '.rewritten_yaml.RewrittenYaml' imported but unused - nav2_common/nav2_common/launch/__init__.py:17:1: F401 '.replace_string.ReplaceString' imported but unused - nav2_common/nav2_common/launch/__init__.py:18:1: F401 '.parse_multirobot_pose.ParseMultiRobotPose' imported but unused * Fix flake8 nav2_common linterr - nav2_common/nav2_common/launch/replace_string.py:97:100: E501 line too long (108 > 99 characters) * Fix flake8 nav2_common linter - nav2_common/nav2_common/launch/rewritten_yaml.py:142:16: E713 test for membership should be 'not in' * Fix flake8 nav_common linter - nav2_common/nav2_common/launch/has_node_params.py:21:1: F401 'sys' imported but unused * Fix nav2_system_tests flake8 linter This came after using black as formatter. So respecting the previous code * Fix flake8-builtins linter * Fix flake8-comprehensions linter * Fix flake8-import-order linter for nav2_sumple_commander * Fix flake8-import-order linter for nav2_system_tests * Fix flake8-import-order linter for nav2_common * Fix flake8-import-order linter for tools * Fix flake8-docstring linter * Fix flake8-quotes linter for nav2_common * Fix flake8-quotes linter for nav2_system_tests * Fix flake8-quotes linter for nav2_collision_monitor * Fix flake8-quotes linter for nav2_map_server * Fix flake8-quotes linter for nav2_bringup * Fix flake8-quotes linter for tools * Fix flake8-quotes linter for nav2_simple_commander * Fix flake8-quotes linter for nav2_lifecycle * Fix flake8-quotes linter for nav2_costmap2d * Fix flake8-quotes linter for nav2_smac_planner * Fix linter... again --- nav2_bringup/launch/bringup_launch.py | 174 ++++++++------- .../cloned_multi_tb3_simulation_launch.py | 173 +++++++++------ nav2_bringup/launch/localization_launch.py | 115 ++++++---- nav2_bringup/launch/navigation_launch.py | 132 +++++++----- nav2_bringup/launch/rviz_launch.py | 42 ++-- nav2_bringup/launch/slam_launch.py | 60 ++++-- nav2_bringup/launch/tb3_simulation_launch.py | 189 +++++++++------- .../unique_multi_tb3_simulation_launch.py | 203 +++++++++++------- .../launch/collision_detector_node.launch.py | 74 ++++--- .../launch/collision_monitor_node.launch.py | 77 ++++--- nav2_common/nav2_common/launch/__init__.py | 11 +- .../nav2_common/launch/has_node_params.py | 56 ++--- .../launch/parse_multirobot_pose.py | 31 ++- .../nav2_common/launch/replace_string.py | 131 +++++------ .../nav2_common/launch/rewritten_yaml.py | 54 +++-- .../test/integration/costmap_tests_launch.py | 29 +-- .../costmap_map_server.launch.py | 19 +- .../test/launch_bond_test.py | 30 +-- .../test/launch_lifecycle_test.py | 32 +-- .../launch/map_saver_server.launch.py | 36 ++-- .../test/component/test_map_saver_launch.py | 8 +- .../test/component/test_map_server_launch.py | 8 +- .../map_saver_node.launch.py | 11 +- .../map_server_node.launch.py | 20 +- .../launch/assisted_teleop_example_launch.py | 43 ++-- .../launch/follow_path_example_launch.py | 43 ++-- .../launch/inspection_demo_launch.py | 43 ++-- .../nav_through_poses_example_launch.py | 43 ++-- .../launch/nav_to_pose_example_launch.py | 43 ++-- .../launch/picking_demo_launch.py | 43 ++-- .../launch/recoveries_example_launch.py | 43 ++-- .../launch/security_demo_launch.py | 43 ++-- .../waypoint_follower_example_launch.py | 43 ++-- .../nav2_simple_commander/demo_inspection.py | 11 +- .../nav2_simple_commander/demo_picking.py | 43 ++-- .../nav2_simple_commander/demo_security.py | 18 +- .../example_follow_path.py | 10 +- .../example_nav_through_poses.py | 11 +- .../example_nav_to_pose.py | 11 +- .../example_waypoint_follower.py | 8 +- .../footprint_collision_checker.py | 32 +-- .../nav2_simple_commander/line_iterator.py | 33 +-- .../nav2_simple_commander/robot_navigator.py | 163 +++++++++----- nav2_simple_commander/setup.py | 21 +- nav2_simple_commander/test/test_flake8.py | 6 +- .../test/test_footprint_collision_checker.py | 1 - .../test/test_line_iterator.py | 5 +- .../generate_motion_primitives.py | 77 ++++--- .../lattice_primitives/helper.py | 9 +- .../lattice_primitives/lattice_generator.py | 6 +- .../tests/test_lattice_generator.py | 14 +- .../tests/test_trajectory_generator.py | 80 ++++--- .../tests/trajectory_visualizer.py | 58 +++-- .../lattice_primitives/trajectory.py | 3 +- .../trajectory_generator.py | 1 + .../test_assisted_teleop_behavior_launch.py | 106 +++++---- .../backup/test_backup_behavior_launch.py | 106 +++++---- .../test_drive_on_heading_behavior_launch.py | 104 +++++---- .../spin/test_spin_behavior_fake_launch.py | 186 ++++++++-------- .../spin/test_spin_behavior_launch.py | 106 +++++---- .../wait/test_wait_behavior_launch.py | 106 +++++---- .../costmap_filters/test_keepout_launch.py | 189 +++++++++------- .../src/costmap_filters/test_speed_launch.py | 175 ++++++++------- .../src/costmap_filters/tester_node.py | 194 ++++++++++------- .../src/error_codes/test_error_codes.py | 30 ++- .../error_codes/test_error_codes_launch.py | 32 +-- .../gps_navigation/dual_ekf_navsat.launch.py | 57 ++--- .../src/gps_navigation/test_case_py.launch.py | 86 ++++---- .../src/gps_navigation/tester.py | 70 +++--- .../localization/test_localization_launch.py | 36 ++-- .../planning/test_planner_costmaps_launch.py | 2 +- .../planning/test_planner_random_launch.py | 2 +- .../system/nav_through_poses_tester_node.py | 87 +++++--- .../src/system/nav_to_pose_tester_node.py | 1 - .../src/system/test_multi_robot_launch.py | 142 ++++++++---- .../src/system/test_system_launch.py | 123 ++++++----- .../src/system/test_wrong_init_pose_launch.py | 125 ++++++----- .../test_system_failure_launch.py | 115 ++++++---- .../src/system_failure/tester_node.py | 78 ++++--- .../src/updown/test_updown_launch.py | 52 +++-- nav2_system_tests/src/updown/updownresults.py | 2 +- .../src/waypoint_follower/tester.py | 44 ++-- tools/bt2img.py | 201 ++++++++++------- tools/planner_benchmarking/metrics.py | 53 +++-- .../planning_benchmark_bringup.py | 103 +++++---- tools/planner_benchmarking/process_data.py | 52 +++-- tools/smoother_benchmarking/metrics.py | 61 +++--- tools/smoother_benchmarking/process_data.py | 123 ++++++----- .../smoother_benchmark_bringup.py | 60 ++++-- 89 files changed, 3410 insertions(+), 2322 deletions(-) diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index ad4c7e091e..f39a0b8e88 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -17,8 +17,12 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import (DeclareLaunchArgument, GroupAction, - IncludeLaunchDescription, SetEnvironmentVariable) +from launch.actions import ( + DeclareLaunchArgument, + GroupAction, + IncludeLaunchDescription, + SetEnvironmentVariable, +) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression @@ -51,8 +55,7 @@ def generate_launch_description(): # 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')] + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] # Only it applys when `use_namespace` is True. # '' keyword shall be replaced by 'namespace' launch argument @@ -61,113 +64,134 @@ def generate_launch_description(): params_file = ReplaceString( source_file=params_file, replacements={'': ('/', namespace)}, - condition=IfCondition(use_namespace)) + condition=IfCondition(use_namespace), + ) configured_params = ParameterFile( RewrittenYaml( source_file=params_file, root_key=namespace, param_rewrites={}, - convert_types=True), - allow_substs=True) + convert_types=True, + ), + allow_substs=True, + ) stdout_linebuf_envvar = SetEnvironmentVariable( - 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') + 'RCUTILS_LOGGING_BUFFERED_STREAM', '1' + ) declare_namespace_cmd = DeclareLaunchArgument( - 'namespace', - default_value='', - description='Top-level namespace') + 'namespace', default_value='', description='Top-level namespace' + ) declare_use_namespace_cmd = DeclareLaunchArgument( 'use_namespace', default_value='false', - description='Whether to apply a namespace to the navigation stack') + description='Whether to apply a namespace to the navigation stack', + ) declare_slam_cmd = DeclareLaunchArgument( - 'slam', - default_value='False', - description='Whether run a SLAM') + 'slam', default_value='False', description='Whether run a SLAM' + ) declare_map_yaml_cmd = DeclareLaunchArgument( - 'map', - default_value='', - description='Full path to map yaml file to load') + 'map', default_value='', description='Full path to map yaml file to load' + ) declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='false', - description='Use simulation (Gazebo) clock if true') + description='Use simulation (Gazebo) clock if true', + ) declare_params_file_cmd = DeclareLaunchArgument( 'params_file', default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), - description='Full path to the ROS2 parameters file to use for all launched nodes') + description='Full path to the ROS2 parameters file to use for all launched nodes', + ) declare_autostart_cmd = DeclareLaunchArgument( - 'autostart', default_value='true', - description='Automatically startup the nav2 stack') + 'autostart', + default_value='true', + description='Automatically startup the nav2 stack', + ) declare_use_composition_cmd = DeclareLaunchArgument( - 'use_composition', default_value='True', - description='Whether to use composed bringup') + 'use_composition', + default_value='True', + description='Whether to use composed bringup', + ) declare_use_respawn_cmd = DeclareLaunchArgument( - 'use_respawn', default_value='False', - description='Whether to respawn if a node crashes. Applied when composition is disabled.') + 'use_respawn', + default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.', + ) declare_log_level_cmd = DeclareLaunchArgument( - 'log_level', default_value='info', - description='log level') + 'log_level', default_value='info', description='log level' + ) # Specify the actions - bringup_cmd_group = GroupAction([ - PushROSNamespace( - condition=IfCondition(use_namespace), - namespace=namespace), - - Node( - condition=IfCondition(use_composition), - name='nav2_container', - package='rclcpp_components', - executable='component_container_isolated', - parameters=[configured_params, {'autostart': autostart}], - arguments=['--ros-args', '--log-level', log_level], - remappings=remappings, - output='screen'), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(launch_dir, 'slam_launch.py')), - condition=IfCondition(slam), - launch_arguments={'namespace': namespace, - 'use_sim_time': use_sim_time, - 'autostart': autostart, - 'use_respawn': use_respawn, - 'params_file': params_file}.items()), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(launch_dir, - 'localization_launch.py')), - condition=IfCondition(PythonExpression(['not ', slam])), - launch_arguments={'namespace': namespace, - 'map': map_yaml_file, - 'use_sim_time': use_sim_time, - 'autostart': autostart, - 'params_file': params_file, - 'use_composition': use_composition, - 'use_respawn': use_respawn, - 'container_name': 'nav2_container'}.items()), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(launch_dir, 'navigation_launch.py')), - launch_arguments={'namespace': namespace, - 'use_sim_time': use_sim_time, - 'autostart': autostart, - 'params_file': params_file, - 'use_composition': use_composition, - 'use_respawn': use_respawn, - 'container_name': 'nav2_container'}.items()), - ]) + bringup_cmd_group = GroupAction( + [ + PushROSNamespace(condition=IfCondition(use_namespace), namespace=namespace), + Node( + condition=IfCondition(use_composition), + name='nav2_container', + package='rclcpp_components', + executable='component_container_isolated', + parameters=[configured_params, {'autostart': autostart}], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings, + output='screen', + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, 'slam_launch.py') + ), + condition=IfCondition(slam), + launch_arguments={ + 'namespace': namespace, + 'use_sim_time': use_sim_time, + 'autostart': autostart, + 'use_respawn': use_respawn, + 'params_file': params_file, + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, 'localization_launch.py') + ), + condition=IfCondition(PythonExpression(['not ', slam])), + launch_arguments={ + 'namespace': namespace, + 'map': map_yaml_file, + 'use_sim_time': use_sim_time, + 'autostart': autostart, + 'params_file': params_file, + 'use_composition': use_composition, + 'use_respawn': use_respawn, + 'container_name': 'nav2_container', + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, 'navigation_launch.py') + ), + launch_arguments={ + 'namespace': namespace, + 'use_sim_time': use_sim_time, + 'autostart': autostart, + 'params_file': params_file, + 'use_composition': use_composition, + 'use_respawn': use_respawn, + 'container_name': 'nav2_container', + }.items(), + ), + ] + ) # Create the launch description and populate ld = LaunchDescription() diff --git a/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py b/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py index c508e1d285..dbff594bc7 100644 --- a/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py +++ b/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py @@ -17,8 +17,13 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import (DeclareLaunchArgument, ExecuteProcess, GroupAction, - IncludeLaunchDescription, LogInfo) +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + GroupAction, + IncludeLaunchDescription, + LogInfo, +) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, TextSubstitution @@ -31,9 +36,9 @@ def generate_launch_description(): Launch arguments consist of robot name(which is namespace) and pose for initialization. Keep general yaml format for pose information. - ex) robots:="robot1={x: 1.0, y: 1.0, yaw: 1.5707}; robot2={x: 1.0, y: 1.0, yaw: 1.5707}" - ex) robots:="robot3={x: 1.0, y: 1.0, z: 1.0, roll: 0.0, pitch: 1.5707, yaw: 1.5707}; - robot4={x: 1.0, y: 1.0, z: 1.0, roll: 0.0, pitch: 1.5707, yaw: 1.5707}" + ex) robots:='robot1={x: 1.0, y: 1.0, yaw: 1.5707}; robot2={x: 1.0, y: 1.0, yaw: 1.5707}' + ex) robots:='robot3={x: 1.0, y: 1.0, z: 1.0, roll: 0.0, pitch: 1.5707, yaw: 1.5707}; + robot4={x: 1.0, y: 1.0, z: 1.0, roll: 0.0, pitch: 1.5707, yaw: 1.5707}' """ # Get the launch directory bringup_dir = get_package_share_directory('nav2_bringup') @@ -56,47 +61,64 @@ def generate_launch_description(): declare_world_cmd = DeclareLaunchArgument( 'world', default_value=os.path.join(bringup_dir, 'worlds', 'world_only.model'), - description='Full path to world file to load') + description='Full path to world file to load', + ) declare_simulator_cmd = DeclareLaunchArgument( 'simulator', default_value='gazebo', - description='The simulator to use (gazebo or gzserver)') + description='The simulator to use (gazebo or gzserver)', + ) declare_map_yaml_cmd = DeclareLaunchArgument( 'map', default_value=os.path.join(bringup_dir, 'maps', 'turtlebot3_world.yaml'), - description='Full path to map file to load') + description='Full path to map file to load', + ) declare_params_file_cmd = DeclareLaunchArgument( 'params_file', - default_value=os.path.join(bringup_dir, 'params', 'nav2_multirobot_params_all.yaml'), - description='Full path to the ROS2 parameters file to use for all launched nodes') + default_value=os.path.join( + bringup_dir, 'params', 'nav2_multirobot_params_all.yaml' + ), + description='Full path to the ROS2 parameters file to use for all launched nodes', + ) declare_autostart_cmd = DeclareLaunchArgument( - 'autostart', default_value='false', - description='Automatically startup the stacks') + 'autostart', + default_value='false', + description='Automatically startup the stacks', + ) declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config', default_value=os.path.join(bringup_dir, 'rviz', 'nav2_namespaced_view.rviz'), - description='Full path to the RVIZ config file to use.') + description='Full path to the RVIZ config file to use.', + ) declare_use_robot_state_pub_cmd = DeclareLaunchArgument( 'use_robot_state_pub', default_value='True', - description='Whether to start the robot state publisher') + description='Whether to start the robot state publisher', + ) declare_use_rviz_cmd = DeclareLaunchArgument( - 'use_rviz', - default_value='True', - description='Whether to start RVIZ') + 'use_rviz', default_value='True', description='Whether to start RVIZ' + ) # Start Gazebo with plugin providing the robot spawning service start_gazebo_cmd = ExecuteProcess( - cmd=[simulator, '--verbose', '-s', 'libgazebo_ros_init.so', - '-s', 'libgazebo_ros_factory.so', world], - output='screen') + cmd=[ + simulator, + '--verbose', + '-s', + 'libgazebo_ros_init.so', + '-s', + 'libgazebo_ros_factory.so', + world, + ], + output='screen', + ) robots_list = ParseMultiRobotPose('robots').value() @@ -104,39 +126,53 @@ def generate_launch_description(): bringup_cmd_group = [] for robot_name in robots_list: init_pose = robots_list[robot_name] - group = GroupAction([ - LogInfo(msg=['Launching namespace=', robot_name, ' init_pose=', str(init_pose)]), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(launch_dir, 'rviz_launch.py')), - condition=IfCondition(use_rviz), - launch_arguments={'namespace': TextSubstitution(text=robot_name), - 'use_namespace': 'True', - 'rviz_config': rviz_config_file}.items()), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(bringup_dir, - 'launch', - 'tb3_simulation_launch.py')), - launch_arguments={'namespace': robot_name, - 'use_namespace': 'True', - 'map': map_yaml_file, - 'use_sim_time': 'True', - 'params_file': params_file, - 'autostart': autostart, - 'use_rviz': 'False', - 'use_simulator': 'False', - 'headless': 'False', - 'use_robot_state_pub': use_robot_state_pub, - 'x_pose': TextSubstitution(text=str(init_pose['x'])), - 'y_pose': TextSubstitution(text=str(init_pose['y'])), - 'z_pose': TextSubstitution(text=str(init_pose['z'])), - 'roll': TextSubstitution(text=str(init_pose['roll'])), - 'pitch': TextSubstitution(text=str(init_pose['pitch'])), - 'yaw': TextSubstitution(text=str(init_pose['yaw'])), - 'robot_name':TextSubstitution(text=robot_name), }.items()) - ]) + group = GroupAction( + [ + LogInfo( + msg=[ + 'Launching namespace=', + robot_name, + ' init_pose=', + str(init_pose), + ] + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, 'rviz_launch.py') + ), + condition=IfCondition(use_rviz), + launch_arguments={ + 'namespace': TextSubstitution(text=robot_name), + 'use_namespace': 'True', + 'rviz_config': rviz_config_file, + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'tb3_simulation_launch.py') + ), + launch_arguments={ + 'namespace': robot_name, + 'use_namespace': 'True', + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': params_file, + 'autostart': autostart, + 'use_rviz': 'False', + 'use_simulator': 'False', + 'headless': 'False', + 'use_robot_state_pub': use_robot_state_pub, + 'x_pose': TextSubstitution(text=str(init_pose['x'])), + 'y_pose': TextSubstitution(text=str(init_pose['y'])), + 'z_pose': TextSubstitution(text=str(init_pose['z'])), + 'roll': TextSubstitution(text=str(init_pose['roll'])), + 'pitch': TextSubstitution(text=str(init_pose['pitch'])), + 'yaw': TextSubstitution(text=str(init_pose['yaw'])), + 'robot_name': TextSubstitution(text=robot_name), + }.items(), + ), + ] + ) bringup_cmd_group.append(group) @@ -158,16 +194,27 @@ def generate_launch_description(): ld.add_action(LogInfo(msg=['number_of_robots=', str(len(robots_list))])) - ld.add_action(LogInfo(condition=IfCondition(log_settings), - msg=['map yaml: ', map_yaml_file])) - ld.add_action(LogInfo(condition=IfCondition(log_settings), - msg=['params yaml: ', params_file])) - ld.add_action(LogInfo(condition=IfCondition(log_settings), - msg=['rviz config file: ', rviz_config_file])) - ld.add_action(LogInfo(condition=IfCondition(log_settings), - msg=['using robot state pub: ', use_robot_state_pub])) - ld.add_action(LogInfo(condition=IfCondition(log_settings), - msg=['autostart: ', autostart])) + ld.add_action( + LogInfo(condition=IfCondition(log_settings), msg=['map yaml: ', map_yaml_file]) + ) + ld.add_action( + LogInfo(condition=IfCondition(log_settings), msg=['params yaml: ', params_file]) + ) + ld.add_action( + LogInfo( + condition=IfCondition(log_settings), + msg=['rviz config file: ', rviz_config_file], + ) + ) + ld.add_action( + LogInfo( + condition=IfCondition(log_settings), + msg=['using robot state pub: ', use_robot_state_pub], + ) + ) + ld.add_action( + LogInfo(condition=IfCondition(log_settings), msg=['autostart: ', autostart]) + ) for cmd in bringup_cmd_group: ld.add_action(cmd) diff --git a/nav2_bringup/launch/localization_launch.py b/nav2_bringup/launch/localization_launch.py index 9a0219c885..42afb5c2cb 100644 --- a/nav2_bringup/launch/localization_launch.py +++ b/nav2_bringup/launch/localization_launch.py @@ -52,66 +52,78 @@ def generate_launch_description(): # 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')] + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] configured_params = ParameterFile( RewrittenYaml( source_file=params_file, root_key=namespace, param_rewrites={}, - convert_types=True), - allow_substs=True) + convert_types=True, + ), + allow_substs=True, + ) stdout_linebuf_envvar = SetEnvironmentVariable( - 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') + 'RCUTILS_LOGGING_BUFFERED_STREAM', '1' + ) declare_namespace_cmd = DeclareLaunchArgument( - 'namespace', - default_value='', - description='Top-level namespace') + 'namespace', default_value='', description='Top-level namespace' + ) declare_map_yaml_cmd = DeclareLaunchArgument( - 'map', - default_value='', - description='Full path to map yaml file to load') + 'map', default_value='', description='Full path to map yaml file to load' + ) declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='false', - description='Use simulation (Gazebo) clock if true') + description='Use simulation (Gazebo) clock if true', + ) declare_params_file_cmd = DeclareLaunchArgument( 'params_file', default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), - description='Full path to the ROS2 parameters file to use for all launched nodes') + description='Full path to the ROS2 parameters file to use for all launched nodes', + ) declare_autostart_cmd = DeclareLaunchArgument( - 'autostart', default_value='true', - description='Automatically startup the nav2 stack') + 'autostart', + default_value='true', + description='Automatically startup the nav2 stack', + ) declare_use_composition_cmd = DeclareLaunchArgument( - 'use_composition', default_value='False', - description='Use composed bringup if True') + 'use_composition', + default_value='False', + description='Use composed bringup if True', + ) declare_container_name_cmd = DeclareLaunchArgument( - 'container_name', default_value='nav2_container', - description='the name of conatiner that nodes will load in if use composition') + 'container_name', + default_value='nav2_container', + description='the name of conatiner that nodes will load in if use composition', + ) declare_use_respawn_cmd = DeclareLaunchArgument( - 'use_respawn', default_value='False', - description='Whether to respawn if a node crashes. Applied when composition is disabled.') + 'use_respawn', + default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.', + ) declare_log_level_cmd = DeclareLaunchArgument( - 'log_level', default_value='info', - description='log level') + 'log_level', default_value='info', description='log level' + ) load_nodes = GroupAction( condition=IfCondition(PythonExpression(['not ', use_composition])), actions=[ SetParameter('use_sim_time', use_sim_time), Node( - condition=IfCondition(EqualsSubstitution(LaunchConfiguration('map'), '')), + condition=IfCondition( + EqualsSubstitution(LaunchConfiguration('map'), '') + ), package='nav2_map_server', executable='map_server', name='map_server', @@ -120,19 +132,22 @@ def generate_launch_description(): respawn_delay=2.0, parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], - remappings=remappings), + remappings=remappings, + ), Node( - condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('map'), '')), + condition=IfCondition( + NotEqualsSubstitution(LaunchConfiguration('map'), '') + ), package='nav2_map_server', executable='map_server', name='map_server', output='screen', respawn=use_respawn, respawn_delay=2.0, - parameters=[configured_params, - {'yaml_filename': map_yaml_file}], + parameters=[configured_params, {'yaml_filename': map_yaml_file}], arguments=['--ros-args', '--log-level', log_level], - remappings=remappings), + remappings=remappings, + ), Node( package='nav2_amcl', executable='amcl', @@ -142,16 +157,17 @@ def generate_launch_description(): respawn_delay=2.0, parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], - remappings=remappings), + remappings=remappings, + ), Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_localization', output='screen', arguments=['--ros-args', '--log-level', log_level], - parameters=[{'autostart': autostart}, - {'node_names': lifecycle_nodes}]) - ] + parameters=[{'autostart': autostart}, {'node_names': lifecycle_nodes}], + ), + ], ) # LoadComposableNode for map server twice depending if we should use the # value of map from a CLI or launch default or user defined value in the @@ -164,27 +180,35 @@ def generate_launch_description(): SetParameter('use_sim_time', use_sim_time), LoadComposableNodes( target_container=container_name_full, - condition=IfCondition(EqualsSubstitution(LaunchConfiguration('map'), '')), + condition=IfCondition( + EqualsSubstitution(LaunchConfiguration('map'), '') + ), composable_node_descriptions=[ ComposableNode( package='nav2_map_server', plugin='nav2_map_server::MapServer', name='map_server', parameters=[configured_params], - remappings=remappings), + remappings=remappings, + ), ], ), LoadComposableNodes( target_container=container_name_full, - condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('map'), '')), + condition=IfCondition( + NotEqualsSubstitution(LaunchConfiguration('map'), '') + ), composable_node_descriptions=[ ComposableNode( package='nav2_map_server', plugin='nav2_map_server::MapServer', name='map_server', - parameters=[configured_params, - {'yaml_filename': map_yaml_file}], - remappings=remappings), + parameters=[ + configured_params, + {'yaml_filename': map_yaml_file}, + ], + remappings=remappings, + ), ], ), LoadComposableNodes( @@ -195,16 +219,19 @@ def generate_launch_description(): plugin='nav2_amcl::AmclNode', name='amcl', parameters=[configured_params], - remappings=remappings), + remappings=remappings, + ), ComposableNode( package='nav2_lifecycle_manager', plugin='nav2_lifecycle_manager::LifecycleManager', name='lifecycle_manager_localization', - parameters=[{'autostart': autostart, - 'node_names': lifecycle_nodes}]), + parameters=[ + {'autostart': autostart, 'node_names': lifecycle_nodes} + ], + ), ], - ) - ] + ), + ], ) # Create the launch description and populate diff --git a/nav2_bringup/launch/navigation_launch.py b/nav2_bringup/launch/navigation_launch.py index f1f17c50e6..2f438c66e0 100644 --- a/nav2_bringup/launch/navigation_launch.py +++ b/nav2_bringup/launch/navigation_launch.py @@ -40,13 +40,15 @@ def generate_launch_description(): use_respawn = LaunchConfiguration('use_respawn') log_level = LaunchConfiguration('log_level') - lifecycle_nodes = ['controller_server', - 'smoother_server', - 'planner_server', - 'behavior_server', - 'bt_navigator', - 'waypoint_follower', - 'velocity_smoother'] + lifecycle_nodes = [ + 'controller_server', + 'smoother_server', + 'planner_server', + 'behavior_server', + 'bt_navigator', + 'waypoint_follower', + 'velocity_smoother', + ] # 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 @@ -54,58 +56,68 @@ def generate_launch_description(): # 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')] + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] # Create our own temporary YAML files that include substitutions - param_substitutions = { - 'autostart': autostart} + param_substitutions = {'autostart': autostart} configured_params = ParameterFile( RewrittenYaml( source_file=params_file, root_key=namespace, param_rewrites=param_substitutions, - convert_types=True), - allow_substs=True) + convert_types=True, + ), + allow_substs=True, + ) stdout_linebuf_envvar = SetEnvironmentVariable( - 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') + 'RCUTILS_LOGGING_BUFFERED_STREAM', '1' + ) declare_namespace_cmd = DeclareLaunchArgument( - 'namespace', - default_value='', - description='Top-level namespace') + 'namespace', default_value='', description='Top-level namespace' + ) declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='false', - description='Use simulation (Gazebo) clock if true') + description='Use simulation (Gazebo) clock if true', + ) declare_params_file_cmd = DeclareLaunchArgument( 'params_file', default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), - description='Full path to the ROS2 parameters file to use for all launched nodes') + description='Full path to the ROS2 parameters file to use for all launched nodes', + ) declare_autostart_cmd = DeclareLaunchArgument( - 'autostart', default_value='true', - description='Automatically startup the nav2 stack') + 'autostart', + default_value='true', + description='Automatically startup the nav2 stack', + ) declare_use_composition_cmd = DeclareLaunchArgument( - 'use_composition', default_value='False', - description='Use composed bringup if True') + 'use_composition', + default_value='False', + description='Use composed bringup if True', + ) declare_container_name_cmd = DeclareLaunchArgument( - 'container_name', default_value='nav2_container', - description='the name of conatiner that nodes will load in if use composition') + 'container_name', + default_value='nav2_container', + description='the name of conatiner that nodes will load in if use composition', + ) declare_use_respawn_cmd = DeclareLaunchArgument( - 'use_respawn', default_value='False', - description='Whether to respawn if a node crashes. Applied when composition is disabled.') + 'use_respawn', + default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.', + ) declare_log_level_cmd = DeclareLaunchArgument( - 'log_level', default_value='info', - description='log level') + 'log_level', default_value='info', description='log level' + ) load_nodes = GroupAction( condition=IfCondition(PythonExpression(['not ', use_composition])), @@ -119,7 +131,8 @@ def generate_launch_description(): respawn_delay=2.0, parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], - remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), + remappings=remappings + [('cmd_vel', 'cmd_vel_nav')], + ), Node( package='nav2_smoother', executable='smoother_server', @@ -129,7 +142,8 @@ def generate_launch_description(): respawn_delay=2.0, parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], - remappings=remappings), + remappings=remappings, + ), Node( package='nav2_planner', executable='planner_server', @@ -139,7 +153,8 @@ def generate_launch_description(): respawn_delay=2.0, parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], - remappings=remappings), + remappings=remappings, + ), Node( package='nav2_behaviors', executable='behavior_server', @@ -149,7 +164,8 @@ def generate_launch_description(): respawn_delay=2.0, parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], - remappings=remappings), + remappings=remappings, + ), Node( package='nav2_bt_navigator', executable='bt_navigator', @@ -159,7 +175,8 @@ def generate_launch_description(): respawn_delay=2.0, parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], - remappings=remappings), + remappings=remappings, + ), Node( package='nav2_waypoint_follower', executable='waypoint_follower', @@ -169,7 +186,8 @@ def generate_launch_description(): respawn_delay=2.0, parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], - remappings=remappings), + remappings=remappings, + ), Node( package='nav2_velocity_smoother', executable='velocity_smoother', @@ -179,17 +197,18 @@ def generate_launch_description(): respawn_delay=2.0, parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], - remappings=remappings + - [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), + remappings=remappings + + [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')], + ), Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_navigation', output='screen', arguments=['--ros-args', '--log-level', log_level], - parameters=[{'autostart': autostart}, - {'node_names': lifecycle_nodes}]), - ] + parameters=[{'autostart': autostart}, {'node_names': lifecycle_nodes}], + ), + ], ) load_composable_nodes = GroupAction( @@ -204,53 +223,62 @@ def generate_launch_description(): plugin='nav2_controller::ControllerServer', name='controller_server', parameters=[configured_params], - remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), + remappings=remappings + [('cmd_vel', 'cmd_vel_nav')], + ), ComposableNode( package='nav2_smoother', plugin='nav2_smoother::SmootherServer', name='smoother_server', parameters=[configured_params], - remappings=remappings), + remappings=remappings, + ), ComposableNode( package='nav2_planner', plugin='nav2_planner::PlannerServer', name='planner_server', parameters=[configured_params], - remappings=remappings), + remappings=remappings, + ), ComposableNode( package='nav2_behaviors', plugin='behavior_server::BehaviorServer', name='behavior_server', parameters=[configured_params], - remappings=remappings), + remappings=remappings, + ), ComposableNode( package='nav2_bt_navigator', plugin='nav2_bt_navigator::BtNavigator', name='bt_navigator', parameters=[configured_params], - remappings=remappings), + remappings=remappings, + ), ComposableNode( package='nav2_waypoint_follower', plugin='nav2_waypoint_follower::WaypointFollower', name='waypoint_follower', parameters=[configured_params], - remappings=remappings), + remappings=remappings, + ), ComposableNode( package='nav2_velocity_smoother', plugin='nav2_velocity_smoother::VelocitySmoother', name='velocity_smoother', parameters=[configured_params], - remappings=remappings + - [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), + remappings=remappings + + [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')], + ), ComposableNode( package='nav2_lifecycle_manager', plugin='nav2_lifecycle_manager::LifecycleManager', name='lifecycle_manager_navigation', - parameters=[{'autostart': autostart, - 'node_names': lifecycle_nodes}]), + parameters=[ + {'autostart': autostart, 'node_names': lifecycle_nodes} + ], + ), ], - ) - ] + ), + ], ) # Create the launch description and populate diff --git a/nav2_bringup/launch/rviz_launch.py b/nav2_bringup/launch/rviz_launch.py index c87ae01b41..dee33f26d6 100644 --- a/nav2_bringup/launch/rviz_launch.py +++ b/nav2_bringup/launch/rviz_launch.py @@ -39,18 +39,23 @@ def generate_launch_description(): declare_namespace_cmd = DeclareLaunchArgument( 'namespace', default_value='navigation', - description=('Top-level namespace. The value will be used to replace the ' - ' keyword on the rviz config file.')) + description=( + 'Top-level namespace. The value will be used to replace the ' + ' keyword on the rviz config file.' + ), + ) declare_use_namespace_cmd = DeclareLaunchArgument( 'use_namespace', default_value='false', - description='Whether to apply a namespace to the navigation stack') + description='Whether to apply a namespace to the navigation stack', + ) declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config', default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'), - description='Full path to the RVIZ config file to use') + description='Full path to the RVIZ config file to use', + ) # Launch rviz start_rviz_cmd = Node( @@ -58,11 +63,13 @@ def generate_launch_description(): package='rviz2', executable='rviz2', arguments=['-d', rviz_config_file], - output='screen') + output='screen', + ) namespaced_rviz_config_file = ReplaceString( source_file=rviz_config_file, - replacements={'': ('/', namespace)}) + replacements={'': ('/', namespace)}, + ) start_namespaced_rviz_cmd = Node( condition=IfCondition(use_namespace), @@ -71,24 +78,31 @@ def generate_launch_description(): namespace=namespace, arguments=['-d', namespaced_rviz_config_file], output='screen', - remappings=[('/map', 'map'), - ('/tf', 'tf'), - ('/tf_static', 'tf_static'), - ('/goal_pose', 'goal_pose'), - ('/clicked_point', 'clicked_point'), - ('/initialpose', 'initialpose')]) + remappings=[ + ('/map', 'map'), + ('/tf', 'tf'), + ('/tf_static', 'tf_static'), + ('/goal_pose', 'goal_pose'), + ('/clicked_point', 'clicked_point'), + ('/initialpose', 'initialpose'), + ], + ) exit_event_handler = RegisterEventHandler( condition=UnlessCondition(use_namespace), event_handler=OnProcessExit( target_action=start_rviz_cmd, - on_exit=EmitEvent(event=Shutdown(reason='rviz exited')))) + on_exit=EmitEvent(event=Shutdown(reason='rviz exited')), + ), + ) exit_event_handler_namespaced = RegisterEventHandler( condition=IfCondition(use_namespace), event_handler=OnProcessExit( target_action=start_namespaced_rviz_cmd, - on_exit=EmitEvent(event=Shutdown(reason='rviz exited')))) + on_exit=EmitEvent(event=Shutdown(reason='rviz exited')), + ), + ) # Create the launch description and populate ld = LaunchDescription() diff --git a/nav2_bringup/launch/slam_launch.py b/nav2_bringup/launch/slam_launch.py index abb3c13d62..a806d260ce 100644 --- a/nav2_bringup/launch/slam_launch.py +++ b/nav2_bringup/launch/slam_launch.py @@ -49,36 +49,43 @@ def generate_launch_description(): source_file=params_file, root_key=namespace, param_rewrites={}, - convert_types=True), - allow_substs=True) + convert_types=True, + ), + allow_substs=True, + ) # Declare the launch arguments declare_namespace_cmd = DeclareLaunchArgument( - 'namespace', - default_value='', - description='Top-level namespace') + 'namespace', default_value='', description='Top-level namespace' + ) declare_params_file_cmd = DeclareLaunchArgument( 'params_file', default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), - description='Full path to the ROS2 parameters file to use for all launched nodes') + description='Full path to the ROS2 parameters file to use for all launched nodes', + ) declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='True', - description='Use simulation (Gazebo) clock if true') + description='Use simulation (Gazebo) clock if true', + ) declare_autostart_cmd = DeclareLaunchArgument( - 'autostart', default_value='True', - description='Automatically startup the nav2 stack') + 'autostart', + default_value='True', + description='Automatically startup the nav2 stack', + ) declare_use_respawn_cmd = DeclareLaunchArgument( - 'use_respawn', default_value='False', - description='Whether to respawn if a node crashes. Applied when composition is disabled.') + 'use_respawn', + default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.', + ) declare_log_level_cmd = DeclareLaunchArgument( - 'log_level', default_value='info', - description='log level') + 'log_level', default_value='info', description='log level' + ) # Nodes launching commands @@ -92,33 +99,40 @@ def generate_launch_description(): respawn=use_respawn, respawn_delay=2.0, arguments=['--ros-args', '--log-level', log_level], - parameters=[configured_params]), + parameters=[configured_params], + ), Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_slam', output='screen', arguments=['--ros-args', '--log-level', log_level], - parameters=[{'autostart': autostart}, - {'node_names': lifecycle_nodes}]) - ]) + parameters=[{'autostart': autostart}, {'node_names': lifecycle_nodes}], + ), + ] + ) # If the provided param file doesn't have slam_toolbox params, we must remove the 'params_file' # LaunchConfiguration, or it will be passed automatically to slam_toolbox and will not load # the default file - has_slam_toolbox_params = HasNodeParams(source_file=params_file, - node_name='slam_toolbox') + has_slam_toolbox_params = HasNodeParams( + source_file=params_file, node_name='slam_toolbox' + ) start_slam_toolbox_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource(slam_launch_file), launch_arguments={'use_sim_time': use_sim_time}.items(), - condition=UnlessCondition(has_slam_toolbox_params)) + condition=UnlessCondition(has_slam_toolbox_params), + ) start_slam_toolbox_cmd_with_params = IncludeLaunchDescription( PythonLaunchDescriptionSource(slam_launch_file), - launch_arguments={'use_sim_time': use_sim_time, - 'slam_params_file': params_file}.items(), - condition=IfCondition(has_slam_toolbox_params)) + launch_arguments={ + 'use_sim_time': use_sim_time, + 'slam_params_file': params_file, + }.items(), + condition=IfCondition(has_slam_toolbox_params), + ) ld = LaunchDescription() diff --git a/nav2_bringup/launch/tb3_simulation_launch.py b/nav2_bringup/launch/tb3_simulation_launch.py index e391fa863c..6598ac94ba 100644 --- a/nav2_bringup/launch/tb3_simulation_launch.py +++ b/nav2_bringup/launch/tb3_simulation_launch.py @@ -19,7 +19,11 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + IncludeLaunchDescription, +) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression @@ -51,12 +55,14 @@ def generate_launch_description(): use_rviz = LaunchConfiguration('use_rviz') headless = LaunchConfiguration('headless') world = LaunchConfiguration('world') - pose = {'x': LaunchConfiguration('x_pose', default='-2.00'), - 'y': LaunchConfiguration('y_pose', default='-0.50'), - 'z': LaunchConfiguration('z_pose', default='0.01'), - 'R': LaunchConfiguration('roll', default='0.00'), - 'P': LaunchConfiguration('pitch', default='0.00'), - 'Y': LaunchConfiguration('yaw', default='0.00')} + pose = { + 'x': LaunchConfiguration('x_pose', default='-2.00'), + 'y': LaunchConfiguration('y_pose', default='-0.50'), + 'z': LaunchConfiguration('z_pose', default='0.01'), + 'R': LaunchConfiguration('roll', default='0.00'), + 'P': LaunchConfiguration('pitch', default='0.00'), + 'Y': LaunchConfiguration('yaw', default='0.00'), + } robot_name = LaunchConfiguration('robot_name') robot_sdf = LaunchConfiguration('robot_sdf') @@ -66,78 +72,84 @@ def generate_launch_description(): # 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')] + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] # Declare the launch arguments declare_namespace_cmd = DeclareLaunchArgument( - 'namespace', - default_value='', - description='Top-level namespace') + 'namespace', default_value='', description='Top-level namespace' + ) declare_use_namespace_cmd = DeclareLaunchArgument( 'use_namespace', default_value='false', - description='Whether to apply a namespace to the navigation stack') + description='Whether to apply a namespace to the navigation stack', + ) declare_slam_cmd = DeclareLaunchArgument( - 'slam', - default_value='False', - description='Whether run a SLAM') + 'slam', default_value='False', description='Whether run a SLAM' + ) declare_map_yaml_cmd = DeclareLaunchArgument( 'map', - default_value=os.path.join( - bringup_dir, 'maps', 'turtlebot3_world.yaml'), - description='Full path to map file to load') + default_value=os.path.join(bringup_dir, 'maps', 'turtlebot3_world.yaml'), + description='Full path to map file to load', + ) declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='true', - description='Use simulation (Gazebo) clock if true') + description='Use simulation (Gazebo) clock if true', + ) declare_params_file_cmd = DeclareLaunchArgument( 'params_file', default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), - description='Full path to the ROS2 parameters file to use for all launched nodes') + description='Full path to the ROS2 parameters file to use for all launched nodes', + ) declare_autostart_cmd = DeclareLaunchArgument( - 'autostart', default_value='true', - description='Automatically startup the nav2 stack') + 'autostart', + default_value='true', + description='Automatically startup the nav2 stack', + ) declare_use_composition_cmd = DeclareLaunchArgument( - 'use_composition', default_value='True', - description='Whether to use composed bringup') + 'use_composition', + default_value='True', + description='Whether to use composed bringup', + ) declare_use_respawn_cmd = DeclareLaunchArgument( - 'use_respawn', default_value='False', - description='Whether to respawn if a node crashes. Applied when composition is disabled.') + 'use_respawn', + default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.', + ) declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config_file', - default_value=os.path.join( - bringup_dir, 'rviz', 'nav2_default_view.rviz'), - description='Full path to the RVIZ config file to use') + default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'), + description='Full path to the RVIZ config file to use', + ) declare_use_simulator_cmd = DeclareLaunchArgument( 'use_simulator', default_value='True', - description='Whether to start the simulator') + description='Whether to start the simulator', + ) declare_use_robot_state_pub_cmd = DeclareLaunchArgument( 'use_robot_state_pub', default_value='True', - description='Whether to start the robot state publisher') + description='Whether to start the robot state publisher', + ) declare_use_rviz_cmd = DeclareLaunchArgument( - 'use_rviz', - default_value='True', - description='Whether to start RVIZ') + 'use_rviz', default_value='True', description='Whether to start RVIZ' + ) declare_simulator_cmd = DeclareLaunchArgument( - 'headless', - default_value='True', - description='Whether to execute gzclient)') + 'headless', default_value='True', description='Whether to execute gzclient)' + ) declare_world_cmd = DeclareLaunchArgument( 'world', @@ -146,30 +158,40 @@ def generate_launch_description(): # default_value=os.path.join(get_package_share_directory('turtlebot3_gazebo'), # worlds/turtlebot3_worlds/waffle.model') default_value=os.path.join(bringup_dir, 'worlds', 'world_only.model'), - description='Full path to world model file to load') + description='Full path to world model file to load', + ) declare_robot_name_cmd = DeclareLaunchArgument( - 'robot_name', - default_value='turtlebot3_waffle', - description='name of the robot') + 'robot_name', default_value='turtlebot3_waffle', description='name of the robot' + ) declare_robot_sdf_cmd = DeclareLaunchArgument( 'robot_sdf', default_value=os.path.join(bringup_dir, 'worlds', 'waffle.model'), - description='Full path to robot sdf file to spawn the robot in gazebo') + description='Full path to robot sdf file to spawn the robot in gazebo', + ) # Specify the actions start_gazebo_server_cmd = ExecuteProcess( condition=IfCondition(use_simulator), - cmd=['gzserver', '-s', 'libgazebo_ros_init.so', - '-s', 'libgazebo_ros_factory.so', world], - cwd=[launch_dir], output='screen') + cmd=[ + 'gzserver', + '-s', + 'libgazebo_ros_init.so', + '-s', + 'libgazebo_ros_factory.so', + world, + ], + cwd=[launch_dir], + output='screen', + ) start_gazebo_client_cmd = ExecuteProcess( - condition=IfCondition(PythonExpression( - [use_simulator, ' and not ', headless])), + condition=IfCondition(PythonExpression([use_simulator, ' and not ', headless])), cmd=['gzclient'], - cwd=[launch_dir], output='screen') + cwd=[launch_dir], + output='screen', + ) urdf = os.path.join(bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') with open(urdf, 'r') as infp: @@ -182,41 +204,62 @@ def generate_launch_description(): name='robot_state_publisher', namespace=namespace, output='screen', - parameters=[{'use_sim_time': use_sim_time, - 'robot_description': robot_description}], - remappings=remappings) + parameters=[ + {'use_sim_time': use_sim_time, 'robot_description': robot_description} + ], + remappings=remappings, + ) start_gazebo_spawner_cmd = Node( package='gazebo_ros', executable='spawn_entity.py', output='screen', arguments=[ - '-entity', robot_name, - '-file', robot_sdf, - '-robot_namespace', namespace, - '-x', pose['x'], '-y', pose['y'], '-z', pose['z'], - '-R', pose['R'], '-P', pose['P'], '-Y', pose['Y']]) + '-entity', + robot_name, + '-file', + robot_sdf, + '-robot_namespace', + namespace, + '-x', + pose['x'], + '-y', + pose['y'], + '-z', + pose['z'], + '-R', + pose['R'], + '-P', + pose['P'], + '-Y', + pose['Y'], + ], + ) rviz_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(launch_dir, 'rviz_launch.py')), + PythonLaunchDescriptionSource(os.path.join(launch_dir, 'rviz_launch.py')), condition=IfCondition(use_rviz), - launch_arguments={'namespace': namespace, - 'use_namespace': use_namespace, - 'rviz_config': rviz_config_file}.items()) + launch_arguments={ + 'namespace': namespace, + 'use_namespace': use_namespace, + 'rviz_config': rviz_config_file, + }.items(), + ) bringup_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(launch_dir, 'bringup_launch.py')), - launch_arguments={'namespace': namespace, - 'use_namespace': use_namespace, - 'slam': slam, - 'map': map_yaml_file, - 'use_sim_time': use_sim_time, - 'params_file': params_file, - 'autostart': autostart, - 'use_composition': use_composition, - 'use_respawn': use_respawn}.items()) + PythonLaunchDescriptionSource(os.path.join(launch_dir, 'bringup_launch.py')), + launch_arguments={ + 'namespace': namespace, + 'use_namespace': use_namespace, + 'slam': slam, + 'map': map_yaml_file, + 'use_sim_time': use_sim_time, + 'params_file': params_file, + 'autostart': autostart, + 'use_composition': use_composition, + 'use_respawn': use_respawn, + }.items(), + ) # Create the launch description and populate ld = LaunchDescription() diff --git a/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py b/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py index 069ae5bb3d..2319def79a 100644 --- a/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py +++ b/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py @@ -25,8 +25,13 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import (DeclareLaunchArgument, ExecuteProcess, GroupAction, - IncludeLaunchDescription, LogInfo) +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + GroupAction, + IncludeLaunchDescription, + LogInfo, +) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, TextSubstitution @@ -39,10 +44,25 @@ def generate_launch_description(): # Names and poses of the robots robots = [ - {'name': 'robot1', 'x_pose': 0.0, 'y_pose': 0.5, 'z_pose': 0.01, - 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0}, - {'name': 'robot2', 'x_pose': 0.0, 'y_pose': -0.5, 'z_pose': 0.01, - 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0}] + { + 'name': 'robot1', + 'x_pose': 0.0, + 'y_pose': 0.5, + 'z_pose': 0.01, + 'roll': 0.0, + 'pitch': 0.0, + 'yaw': 0.0, + }, + { + 'name': 'robot2', + 'x_pose': 0.0, + 'y_pose': -0.5, + 'z_pose': 0.01, + 'roll': 0.0, + 'pitch': 0.0, + 'yaw': 0.0, + }, + ] # Simulation settings world = LaunchConfiguration('world') @@ -61,108 +81,145 @@ def generate_launch_description(): declare_world_cmd = DeclareLaunchArgument( 'world', default_value=os.path.join(bringup_dir, 'worlds', 'world_only.model'), - description='Full path to world file to load') + description='Full path to world file to load', + ) declare_simulator_cmd = DeclareLaunchArgument( 'simulator', default_value='gazebo', - description='The simulator to use (gazebo or gzserver)') + description='The simulator to use (gazebo or gzserver)', + ) declare_map_yaml_cmd = DeclareLaunchArgument( 'map', default_value=os.path.join(bringup_dir, 'maps', 'turtlebot3_world.yaml'), - description='Full path to map file to load') + description='Full path to map file to load', + ) declare_robot1_params_file_cmd = DeclareLaunchArgument( 'robot1_params_file', - default_value=os.path.join(bringup_dir, 'params', 'nav2_multirobot_params_1.yaml'), - description='Full path to the ROS2 parameters file to use for robot1 launched nodes') + default_value=os.path.join( + bringup_dir, 'params', 'nav2_multirobot_params_1.yaml' + ), + description='Full path to the ROS2 parameters file to use for robot1 launched nodes', + ) declare_robot2_params_file_cmd = DeclareLaunchArgument( 'robot2_params_file', - default_value=os.path.join(bringup_dir, 'params', 'nav2_multirobot_params_2.yaml'), - description='Full path to the ROS2 parameters file to use for robot2 launched nodes') + default_value=os.path.join( + bringup_dir, 'params', 'nav2_multirobot_params_2.yaml' + ), + description='Full path to the ROS2 parameters file to use for robot2 launched nodes', + ) declare_autostart_cmd = DeclareLaunchArgument( - 'autostart', default_value='false', - description='Automatically startup the stacks') + 'autostart', + default_value='false', + description='Automatically startup the stacks', + ) declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config', default_value=os.path.join(bringup_dir, 'rviz', 'nav2_namespaced_view.rviz'), - description='Full path to the RVIZ config file to use.') + description='Full path to the RVIZ config file to use.', + ) declare_use_robot_state_pub_cmd = DeclareLaunchArgument( 'use_robot_state_pub', default_value='True', - description='Whether to start the robot state publisher') + description='Whether to start the robot state publisher', + ) declare_use_rviz_cmd = DeclareLaunchArgument( - 'use_rviz', - default_value='True', - description='Whether to start RVIZ') + 'use_rviz', default_value='True', description='Whether to start RVIZ' + ) # Start Gazebo with plugin providing the robot spawning service start_gazebo_cmd = ExecuteProcess( - cmd=[simulator, '--verbose', '-s', 'libgazebo_ros_init.so', - '-s', 'libgazebo_ros_factory.so', world], - output='screen') + cmd=[ + simulator, + '--verbose', + '-s', + 'libgazebo_ros_init.so', + '-s', + 'libgazebo_ros_factory.so', + world, + ], + output='screen', + ) # Define commands for launching the navigation instances nav_instances_cmds = [] for robot in robots: params_file = LaunchConfiguration(f"{robot['name']}_params_file") - group = GroupAction([ - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(launch_dir, 'rviz_launch.py')), - condition=IfCondition(use_rviz), - launch_arguments={'namespace': TextSubstitution(text=robot['name']), - 'use_namespace': 'True', - 'rviz_config': rviz_config_file}.items()), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(bringup_dir, - 'launch', - 'tb3_simulation_launch.py')), - launch_arguments={'namespace': robot['name'], - 'use_namespace': 'True', - 'map': map_yaml_file, - 'use_sim_time': 'True', - 'params_file': params_file, - 'autostart': autostart, - 'use_rviz': 'False', - 'use_simulator': 'False', - 'headless': 'False', - 'use_robot_state_pub': use_robot_state_pub, - 'x_pose': TextSubstitution(text=str(robot['x_pose'])), - 'y_pose': TextSubstitution(text=str(robot['y_pose'])), - 'z_pose': TextSubstitution(text=str(robot['z_pose'])), - 'roll': TextSubstitution(text=str(robot['roll'])), - 'pitch': TextSubstitution(text=str(robot['pitch'])), - 'yaw': TextSubstitution(text=str(robot['yaw'])), - 'robot_name':TextSubstitution(text=robot['name']), }.items()), - - LogInfo( - condition=IfCondition(log_settings), - msg=['Launching ', robot['name']]), - LogInfo( - condition=IfCondition(log_settings), - msg=[robot['name'], ' map yaml: ', map_yaml_file]), - LogInfo( - condition=IfCondition(log_settings), - msg=[robot['name'], ' params yaml: ', params_file]), - LogInfo( - condition=IfCondition(log_settings), - msg=[robot['name'], ' rviz config file: ', rviz_config_file]), - LogInfo( - condition=IfCondition(log_settings), - msg=[robot['name'], ' using robot state pub: ', use_robot_state_pub]), - LogInfo( - condition=IfCondition(log_settings), - msg=[robot['name'], ' autostart: ', autostart]) - ]) + group = GroupAction( + [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, 'rviz_launch.py') + ), + condition=IfCondition(use_rviz), + launch_arguments={ + 'namespace': TextSubstitution(text=robot['name']), + 'use_namespace': 'True', + 'rviz_config': rviz_config_file, + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'tb3_simulation_launch.py') + ), + launch_arguments={ + 'namespace': robot['name'], + 'use_namespace': 'True', + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': params_file, + 'autostart': autostart, + 'use_rviz': 'False', + 'use_simulator': 'False', + 'headless': 'False', + 'use_robot_state_pub': use_robot_state_pub, + 'x_pose': TextSubstitution(text=str(robot['x_pose'])), + 'y_pose': TextSubstitution(text=str(robot['y_pose'])), + 'z_pose': TextSubstitution(text=str(robot['z_pose'])), + 'roll': TextSubstitution(text=str(robot['roll'])), + 'pitch': TextSubstitution(text=str(robot['pitch'])), + 'yaw': TextSubstitution(text=str(robot['yaw'])), + 'robot_name': TextSubstitution(text=robot['name']), + }.items(), + ), + LogInfo( + condition=IfCondition(log_settings), + msg=['Launching ', robot['name']], + ), + LogInfo( + condition=IfCondition(log_settings), + msg=[robot['name'], ' map yaml: ', map_yaml_file], + ), + LogInfo( + condition=IfCondition(log_settings), + msg=[robot['name'], ' params yaml: ', params_file], + ), + LogInfo( + condition=IfCondition(log_settings), + msg=[robot['name'], ' rviz config file: ', rviz_config_file], + ), + LogInfo( + condition=IfCondition(log_settings), + msg=[ + robot['name'], + ' using robot state pub: ', + use_robot_state_pub, + ], + ), + LogInfo( + condition=IfCondition(log_settings), + msg=[robot['name'], ' autostart: ', autostart], + ), + ] + ) nav_instances_cmds.append(group) diff --git a/nav2_collision_monitor/launch/collision_detector_node.launch.py b/nav2_collision_monitor/launch/collision_detector_node.launch.py index 5440cfedc3..c16bec55a4 100644 --- a/nav2_collision_monitor/launch/collision_detector_node.launch.py +++ b/nav2_collision_monitor/launch/collision_detector_node.launch.py @@ -37,8 +37,7 @@ def generate_launch_description(): # Constant parameters lifecycle_nodes = ['collision_detector'] autostart = True - remappings = [('/tf', 'tf'), - ('/tf_static', 'tf_static')] + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] # Launch arguments # 1. Create the launch configuration variables @@ -51,33 +50,41 @@ def generate_launch_description(): # 2. Declare the launch arguments declare_namespace_cmd = DeclareLaunchArgument( - 'namespace', - default_value='', - description='Top-level namespace') + 'namespace', default_value='', description='Top-level namespace' + ) declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='True', - description='Use simulation (Gazebo) clock if true') + description='Use simulation (Gazebo) clock if true', + ) declare_params_file_cmd = DeclareLaunchArgument( 'params_file', - default_value=os.path.join(package_dir, 'params', 'collision_monitor_params.yaml'), - description='Full path to the ROS2 parameters file to use for all launched nodes') + default_value=os.path.join( + package_dir, 'params', 'collision_monitor_params.yaml' + ), + description='Full path to the ROS2 parameters file to use for all launched nodes', + ) declare_use_composition_cmd = DeclareLaunchArgument( - 'use_composition', default_value='True', - description='Use composed bringup if True') + 'use_composition', + default_value='True', + description='Use composed bringup if True', + ) declare_container_name_cmd = DeclareLaunchArgument( - 'container_name', default_value='nav2_container', - description='the name of conatiner that nodes will load in if use composition') + 'container_name', + default_value='nav2_container', + description='the name of conatiner that nodes will load in if use composition', + ) configured_params = RewrittenYaml( source_file=params_file, root_key=namespace, param_rewrites={}, - convert_types=True) + convert_types=True, + ) # Declare node launching commands load_nodes = GroupAction( @@ -85,25 +92,29 @@ def generate_launch_description(): actions=[ SetParameter('use_sim_time', use_sim_time), PushROSNamespace( - condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')), - namespace=namespace), + condition=IfCondition( + NotEqualsSubstitution(LaunchConfiguration('namespace'), '') + ), + namespace=namespace, + ), Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_collision_detector', output='screen', emulate_tty=True, # https://github.com/ros2/launch/issues/188 - parameters=[{'autostart': autostart}, - {'node_names': lifecycle_nodes}], - remappings=remappings), + parameters=[{'autostart': autostart}, {'node_names': lifecycle_nodes}], + remappings=remappings, + ), Node( package='nav2_collision_monitor', executable='collision_detector', output='screen', emulate_tty=True, # https://github.com/ros2/launch/issues/188 parameters=[configured_params], - remappings=remappings) - ] + remappings=remappings, + ), + ], ) load_composable_nodes = GroupAction( @@ -111,8 +122,11 @@ def generate_launch_description(): actions=[ SetParameter('use_sim_time', use_sim_time), PushROSNamespace( - condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')), - namespace=namespace), + condition=IfCondition( + NotEqualsSubstitution(LaunchConfiguration('namespace'), '') + ), + namespace=namespace, + ), LoadComposableNodes( target_container=container_name_full, composable_node_descriptions=[ @@ -120,18 +134,22 @@ def generate_launch_description(): package='nav2_lifecycle_manager', plugin='nav2_lifecycle_manager::LifecycleManager', name='lifecycle_manager_collision_detector', - parameters=[{'autostart': autostart}, - {'node_names': lifecycle_nodes}], - remappings=remappings), + parameters=[ + {'autostart': autostart}, + {'node_names': lifecycle_nodes}, + ], + remappings=remappings, + ), ComposableNode( package='nav2_collision_monitor', plugin='nav2_collision_monitor::CollisionDetector', name='collision_detector', parameters=[configured_params], - remappings=remappings) + remappings=remappings, + ), ], - ) - ] + ), + ], ) ld = LaunchDescription() diff --git a/nav2_collision_monitor/launch/collision_monitor_node.launch.py b/nav2_collision_monitor/launch/collision_monitor_node.launch.py index f4d0a953e3..b00b43b2aa 100644 --- a/nav2_collision_monitor/launch/collision_monitor_node.launch.py +++ b/nav2_collision_monitor/launch/collision_monitor_node.launch.py @@ -37,8 +37,7 @@ def generate_launch_description(): # Constant parameters lifecycle_nodes = ['collision_monitor'] autostart = True - remappings = [('/tf', 'tf'), - ('/tf_static', 'tf_static')] + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] # Launch arguments # 1. Create the launch configuration variables @@ -51,35 +50,44 @@ def generate_launch_description(): # 2. Declare the launch arguments declare_namespace_cmd = DeclareLaunchArgument( - 'namespace', - default_value='', - description='Top-level namespace') + 'namespace', default_value='', description='Top-level namespace' + ) declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='True', - description='Use simulation (Gazebo) clock if true') + description='Use simulation (Gazebo) clock if true', + ) declare_params_file_cmd = DeclareLaunchArgument( 'params_file', - default_value=os.path.join(package_dir, 'params', 'collision_monitor_params.yaml'), - description='Full path to the ROS2 parameters file to use for all launched nodes') + default_value=os.path.join( + package_dir, 'params', 'collision_monitor_params.yaml' + ), + description='Full path to the ROS2 parameters file to use for all launched nodes', + ) declare_use_composition_cmd = DeclareLaunchArgument( - 'use_composition', default_value='True', - description='Use composed bringup if True') + 'use_composition', + default_value='True', + description='Use composed bringup if True', + ) declare_container_name_cmd = DeclareLaunchArgument( - 'container_name', default_value='nav2_container', - description='the name of conatiner that nodes will load in if use composition') + 'container_name', + default_value='nav2_container', + description='the name of conatiner that nodes will load in if use composition', + ) configured_params = ParameterFile( RewrittenYaml( source_file=params_file, root_key=namespace, param_rewrites={}, - convert_types=True), - allow_substs=True) + convert_types=True, + ), + allow_substs=True, + ) # Declare node launching commands load_nodes = GroupAction( @@ -87,25 +95,29 @@ def generate_launch_description(): actions=[ SetParameter('use_sim_time', use_sim_time), PushROSNamespace( - condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')), - namespace=namespace), + condition=IfCondition( + NotEqualsSubstitution(LaunchConfiguration('namespace'), '') + ), + namespace=namespace, + ), Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_collision_monitor', output='screen', emulate_tty=True, # https://github.com/ros2/launch/issues/188 - parameters=[{'autostart': autostart}, - {'node_names': lifecycle_nodes}], - remappings=remappings), + parameters=[{'autostart': autostart}, {'node_names': lifecycle_nodes}], + remappings=remappings, + ), Node( package='nav2_collision_monitor', executable='collision_monitor', output='screen', emulate_tty=True, # https://github.com/ros2/launch/issues/188 parameters=[configured_params], - remappings=remappings) - ] + remappings=remappings, + ), + ], ) load_composable_nodes = GroupAction( @@ -113,8 +125,11 @@ def generate_launch_description(): actions=[ SetParameter('use_sim_time', use_sim_time), PushROSNamespace( - condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')), - namespace=namespace), + condition=IfCondition( + NotEqualsSubstitution(LaunchConfiguration('namespace'), '') + ), + namespace=namespace, + ), LoadComposableNodes( target_container=container_name_full, composable_node_descriptions=[ @@ -122,18 +137,22 @@ def generate_launch_description(): package='nav2_lifecycle_manager', plugin='nav2_lifecycle_manager::LifecycleManager', name='lifecycle_manager_collision_monitor', - parameters=[{'autostart': autostart}, - {'node_names': lifecycle_nodes}], - remappings=remappings), + parameters=[ + {'autostart': autostart}, + {'node_names': lifecycle_nodes}, + ], + remappings=remappings, + ), ComposableNode( package='nav2_collision_monitor', plugin='nav2_collision_monitor::CollisionMonitor', name='collision_monitor', parameters=[configured_params], - remappings=remappings) + remappings=remappings, + ), ], - ) - ] + ), + ], ) ld = LaunchDescription() diff --git a/nav2_common/nav2_common/launch/__init__.py b/nav2_common/nav2_common/launch/__init__.py index 7eba78e192..61ea85dd79 100644 --- a/nav2_common/nav2_common/launch/__init__.py +++ b/nav2_common/nav2_common/launch/__init__.py @@ -13,6 +13,13 @@ # limitations under the License. from .has_node_params import HasNodeParams -from .rewritten_yaml import RewrittenYaml -from .replace_string import ReplaceString from .parse_multirobot_pose import ParseMultiRobotPose +from .replace_string import ReplaceString +from .rewritten_yaml import RewrittenYaml + +__all__ = [ + 'HasNodeParams', + 'RewrittenYaml', + 'ReplaceString', + 'ParseMultiRobotPose', +] diff --git a/nav2_common/nav2_common/launch/has_node_params.py b/nav2_common/nav2_common/launch/has_node_params.py index 92f96790b5..41f005bbc5 100644 --- a/nav2_common/nav2_common/launch/has_node_params.py +++ b/nav2_common/nav2_common/launch/has_node_params.py @@ -15,46 +15,48 @@ from typing import List from typing import Text -import yaml import launch +import yaml -import sys # delete this class HasNodeParams(launch.Substitution): - """ - Substitution that checks if a param file contains parameters for a node - - Used in launch system - """ + """ + Substitution that checks if a param file contains parameters for a node. - def __init__(self, - source_file: launch.SomeSubstitutionsType, - node_name: Text) -> None: - super().__init__() + Used in launch system """ + + def __init__( + self, source_file: launch.SomeSubstitutionsType, node_name: Text + ) -> None: + super().__init__() + """ Construct the substitution :param: source_file the parameter YAML file :param: node_name the name of the node to check """ - from launch.utilities import normalize_to_list_of_substitutions # import here to avoid loop - self.__source_file = normalize_to_list_of_substitutions(source_file) - self.__node_name = node_name + from launch.utilities import ( + normalize_to_list_of_substitutions, + ) # import here to avoid loop + + self.__source_file = normalize_to_list_of_substitutions(source_file) + self.__node_name = node_name - @property - def name(self) -> List[launch.Substitution]: - """Getter for name.""" - return self.__source_file + @property + def name(self) -> List[launch.Substitution]: + """Getter for name.""" + return self.__source_file - def describe(self) -> Text: - """Return a description of this substitution as a string.""" - return '' + def describe(self) -> Text: + """Return a description of this substitution as a string.""" + return '' - def perform(self, context: launch.LaunchContext) -> Text: - yaml_filename = launch.utilities.perform_substitutions(context, self.name) - data = yaml.safe_load(open(yaml_filename, 'r')) + def perform(self, context: launch.LaunchContext) -> Text: + yaml_filename = launch.utilities.perform_substitutions(context, self.name) + data = yaml.safe_load(open(yaml_filename, 'r')) - if self.__node_name in data.keys(): - return "True" - return "False" + if self.__node_name in data.keys(): + return 'True' + return 'False' diff --git a/nav2_common/nav2_common/launch/parse_multirobot_pose.py b/nav2_common/nav2_common/launch/parse_multirobot_pose.py index 9025f967fc..bb1725d36d 100644 --- a/nav2_common/nav2_common/launch/parse_multirobot_pose.py +++ b/nav2_common/nav2_common/launch/parse_multirobot_pose.py @@ -12,19 +12,18 @@ # See the License for the specific language governing permissions and # limitations under the License. -import yaml import sys -from typing import Text, Dict +from typing import Dict, Text + +import yaml -class ParseMultiRobotPose(): - """ - Parsing argument using sys module - """ +class ParseMultiRobotPose: + """Parsing argument using sys module.""" def __init__(self, target_argument: Text): """ - Parse arguments for multi-robot's pose + Parse arguments for multi-robot's pose. for example, `ros2 launch nav2_bringup bringup_multirobot_launch.py @@ -42,23 +41,19 @@ def __init__(self, target_argument: Text): self.__args: Text = self.__parse_argument(target_argument) def __parse_argument(self, target_argument: Text) -> Text: - """ - get value of target argument - """ + """Get value of target argument.""" if len(sys.argv) > 4: argv = sys.argv[4:] for arg in argv: - if arg.startswith(target_argument + ":="): - return arg.replace(target_argument + ":=", "") - return "" + if arg.startswith(target_argument + ':='): + return arg.replace(target_argument + ':=', '') + return '' def value(self) -> Dict: - """ - get value of target argument - """ + """Get value of target argument.""" args = self.__args - parsed_args = list() if len(args) == 0 else args.split(';') - multirobots = dict() + parsed_args = [] if len(args) == 0 else args.split(';') + multirobots = {} for arg in parsed_args: key_val = arg.strip().split('=') if len(key_val) != 2: diff --git a/nav2_common/nav2_common/launch/replace_string.py b/nav2_common/nav2_common/launch/replace_string.py index 9d22f1732d..076ac16f21 100644 --- a/nav2_common/nav2_common/launch/replace_string.py +++ b/nav2_common/nav2_common/launch/replace_string.py @@ -12,76 +12,87 @@ # See the License for the specific language governing permissions and # limitations under the License. -from typing import Dict -from typing import List -from typing import Text -from typing import Optional import tempfile +from typing import Dict, List, Optional, Text + import launch -class ReplaceString(launch.Substitution): - """ - Substitution that replaces strings on a given file. - Used in launch system - """ +class ReplaceString(launch.Substitution): + """ + Substitution that replaces strings on a given file. - def __init__(self, - source_file: launch.SomeSubstitutionsType, - replacements: Dict, - condition: Optional[launch.Condition] = None) -> None: - super().__init__() + Used in launch system + """ - from launch.utilities import normalize_to_list_of_substitutions # import here to avoid loop - self.__source_file = normalize_to_list_of_substitutions(source_file) - self.__replacements = {} - for key in replacements: - self.__replacements[key] = normalize_to_list_of_substitutions(replacements[key]) - self.__condition = condition + def __init__( + self, + source_file: launch.SomeSubstitutionsType, + replacements: Dict, + condition: Optional[launch.Condition] = None, + ) -> None: + super().__init__() - @property - def name(self) -> List[launch.Substitution]: - """Getter for name.""" - return self.__source_file + from launch.utilities import ( + normalize_to_list_of_substitutions, + ) # import here to avoid loop - @property - def condition(self) -> Optional[launch.Condition]: - """Getter for condition.""" - return self.__condition + self.__source_file = normalize_to_list_of_substitutions(source_file) + self.__replacements = {} + for key in replacements: + self.__replacements[key] = normalize_to_list_of_substitutions( + replacements[key] + ) + self.__condition = condition - def describe(self) -> Text: - """Return a description of this substitution as a string.""" - return '' + @property + def name(self) -> List[launch.Substitution]: + """Getter for name.""" + return self.__source_file - def perform(self, context: launch.LaunchContext) -> Text: - yaml_filename = launch.utilities.perform_substitutions(context, self.name) - if self.__condition is None or self.__condition.evaluate(context): - output_file = tempfile.NamedTemporaryFile(mode='w', delete=False) - replacements = self.resolve_replacements(context) - try: - input_file = open(yaml_filename, 'r') - self.replace(input_file, output_file, replacements) - except Exception as err: # noqa: B902 - print('ReplaceString substitution error: ', err) - finally: - input_file.close() - output_file.close() - return output_file.name - else: - return yaml_filename + @property + def condition(self) -> Optional[launch.Condition]: + """Getter for condition.""" + return self.__condition - def resolve_replacements(self, context): - resolved_replacements = {} - for key in self.__replacements: - resolved_replacements[key] = launch.utilities.perform_substitutions(context, self.__replacements[key]) - return resolved_replacements + def describe(self) -> Text: + """Return a description of this substitution as a string.""" + return '' - def replace(self, input_file, output_file, replacements): - for line in input_file: - for key, value in replacements.items(): - if isinstance(key, str) and isinstance(value, str): - if key in line: - line = line.replace(key, value) + def perform(self, context: launch.LaunchContext) -> Text: + yaml_filename = launch.utilities.perform_substitutions(context, self.name) + if self.__condition is None or self.__condition.evaluate(context): + output_file = tempfile.NamedTemporaryFile(mode='w', delete=False) + replacements = self.resolve_replacements(context) + try: + input_file = open(yaml_filename, 'r') + self.replace(input_file, output_file, replacements) + except Exception as err: # noqa: B902 + print('ReplaceString substitution error: ', err) + finally: + input_file.close() + output_file.close() + return output_file.name else: - raise TypeError('A provided replacement pair is not a string. Both key and value should be strings.') - output_file.write(line) + return yaml_filename + + def resolve_replacements(self, context): + resolved_replacements = {} + for key in self.__replacements: + resolved_replacements[key] = launch.utilities.perform_substitutions( + context, self.__replacements[key] + ) + return resolved_replacements + + def replace(self, input_file, output_file, replacements): + for line in input_file: + for key, value in replacements.items(): + if isinstance(key, str) and isinstance(value, str): + if key in line: + line = line.replace(key, value) + else: + raise TypeError( + 'A provided replacement pair is not a string. Both key and value should be' + 'strings.' + ) + output_file.write(line) diff --git a/nav2_common/nav2_common/launch/rewritten_yaml.py b/nav2_common/nav2_common/launch/rewritten_yaml.py index 071c884864..c28dc88cc0 100644 --- a/nav2_common/nav2_common/launch/rewritten_yaml.py +++ b/nav2_common/nav2_common/launch/rewritten_yaml.py @@ -12,14 +12,11 @@ # See the License for the specific language governing permissions and # limitations under the License. -from typing import Dict -from typing import List -from typing import Text -from typing import Optional - -import yaml import tempfile +from typing import Dict, List, Optional, Text + import launch +import yaml class DictItemReference: @@ -41,12 +38,14 @@ class RewrittenYaml(launch.Substitution): Used in launch system """ - def __init__(self, + def __init__( + self, source_file: launch.SomeSubstitutionsType, param_rewrites: Dict, root_key: Optional[launch.SomeSubstitutionsType] = None, key_rewrites: Optional[Dict] = None, - convert_types = False) -> None: + convert_types=False, + ) -> None: super().__init__() """ Construct the substitution @@ -58,17 +57,24 @@ def __init__(self, :param: convert_types whether to attempt converting the string to a number or boolean """ - from launch.utilities import normalize_to_list_of_substitutions # import here to avoid loop + from launch.utilities import ( + normalize_to_list_of_substitutions, + ) # import here to avoid loop + self.__source_file = normalize_to_list_of_substitutions(source_file) self.__param_rewrites = {} self.__key_rewrites = {} self.__convert_types = convert_types self.__root_key = None for key in param_rewrites: - self.__param_rewrites[key] = normalize_to_list_of_substitutions(param_rewrites[key]) + self.__param_rewrites[key] = normalize_to_list_of_substitutions( + param_rewrites[key] + ) if key_rewrites is not None: for key in key_rewrites: - self.__key_rewrites[key] = normalize_to_list_of_substitutions(key_rewrites[key]) + self.__key_rewrites[key] = normalize_to_list_of_substitutions( + key_rewrites[key] + ) if root_key is not None: self.__root_key = normalize_to_list_of_substitutions(root_key) @@ -100,10 +106,14 @@ def perform(self, context: launch.LaunchContext) -> Text: def resolve_rewrites(self, context): resolved_params = {} for key in self.__param_rewrites: - resolved_params[key] = launch.utilities.perform_substitutions(context, self.__param_rewrites[key]) + resolved_params[key] = launch.utilities.perform_substitutions( + context, self.__param_rewrites[key] + ) resolved_keys = {} for key in self.__key_rewrites: - resolved_keys[key] = launch.utilities.perform_substitutions(context, self.__key_rewrites[key]) + resolved_keys[key] = launch.utilities.perform_substitutions( + context, self.__key_rewrites[key] + ) return resolved_params, resolved_keys def substitute_params(self, yaml, param_rewrites): @@ -126,7 +136,7 @@ def add_params(self, yaml, param_rewrites): # add new total path parameters yaml_paths = self.pathify(yaml) for path in param_rewrites: - if not path in yaml_paths: + if not path in yaml_paths: # noqa: E713 new_val = self.convert(param_rewrites[path]) yaml_keys = path.split('.') if 'ros__parameters' in yaml_keys: @@ -139,9 +149,13 @@ def updateYamlPathVals(self, yaml, yaml_key_list, rewrite_val): break key = yaml_key_list.pop(0) if isinstance(yaml, list): - yaml[int(key)] = self.updateYamlPathVals(yaml[int(key)], yaml_key_list, rewrite_val) + yaml[int(key)] = self.updateYamlPathVals( + yaml[int(key)], yaml_key_list, rewrite_val + ) else: - yaml[key] = self.updateYamlPathVals(yaml.get(key, {}), yaml_key_list, rewrite_val) + yaml[key] = self.updateYamlPathVals( + yaml.get(key, {}), yaml_key_list, rewrite_val + ) return yaml def substitute_keys(self, yaml, key_rewrites): @@ -167,10 +181,10 @@ def getYamlLeafKeys(self, yamlData): def pathify(self, d, p=None, paths=None, joinchar='.'): if p is None: paths = {} - self.pathify(d, "", paths, joinchar=joinchar) + self.pathify(d, '', paths, joinchar=joinchar) return paths pn = p - if p != "": + if p != '': pn += joinchar if isinstance(d, dict): for k in d: @@ -191,9 +205,9 @@ def convert(self, text_value): pass # try converting to bool - if text_value.lower() == "true": + if text_value.lower() == 'true': return True - if text_value.lower() == "false": + if text_value.lower() == 'false': return False # nothing else worked so fall through and return text diff --git a/nav2_costmap_2d/test/integration/costmap_tests_launch.py b/nav2_costmap_2d/test/integration/costmap_tests_launch.py index 706290d826..692c4f9fce 100755 --- a/nav2_costmap_2d/test/integration/costmap_tests_launch.py +++ b/nav2_costmap_2d/test/integration/costmap_tests_launch.py @@ -27,21 +27,23 @@ def main(argv=sys.argv[1:]): - launchFile = os.path.join(os.getenv('TEST_LAUNCH_DIR'), 'costmap_map_server.launch.py') + launchFile = os.path.join( + os.getenv('TEST_LAUNCH_DIR'), 'costmap_map_server.launch.py' + ) testExecutable = os.getenv('TEST_EXECUTABLE') map_to_odom = launch_ros.actions.Node( package='tf2_ros', executable='static_transform_publisher', output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'] + arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'], ) odom_to_base_link = launch_ros.actions.Node( package='tf2_ros', executable='static_transform_publisher', output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'odom', 'base_link'] + arguments=['0', '0', '0', '0', '0', '0', 'odom', 'base_link'], ) lifecycle_manager = launch_ros.actions.Node( @@ -49,19 +51,20 @@ def main(argv=sys.argv[1:]): executable='lifecycle_manager', name='lifecycle_manager', output='screen', - parameters=[{'node_names': ['map_server']}, {'autostart': True}]) + parameters=[{'node_names': ['map_server']}, {'autostart': True}], + ) - ld = LaunchDescription([ - IncludeLaunchDescription(PythonLaunchDescriptionSource([launchFile])), - map_to_odom, - odom_to_base_link, - lifecycle_manager - ]) + ld = LaunchDescription( + [ + IncludeLaunchDescription(PythonLaunchDescriptionSource([launchFile])), + map_to_odom, + odom_to_base_link, + lifecycle_manager, + ] + ) test1_action = ExecuteProcess( - cmd=[testExecutable], - name='costmap_tests', - output='screen' + cmd=[testExecutable], name='costmap_tests', output='screen' ) lts = LaunchTestService() diff --git a/nav2_costmap_2d/test/test_launch_files/costmap_map_server.launch.py b/nav2_costmap_2d/test/test_launch_files/costmap_map_server.launch.py index d4b31798f4..622d55e074 100644 --- a/nav2_costmap_2d/test/test_launch_files/costmap_map_server.launch.py +++ b/nav2_costmap_2d/test/test_launch_files/costmap_map_server.launch.py @@ -22,11 +22,14 @@ def generate_launch_description(): mapFile = os.getenv('TEST_MAP') - return LaunchDescription([ - launch_ros.actions.Node( - package='nav2_map_server', - executable='map_server', - name='map_server', - output='screen', - parameters=[{'yaml_filename': mapFile}]) - ]) + return LaunchDescription( + [ + launch_ros.actions.Node( + package='nav2_map_server', + executable='map_server', + name='map_server', + output='screen', + parameters=[{'yaml_filename': mapFile}], + ) + ] + ) diff --git a/nav2_lifecycle_manager/test/launch_bond_test.py b/nav2_lifecycle_manager/test/launch_bond_test.py index 061375db45..728a061001 100755 --- a/nav2_lifecycle_manager/test/launch_bond_test.py +++ b/nav2_lifecycle_manager/test/launch_bond_test.py @@ -24,16 +24,21 @@ def generate_launch_description(): - return LaunchDescription([ - Node( - package='nav2_lifecycle_manager', - executable='lifecycle_manager', - name='lifecycle_manager_test', - output='screen', - parameters=[{'use_sim_time': False}, - {'autostart': False}, - {'node_names': ['bond_tester']}]), - ]) + return LaunchDescription( + [ + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_test', + output='screen', + parameters=[ + {'use_sim_time': False}, + {'autostart': False}, + {'node_names': ['bond_tester']}, + ], + ), + ] + ) def main(argv=sys.argv[1:]): @@ -42,9 +47,8 @@ def main(argv=sys.argv[1:]): testExecutable = os.getenv('TEST_EXECUTABLE') test1_action = ExecuteProcess( - cmd=[testExecutable], - name='test_bond_gtest', - output='screen') + cmd=[testExecutable], name='test_bond_gtest', output='screen' + ) lts = LaunchTestService() lts.add_test_action(ld, test1_action) diff --git a/nav2_lifecycle_manager/test/launch_lifecycle_test.py b/nav2_lifecycle_manager/test/launch_lifecycle_test.py index c77d58d76a..0658236185 100755 --- a/nav2_lifecycle_manager/test/launch_lifecycle_test.py +++ b/nav2_lifecycle_manager/test/launch_lifecycle_test.py @@ -24,17 +24,22 @@ def generate_launch_description(): - return LaunchDescription([ - Node( - package='nav2_lifecycle_manager', - executable='lifecycle_manager', - name='lifecycle_manager_test', - output='screen', - parameters=[{'use_sim_time': False}, - {'autostart': False}, - {'bond_timeout': 0.0}, - {'node_names': ['lifecycle_node_test']}]), - ]) + return LaunchDescription( + [ + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_test', + output='screen', + parameters=[ + {'use_sim_time': False}, + {'autostart': False}, + {'bond_timeout': 0.0}, + {'node_names': ['lifecycle_node_test']}, + ], + ), + ] + ) def main(argv=sys.argv[1:]): @@ -43,9 +48,8 @@ def main(argv=sys.argv[1:]): testExecutable = os.getenv('TEST_EXECUTABLE') test1_action = ExecuteProcess( - cmd=[testExecutable], - name='test_lifecycle_node_gtest', - output='screen') + cmd=[testExecutable], name='test_lifecycle_node_gtest', output='screen' + ) lts = LaunchTestService() lts.add_test_action(ld, test1_action) diff --git a/nav2_map_server/launch/map_saver_server.launch.py b/nav2_map_server/launch/map_saver_server.launch.py index 53731569bb..aa3f2c4f7b 100644 --- a/nav2_map_server/launch/map_saver_server.launch.py +++ b/nav2_map_server/launch/map_saver_server.launch.py @@ -29,23 +29,29 @@ def generate_launch_description(): # Nodes launching commands start_map_saver_server_cmd = launch_ros.actions.Node( - package='nav2_map_server', - executable='map_saver_server', - output='screen', - emulate_tty=True, # https://github.com/ros2/launch/issues/188 - parameters=[{'save_map_timeout': save_map_timeout}, - {'free_thresh_default': free_thresh_default}, - {'occupied_thresh_default': occupied_thresh_default}]) + package='nav2_map_server', + executable='map_saver_server', + output='screen', + emulate_tty=True, # https://github.com/ros2/launch/issues/188 + parameters=[ + {'save_map_timeout': save_map_timeout}, + {'free_thresh_default': free_thresh_default}, + {'occupied_thresh_default': occupied_thresh_default}, + ], + ) start_lifecycle_manager_cmd = launch_ros.actions.Node( - package='nav2_lifecycle_manager', - executable='lifecycle_manager', - name='lifecycle_manager', - output='screen', - emulate_tty=True, # https://github.com/ros2/launch/issues/188 - parameters=[{'use_sim_time': use_sim_time}, - {'autostart': autostart}, - {'node_names': lifecycle_nodes}]) + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager', + output='screen', + emulate_tty=True, # https://github.com/ros2/launch/issues/188 + parameters=[ + {'use_sim_time': use_sim_time}, + {'autostart': autostart}, + {'node_names': lifecycle_nodes}, + ], + ) ld = LaunchDescription() diff --git a/nav2_map_server/test/component/test_map_saver_launch.py b/nav2_map_server/test/component/test_map_saver_launch.py index a491a2d8de..e33e03a986 100755 --- a/nav2_map_server/test/component/test_map_saver_launch.py +++ b/nav2_map_server/test/component/test_map_saver_launch.py @@ -28,9 +28,11 @@ def main(argv=sys.argv[1:]): launchFile = os.path.join(os.getenv('TEST_LAUNCH_DIR'), 'map_saver_node.launch.py') testExecutable = os.getenv('TEST_EXECUTABLE') - ld = LaunchDescription([ - IncludeLaunchDescription(PythonLaunchDescriptionSource([launchFile])), - ]) + ld = LaunchDescription( + [ + IncludeLaunchDescription(PythonLaunchDescriptionSource([launchFile])), + ] + ) test1_action = ExecuteProcess( cmd=[testExecutable], name='test_map_saver_node', diff --git a/nav2_map_server/test/component/test_map_server_launch.py b/nav2_map_server/test/component/test_map_server_launch.py index 8164294085..473feea97e 100755 --- a/nav2_map_server/test/component/test_map_server_launch.py +++ b/nav2_map_server/test/component/test_map_server_launch.py @@ -28,9 +28,11 @@ def main(argv=sys.argv[1:]): launchFile = os.path.join(os.getenv('TEST_LAUNCH_DIR'), 'map_server_node.launch.py') testExecutable = os.getenv('TEST_EXECUTABLE') - ld = LaunchDescription([ - IncludeLaunchDescription(PythonLaunchDescriptionSource([launchFile])), - ]) + ld = LaunchDescription( + [ + IncludeLaunchDescription(PythonLaunchDescriptionSource([launchFile])), + ] + ) test1_action = ExecuteProcess( cmd=[testExecutable], name='test_map_server_node', diff --git a/nav2_map_server/test/test_launch_files/map_saver_node.launch.py b/nav2_map_server/test/test_launch_files/map_saver_node.launch.py index 7287f73594..a0ac0aeb8a 100644 --- a/nav2_map_server/test/test_launch_files/map_saver_node.launch.py +++ b/nav2_map_server/test/test_launch_files/map_saver_node.launch.py @@ -22,7 +22,9 @@ def generate_launch_description(): - map_publisher = f"{os.path.dirname(os.getenv('TEST_EXECUTABLE'))}/test_map_saver_publisher" + map_publisher = ( + f"{os.path.dirname(os.getenv('TEST_EXECUTABLE'))}/test_map_saver_publisher" + ) ld = LaunchDescription() @@ -30,11 +32,10 @@ def generate_launch_description(): package='nav2_map_server', executable='map_saver_server', output='screen', - parameters=[os.path.join(os.getenv('TEST_DIR'), - 'map_saver_params.yaml')]) + parameters=[os.path.join(os.getenv('TEST_DIR'), 'map_saver_params.yaml')], + ) - map_publisher_cmd = ExecuteProcess( - cmd=[map_publisher]) + map_publisher_cmd = ExecuteProcess(cmd=[map_publisher]) ld.add_action(map_saver_server_cmd) ld.add_action(map_publisher_cmd) diff --git a/nav2_map_server/test/test_launch_files/map_server_node.launch.py b/nav2_map_server/test/test_launch_files/map_server_node.launch.py index 78d5e8b6d1..813abf7ea1 100644 --- a/nav2_map_server/test/test_launch_files/map_server_node.launch.py +++ b/nav2_map_server/test/test_launch_files/map_server_node.launch.py @@ -21,11 +21,15 @@ def generate_launch_description(): - return LaunchDescription([ - launch_ros.actions.Node( - package='nav2_map_server', - executable='map_server', - output='screen', - parameters=[os.path.join(os.getenv('TEST_DIR'), - 'map_server_params.yaml')]) - ]) + return LaunchDescription( + [ + launch_ros.actions.Node( + package='nav2_map_server', + executable='map_server', + output='screen', + parameters=[ + os.path.join(os.getenv('TEST_DIR'), 'map_server_params.yaml') + ], + ) + ] + ) diff --git a/nav2_simple_commander/launch/assisted_teleop_example_launch.py b/nav2_simple_commander/launch/assisted_teleop_example_launch.py index cd37de2bdf..3324b0ee4e 100644 --- a/nav2_simple_commander/launch/assisted_teleop_example_launch.py +++ b/nav2_simple_commander/launch/assisted_teleop_example_launch.py @@ -18,7 +18,11 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + IncludeLaunchDescription, +) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression @@ -39,24 +43,26 @@ def generate_launch_description(): # Declare the launch arguments declare_use_rviz_cmd = DeclareLaunchArgument( - 'use_rviz', - default_value='True', - description='Whether to start RVIZ') + 'use_rviz', default_value='True', description='Whether to start RVIZ' + ) declare_simulator_cmd = DeclareLaunchArgument( - 'headless', - default_value='False', - description='Whether to execute gzclient)') + 'headless', default_value='False', description='Whether to execute gzclient)' + ) # start the simulation start_gazebo_server_cmd = ExecuteProcess( cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], - cwd=[warehouse_dir], output='screen') + cwd=[warehouse_dir], + output='screen', + ) start_gazebo_client_cmd = ExecuteProcess( condition=IfCondition(PythonExpression(['not ', headless])), cmd=['gzclient'], - cwd=[warehouse_dir], output='screen') + cwd=[warehouse_dir], + output='screen', + ) urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') start_robot_state_publisher_cmd = Node( @@ -64,28 +70,33 @@ def generate_launch_description(): executable='robot_state_publisher', name='robot_state_publisher', output='screen', - arguments=[urdf]) + arguments=[urdf], + ) # start the visualization rviz_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py') + ), condition=IfCondition(use_rviz), - launch_arguments={'namespace': '', - 'use_namespace': 'False'}.items()) + 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()) + 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_assisted_teleop', emulate_tty=True, - output='screen') + output='screen', + ) ld = LaunchDescription() ld.add_action(declare_use_rviz_cmd) diff --git a/nav2_simple_commander/launch/follow_path_example_launch.py b/nav2_simple_commander/launch/follow_path_example_launch.py index e933ed421c..b4fe9b586f 100644 --- a/nav2_simple_commander/launch/follow_path_example_launch.py +++ b/nav2_simple_commander/launch/follow_path_example_launch.py @@ -17,7 +17,11 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + IncludeLaunchDescription, +) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression @@ -38,24 +42,26 @@ def generate_launch_description(): # Declare the launch arguments declare_use_rviz_cmd = DeclareLaunchArgument( - 'use_rviz', - default_value='True', - description='Whether to start RVIZ') + 'use_rviz', default_value='True', description='Whether to start RVIZ' + ) declare_simulator_cmd = DeclareLaunchArgument( - 'headless', - default_value='False', - description='Whether to execute gzclient)') + 'headless', default_value='False', description='Whether to execute gzclient)' + ) # start the simulation start_gazebo_server_cmd = ExecuteProcess( cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], - cwd=[warehouse_dir], output='screen') + cwd=[warehouse_dir], + output='screen', + ) start_gazebo_client_cmd = ExecuteProcess( condition=IfCondition(PythonExpression(['not ', headless])), cmd=['gzclient'], - cwd=[warehouse_dir], output='screen') + cwd=[warehouse_dir], + output='screen', + ) urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') start_robot_state_publisher_cmd = Node( @@ -63,28 +69,33 @@ def generate_launch_description(): executable='robot_state_publisher', name='robot_state_publisher', output='screen', - arguments=[urdf]) + arguments=[urdf], + ) # start the visualization rviz_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py') + ), condition=IfCondition(use_rviz), - launch_arguments={'namespace': '', - 'use_namespace': 'False'}.items()) + 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()) + 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') + output='screen', + ) ld = LaunchDescription() ld.add_action(declare_use_rviz_cmd) diff --git a/nav2_simple_commander/launch/inspection_demo_launch.py b/nav2_simple_commander/launch/inspection_demo_launch.py index 43456f6398..285f26099e 100644 --- a/nav2_simple_commander/launch/inspection_demo_launch.py +++ b/nav2_simple_commander/launch/inspection_demo_launch.py @@ -17,7 +17,11 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + IncludeLaunchDescription, +) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression @@ -38,24 +42,26 @@ def generate_launch_description(): # Declare the launch arguments declare_use_rviz_cmd = DeclareLaunchArgument( - 'use_rviz', - default_value='True', - description='Whether to start RVIZ') + 'use_rviz', default_value='True', description='Whether to start RVIZ' + ) declare_simulator_cmd = DeclareLaunchArgument( - 'headless', - default_value='False', - description='Whether to execute gzclient)') + 'headless', default_value='False', description='Whether to execute gzclient)' + ) # start the simulation start_gazebo_server_cmd = ExecuteProcess( cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], - cwd=[warehouse_dir], output='screen') + cwd=[warehouse_dir], + output='screen', + ) start_gazebo_client_cmd = ExecuteProcess( condition=IfCondition(PythonExpression(['not ', headless])), cmd=['gzclient'], - cwd=[warehouse_dir], output='screen') + cwd=[warehouse_dir], + output='screen', + ) urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') start_robot_state_publisher_cmd = Node( @@ -63,28 +69,33 @@ def generate_launch_description(): executable='robot_state_publisher', name='robot_state_publisher', output='screen', - arguments=[urdf]) + arguments=[urdf], + ) # start the visualization rviz_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py') + ), condition=IfCondition(use_rviz), - launch_arguments={'namespace': '', - 'use_namespace': 'False'}.items()) + 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()) + 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='demo_inspection', emulate_tty=True, - output='screen') + output='screen', + ) ld = LaunchDescription() ld.add_action(declare_use_rviz_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 0c9fedaad9..e8b0fefba0 100644 --- a/nav2_simple_commander/launch/nav_through_poses_example_launch.py +++ b/nav2_simple_commander/launch/nav_through_poses_example_launch.py @@ -17,7 +17,11 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + IncludeLaunchDescription, +) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression @@ -38,24 +42,26 @@ def generate_launch_description(): # Declare the launch arguments declare_use_rviz_cmd = DeclareLaunchArgument( - 'use_rviz', - default_value='True', - description='Whether to start RVIZ') + 'use_rviz', default_value='True', description='Whether to start RVIZ' + ) declare_simulator_cmd = DeclareLaunchArgument( - 'headless', - default_value='False', - description='Whether to execute gzclient)') + 'headless', default_value='False', description='Whether to execute gzclient)' + ) # start the simulation start_gazebo_server_cmd = ExecuteProcess( cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], - cwd=[warehouse_dir], output='screen') + cwd=[warehouse_dir], + output='screen', + ) start_gazebo_client_cmd = ExecuteProcess( condition=IfCondition(PythonExpression(['not ', headless])), cmd=['gzclient'], - cwd=[warehouse_dir], output='screen') + cwd=[warehouse_dir], + output='screen', + ) urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') start_robot_state_publisher_cmd = Node( @@ -63,28 +69,33 @@ def generate_launch_description(): executable='robot_state_publisher', name='robot_state_publisher', output='screen', - arguments=[urdf]) + arguments=[urdf], + ) # start the visualization rviz_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py') + ), condition=IfCondition(use_rviz), - launch_arguments={'namespace': '', - 'use_namespace': 'False'}.items()) + 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()) + 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_nav_through_poses', emulate_tty=True, - output='screen') + output='screen', + ) ld = LaunchDescription() ld.add_action(declare_use_rviz_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 d425949409..29aeddebd1 100644 --- a/nav2_simple_commander/launch/nav_to_pose_example_launch.py +++ b/nav2_simple_commander/launch/nav_to_pose_example_launch.py @@ -17,7 +17,11 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + IncludeLaunchDescription, +) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression @@ -38,24 +42,26 @@ def generate_launch_description(): # Declare the launch arguments declare_use_rviz_cmd = DeclareLaunchArgument( - 'use_rviz', - default_value='True', - description='Whether to start RVIZ') + 'use_rviz', default_value='True', description='Whether to start RVIZ' + ) declare_simulator_cmd = DeclareLaunchArgument( - 'headless', - default_value='False', - description='Whether to execute gzclient)') + 'headless', default_value='False', description='Whether to execute gzclient)' + ) # start the simulation start_gazebo_server_cmd = ExecuteProcess( cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], - cwd=[warehouse_dir], output='screen') + cwd=[warehouse_dir], + output='screen', + ) start_gazebo_client_cmd = ExecuteProcess( condition=IfCondition(PythonExpression(['not ', headless])), cmd=['gzclient'], - cwd=[warehouse_dir], output='screen') + cwd=[warehouse_dir], + output='screen', + ) urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') start_robot_state_publisher_cmd = Node( @@ -63,28 +69,33 @@ def generate_launch_description(): executable='robot_state_publisher', name='robot_state_publisher', output='screen', - arguments=[urdf]) + arguments=[urdf], + ) # start the visualization rviz_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py') + ), condition=IfCondition(use_rviz), - launch_arguments={'namespace': '', - 'use_namespace': 'False'}.items()) + 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()) + 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_nav_to_pose', emulate_tty=True, - output='screen') + output='screen', + ) ld = LaunchDescription() ld.add_action(declare_use_rviz_cmd) diff --git a/nav2_simple_commander/launch/picking_demo_launch.py b/nav2_simple_commander/launch/picking_demo_launch.py index 5fce570106..8505b6bbc3 100644 --- a/nav2_simple_commander/launch/picking_demo_launch.py +++ b/nav2_simple_commander/launch/picking_demo_launch.py @@ -17,7 +17,11 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + IncludeLaunchDescription, +) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression @@ -38,24 +42,26 @@ def generate_launch_description(): # Declare the launch arguments declare_use_rviz_cmd = DeclareLaunchArgument( - 'use_rviz', - default_value='True', - description='Whether to start RVIZ') + 'use_rviz', default_value='True', description='Whether to start RVIZ' + ) declare_simulator_cmd = DeclareLaunchArgument( - 'headless', - default_value='False', - description='Whether to execute gzclient)') + 'headless', default_value='False', description='Whether to execute gzclient)' + ) # start the simulation start_gazebo_server_cmd = ExecuteProcess( cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], - cwd=[warehouse_dir], output='screen') + cwd=[warehouse_dir], + output='screen', + ) start_gazebo_client_cmd = ExecuteProcess( condition=IfCondition(PythonExpression(['not ', headless])), cmd=['gzclient'], - cwd=[warehouse_dir], output='screen') + cwd=[warehouse_dir], + output='screen', + ) urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') start_robot_state_publisher_cmd = Node( @@ -63,28 +69,33 @@ def generate_launch_description(): executable='robot_state_publisher', name='robot_state_publisher', output='screen', - arguments=[urdf]) + arguments=[urdf], + ) # start the visualization rviz_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py') + ), condition=IfCondition(use_rviz), - launch_arguments={'namespace': '', - 'use_namespace': 'False'}.items()) + 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()) + 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='demo_picking', emulate_tty=True, - output='screen') + output='screen', + ) ld = LaunchDescription() ld.add_action(declare_use_rviz_cmd) diff --git a/nav2_simple_commander/launch/recoveries_example_launch.py b/nav2_simple_commander/launch/recoveries_example_launch.py index d7b419711e..638550c44e 100644 --- a/nav2_simple_commander/launch/recoveries_example_launch.py +++ b/nav2_simple_commander/launch/recoveries_example_launch.py @@ -17,7 +17,11 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + IncludeLaunchDescription, +) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression @@ -38,24 +42,26 @@ def generate_launch_description(): # Declare the launch arguments declare_use_rviz_cmd = DeclareLaunchArgument( - 'use_rviz', - default_value='True', - description='Whether to start RVIZ') + 'use_rviz', default_value='True', description='Whether to start RVIZ' + ) declare_simulator_cmd = DeclareLaunchArgument( - 'headless', - default_value='False', - description='Whether to execute gzclient)') + 'headless', default_value='False', description='Whether to execute gzclient)' + ) # start the simulation start_gazebo_server_cmd = ExecuteProcess( cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], - cwd=[warehouse_dir], output='screen') + cwd=[warehouse_dir], + output='screen', + ) start_gazebo_client_cmd = ExecuteProcess( condition=IfCondition(PythonExpression(['not ', headless])), cmd=['gzclient'], - cwd=[warehouse_dir], output='screen') + cwd=[warehouse_dir], + output='screen', + ) urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') start_robot_state_publisher_cmd = Node( @@ -63,28 +69,33 @@ def generate_launch_description(): executable='robot_state_publisher', name='robot_state_publisher', output='screen', - arguments=[urdf]) + arguments=[urdf], + ) # start the visualization rviz_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py') + ), condition=IfCondition(use_rviz), - launch_arguments={'namespace': '', - 'use_namespace': 'False'}.items()) + 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()) + 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='demo_recoveries', emulate_tty=True, - output='screen') + output='screen', + ) ld = LaunchDescription() ld.add_action(declare_use_rviz_cmd) diff --git a/nav2_simple_commander/launch/security_demo_launch.py b/nav2_simple_commander/launch/security_demo_launch.py index c04de67d45..a958a75f73 100644 --- a/nav2_simple_commander/launch/security_demo_launch.py +++ b/nav2_simple_commander/launch/security_demo_launch.py @@ -17,7 +17,11 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + IncludeLaunchDescription, +) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression @@ -38,24 +42,26 @@ def generate_launch_description(): # Declare the launch arguments declare_use_rviz_cmd = DeclareLaunchArgument( - 'use_rviz', - default_value='True', - description='Whether to start RVIZ') + 'use_rviz', default_value='True', description='Whether to start RVIZ' + ) declare_simulator_cmd = DeclareLaunchArgument( - 'headless', - default_value='False', - description='Whether to execute gzclient)') + 'headless', default_value='False', description='Whether to execute gzclient)' + ) # start the simulation start_gazebo_server_cmd = ExecuteProcess( cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], - cwd=[warehouse_dir], output='screen') + cwd=[warehouse_dir], + output='screen', + ) start_gazebo_client_cmd = ExecuteProcess( condition=IfCondition(PythonExpression(['not ', headless])), cmd=['gzclient'], - cwd=[warehouse_dir], output='screen') + cwd=[warehouse_dir], + output='screen', + ) urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') start_robot_state_publisher_cmd = Node( @@ -63,28 +69,33 @@ def generate_launch_description(): executable='robot_state_publisher', name='robot_state_publisher', output='screen', - arguments=[urdf]) + arguments=[urdf], + ) # start the visualization rviz_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py') + ), condition=IfCondition(use_rviz), - launch_arguments={'namespace': '', - 'use_namespace': 'False'}.items()) + 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()) + 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='demo_security', emulate_tty=True, - output='screen') + output='screen', + ) ld = LaunchDescription() ld.add_action(declare_use_rviz_cmd) diff --git a/nav2_simple_commander/launch/waypoint_follower_example_launch.py b/nav2_simple_commander/launch/waypoint_follower_example_launch.py index 343017c0e4..f164035e3b 100644 --- a/nav2_simple_commander/launch/waypoint_follower_example_launch.py +++ b/nav2_simple_commander/launch/waypoint_follower_example_launch.py @@ -17,7 +17,11 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + IncludeLaunchDescription, +) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression @@ -38,24 +42,26 @@ def generate_launch_description(): # Declare the launch arguments declare_use_rviz_cmd = DeclareLaunchArgument( - 'use_rviz', - default_value='True', - description='Whether to start RVIZ') + 'use_rviz', default_value='True', description='Whether to start RVIZ' + ) declare_simulator_cmd = DeclareLaunchArgument( - 'headless', - default_value='False', - description='Whether to execute gzclient)') + 'headless', default_value='False', description='Whether to execute gzclient)' + ) # start the simulation start_gazebo_server_cmd = ExecuteProcess( cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], - cwd=[warehouse_dir], output='screen') + cwd=[warehouse_dir], + output='screen', + ) start_gazebo_client_cmd = ExecuteProcess( condition=IfCondition(PythonExpression(['not ', headless])), cmd=['gzclient'], - cwd=[warehouse_dir], output='screen') + cwd=[warehouse_dir], + output='screen', + ) urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') start_robot_state_publisher_cmd = Node( @@ -63,28 +69,33 @@ def generate_launch_description(): executable='robot_state_publisher', name='robot_state_publisher', output='screen', - arguments=[urdf]) + arguments=[urdf], + ) # start the visualization rviz_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py') + ), condition=IfCondition(use_rviz), - launch_arguments={'namespace': '', - 'use_namespace': 'False'}.items()) + 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()) + 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_waypoint_follower', emulate_tty=True, - output='screen') + output='screen', + ) ld = LaunchDescription() ld.add_action(declare_use_rviz_cmd) diff --git a/nav2_simple_commander/nav2_simple_commander/demo_inspection.py b/nav2_simple_commander/nav2_simple_commander/demo_inspection.py index 429e11b68f..5f6d04769a 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_inspection.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_inspection.py @@ -41,7 +41,8 @@ def main(): [3.661, -4.121], [5.431, -4.121], [3.661, -5.850], - [5.431, -5.800]] + [5.431, -5.800], + ] # Set our demo's initial pose initial_pose = PoseStamped() @@ -76,8 +77,12 @@ def main(): i += 1 feedback = navigator.getFeedback() if feedback and i % 5 == 0: - print('Executing current waypoint: ' + - str(feedback.current_waypoint + 1) + '/' + str(len(inspection_points))) + print( + 'Executing current waypoint: ' + + str(feedback.current_waypoint + 1) + + '/' + + str(len(inspection_points)) + ) result = navigator.getResult() if result == TaskResult.SUCCEEDED: diff --git a/nav2_simple_commander/nav2_simple_commander/demo_picking.py b/nav2_simple_commander/nav2_simple_commander/demo_picking.py index 6a7a177acb..48c0551c2c 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_picking.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_picking.py @@ -25,14 +25,16 @@ '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_D': [-3.24, 5.861], +} # 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_3': [-6.349, 9.147], +} """ Basic item picking demo. In this demonstration, the expectation @@ -45,7 +47,7 @@ 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 ('pallet_jack7') #################### request_item_location = 'shelf_C' request_destination = 'pallet_jack7' @@ -86,26 +88,43 @@ def main(): i += 1 feedback = navigator.getFeedback() if feedback and i % 5 == 0: - print('Estimated time of arrival at ' + request_item_location + - ' for worker: ' + '{0:.0f}'.format( - Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9) - + ' seconds.') + print( + 'Estimated time of arrival at ' + + request_item_location + + ' for worker: ' + + '{0:.0f}'.format( + Duration.from_msg(feedback.estimated_time_remaining).nanoseconds + / 1e9 + ) + + ' seconds.' + ) result = navigator.getResult() if result == TaskResult.SUCCEEDED: - print('Got product from ' + request_item_location + - '! Bringing product to shipping destination (' + request_destination + ')...') + print( + 'Got product from ' + + request_item_location + + '! Bringing product to shipping destination (' + + request_destination + + ')...' + ) shipping_destination = PoseStamped() shipping_destination.header.frame_id = 'map' shipping_destination.header.stamp = navigator.get_clock().now().to_msg() - shipping_destination.pose.position.x = shipping_destinations[request_destination][0] - shipping_destination.pose.position.y = shipping_destinations[request_destination][1] + shipping_destination.pose.position.x = shipping_destinations[ + request_destination + ][0] + shipping_destination.pose.position.y = shipping_destinations[ + request_destination + ][1] shipping_destination.pose.orientation.z = 1.0 shipping_destination.pose.orientation.w = 0.0 navigator.goToPose(shipping_destination) elif result == TaskResult.CANCELED: - print(f'Task at {request_item_location} was canceled. Returning to staging point...') + print( + f'Task at {request_item_location} was canceled. Returning to staging point...' + ) initial_pose.header.stamp = navigator.get_clock().now().to_msg() navigator.goToPose(initial_pose) diff --git a/nav2_simple_commander/nav2_simple_commander/demo_security.py b/nav2_simple_commander/nav2_simple_commander/demo_security.py index f727cc8d72..24cc05feff 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_security.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_security.py @@ -43,7 +43,8 @@ def main(): [-3.665, -9.427], [-3.665, -4.303], [-3.665, 2.330], - [-3.665, 9.283]] + [-3.665, 9.283], + ] # Set our demo's initial pose initial_pose = PoseStamped() @@ -79,12 +80,19 @@ def main(): i += 1 feedback = navigator.getFeedback() if feedback and i % 5 == 0: - print('Estimated time to complete current route: ' + '{0:.0f}'.format( - Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9) - + ' seconds.') + print( + 'Estimated time to complete current route: ' + + '{0:.0f}'.format( + Duration.from_msg(feedback.estimated_time_remaining).nanoseconds + / 1e9 + ) + + ' seconds.' + ) # Some failure mode, must stop since the robot is clearly stuck - if Duration.from_msg(feedback.navigation_time) > Duration(seconds=180.0): + if Duration.from_msg(feedback.navigation_time) > Duration( + seconds=180.0 + ): print('Navigation has exceeded timeout of 180s, canceling request.') navigator.cancelTask() 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 963dacf942..104083f34a 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_follow_path.py +++ b/nav2_simple_commander/nav2_simple_commander/example_follow_path.py @@ -68,10 +68,12 @@ def main(): i += 1 feedback = navigator.getFeedback() if feedback and i % 5 == 0: - print('Estimated distance remaining to goal position: ' + - '{0:.3f}'.format(feedback.distance_to_goal) + - '\nCurrent speed of the robot: ' + - '{0:.3f}'.format(feedback.speed)) + print( + 'Estimated distance remaining to goal position: ' + + '{0:.3f}'.format(feedback.distance_to_goal) + + '\nCurrent speed of the robot: ' + + '{0:.3f}'.format(feedback.speed) + ) # Do something depending on the return code result = navigator.getResult() 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 58c47c75e1..66023ec11a 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 @@ -100,9 +100,14 @@ def main(): i = i + 1 feedback = navigator.getFeedback() if feedback and i % 5 == 0: - print('Estimated time of arrival: ' + '{0:.0f}'.format( - Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9) - + ' seconds.') + print( + 'Estimated time of arrival: ' + + '{0:.0f}'.format( + Duration.from_msg(feedback.estimated_time_remaining).nanoseconds + / 1e9 + ) + + ' seconds.' + ) # Some navigation timeout to demo cancellation if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0): 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 58dee813c9..d7fb850d12 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 @@ -79,9 +79,14 @@ def main(): i = i + 1 feedback = navigator.getFeedback() if feedback and i % 5 == 0: - print('Estimated time of arrival: ' + '{0:.0f}'.format( - Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9) - + ' seconds.') + print( + 'Estimated time of arrival: ' + + '{0:.0f}'.format( + Duration.from_msg(feedback.estimated_time_remaining).nanoseconds + / 1e9 + ) + + ' seconds.' + ) # Some navigation timeout to demo cancellation if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0): 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 9d02341a66..3044465851 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py +++ b/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py @@ -101,8 +101,12 @@ def main(): i = i + 1 feedback = navigator.getFeedback() if feedback and i % 5 == 0: - print('Executing current waypoint: ' + - str(feedback.current_waypoint + 1) + '/' + str(len(goal_poses))) + print( + 'Executing current waypoint: ' + + str(feedback.current_waypoint + 1) + + '/' + + str(len(goal_poses)) + ) now = navigator.get_clock().now() # Some navigation timeout to demo cancellation diff --git a/nav2_simple_commander/nav2_simple_commander/footprint_collision_checker.py b/nav2_simple_commander/nav2_simple_commander/footprint_collision_checker.py index 2117e21a55..5cf2f81794 100644 --- a/nav2_simple_commander/nav2_simple_commander/footprint_collision_checker.py +++ b/nav2_simple_commander/nav2_simple_commander/footprint_collision_checker.py @@ -34,7 +34,7 @@ FREE_SPACE = 0 -class FootprintCollisionChecker(): +class FootprintCollisionChecker: """ FootprintCollisionChecker. @@ -65,8 +65,7 @@ def footprintCost(self, footprint: Polygon): x1 = 0.0 y1 = 0.0 - x0, y0 = self.worldToMapValidated( - footprint.points[0].x, footprint.points[0].y) + x0, y0 = self.worldToMapValidated(footprint.points[0].x, footprint.points[0].y) if x0 is None or y0 is None: return LETHAL_OBSTACLE @@ -76,13 +75,13 @@ def footprintCost(self, footprint: Polygon): for i in range(len(footprint.points) - 1): x1, y1 = self.worldToMapValidated( - footprint.points[i + 1].x, footprint.points[i + 1].y) + footprint.points[i + 1].x, footprint.points[i + 1].y + ) if x1 is None or y1 is None: return LETHAL_OBSTACLE - footprint_cost = max( - float(self.lineCost(x0, x1, y0, y1)), footprint_cost) + footprint_cost = max(float(self.lineCost(x0, x1, y0, y1)), footprint_cost) x0 = x1 y0 = y1 @@ -115,7 +114,8 @@ def lineCost(self, x0, x1, y0, y1, step_size=0.5): while line_iterator.isValid(): point_cost = self.pointCost( - int(line_iterator.getX()), int(line_iterator.getY())) + int(line_iterator.getX()), int(line_iterator.getY()) + ) if point_cost == LETHAL_OBSTACLE: return point_cost @@ -146,7 +146,8 @@ def worldToMapValidated(self, wx: float, wy: float): """ if self.costmap_ is None: raise ValueError( - 'Costmap not specified, use setCostmap to specify the costmap first') + 'Costmap not specified, use setCostmap to specify the costmap first' + ) return self.costmap_.worldToMapValidated(wx, wy) def pointCost(self, x: int, y: int): @@ -165,7 +166,8 @@ def pointCost(self, x: int, y: int): """ if self.costmap_ is None: raise ValueError( - 'Costmap not specified, use setCostmap to specify the costmap first') + 'Costmap not specified, use setCostmap to specify the costmap first' + ) return self.costmap_.getCostXY(x, y) def setCostmap(self, costmap: PyCostmap2D): @@ -207,12 +209,12 @@ def footprintCostAtPose(self, x: float, y: float, theta: float, footprint: Polyg for i in range(len(footprint.points)): new_pt = Point32() - new_pt.x = x + \ - (footprint.points[i].x * cos_th - - footprint.points[i].y * sin_th) - new_pt.y = y + \ - (footprint.points[i].x * sin_th + - footprint.points[i].y * cos_th) + new_pt.x = x + ( + footprint.points[i].x * cos_th - footprint.points[i].y * sin_th + ) + new_pt.y = y + ( + footprint.points[i].x * sin_th + footprint.points[i].y * cos_th + ) oriented_footprint.points.append(new_pt) return self.footprintCost(oriented_footprint) diff --git a/nav2_simple_commander/nav2_simple_commander/line_iterator.py b/nav2_simple_commander/nav2_simple_commander/line_iterator.py index 7fe50a11ec..9f6c092570 100644 --- a/nav2_simple_commander/nav2_simple_commander/line_iterator.py +++ b/nav2_simple_commander/nav2_simple_commander/line_iterator.py @@ -24,7 +24,7 @@ from cmath import sqrt -class LineIterator(): +class LineIterator: """ LineIterator. @@ -77,18 +77,19 @@ def __init__(self, x0, y0, x1, y1, step_size=1.0): if x1 != x0 and y1 != y0: self.valid_ = True - self.m_ = (y1-y0)/(x1-x0) - self.b_ = y1 - (self.m_*x1) + self.m_ = (y1 - y0) / (x1 - x0) + self.b_ = y1 - (self.m_ * x1) elif x1 == x0 and y1 != y0: self.valid_ = True elif y1 == y1 and x1 != x0: self.valid_ = True - self.m_ = (y1-y0)/(x1-x0) - self.b_ = y1 - (self.m_*x1) + self.m_ = (y1 - y0) / (x1 - x0) + self.b_ = y1 - (self.m_ * x1) else: self.valid_ = False raise ValueError( - 'Line has zero length (All 4 points have same coordinates)') + 'Line has zero length (All 4 points have same coordinates)' + ) def isValid(self): """Check if line is valid.""" @@ -98,29 +99,33 @@ def advance(self): """Advance to the next point in the line.""" if self.x1_ > self.x0_: if self.x_ < self.x1_: - self.x_ = round(self.clamp( - self.x_ + self.step_size_, self.x0_, self.x1_), 5) + self.x_ = round( + self.clamp(self.x_ + self.step_size_, self.x0_, self.x1_), 5 + ) self.y_ = round(self.m_ * self.x_ + self.b_, 5) else: self.valid_ = False elif self.x1_ < self.x0_: if self.x_ > self.x1_: - self.x_ = round(self.clamp( - self.x_ - self.step_size_, self.x1_, self.x0_), 5) + self.x_ = round( + self.clamp(self.x_ - self.step_size_, self.x1_, self.x0_), 5 + ) self.y_ = round(self.m_ * self.x_ + self.b_, 5) else: self.valid_ = False else: if self.y1_ > self.y0_: if self.y_ < self.y1_: - self.y_ = round(self.clamp( - self.y_ + self.step_size_, self.y0_, self.y1_), 5) + self.y_ = round( + self.clamp(self.y_ + self.step_size_, self.y0_, self.y1_), 5 + ) else: self.valid_ = False elif self.y1_ < self.y0_: if self.y_ > self.y1_: - self.y_ = round(self.clamp( - self.y_ - self.step_size_, self.y1_, self.y0_), 5) + self.y_ = round( + self.clamp(self.y_ - self.step_size_, self.y1_, self.y0_), 5 + ) else: self.valid_ = False else: diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index 76a6c3d79b..a8ded664cd 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -25,8 +25,13 @@ from lifecycle_msgs.srv import GetState from nav2_msgs.action import AssistedTeleop, BackUp, Spin from nav2_msgs.action import ComputePathThroughPoses, ComputePathToPose -from nav2_msgs.action import FollowPath, FollowWaypoints, FollowGPSWaypoints, \ - NavigateThroughPoses, NavigateToPose +from nav2_msgs.action import ( + FollowGPSWaypoints, + FollowPath, + FollowWaypoints, + NavigateThroughPoses, + NavigateToPose, +) from nav2_msgs.action import SmoothPath from nav2_msgs.srv import ClearEntireCostmap, GetCostmap, LoadMap, ManageLifecycleNodes @@ -46,7 +51,6 @@ class TaskResult(Enum): class BasicNavigator(Node): - def __init__(self, node_name='basic_navigator', namespace=''): super().__init__(node_name=node_name, namespace=namespace) self.initial_pose = PoseStamped() @@ -57,42 +61,58 @@ def __init__(self, node_name='basic_navigator', namespace=''): self.status = None amcl_pose_qos = QoSProfile( - durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1) + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ) self.initial_pose_received = False - self.nav_through_poses_client = ActionClient(self, - NavigateThroughPoses, - 'navigate_through_poses') + self.nav_through_poses_client = ActionClient( + self, NavigateThroughPoses, 'navigate_through_poses' + ) self.nav_to_pose_client = ActionClient(self, NavigateToPose, 'navigate_to_pose') - self.follow_waypoints_client = ActionClient(self, FollowWaypoints, 'follow_waypoints') - self.follow_gps_waypoints_client = ActionClient(self, FollowGPSWaypoints, - 'follow_gps_waypoints') + self.follow_waypoints_client = ActionClient( + self, FollowWaypoints, 'follow_waypoints' + ) + self.follow_gps_waypoints_client = ActionClient( + self, FollowGPSWaypoints, 'follow_gps_waypoints' + ) self.follow_path_client = ActionClient(self, FollowPath, 'follow_path') - self.compute_path_to_pose_client = ActionClient(self, ComputePathToPose, - 'compute_path_to_pose') - self.compute_path_through_poses_client = ActionClient(self, ComputePathThroughPoses, - 'compute_path_through_poses') + self.compute_path_to_pose_client = ActionClient( + self, ComputePathToPose, 'compute_path_to_pose' + ) + self.compute_path_through_poses_client = ActionClient( + self, ComputePathThroughPoses, 'compute_path_through_poses' + ) self.smoother_client = ActionClient(self, SmoothPath, 'smooth_path') self.spin_client = ActionClient(self, Spin, 'spin') self.backup_client = ActionClient(self, BackUp, 'backup') - self.assisted_teleop_client = ActionClient(self, AssistedTeleop, 'assisted_teleop') - self.localization_pose_sub = self.create_subscription(PoseWithCovarianceStamped, - 'amcl_pose', - self._amclPoseCallback, - amcl_pose_qos) - self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped, - 'initialpose', - 10) + self.assisted_teleop_client = ActionClient( + self, AssistedTeleop, 'assisted_teleop' + ) + self.localization_pose_sub = self.create_subscription( + PoseWithCovarianceStamped, + 'amcl_pose', + self._amclPoseCallback, + amcl_pose_qos, + ) + self.initial_pose_pub = self.create_publisher( + PoseWithCovarianceStamped, 'initialpose', 10 + ) self.change_maps_srv = self.create_client(LoadMap, 'map_server/load_map') self.clear_costmap_global_srv = self.create_client( - ClearEntireCostmap, 'global_costmap/clear_entirely_global_costmap') + ClearEntireCostmap, 'global_costmap/clear_entirely_global_costmap' + ) self.clear_costmap_local_srv = self.create_client( - ClearEntireCostmap, 'local_costmap/clear_entirely_local_costmap') - self.get_costmap_global_srv = self.create_client(GetCostmap, 'global_costmap/get_costmap') - self.get_costmap_local_srv = self.create_client(GetCostmap, 'local_costmap/get_costmap') + ClearEntireCostmap, 'local_costmap/clear_entirely_local_costmap' + ) + self.get_costmap_global_srv = self.create_client( + GetCostmap, 'global_costmap/get_costmap' + ) + self.get_costmap_local_srv = self.create_client( + GetCostmap, 'local_costmap/get_costmap' + ) def destroyNode(self): self.destroy_node() @@ -126,8 +146,9 @@ def goThroughPoses(self, poses, behavior_tree=''): goal_msg.behavior_tree = behavior_tree self.info(f'Navigating with {len(goal_msg.poses)} goals....') - send_goal_future = self.nav_through_poses_client.send_goal_async(goal_msg, - self._feedbackCallback) + send_goal_future = self.nav_through_poses_client.send_goal_async( + goal_msg, self._feedbackCallback + ) rclpy.spin_until_future_complete(self, send_goal_future) self.goal_handle = send_goal_future.result() @@ -148,16 +169,27 @@ def goToPose(self, pose, behavior_tree=''): goal_msg.pose = pose goal_msg.behavior_tree = behavior_tree - self.info('Navigating to goal: ' + str(pose.pose.position.x) + ' ' + - str(pose.pose.position.y) + '...') - send_goal_future = self.nav_to_pose_client.send_goal_async(goal_msg, - self._feedbackCallback) + self.info( + 'Navigating to goal: ' + + str(pose.pose.position.x) + + ' ' + + str(pose.pose.position.y) + + '...' + ) + send_goal_future = self.nav_to_pose_client.send_goal_async( + goal_msg, self._feedbackCallback + ) rclpy.spin_until_future_complete(self, send_goal_future) self.goal_handle = send_goal_future.result() if not self.goal_handle.accepted: - self.error('Goal to ' + str(pose.pose.position.x) + ' ' + - str(pose.pose.position.y) + ' was rejected!') + self.error( + 'Goal to ' + + str(pose.pose.position.x) + + ' ' + + str(pose.pose.position.y) + + ' was rejected!' + ) return False self.result_future = self.goal_handle.get_result_async() @@ -173,8 +205,9 @@ def followWaypoints(self, poses): goal_msg.poses = poses self.info(f'Following {len(goal_msg.poses)} goals....') - send_goal_future = self.follow_waypoints_client.send_goal_async(goal_msg, - self._feedbackCallback) + send_goal_future = self.follow_waypoints_client.send_goal_async( + goal_msg, self._feedbackCallback + ) rclpy.spin_until_future_complete(self, send_goal_future) self.goal_handle = send_goal_future.result() @@ -195,13 +228,16 @@ def followGpsWaypoints(self, gps_poses): goal_msg.gps_poses = gps_poses self.info(f'Following {len(goal_msg.gps_poses)} gps goals....') - send_goal_future = self.follow_gps_waypoints_client.send_goal_async(goal_msg, - self._feedbackCallback) + send_goal_future = self.follow_gps_waypoints_client.send_goal_async( + goal_msg, self._feedbackCallback + ) rclpy.spin_until_future_complete(self, send_goal_future) self.goal_handle = send_goal_future.result() if not self.goal_handle.accepted: - self.error(f'Following {len(gps_poses)} gps waypoints request was rejected!') + self.error( + f'Following {len(gps_poses)} gps waypoints request was rejected!' + ) return False self.result_future = self.goal_handle.get_result_async() @@ -216,7 +252,9 @@ def spin(self, spin_dist=1.57, time_allowance=10): goal_msg.time_allowance = Duration(sec=time_allowance) self.info(f'Spinning to angle {goal_msg.target_yaw}....') - send_goal_future = self.spin_client.send_goal_async(goal_msg, self._feedbackCallback) + send_goal_future = self.spin_client.send_goal_async( + goal_msg, self._feedbackCallback + ) rclpy.spin_until_future_complete(self, send_goal_future) self.goal_handle = send_goal_future.result() @@ -237,7 +275,9 @@ def backup(self, backup_dist=0.15, backup_speed=0.025, time_allowance=10): goal_msg.time_allowance = Duration(sec=time_allowance) self.info(f'Backing up {goal_msg.target.x} m at {goal_msg.speed} m/s....') - send_goal_future = self.backup_client.send_goal_async(goal_msg, self._feedbackCallback) + send_goal_future = self.backup_client.send_goal_async( + goal_msg, self._feedbackCallback + ) rclpy.spin_until_future_complete(self, send_goal_future) self.goal_handle = send_goal_future.result() @@ -256,8 +296,9 @@ def assistedTeleop(self, time_allowance=30): goal_msg.time_allowance = Duration(sec=time_allowance) self.info("Running 'assisted_teleop'....") - send_goal_future = \ - self.assisted_teleop_client.send_goal_async(goal_msg, self._feedbackCallback) + send_goal_future = self.assisted_teleop_client.send_goal_async( + goal_msg, self._feedbackCallback + ) rclpy.spin_until_future_complete(self, send_goal_future) self.goal_handle = send_goal_future.result() @@ -280,8 +321,9 @@ def followPath(self, path, controller_id='', goal_checker_id=''): goal_msg.goal_checker_id = goal_checker_id self.info('Executing path...') - send_goal_future = self.follow_path_client.send_goal_async(goal_msg, - self._feedbackCallback) + send_goal_future = self.follow_path_client.send_goal_async( + goal_msg, self._feedbackCallback + ) rclpy.spin_until_future_complete(self, send_goal_future) self.goal_handle = send_goal_future.result() @@ -335,7 +377,7 @@ def getResult(self): def waitUntilNav2Active(self, navigator='bt_navigator', localizer='amcl'): """Block until the full navigation system is up and running.""" - if localizer != "robot_localization": # non-lifecycle node + if localizer != 'robot_localization': # non-lifecycle node self._waitForNodeToActivate(localizer) if localizer == 'amcl': self._waitForInitialPose() @@ -394,8 +436,12 @@ def _getPathThroughPosesImpl(self, start, goals, planner_id='', use_start=False) Internal implementation to get the full result, not just the path. """ self.debug("Waiting for 'ComputePathThroughPoses' action server") - while not self.compute_path_through_poses_client.wait_for_server(timeout_sec=1.0): - self.info("'ComputePathThroughPoses' action server not available, waiting...") + while not self.compute_path_through_poses_client.wait_for_server( + timeout_sec=1.0 + ): + self.info( + "'ComputePathThroughPoses' action server not available, waiting..." + ) goal_msg = ComputePathThroughPoses.Goal() goal_msg.start = start @@ -404,7 +450,9 @@ def _getPathThroughPosesImpl(self, start, goals, planner_id='', use_start=False) goal_msg.use_start = use_start self.info('Getting path...') - send_goal_future = self.compute_path_through_poses_client.send_goal_async(goal_msg) + send_goal_future = self.compute_path_through_poses_client.send_goal_async( + goal_msg + ) rclpy.spin_until_future_complete(self, send_goal_future) self.goal_handle = send_goal_future.result() @@ -431,7 +479,9 @@ def getPathThroughPoses(self, start, goals, planner_id='', use_start=False): else: return rtn.path - def _smoothPathImpl(self, path, smoother_id='', max_duration=2.0, check_for_collision=False): + def _smoothPathImpl( + self, path, smoother_id='', max_duration=2.0, check_for_collision=False + ): """ Send a `SmoothPath` action request. @@ -462,10 +512,11 @@ def _smoothPathImpl(self, path, smoother_id='', max_duration=2.0, check_for_coll return self.result_future.result().result - def smoothPath(self, path, smoother_id='', max_duration=2.0, check_for_collision=False): + def smoothPath( + self, path, smoother_id='', max_duration=2.0, check_for_collision=False + ): """Send a `SmoothPath` action request.""" - rtn = self._smoothPathImpl( - path, smoother_id, max_duration, check_for_collision) + rtn = self._smoothPathImpl(path, smoother_id, max_duration, check_for_collision) if self.status != GoalStatus.STATUS_SUCCEEDED: self.warn(f'Getting path failed with status code: {self.status}') diff --git a/nav2_simple_commander/setup.py b/nav2_simple_commander/setup.py index c7b12a3fcf..4b49f89a45 100644 --- a/nav2_simple_commander/setup.py +++ b/nav2_simple_commander/setup.py @@ -11,8 +11,7 @@ version='1.0.0', packages=[package_name], data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), (os.path.join('share', package_name), glob('launch/*')), ], @@ -25,15 +24,15 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'example_nav_to_pose = nav2_simple_commander.example_nav_to_pose:main', - 'example_nav_through_poses = nav2_simple_commander.example_nav_through_poses:main', - 'example_waypoint_follower = nav2_simple_commander.example_waypoint_follower:main', - 'example_follow_path = nav2_simple_commander.example_follow_path:main', - 'demo_picking = nav2_simple_commander.demo_picking:main', - 'demo_inspection = nav2_simple_commander.demo_inspection:main', - 'demo_security = nav2_simple_commander.demo_security:main', - 'demo_recoveries = nav2_simple_commander.demo_recoveries:main', - 'example_assisted_teleop = nav2_simple_commander.example_assisted_teleop:main', + 'example_nav_to_pose = nav2_simple_commander.example_nav_to_pose:main', + 'example_nav_through_poses = nav2_simple_commander.example_nav_through_poses:main', + 'example_waypoint_follower = nav2_simple_commander.example_waypoint_follower:main', + 'example_follow_path = nav2_simple_commander.example_follow_path:main', + 'demo_picking = nav2_simple_commander.demo_picking:main', + 'demo_inspection = nav2_simple_commander.demo_inspection:main', + 'demo_security = nav2_simple_commander.demo_security:main', + 'demo_recoveries = nav2_simple_commander.demo_recoveries:main', + 'example_assisted_teleop = nav2_simple_commander.example_assisted_teleop:main', ], }, ) diff --git a/nav2_simple_commander/test/test_flake8.py b/nav2_simple_commander/test/test_flake8.py index 27ee1078ff..26030113cc 100644 --- a/nav2_simple_commander/test/test_flake8.py +++ b/nav2_simple_commander/test/test_flake8.py @@ -20,6 +20,6 @@ @pytest.mark.linter def test_flake8(): rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) + assert rc == 0, 'Found %d code style errors / warnings:\n' % len( + errors + ) + '\n'.join(errors) diff --git a/nav2_simple_commander/test/test_footprint_collision_checker.py b/nav2_simple_commander/test/test_footprint_collision_checker.py index 8ffc4b653b..070a7bc6e4 100644 --- a/nav2_simple_commander/test/test_footprint_collision_checker.py +++ b/nav2_simple_commander/test/test_footprint_collision_checker.py @@ -23,7 +23,6 @@ class TestFootprintCollisionChecker(unittest.TestCase): - def test_no_costmap(self): # Test if a type error raised when costmap is not specified yet fcc_ = FootprintCollisionChecker() diff --git a/nav2_simple_commander/test/test_line_iterator.py b/nav2_simple_commander/test/test_line_iterator.py index 87c348ea70..1360c183e7 100644 --- a/nav2_simple_commander/test/test_line_iterator.py +++ b/nav2_simple_commander/test/test_line_iterator.py @@ -19,7 +19,6 @@ class TestLineIterator(unittest.TestCase): - def test_type_error(self): # Test if a type error raised when passing invalid arguements types self.assertRaises(TypeError, LineIterator, 0, 0, '10', 10, '1') @@ -58,7 +57,7 @@ def test_straight_line(self): i = 0 while lt.isValid(): self.assertEqual(lt.getX(), lt.getX0() + i) - self.assertEqual(lt.getY(), lt.getY0() + (i*2)) + self.assertEqual(lt.getY(), lt.getY0() + (i * 2)) lt.advance() i += 1 @@ -67,7 +66,7 @@ def test_straight_line(self): i = 0 while lt.isValid(): self.assertEqual(lt.getX(), lt.getX0() + i) - self.assertEqual(lt.getY(), lt.getY0() + (-i*2)) + self.assertEqual(lt.getY(), lt.getY0() + (-i * 2)) lt.advance() i += 1 diff --git a/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py b/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py index e70c235e83..d19ef987ec 100644 --- a/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py +++ b/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py @@ -40,24 +40,28 @@ def handle_arg_parsing(): An object containing all parsed arguments """ - parser = argparse.ArgumentParser(description='Generate motionprimitives ' - "for Nav2's State " - 'Lattice Planner') - parser.add_argument('--config', - type=Path, - default='./config.json', - help='The config file containing the ' - 'parameters to be used') - parser.add_argument('--output', - type=Path, - default='./output.json', - help='The output file containing the ' - 'trajectory data') - parser.add_argument('--visualizations', - type=Path, - default='./visualizations', - help='The output folder where the ' - 'visualizations of the trajectories will be saved') + parser = argparse.ArgumentParser( + description="Generate motionprimitives for Nav2's State Lattice Planner" + ) + parser.add_argument( + '--config', + type=Path, + default='./config.json', + help='The config file containing the ' 'parameters to be used', + ) + parser.add_argument( + '--output', + type=Path, + default='./output.json', + help='The output file containing the ' 'trajectory data', + ) + parser.add_argument( + '--visualizations', + type=Path, + default='./visualizations', + help='The output folder where the ' + 'visualizations of the trajectories will be saved', + ) return parser.parse_args() @@ -130,14 +134,18 @@ def create_header(config: dict, minimal_set_trajectories: dict) -> dict: header_dict['lattice_metadata'][key] = value heading_angles = create_heading_angle_list(minimal_set_trajectories) - adjusted_heading_angles = [angle + 2*np.pi if angle < 0 else angle for angle in heading_angles] + adjusted_heading_angles = [ + angle + 2 * np.pi if angle < 0 else angle for angle in heading_angles + ] header_dict['lattice_metadata']['heading_angles'] = adjusted_heading_angles return header_dict -def write_to_json(output_path: Path, minimal_set_trajectories: dict, config: dict) -> None: +def write_to_json( + output_path: Path, minimal_set_trajectories: dict, config: dict +) -> None: """ Write the minimal spanning set to an output file. @@ -156,32 +164,29 @@ def write_to_json(output_path: Path, minimal_set_trajectories: dict, config: dic trajectory_start_angles = list(minimal_set_trajectories.keys()) heading_angle_list = create_heading_angle_list(minimal_set_trajectories) - heading_lookup = {angle: idx for idx, angle in - enumerate(heading_angle_list)} + heading_lookup = {angle: idx for idx, angle in enumerate(heading_angle_list)} idx = 0 - for start_angle in sorted(trajectory_start_angles, - key=lambda x: (x < 0, x)): + for start_angle in sorted(trajectory_start_angles, key=lambda x: (x < 0, x)): for trajectory in sorted( - minimal_set_trajectories[start_angle], - key=lambda x: x.parameters.end_angle + minimal_set_trajectories[start_angle], key=lambda x: x.parameters.end_angle ): traj_info = {} traj_info['trajectory_id'] = idx - traj_info['start_angle_index'] = heading_lookup[trajectory.parameters.start_angle] - traj_info['end_angle_index'] = heading_lookup[trajectory.parameters.end_angle] + traj_info['start_angle_index'] = heading_lookup[ + trajectory.parameters.start_angle + ] + traj_info['end_angle_index'] = heading_lookup[ + trajectory.parameters.end_angle + ] traj_info['left_turn'] = bool(trajectory.parameters.left_turn) - traj_info['trajectory_radius'] = \ - trajectory.parameters.turning_radius + traj_info['trajectory_radius'] = trajectory.parameters.turning_radius traj_info['trajectory_length'] = round( trajectory.parameters.total_length, 5 ) - traj_info['arc_length'] = round( - trajectory.parameters.arc_length, - 5 - ) + traj_info['arc_length'] = round(trajectory.parameters.arc_length, 5) traj_info['straight_length'] = round( trajectory.parameters.start_straight_length + trajectory.parameters.end_straight_length, @@ -198,7 +203,9 @@ def write_to_json(output_path: Path, minimal_set_trajectories: dict, config: dic json.dump(output_dict, output_file, indent='\t') -def save_visualizations(visualizations_folder: Path, minimal_set_trajectories: dict) -> None: +def save_visualizations( + visualizations_folder: Path, minimal_set_trajectories: dict +) -> None: """ Draw the visualizations for every trajectory and save it as an image. diff --git a/nav2_smac_planner/lattice_primitives/helper.py b/nav2_smac_planner/lattice_primitives/helper.py index 24d3cf9140..e29c787738 100644 --- a/nav2_smac_planner/lattice_primitives/helper.py +++ b/nav2_smac_planner/lattice_primitives/helper.py @@ -28,11 +28,11 @@ def normalize_angle(angle): The normalized angle in the range [0,2pi) """ - while angle >= 2*np.pi: - angle -= 2*np.pi + while angle >= 2 * np.pi: + angle -= 2 * np.pi while angle < 0: - angle += 2*np.pi + angle += 2 * np.pi return angle @@ -123,5 +123,4 @@ def get_rotation_matrix(angle): A 2x2 matrix representing a 2D rotation by angle """ - return np.array([[np.cos(angle), -np.sin(angle)], - [np.sin(angle), np.cos(angle)]]) + return np.array([[np.cos(angle), -np.sin(angle)], [np.sin(angle), np.cos(angle)]]) diff --git a/nav2_smac_planner/lattice_primitives/lattice_generator.py b/nav2_smac_planner/lattice_primitives/lattice_generator.py index e8968c2009..185cc0f839 100644 --- a/nav2_smac_planner/lattice_primitives/lattice_generator.py +++ b/nav2_smac_planner/lattice_primitives/lattice_generator.py @@ -57,8 +57,7 @@ def __init__(self, config: dict): self.turning_radius = config['turning_radius'] self.stopping_threshold = config['stopping_threshold'] self.num_of_headings = config['num_of_headings'] - self.headings = \ - self._get_heading_discretization(config['num_of_headings']) + self.headings = self._get_heading_discretization(config['num_of_headings']) self.motion_model = self.MotionModel[config['motion_model'].upper()] @@ -567,8 +566,7 @@ def _handle_motion_model(self, spanning_set: dict) -> dict: return omni_spanning_set else: - print('No handling implemented for Motion Model: ' + - f'{self.motion_model}') + print('No handling implemented for Motion Model: ' + f'{self.motion_model}') raise NotImplementedError def _add_in_place_turns(self, spanning_set: dict) -> dict: diff --git a/nav2_smac_planner/lattice_primitives/tests/test_lattice_generator.py b/nav2_smac_planner/lattice_primitives/tests/test_lattice_generator.py index 01476decd0..30a7a24eb2 100644 --- a/nav2_smac_planner/lattice_primitives/tests/test_lattice_generator.py +++ b/nav2_smac_planner/lattice_primitives/tests/test_lattice_generator.py @@ -28,11 +28,13 @@ class TestLatticeGenerator(unittest.TestCase): """Contains the unit tests for the TrajectoryGenerator.""" def setUp(self) -> None: - config = {'motion_model': MOTION_MODEL, - 'turning_radius': TURNING_RADIUS, - 'grid_resolution': GRID_RESOLUTION, - 'stopping_threshold': STOPPING_THRESHOLD, - 'num_of_headings': NUM_OF_HEADINGS} + config = { + 'motion_model': MOTION_MODEL, + 'turning_radius': TURNING_RADIUS, + 'grid_resolution': GRID_RESOLUTION, + 'stopping_threshold': STOPPING_THRESHOLD, + 'num_of_headings': NUM_OF_HEADINGS, + } lattice_gen = LatticeGenerator(config) @@ -83,7 +85,7 @@ def test_output_angles_in_correct_range(self): for x, y, angle in output: self.assertGreaterEqual(angle, 0) - self.assertLessEqual(angle, 2*np.pi) + self.assertLessEqual(angle, 2 * np.pi) if __name__ == '__main__': diff --git a/nav2_smac_planner/lattice_primitives/tests/test_trajectory_generator.py b/nav2_smac_planner/lattice_primitives/tests/test_trajectory_generator.py index 0a994cac49..ea3f51930f 100644 --- a/nav2_smac_planner/lattice_primitives/tests/test_trajectory_generator.py +++ b/nav2_smac_planner/lattice_primitives/tests/test_trajectory_generator.py @@ -32,7 +32,8 @@ def test_generate_trajectory_only_arc(self): # Quadrant 1 end_point = np.array([1, 1]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE) + end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE + ) self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -40,7 +41,8 @@ def test_generate_trajectory_only_arc(self): # Quadrant 2 end_point = np.array([-1, 1]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE) + end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE + ) self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -48,7 +50,8 @@ def test_generate_trajectory_only_arc(self): # Quadrant 3 end_point = np.array([-1, -1]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE) + end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE + ) self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -56,7 +59,8 @@ def test_generate_trajectory_only_arc(self): # Quadrant 4 end_point = np.array([1, -1]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE) + end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE + ) self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -65,7 +69,8 @@ def test_generate_trajectory_only_line(self): # Quadrant 1 end_point = np.array([1, 1]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, np.deg2rad(45), np.deg2rad(45), STEP_DISTANCE) + end_point, np.deg2rad(45), np.deg2rad(45), STEP_DISTANCE + ) self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -73,7 +78,8 @@ def test_generate_trajectory_only_line(self): # Quadrant 2 end_point = np.array([-1, 1]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, np.deg2rad(135), np.deg2rad(135), STEP_DISTANCE) + end_point, np.deg2rad(135), np.deg2rad(135), STEP_DISTANCE + ) self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -81,7 +87,8 @@ def test_generate_trajectory_only_line(self): # Quadrant 3 end_point = np.array([-1, -1]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, -np.deg2rad(135), -np.deg2rad(135), STEP_DISTANCE) + end_point, -np.deg2rad(135), -np.deg2rad(135), STEP_DISTANCE + ) self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -89,7 +96,8 @@ def test_generate_trajectory_only_line(self): # Quadrant 4 end_point = np.array([1, -1]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, -np.deg2rad(45), -np.deg2rad(45), STEP_DISTANCE) + end_point, -np.deg2rad(45), -np.deg2rad(45), STEP_DISTANCE + ) self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -98,7 +106,8 @@ def test_generate_trajectory_line_to_arc(self): # Quadrant 1 end_point = np.array([2, 1]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE) + end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE + ) self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -106,7 +115,8 @@ def test_generate_trajectory_line_to_arc(self): # Quadrant 2 end_point = np.array([-2, 1]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE) + end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE + ) self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -114,7 +124,8 @@ def test_generate_trajectory_line_to_arc(self): # Quadrant 3 end_point = np.array([-2, -1]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE) + end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE + ) self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -122,7 +133,8 @@ def test_generate_trajectory_line_to_arc(self): # Quadrant 1 end_point = np.array([2, -1]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE) + end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE + ) self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -131,7 +143,8 @@ def test_generate_trajectory_line_to_end(self): # Quadrant 1 end_point = np.array([1, 2]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE) + end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE + ) self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -139,7 +152,8 @@ def test_generate_trajectory_line_to_end(self): # Quadrant 2 end_point = np.array([-1, 2]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE) + end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE + ) self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -147,7 +161,8 @@ def test_generate_trajectory_line_to_end(self): # Quadrant 3 end_point = np.array([-1, -2]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE) + end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE + ) self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -155,37 +170,42 @@ def test_generate_trajectory_line_to_end(self): # Quadrant 4 end_point = np.array([1, -2]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE) + end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE + ) self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) def test_generate_trajectory_radius_too_small(self): # Quadrant 1 - end_point = np.array([.9, .9]) + end_point = np.array([0.9, 0.9]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE) + end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE + ) self.assertEqual(trajectory, None) # Quadrant 2 - end_point = np.array([-.9, -.9]) + end_point = np.array([-0.9, -0.9]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE) + end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE + ) self.assertEqual(trajectory, None) # Quadrant 3 - end_point = np.array([-.9, -.9]) + end_point = np.array([-0.9, -0.9]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE) + end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE + ) self.assertEqual(trajectory, None) # Quadrant 4 - end_point = np.array([.9, -.9]) + end_point = np.array([0.9, -0.9]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE) + end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE + ) self.assertEqual(trajectory, None) @@ -193,7 +213,8 @@ def test_generate_trajectory_parallel_lines_coincident(self): # Quadrant 1 end_point = np.array([5, 0]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, np.deg2rad(0), np.deg2rad(0), STEP_DISTANCE) + end_point, np.deg2rad(0), np.deg2rad(0), STEP_DISTANCE + ) self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -201,7 +222,8 @@ def test_generate_trajectory_parallel_lines_coincident(self): # Quadrant 2 end_point = np.array([-5, 0]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, -np.deg2rad(180), -np.deg2rad(180), STEP_DISTANCE) + end_point, -np.deg2rad(180), -np.deg2rad(180), STEP_DISTANCE + ) self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -210,14 +232,16 @@ def test_generate_trajectory_parallel_lines_not_coincident(self): # Quadrant 1 end_point = np.array([0, 3]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, np.deg2rad(0), np.deg2rad(0), STEP_DISTANCE) + end_point, np.deg2rad(0), np.deg2rad(0), STEP_DISTANCE + ) self.assertEqual(trajectory, None) # Quadrant 2 end_point = np.array([0, 3]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, -np.deg2rad(180), -np.deg2rad(180), STEP_DISTANCE) + end_point, -np.deg2rad(180), -np.deg2rad(180), STEP_DISTANCE + ) self.assertEqual(trajectory, None) diff --git a/nav2_smac_planner/lattice_primitives/tests/trajectory_visualizer.py b/nav2_smac_planner/lattice_primitives/tests/trajectory_visualizer.py index ef1ce0f472..9187d0b1e4 100644 --- a/nav2_smac_planner/lattice_primitives/tests/trajectory_visualizer.py +++ b/nav2_smac_planner/lattice_primitives/tests/trajectory_visualizer.py @@ -32,8 +32,14 @@ def plot_arrow(x, y, yaw, length=1.0, fc='r', ec='k'): """Plot arrow.""" - plt.arrow(x, y, length * np.cos(yaw), length * - np.sin(yaw), width=0.05*length, length_includes_head=True) + plt.arrow( + x, + y, + length * np.cos(yaw), + length * np.sin(yaw), + width=0.05 * length, + length_includes_head=True, + ) plt.plot(x, y) plt.plot(0, 0) @@ -51,21 +57,38 @@ def read_trajectories_data(file_path): trajectory_data = read_trajectories_data(trajectory_file_path) -min_x = min([min([pose[0] for pose in primitive['poses']]) - for primitive in trajectory_data['primitives']]) -max_x = max([max([pose[0] for pose in primitive['poses']]) - for primitive in trajectory_data['primitives']]) - -min_y = min([min([pose[1] for pose in primitive['poses']]) - for primitive in trajectory_data['primitives']]) -max_y = max([max([pose[1] for pose in primitive['poses']]) - for primitive in trajectory_data['primitives']]) +min_x = min( + [ + min([pose[0] for pose in primitive['poses']]) + for primitive in trajectory_data['primitives'] + ] +) +max_x = max( + [ + max([pose[0] for pose in primitive['poses']]) + for primitive in trajectory_data['primitives'] + ] +) + +min_y = min( + [ + min([pose[1] for pose in primitive['poses']]) + for primitive in trajectory_data['primitives'] + ] +) +max_y = max( + [ + max([pose[1] for pose in primitive['poses']]) + for primitive in trajectory_data['primitives'] + ] +) heading_angles = trajectory_data['lattice_metadata']['heading_angles'] for primitive in trajectory_data['primitives']: - arrow_length = (primitive['arc_length'] + - primitive['straight_length']) / len(primitive['poses']) + arrow_length = (primitive['arc_length'] + primitive['straight_length']) / len( + primitive['poses'] + ) if arrow_length == 0: arrow_length = max_x / len(primitive['poses']) @@ -73,7 +96,7 @@ def read_trajectories_data(file_path): xs = np.array([pose[0] for pose in primitive['poses']]) ys = np.array([pose[1] for pose in primitive['poses']]) - lengths = np.sqrt((xs[1:] - xs[:-1])**2 + (ys[1:] - ys[:-1])**2) + lengths = np.sqrt((xs[1:] - xs[:-1]) ** 2 + (ys[1:] - ys[:-1]) ** 2) print('Distances between points: ', lengths) for x, y, yaw in primitive['poses']: @@ -85,14 +108,13 @@ def read_trajectories_data(file_path): left_x, right_x = plt.xlim() left_y, right_y = plt.ylim() - plt.xlim(1.2*min_x, 1.2*max_x) - plt.ylim(1.2*min_y, 1.2*max_y) + plt.xlim(1.2 * min_x, 1.2 * max_x) + plt.ylim(1.2 * min_y, 1.2 * max_y) start_angle = np.rad2deg(heading_angles[primitive['start_angle_index']]) end_angle = np.rad2deg(heading_angles[primitive['end_angle_index']]) plt.title(f"Trajectory ID: {primitive['trajectory_id']}") - plt.figtext( - 0.7, 0.9, f'Start: {start_angle}\nEnd: {end_angle}') + plt.figtext(0.7, 0.9, f'Start: {start_angle}\nEnd: {end_angle}') plt.show() diff --git a/nav2_smac_planner/lattice_primitives/trajectory.py b/nav2_smac_planner/lattice_primitives/trajectory.py index 520ded328d..0dcb6edc07 100644 --- a/nav2_smac_planner/lattice_primitives/trajectory.py +++ b/nav2_smac_planner/lattice_primitives/trajectory.py @@ -73,8 +73,7 @@ def end_straight_length(self): @property def total_length(self): """Total length of trajectory.""" - return self.arc_length + self.start_straight_length + \ - self.end_straight_length + return self.arc_length + self.start_straight_length + self.end_straight_length @staticmethod def no_arc(end_point, start_angle, end_angle): diff --git a/nav2_smac_planner/lattice_primitives/trajectory_generator.py b/nav2_smac_planner/lattice_primitives/trajectory_generator.py index f2f51baf01..bd47d055cd 100644 --- a/nav2_smac_planner/lattice_primitives/trajectory_generator.py +++ b/nav2_smac_planner/lattice_primitives/trajectory_generator.py @@ -243,6 +243,7 @@ def _get_intersection_point( The intersection point of line 1 and 2 """ + def line1(x): return m1 * x + c1 diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py b/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py index 57f2b65625..b262d36efb 100755 --- a/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py @@ -20,7 +20,11 @@ from launch import LaunchDescription from launch import LaunchService -from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.actions import ( + ExecuteProcess, + IncludeLaunchDescription, + SetEnvironmentVariable, +) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from launch_testing.legacy import LaunchTestService @@ -32,55 +36,64 @@ def generate_launch_description(): map_yaml_file = os.getenv('TEST_MAP') world = os.getenv('TEST_WORLD') - bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', - os.getenv('BT_NAVIGATOR_XML')) + bt_navigator_xml = os.path.join( + get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML'), + ) bringup_dir = get_package_share_directory('nav2_bringup') params_file = os.path.join(bringup_dir, 'params/nav2_params.yaml') # Replace the `use_astar` setting on the params file configured_params = RewrittenYaml( - source_file=params_file, - root_key='', - param_rewrites='', - convert_types=True) - - return LaunchDescription([ - SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), - SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), - - # Launch gazebo server for simulation - ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_init.so', - '--minimal_comms', world], - output='screen'), - - # TODO(orduno) Launch the robot state publisher instead - # using a local copy of TB3 urdf file - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), - - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), - launch_arguments={ - 'map': map_yaml_file, - 'use_sim_time': 'True', - 'params_file': configured_params, - 'bt_xml_file': bt_navigator_xml, - 'use_composition': 'False', - 'autostart': 'True'}.items()), - ]) + source_file=params_file, root_key='', param_rewrites='', convert_types=True + ) + + return LaunchDescription( + [ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + # Launch gazebo server for simulation + ExecuteProcess( + cmd=[ + 'gzserver', + '-s', + 'libgazebo_ros_init.so', + '--minimal_comms', + world, + ], + output='screen', + ), + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], + ), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py') + ), + launch_arguments={ + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': configured_params, + 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', + 'autostart': 'True', + }.items(), + ), + ] + ) def main(argv=sys.argv[1:]): @@ -89,9 +102,8 @@ def main(argv=sys.argv[1:]): testExecutable = os.getenv('TEST_EXECUTABLE') test1_action = ExecuteProcess( - cmd=[testExecutable], - name='test_assisted_teleop_behavior_node', - output='screen') + cmd=[testExecutable], name='test_assisted_teleop_behavior_node', output='screen' + ) lts = LaunchTestService() lts.add_test_action(ld, test1_action) diff --git a/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py b/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py index 6091ffb40c..6f4a6c3c16 100755 --- a/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py @@ -20,7 +20,11 @@ from launch import LaunchDescription from launch import LaunchService -from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.actions import ( + ExecuteProcess, + IncludeLaunchDescription, + SetEnvironmentVariable, +) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from launch_testing.legacy import LaunchTestService @@ -32,55 +36,64 @@ def generate_launch_description(): map_yaml_file = os.getenv('TEST_MAP') world = os.getenv('TEST_WORLD') - bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', - os.getenv('BT_NAVIGATOR_XML')) + bt_navigator_xml = os.path.join( + get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML'), + ) bringup_dir = get_package_share_directory('nav2_bringup') params_file = os.path.join(bringup_dir, 'params/nav2_params.yaml') # Replace the `use_astar` setting on the params file configured_params = RewrittenYaml( - source_file=params_file, - root_key='', - param_rewrites='', - convert_types=True) - - return LaunchDescription([ - SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), - SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), - - # Launch gazebo server for simulation - ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_init.so', - '--minimal_comms', world], - output='screen'), - - # TODO(orduno) Launch the robot state publisher instead - # using a local copy of TB3 urdf file - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), - - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), - launch_arguments={ - 'map': map_yaml_file, - 'use_sim_time': 'True', - 'params_file': configured_params, - 'bt_xml_file': bt_navigator_xml, - 'use_composition': 'False', - 'autostart': 'True'}.items()), - ]) + source_file=params_file, root_key='', param_rewrites='', convert_types=True + ) + + return LaunchDescription( + [ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + # Launch gazebo server for simulation + ExecuteProcess( + cmd=[ + 'gzserver', + '-s', + 'libgazebo_ros_init.so', + '--minimal_comms', + world, + ], + output='screen', + ), + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], + ), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py') + ), + launch_arguments={ + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': configured_params, + 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', + 'autostart': 'True', + }.items(), + ), + ] + ) def main(argv=sys.argv[1:]): @@ -89,9 +102,8 @@ def main(argv=sys.argv[1:]): testExecutable = os.getenv('TEST_EXECUTABLE') test1_action = ExecuteProcess( - cmd=[testExecutable], - name='test_backup_behavior_node', - output='screen') + cmd=[testExecutable], name='test_backup_behavior_node', output='screen' + ) lts = LaunchTestService() lts.add_test_action(ld, test1_action) diff --git a/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_launch.py b/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_launch.py index 49959ec86d..8f23ce67b2 100755 --- a/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_launch.py @@ -20,7 +20,11 @@ from launch import LaunchDescription from launch import LaunchService -from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.actions import ( + ExecuteProcess, + IncludeLaunchDescription, + SetEnvironmentVariable, +) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from launch_testing.legacy import LaunchTestService @@ -32,55 +36,64 @@ def generate_launch_description(): map_yaml_file = os.getenv('TEST_MAP') world = os.getenv('TEST_WORLD') - bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', - os.getenv('BT_NAVIGATOR_XML')) + bt_navigator_xml = os.path.join( + get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML'), + ) bringup_dir = get_package_share_directory('nav2_bringup') params_file = os.path.join(bringup_dir, 'params/nav2_params.yaml') # Replace the `use_astar` setting on the params file configured_params = RewrittenYaml( - source_file=params_file, - root_key='', - param_rewrites='', - convert_types=True) - - return LaunchDescription([ - SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), - SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), - - # Launch gazebo server for simulation - ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_init.so', - '--minimal_comms', world], - output='screen'), - - # TODO(orduno) Launch the robot state publisher instead - # using a local copy of TB3 urdf file - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), - - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), - launch_arguments={ - 'map': map_yaml_file, - 'use_sim_time': 'True', - 'params_file': configured_params, - 'bt_xml_file': bt_navigator_xml, - 'use_composition': 'False', - 'autostart': 'True'}.items()), - ]) + source_file=params_file, root_key='', param_rewrites='', convert_types=True + ) + + return LaunchDescription( + [ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + # Launch gazebo server for simulation + ExecuteProcess( + cmd=[ + 'gzserver', + '-s', + 'libgazebo_ros_init.so', + '--minimal_comms', + world, + ], + output='screen', + ), + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], + ), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py') + ), + launch_arguments={ + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': configured_params, + 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', + 'autostart': 'True', + }.items(), + ), + ] + ) def main(argv=sys.argv[1:]): @@ -91,7 +104,8 @@ def main(argv=sys.argv[1:]): test1_action = ExecuteProcess( cmd=[testExecutable], name='test_drive_on_heading_behavior_node', - output='screen') + output='screen', + ) lts = LaunchTestService() lts.add_test_action(ld, test1_action) diff --git a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_fake_launch.py b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_fake_launch.py index 1b0ace8f1b..22b8327cd2 100755 --- a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_fake_launch.py +++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_fake_launch.py @@ -35,7 +35,9 @@ def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time') autostart = LaunchConfiguration('autostart') params_file = LaunchConfiguration('params_file') - default_nav_through_poses_bt_xml = LaunchConfiguration('default_nav_through_poses_bt_xml') + default_nav_through_poses_bt_xml = LaunchConfiguration( + 'default_nav_through_poses_bt_xml' + ) default_nav_to_pose_bt_xml = LaunchConfiguration('default_nav_to_pose_bt_xml') map_subscribe_transient_local = LaunchConfiguration('map_subscribe_transient_local') @@ -45,13 +47,15 @@ def generate_launch_description(): 'default_nav_through_poses_bt_xml': default_nav_through_poses_bt_xml, 'default_nav_to_pose_bt_xml': default_nav_to_pose_bt_xml, 'autostart': autostart, - 'map_subscribe_transient_local': map_subscribe_transient_local} + 'map_subscribe_transient_local': map_subscribe_transient_local, + } configured_params = RewrittenYaml( - source_file=params_file, - root_key=namespace, - param_rewrites=param_substitutions, - convert_types=True) + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True, + ) lifecycle_nodes = ['behavior_server'] @@ -61,83 +65,94 @@ def generate_launch_description(): # 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')] - - return LaunchDescription([ - SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), - SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), - - DeclareLaunchArgument( - 'namespace', default_value='', - description='Top-level namespace'), - - DeclareLaunchArgument( - 'use_sim_time', default_value='false', - description='Use simulation (Gazebo) clock if true'), - - DeclareLaunchArgument( - 'autostart', default_value='true', - description='Automatically startup the nav2 stack'), - - DeclareLaunchArgument( - 'params_file', - default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), - description='Full path to the ROS2 parameters file to use'), - - DeclareLaunchArgument( - 'default_nav_through_poses_bt_xml', - default_value=os.path.join( - get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', 'navigate_through_poses_w_replanning_and_recovery.xml'), - description='Full path to the behavior tree xml file to use'), - - DeclareLaunchArgument( - 'default_nav_to_pose_bt_xml', - default_value=os.path.join( - get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', 'navigate_to_pose_w_replanning_and_recovery.xml'), - description='Full path to the behavior tree xml file to use'), - - DeclareLaunchArgument( - 'map_subscribe_transient_local', default_value='false', - description='Whether to set the map subscriber QoS to transient local'), - - # TODO(orduno) Launch the robot state publisher instead - # using a local copy of TB3 urdf file - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom']), - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), - - Node( - package='nav2_behaviors', - executable='behavior_server', - name='behavior_server', - output='screen', - parameters=[configured_params], - remappings=remappings), - - Node( - package='nav2_lifecycle_manager', - executable='lifecycle_manager', - name='lifecycle_manager_navigation', - output='screen', - parameters=[{'use_sim_time': use_sim_time}, - {'autostart': autostart}, - {'node_names': lifecycle_nodes}]), - ]) + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] + + return LaunchDescription( + [ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + DeclareLaunchArgument( + 'namespace', default_value='', description='Top-level namespace' + ), + DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation (Gazebo) clock if true', + ), + DeclareLaunchArgument( + 'autostart', + default_value='true', + description='Automatically startup the nav2 stack', + ), + DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use', + ), + DeclareLaunchArgument( + 'default_nav_through_poses_bt_xml', + default_value=os.path.join( + get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + 'navigate_through_poses_w_replanning_and_recovery.xml', + ), + description='Full path to the behavior tree xml file to use', + ), + DeclareLaunchArgument( + 'default_nav_to_pose_bt_xml', + default_value=os.path.join( + get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + 'navigate_to_pose_w_replanning_and_recovery.xml', + ), + description='Full path to the behavior tree xml file to use', + ), + DeclareLaunchArgument( + 'map_subscribe_transient_local', + default_value='false', + description='Whether to set the map subscriber QoS to transient local', + ), + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'], + ), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], + ), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + ), + Node( + package='nav2_behaviors', + executable='behavior_server', + name='behavior_server', + output='screen', + parameters=[configured_params], + remappings=remappings, + ), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_navigation', + output='screen', + parameters=[ + {'use_sim_time': use_sim_time}, + {'autostart': autostart}, + {'node_names': lifecycle_nodes}, + ], + ), + ] + ) def main(argv=sys.argv[1:]): @@ -146,9 +161,8 @@ def main(argv=sys.argv[1:]): testExecutable = os.getenv('TEST_EXECUTABLE') test1_action = ExecuteProcess( - cmd=[testExecutable], - name='test_spin_behavior_fake_node', - output='screen') + cmd=[testExecutable], name='test_spin_behavior_fake_node', output='screen' + ) lts = LaunchTestService() lts.add_test_action(ld, test1_action) diff --git a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py index 154f9ed533..4552f51768 100755 --- a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py @@ -20,7 +20,11 @@ from launch import LaunchDescription from launch import LaunchService -from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.actions import ( + ExecuteProcess, + IncludeLaunchDescription, + SetEnvironmentVariable, +) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from launch_testing.legacy import LaunchTestService @@ -32,55 +36,64 @@ def generate_launch_description(): map_yaml_file = os.getenv('TEST_MAP') world = os.getenv('TEST_WORLD') - bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', - os.getenv('BT_NAVIGATOR_XML')) + bt_navigator_xml = os.path.join( + get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML'), + ) bringup_dir = get_package_share_directory('nav2_bringup') params_file = os.path.join(bringup_dir, 'params/nav2_params.yaml') # Replace the `use_astar` setting on the params file configured_params = RewrittenYaml( - source_file=params_file, - root_key='', - param_rewrites='', - convert_types=True) - - return LaunchDescription([ - SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), - SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), - - # Launch gazebo server for simulation - ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_init.so', - '--minimal_comms', world], - output='screen'), - - # TODO(orduno) Launch the robot state publisher instead - # using a local copy of TB3 urdf file - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), - - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), - launch_arguments={ - 'map': map_yaml_file, - 'use_sim_time': 'True', - 'params_file': configured_params, - 'bt_xml_file': bt_navigator_xml, - 'use_composition': 'False', - 'autostart': 'True'}.items()), - ]) + source_file=params_file, root_key='', param_rewrites='', convert_types=True + ) + + return LaunchDescription( + [ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + # Launch gazebo server for simulation + ExecuteProcess( + cmd=[ + 'gzserver', + '-s', + 'libgazebo_ros_init.so', + '--minimal_comms', + world, + ], + output='screen', + ), + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], + ), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py') + ), + launch_arguments={ + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': configured_params, + 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', + 'autostart': 'True', + }.items(), + ), + ] + ) def main(argv=sys.argv[1:]): @@ -89,9 +102,8 @@ def main(argv=sys.argv[1:]): testExecutable = os.getenv('TEST_EXECUTABLE') test1_action = ExecuteProcess( - cmd=[testExecutable], - name='test_spin_behavior_node', - output='screen') + cmd=[testExecutable], name='test_spin_behavior_node', output='screen' + ) lts = LaunchTestService() lts.add_test_action(ld, test1_action) diff --git a/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py index ee89e416de..f4c4e5bc16 100755 --- a/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py @@ -20,7 +20,11 @@ from launch import LaunchDescription from launch import LaunchService -from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.actions import ( + ExecuteProcess, + IncludeLaunchDescription, + SetEnvironmentVariable, +) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from launch_testing.legacy import LaunchTestService @@ -32,55 +36,64 @@ def generate_launch_description(): map_yaml_file = os.getenv('TEST_MAP') world = os.getenv('TEST_WORLD') - bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', - os.getenv('BT_NAVIGATOR_XML')) + bt_navigator_xml = os.path.join( + get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML'), + ) bringup_dir = get_package_share_directory('nav2_bringup') params_file = os.path.join(bringup_dir, 'params/nav2_params.yaml') # Replace the `use_astar` setting on the params file configured_params = RewrittenYaml( - source_file=params_file, - root_key='', - param_rewrites='', - convert_types=True) - - return LaunchDescription([ - SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), - SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), - - # Launch gazebo server for simulation - ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_init.so', - '--minimal_comms', world], - output='screen'), - - # TODO(orduno) Launch the robot state publisher instead - # using a local copy of TB3 urdf file - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), - - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), - launch_arguments={ - 'map': map_yaml_file, - 'use_sim_time': 'True', - 'params_file': configured_params, - 'bt_xml_file': bt_navigator_xml, - 'use_composition': 'False', - 'autostart': 'True'}.items()), - ]) + source_file=params_file, root_key='', param_rewrites='', convert_types=True + ) + + return LaunchDescription( + [ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + # Launch gazebo server for simulation + ExecuteProcess( + cmd=[ + 'gzserver', + '-s', + 'libgazebo_ros_init.so', + '--minimal_comms', + world, + ], + output='screen', + ), + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], + ), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py') + ), + launch_arguments={ + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': configured_params, + 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', + 'autostart': 'True', + }.items(), + ), + ] + ) def main(argv=sys.argv[1:]): @@ -89,9 +102,8 @@ def main(argv=sys.argv[1:]): testExecutable = os.getenv('TEST_EXECUTABLE') test1_action = ExecuteProcess( - cmd=[testExecutable], - name='test_wait_behavior_node', - output='screen') + cmd=[testExecutable], name='test_wait_behavior_node', output='screen' + ) lts = LaunchTestService() lts.add_test_action(ld, test1_action) diff --git a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py index 61c5392137..dcace8aeb6 100755 --- a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py +++ b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py @@ -22,7 +22,11 @@ from launch import LaunchDescription from launch import LaunchService -from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.actions import ( + ExecuteProcess, + IncludeLaunchDescription, + SetEnvironmentVariable, +) from launch.launch_context import LaunchContext from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node @@ -36,9 +40,11 @@ def generate_launch_description(): filter_mask_file = os.getenv('TEST_MASK') world = os.getenv('TEST_WORLD') - bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', - os.getenv('BT_NAVIGATOR_XML')) + bt_navigator_xml = os.path.join( + get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML'), + ) bringup_dir = get_package_share_directory('nav2_bringup') script_dir = os.path.dirname(os.path.realpath(__file__)) @@ -48,96 +54,119 @@ def generate_launch_description(): param_substitutions = { 'planner_server.ros__parameters.GridBased.use_astar': os.getenv('ASTAR'), 'filter_mask_server.ros__parameters.yaml_filename': filter_mask_file, - 'map_server.ros__parameters.yaml_filename': map_yaml_file} + 'map_server.ros__parameters.yaml_filename': map_yaml_file, + } configured_params = RewrittenYaml( source_file=params_file, root_key='', param_rewrites=param_substitutions, - convert_types=True) + convert_types=True, + ) context = LaunchContext() new_yaml = configured_params.perform(context) - return LaunchDescription([ - SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), - SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), - - # Launch gazebo server for simulation - ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_init.so', - '--minimal_comms', world], - output='screen'), - - # TODO(orduno) Launch the robot state publisher instead - # using a local copy of TB3 urdf file - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), - - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), - - Node( - package='nav2_lifecycle_manager', - executable='lifecycle_manager', - name='lifecycle_manager_filters', - output='screen', - parameters=[{ - 'node_names': - [ - 'filter_mask_server', 'costmap_filter_info_server' - ] - }, - {'autostart': True}]), - - # Nodes required for Costmap Filters configuration - Node( - package='nav2_map_server', - executable='map_server', - name='filter_mask_server', - output='screen', - parameters=[new_yaml]), - - Node( - package='nav2_map_server', - executable='costmap_filter_info_server', - name='costmap_filter_info_server', - output='screen', - parameters=[new_yaml]), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), - launch_arguments={'namespace': '', - 'use_namespace': 'False', - 'map': map_yaml_file, - 'use_sim_time': 'True', - 'params_file': new_yaml, - 'bt_xml_file': bt_navigator_xml, - 'use_composition': 'False', - 'autostart': 'True'}.items()), - - Node( - package='nav2_costmap_2d', - executable='nav2_costmap_2d_cloud', - name='costmap_2d_cloud', - output='screen', - remappings=[('voxel_grid', 'local_costmap/voxel_grid')]), - ]) + return LaunchDescription( + [ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + # Launch gazebo server for simulation + ExecuteProcess( + cmd=[ + 'gzserver', + '-s', + 'libgazebo_ros_init.so', + '--minimal_comms', + world, + ], + output='screen', + ), + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], + ), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + ), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_filters', + output='screen', + parameters=[ + { + 'node_names': [ + 'filter_mask_server', + 'costmap_filter_info_server', + ] + }, + {'autostart': True}, + ], + ), + # Nodes required for Costmap Filters configuration + Node( + package='nav2_map_server', + executable='map_server', + name='filter_mask_server', + output='screen', + parameters=[new_yaml], + ), + Node( + package='nav2_map_server', + executable='costmap_filter_info_server', + name='costmap_filter_info_server', + output='screen', + parameters=[new_yaml], + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py') + ), + launch_arguments={ + 'namespace': '', + 'use_namespace': 'False', + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': new_yaml, + 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', + 'autostart': 'True', + }.items(), + ), + Node( + package='nav2_costmap_2d', + executable='nav2_costmap_2d_cloud', + name='costmap_2d_cloud', + output='screen', + remappings=[('voxel_grid', 'local_costmap/voxel_grid')], + ), + ] + ) def main(argv=sys.argv[1:]): ld = generate_launch_description() test1_action = ExecuteProcess( - cmd=[os.path.join(os.getenv('TEST_DIR'), 'tester_node.py'), - '-t', 'keepout', '-r', '-2.0', '-0.5', '0.0', '-0.5'], + cmd=[ + os.path.join(os.getenv('TEST_DIR'), 'tester_node.py'), + '-t', + 'keepout', + '-r', + '-2.0', + '-0.5', + '0.0', + '-0.5', + ], name='tester_node', - output='screen') + output='screen', + ) lts = LaunchTestService() lts.add_test_action(ld, test1_action) diff --git a/nav2_system_tests/src/costmap_filters/test_speed_launch.py b/nav2_system_tests/src/costmap_filters/test_speed_launch.py index 971fcdd5e0..95d6fb412f 100755 --- a/nav2_system_tests/src/costmap_filters/test_speed_launch.py +++ b/nav2_system_tests/src/costmap_filters/test_speed_launch.py @@ -22,7 +22,11 @@ from launch import LaunchDescription from launch import LaunchService -from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.actions import ( + ExecuteProcess, + IncludeLaunchDescription, + SetEnvironmentVariable, +) from launch.launch_context import LaunchContext from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node @@ -36,9 +40,11 @@ def generate_launch_description(): filter_mask_file = os.getenv('TEST_MASK') world = os.getenv('TEST_WORLD') - bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', - os.getenv('BT_NAVIGATOR_XML')) + bt_navigator_xml = os.path.join( + get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML'), + ) bringup_dir = get_package_share_directory('nav2_bringup') params_file = os.getenv('PARAMS_FILE') @@ -47,88 +53,111 @@ def generate_launch_description(): param_substitutions = { 'planner_server.ros__parameters.GridBased.use_astar': os.getenv('ASTAR'), 'filter_mask_server.ros__parameters.yaml_filename': filter_mask_file, - 'map_server.ros__parameters.yaml_filename': map_yaml_file} + 'map_server.ros__parameters.yaml_filename': map_yaml_file, + } configured_params = RewrittenYaml( source_file=params_file, root_key='', param_rewrites=param_substitutions, - convert_types=True) + convert_types=True, + ) context = LaunchContext() new_yaml = configured_params.perform(context) - return LaunchDescription([ - SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), - SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), - - # Launch gazebo server for simulation - ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_init.so', - '--minimal_comms', world], - output='screen'), - - # TODO(orduno) Launch the robot state publisher instead - # using a local copy of TB3 urdf file - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), - - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), - - Node( - package='nav2_lifecycle_manager', - executable='lifecycle_manager', - name='lifecycle_manager_filters', - output='screen', - parameters=[{ - 'node_names': - [ - 'filter_mask_server', 'costmap_filter_info_server' - ] - }, - {'autostart': True}]), - - # Nodes required for Costmap Filters configuration - Node( - package='nav2_map_server', - executable='map_server', - name='filter_mask_server', - output='screen', - parameters=[new_yaml]), - - Node( - package='nav2_map_server', - executable='costmap_filter_info_server', - name='costmap_filter_info_server', - output='screen', - parameters=[new_yaml]), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), - launch_arguments={'namespace': '', - 'use_namespace': 'False', - 'map': map_yaml_file, - 'use_sim_time': 'True', - 'params_file': new_yaml, - 'bt_xml_file': bt_navigator_xml, - 'use_composition': 'False', - 'autostart': 'True'}.items()), - ]) + return LaunchDescription( + [ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + # Launch gazebo server for simulation + ExecuteProcess( + cmd=[ + 'gzserver', + '-s', + 'libgazebo_ros_init.so', + '--minimal_comms', + world, + ], + output='screen', + ), + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], + ), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + ), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_filters', + output='screen', + parameters=[ + { + 'node_names': [ + 'filter_mask_server', + 'costmap_filter_info_server', + ] + }, + {'autostart': True}, + ], + ), + # Nodes required for Costmap Filters configuration + Node( + package='nav2_map_server', + executable='map_server', + name='filter_mask_server', + output='screen', + parameters=[new_yaml], + ), + Node( + package='nav2_map_server', + executable='costmap_filter_info_server', + name='costmap_filter_info_server', + output='screen', + parameters=[new_yaml], + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py') + ), + launch_arguments={ + 'namespace': '', + 'use_namespace': 'False', + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': new_yaml, + 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', + 'autostart': 'True', + }.items(), + ), + ] + ) def main(argv=sys.argv[1:]): ld = generate_launch_description() test1_action = ExecuteProcess( - cmd=[os.path.join(os.getenv('TEST_DIR'), 'tester_node.py'), - '-t', 'speed', '-r', '-2.0', '-0.5', '0.0', '-0.5'], + cmd=[ + os.path.join(os.getenv('TEST_DIR'), 'tester_node.py'), + '-t', + 'speed', + '-r', + '-2.0', + '-0.5', + '0.0', + '-0.5', + ], name='tester_node', - output='screen') + output='screen', + ) lts = LaunchTestService() lts.add_test_action(ld, test1_action) diff --git a/nav2_system_tests/src/costmap_filters/tester_node.py b/nav2_system_tests/src/costmap_filters/tester_node.py index 0a61116254..da8ab82a20 100755 --- a/nav2_system_tests/src/costmap_filters/tester_node.py +++ b/nav2_system_tests/src/costmap_filters/tester_node.py @@ -46,12 +46,8 @@ class TestType(Enum): SPEED = 1 -class FilterMask(): - - def __init__( - self, - filter_mask: OccupancyGrid - ): +class FilterMask: + def __init__(self, filter_mask: OccupancyGrid): self.filter_mask = filter_mask # Converts world coordinates into filter mask map coordinate. @@ -82,38 +78,45 @@ def getValue(self, mx, my): class NavTester(Node): - def __init__( self, test_type: TestType, initial_pose: Pose, goal_pose: Pose, - namespace: str = '' + namespace: str = '', ): super().__init__(node_name='nav2_tester', namespace=namespace) - self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped, - 'initialpose', 10) + self.initial_pose_pub = self.create_publisher( + PoseWithCovarianceStamped, 'initialpose', 10 + ) self.goal_pub = self.create_publisher(PoseStamped, 'goal_pose', 10) transient_local_qos = QoSProfile( durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, reliability=QoSReliabilityPolicy.RELIABLE, history=QoSHistoryPolicy.KEEP_LAST, - depth=1) + depth=1, + ) volatile_qos = QoSProfile( durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE, reliability=QoSReliabilityPolicy.RELIABLE, history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, - depth=1) - - self.model_pose_sub = self.create_subscription(PoseWithCovarianceStamped, - 'amcl_pose', self.poseCallback, - transient_local_qos) - self.clearing_ep_sub = self.create_subscription(PointCloud2, - 'local_costmap/clearing_endpoints', - self.clearingEndpointsCallback, - transient_local_qos) + depth=1, + ) + + self.model_pose_sub = self.create_subscription( + PoseWithCovarianceStamped, + 'amcl_pose', + self.poseCallback, + transient_local_qos, + ) + self.clearing_ep_sub = self.create_subscription( + PointCloud2, + 'local_costmap/clearing_endpoints', + self.clearingEndpointsCallback, + transient_local_qos, + ) self.test_type = test_type self.filter_test_result = True self.clearing_endpoints_received = False @@ -122,38 +125,37 @@ def __init__( self.cost_cloud_received = False if self.test_type == TestType.KEEPOUT: - self.plan_sub = self.create_subscription(Path, 'plan', - self.planCallback, volatile_qos) - self.voxel_marked_sub = self.create_subscription(PointCloud2, - 'voxel_marked_cloud', - self.voxelMarkedCallback, - 1) - self.voxel_unknown_sub = self.create_subscription(PointCloud2, - 'voxel_unknown_cloud', - self.voxelUnknownCallback, - 1) - self.cost_cloud_sub = self.create_subscription(PointCloud2, - 'cost_cloud', - self.dwbCostCloudCallback, - 1) + self.plan_sub = self.create_subscription( + Path, 'plan', self.planCallback, volatile_qos + ) + self.voxel_marked_sub = self.create_subscription( + PointCloud2, 'voxel_marked_cloud', self.voxelMarkedCallback, 1 + ) + self.voxel_unknown_sub = self.create_subscription( + PointCloud2, 'voxel_unknown_cloud', self.voxelUnknownCallback, 1 + ) + self.cost_cloud_sub = self.create_subscription( + PointCloud2, 'cost_cloud', self.dwbCostCloudCallback, 1 + ) elif self.test_type == TestType.SPEED: self.speed_it = 0 # Expected chain of speed limits self.limits = [50.0, 0.0] - # Permissive array: all received speed limits must match to "limits" from above + # Permissive array: all received speed limits must match to 'limits' from above self.limit_passed = [False, False] - self.plan_sub = self.create_subscription(SpeedLimit, 'speed_limit', - self.speedLimitCallback, volatile_qos) + self.plan_sub = self.create_subscription( + SpeedLimit, 'speed_limit', self.speedLimitCallback, volatile_qos + ) self.mask_received = False - self.mask_sub = self.create_subscription(OccupancyGrid, 'filter_mask', - self.maskCallback, transient_local_qos) + self.mask_sub = self.create_subscription( + OccupancyGrid, 'filter_mask', self.maskCallback, transient_local_qos + ) self.initial_pose_received = False self.initial_pose = initial_pose self.goal_pose = goal_pose - self.action_client = ActionClient( - self, NavigateToPose, 'navigate_to_pose') + self.action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose') def info_msg(self, msg: str): self.get_logger().info('\033[1;37;44m' + msg + '\033[0m') @@ -186,8 +188,7 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None): # Sends a `NavToPose` action request and waits for completion self.info_msg("Waiting for 'NavigateToPose' action server") while not self.action_client.wait_for_server(timeout_sec=1.0): - self.info_msg( - "'NavigateToPose' action server not available, waiting...") + self.info_msg("'NavigateToPose' action server not available, waiting...") self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose goal_msg = NavigateToPose.Goal() @@ -235,12 +236,12 @@ def checkKeepout(self, x, y): return True # Checks that currently received speed_limit is equal to the it-th item - # of expected speed "limits" array. - # If so, sets it-th item of permissive array "limit_passed" to be true. + # of expected speed 'limits' array. + # If so, sets it-th item of permissive array 'limit_passed' to be true. # Otherwise it will be remained to be false. # Also verifies that speed limit messages received no more than N-times - # (where N - is the length of "limits" array), - # otherwise sets overall "filter_test_result" to be false. + # (where N - is the length of 'limits' array), + # otherwise sets overall 'filter_test_result' to be false. def checkSpeed(self, it, speed_limit): if it >= len(self.limits): self.error_msg('Got excess speed limit') @@ -249,15 +250,21 @@ def checkSpeed(self, it, speed_limit): if speed_limit == self.limits[it]: self.limit_passed[it] = True else: - self.error_msg('Incorrect speed limit received: ' + str(speed_limit) + - ', but should be: ' + str(self.limits[it])) + self.error_msg( + 'Incorrect speed limit received: ' + + str(speed_limit) + + ', but should be: ' + + str(self.limits[it]) + ) def poseCallback(self, msg): self.info_msg('Received amcl_pose') self.current_pose = msg.pose.pose self.initial_pose_received = True if self.test_type == TestType.KEEPOUT: - if not self.checkKeepout(msg.pose.pose.position.x, msg.pose.pose.position.y): + if not self.checkKeepout( + msg.pose.pose.position.x, msg.pose.pose.position.y + ): self.error_msg('Robot goes into keepout zone') def planCallback(self, msg): @@ -307,16 +314,21 @@ def wait_for_filter_mask(self, timeout): def wait_for_pointcloud_subscribers(self, timeout): start_time = time.time() - while not self.voxel_unknown_received or not self.voxel_marked_received \ - or not self.clearing_endpoints_received: + while ( + not self.voxel_unknown_received + or not self.voxel_marked_received + or not self.clearing_endpoints_received + ): self.info_msg( 'Waiting for voxel_marked_cloud/voxel_unknown_cloud/\ - clearing_endpoints msg to be received ...') + clearing_endpoints msg to be received ...' + ) rclpy.spin_once(self, timeout_sec=1) if (time.time() - start_time) > timeout: self.error_msg( 'Time out to waiting for voxel_marked_cloud/voxel_unknown_cloud/\ - clearing_endpoints msgs') + clearing_endpoints msgs' + ) return False return True @@ -351,7 +363,7 @@ def wait_for_node_active(self, node_name: str): self.info_msg(f'{node_service} service not available, waiting...') req = GetState.Request() # empty request state = 'UNKNOWN' - while (state != 'active'): + while state != 'active': self.info_msg(f'Getting {node_name} state...') future = state_client.call_async(req) rclpy.spin_until_future_complete(self, future) @@ -359,8 +371,9 @@ def wait_for_node_active(self, node_name: str): state = future.result().current_state.label self.info_msg(f'Result of get_state: {state}') else: - self.error_msg('Exception while calling service: %r' % - future.exception()) + self.error_msg( + 'Exception while calling service: %r' % future.exception() + ) time.sleep(5) def shutdown(self): @@ -368,8 +381,7 @@ def shutdown(self): self.action_client.destroy() transition_service = 'lifecycle_manager_navigation/manage_nodes' - mgr_client = self.create_client( - ManageLifecycleNodes, transition_service) + mgr_client = self.create_client(ManageLifecycleNodes, transition_service) while not mgr_client.wait_for_service(timeout_sec=1.0): self.info_msg(f'{transition_service} service not available, waiting...') @@ -380,13 +392,11 @@ def shutdown(self): self.info_msg('Shutting down navigation lifecycle manager...') rclpy.spin_until_future_complete(self, future) future.result() - self.info_msg( - 'Shutting down navigation lifecycle manager complete.') + self.info_msg('Shutting down navigation lifecycle manager complete.') except Exception as e: # noqa: B902 self.error_msg(f'Service call failed {e!r}') transition_service = 'lifecycle_manager_localization/manage_nodes' - mgr_client = self.create_client( - ManageLifecycleNodes, transition_service) + mgr_client = self.create_client(ManageLifecycleNodes, transition_service) while not mgr_client.wait_for_service(timeout_sec=1.0): self.info_msg(f'{transition_service} service not available, waiting...') @@ -397,8 +407,7 @@ def shutdown(self): self.info_msg('Shutting down localization lifecycle manager...') rclpy.spin_until_future_complete(self, future) future.result() - self.info_msg( - 'Shutting down localization lifecycle manager complete') + self.info_msg('Shutting down localization lifecycle manager complete') except Exception as e: # noqa: B902 self.error_msg(f'Service call failed {e!r}') @@ -419,10 +428,10 @@ def test_RobotMovesToGoal(robot_tester): # Tests that all received speed limits are correct: -# If overall "filter_test_result" is true -# checks that all items in "limit_passed" permissive array are also true. +# If overall 'filter_test_result' is true +# checks that all items in 'limit_passed' permissive array are also true. # In other words, it verifies that all speed limits are received -# exactly (by count and values) as expected by "limits" array. +# exactly (by count and values) as expected by 'limits' array. def test_SpeedLimitsAllCorrect(robot_tester): if not robot_tester.filter_test_result: return False @@ -436,21 +445,21 @@ def test_SpeedLimitsAllCorrect(robot_tester): def run_all_tests(robot_tester): # set transforms to use_sim_time result = True - if (result): + if result: robot_tester.wait_for_node_active('amcl') robot_tester.wait_for_initial_pose() robot_tester.wait_for_node_active('bt_navigator') result = robot_tester.wait_for_filter_mask(10) - if (result): + if result: result = robot_tester.runNavigateAction() if robot_tester.test_type == TestType.KEEPOUT: result = result and robot_tester.wait_for_pointcloud_subscribers(10) - if (result): + if result: result = test_RobotMovesToGoal(robot_tester) - if (result): + if result: if robot_tester.test_type == TestType.KEEPOUT: result = robot_tester.filter_test_result result = result and robot_tester.cost_cloud_received @@ -459,7 +468,7 @@ def run_all_tests(robot_tester): # Add more tests here if desired - if (result): + if result: robot_tester.info_msg('Test PASSED') else: robot_tester.error_msg('Test FAILED') @@ -491,23 +500,44 @@ def get_tester(args): tester = NavTester( test_type, initial_pose=fwd_pose(float(init_x), float(init_y)), - goal_pose=fwd_pose(float(final_x), float(final_y))) + goal_pose=fwd_pose(float(final_x), float(final_y)), + ) tester.info_msg( - 'Starting tester, robot going from ' + init_x + ', ' + init_y + - ' to ' + final_x + ', ' + final_y + '.') + 'Starting tester, robot going from ' + + init_x + + ', ' + + init_y + + ' to ' + + final_x + + ', ' + + final_y + + '.' + ) return tester def main(argv=sys.argv[1:]): # The robot(s) positions from the input arguments parser = argparse.ArgumentParser( - description='System-level costmap filters tester node') - parser.add_argument('-t', '--type', type=str, action='store', dest='type', - help='Type of costmap filter being tested.') + description='System-level costmap filters tester node' + ) + parser.add_argument( + '-t', + '--type', + type=str, + action='store', + dest='type', + help='Type of costmap filter being tested.', + ) group = parser.add_mutually_exclusive_group(required=True) - group.add_argument('-r', '--robot', action='append', nargs=4, - metavar=('init_x', 'init_y', 'final_x', 'final_y'), - help='The robot starting and final positions.') + group.add_argument( + '-r', + '--robot', + action='append', + nargs=4, + metavar=('init_x', 'init_y', 'final_x', 'final_y'), + help='The robot starting and final positions.', + ) args, unknown = parser.parse_known_args() diff --git a/nav2_system_tests/src/error_codes/test_error_codes.py b/nav2_system_tests/src/error_codes/test_error_codes.py index 7a46f5fd81..f023bbb27c 100755 --- a/nav2_system_tests/src/error_codes/test_error_codes.py +++ b/nav2_system_tests/src/error_codes/test_error_codes.py @@ -17,7 +17,12 @@ import time from geometry_msgs.msg import PoseStamped -from nav2_msgs.action import ComputePathThroughPoses, ComputePathToPose, FollowPath, SmoothPath +from nav2_msgs.action import ( + ComputePathThroughPoses, + ComputePathToPose, + FollowPath, + SmoothPath, +) from nav2_simple_commander.robot_navigator import BasicNavigator from nav_msgs.msg import Path import rclpy @@ -60,7 +65,7 @@ def main(argv=sys.argv[1:]): 'invalid_path': FollowPath.Result().INVALID_PATH, 'patience_exceeded': FollowPath.Result().PATIENCE_EXCEEDED, 'failed_to_make_progress': FollowPath.Result().FAILED_TO_MAKE_PROGRESS, - 'no_valid_control': FollowPath.Result().NO_VALID_CONTROL + 'no_valid_control': FollowPath.Result().NO_VALID_CONTROL, } for controller, error_code in follow_path.items(): @@ -70,8 +75,9 @@ def main(argv=sys.argv[1:]): while not navigator.isTaskComplete(): time.sleep(0.5) - assert navigator.result_future.result().result.error_code == error_code, \ - 'Follow path error code does not match' + assert ( + navigator.result_future.result().result.error_code == error_code + ), 'Follow path error code does not match' else: assert False, 'Follow path was rejected' @@ -96,11 +102,14 @@ def main(argv=sys.argv[1:]): 'start_occupied': ComputePathToPose.Result().START_OCCUPIED, 'goal_occupied': ComputePathToPose.Result().GOAL_OCCUPIED, 'timeout': ComputePathToPose.Result().TIMEOUT, - 'no_valid_path': ComputePathToPose.Result().NO_VALID_PATH} + 'no_valid_path': ComputePathToPose.Result().NO_VALID_PATH, + } for planner, error_code in compute_path_to_pose.items(): result = navigator._getPathImpl(initial_pose, goal_pose, planner) - assert result.error_code == error_code, 'Compute path to pose error does not match' + assert ( + result.error_code == error_code + ), 'Compute path to pose error does not match' # Check compute path through error codes goal_pose1 = goal_pose @@ -116,11 +125,14 @@ def main(argv=sys.argv[1:]): 'goal_occupied': ComputePathThroughPoses.Result().GOAL_OCCUPIED, 'timeout': ComputePathThroughPoses.Result().TIMEOUT, 'no_valid_path': ComputePathThroughPoses.Result().NO_VALID_PATH, - 'no_viapoints_given': ComputePathThroughPoses.Result().NO_VIAPOINTS_GIVEN} + 'no_viapoints_given': ComputePathThroughPoses.Result().NO_VIAPOINTS_GIVEN, + } for planner, error_code in compute_path_through_poses.items(): result = navigator._getPathThroughPosesImpl(initial_pose, goal_poses, planner) - assert result.error_code == error_code, 'Compute path through pose error does not match' + assert ( + result.error_code == error_code + ), 'Compute path through pose error does not match' # Check compute path to pose error codes pose = PoseStamped() @@ -146,7 +158,7 @@ def main(argv=sys.argv[1:]): 'timeout': SmoothPath.Result().TIMEOUT, 'smoothed_path_in_collision': SmoothPath.Result().SMOOTHED_PATH_IN_COLLISION, 'failed_to_smooth_path': SmoothPath.Result().FAILED_TO_SMOOTH_PATH, - 'invalid_path': SmoothPath.Result().INVALID_PATH + 'invalid_path': SmoothPath.Result().INVALID_PATH, } for smoother, error_code in smoother.items(): diff --git a/nav2_system_tests/src/error_codes/test_error_codes_launch.py b/nav2_system_tests/src/error_codes/test_error_codes_launch.py index d70c56ac8e..98a33d38d6 100755 --- a/nav2_system_tests/src/error_codes/test_error_codes_launch.py +++ b/nav2_system_tests/src/error_codes/test_error_codes_launch.py @@ -27,12 +27,9 @@ def generate_launch_description(): params_file = os.path.join(os.getenv('TEST_DIR'), 'error_code_param.yaml') - remappings = [('/tf', 'tf'), - ('/tf_static', 'tf_static')] + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] - lifecycle_nodes = ['controller_server', - 'planner_server', - 'smoother_server'] + lifecycle_nodes = ['controller_server', 'planner_server', 'smoother_server'] load_nodes = GroupAction( actions=[ @@ -40,19 +37,22 @@ def generate_launch_description(): package='tf2_ros', executable='static_transform_publisher', output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom']), + arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'], + ), Node( package='tf2_ros', executable='static_transform_publisher', output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'odom', 'base_link']), + arguments=['0', '0', '0', '0', '0', '0', 'odom', 'base_link'], + ), Node( package='nav2_controller', executable='controller_server', output='screen', respawn_delay=2.0, parameters=[params_file], - remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), + remappings=remappings + [('cmd_vel', 'cmd_vel_nav')], + ), Node( package='nav2_planner', executable='planner_server', @@ -60,7 +60,8 @@ def generate_launch_description(): output='screen', respawn_delay=2.0, parameters=[params_file], - remappings=remappings), + remappings=remappings, + ), Node( package='nav2_smoother', executable='smoother_server', @@ -68,15 +69,17 @@ def generate_launch_description(): output='screen', respawn_delay=2.0, parameters=[params_file], - remappings=remappings), + remappings=remappings, + ), Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_navigation', output='screen', - parameters=[{'autostart': True}, - {'node_names': lifecycle_nodes}]) - ]) + parameters=[{'autostart': True}, {'node_names': lifecycle_nodes}], + ), + ] + ) ld = LaunchDescription() ld.add_action(load_nodes) @@ -90,7 +93,8 @@ def main(argv=sys.argv[1:]): test_error_codes_action = ExecuteProcess( cmd=[os.path.join(os.getenv('TEST_DIR'), 'test_error_codes.py')], name='test_error_codes', - output='screen') + output='screen', + ) lts = LaunchTestService() lts.add_test_action(ld, test_error_codes_action) diff --git a/nav2_system_tests/src/gps_navigation/dual_ekf_navsat.launch.py b/nav2_system_tests/src/gps_navigation/dual_ekf_navsat.launch.py index 852436615b..8cc64ec69e 100644 --- a/nav2_system_tests/src/gps_navigation/dual_ekf_navsat.launch.py +++ b/nav2_system_tests/src/gps_navigation/dual_ekf_navsat.launch.py @@ -11,52 +11,53 @@ # See the License for the specific language governing permissions and # limitations under the License. -from launch import LaunchDescription -import launch_ros.actions import os + +from launch import LaunchDescription import launch.actions +import launch_ros.actions def generate_launch_description(): launch_dir = os.path.dirname(os.path.realpath(__file__)) - params_file = os.path.join(launch_dir, "dual_ekf_navsat_params.yaml") - os.environ["FILE_PATH"] = str(launch_dir) + params_file = os.path.join(launch_dir, 'dual_ekf_navsat_params.yaml') + os.environ['FILE_PATH'] = str(launch_dir) return LaunchDescription( [ launch.actions.DeclareLaunchArgument( - "output_final_position", default_value="false" + 'output_final_position', default_value='false' ), launch.actions.DeclareLaunchArgument( - "output_location", default_value="~/dual_ekf_navsat_example_debug.txt" + 'output_location', default_value='~/dual_ekf_navsat_example_debug.txt' ), launch_ros.actions.Node( - package="robot_localization", - executable="ekf_node", - name="ekf_filter_node_odom", - output="screen", - parameters=[params_file, {"use_sim_time": True}], - remappings=[("odometry/filtered", "odometry/local")], + package='robot_localization', + executable='ekf_node', + name='ekf_filter_node_odom', + output='screen', + parameters=[params_file, {'use_sim_time': True}], + remappings=[('odometry/filtered', 'odometry/local')], ), launch_ros.actions.Node( - package="robot_localization", - executable="ekf_node", - name="ekf_filter_node_map", - output="screen", - parameters=[params_file, {"use_sim_time": True}], - remappings=[("odometry/filtered", "odometry/global")], + package='robot_localization', + executable='ekf_node', + name='ekf_filter_node_map', + output='screen', + parameters=[params_file, {'use_sim_time': True}], + remappings=[('odometry/filtered', 'odometry/global')], ), launch_ros.actions.Node( - package="robot_localization", - executable="navsat_transform_node", - name="navsat_transform", - output="screen", - parameters=[params_file, {"use_sim_time": True}], + package='robot_localization', + executable='navsat_transform_node', + name='navsat_transform', + output='screen', + parameters=[params_file, {'use_sim_time': True}], remappings=[ - ("imu/data", "imu/data"), - ("gps/fix", "gps/fix"), - ("gps/filtered", "gps/filtered"), - ("odometry/gps", "odometry/gps"), - ("odometry/filtered", "odometry/global"), + ('imu/data', 'imu/data'), + ('gps/fix', 'gps/fix'), + ('gps/filtered', 'gps/filtered'), + ('odometry/gps', 'odometry/gps'), + ('odometry/filtered', 'odometry/global'), ], ), ] diff --git a/nav2_system_tests/src/gps_navigation/test_case_py.launch.py b/nav2_system_tests/src/gps_navigation/test_case_py.launch.py index cd90fdb620..db949b7756 100755 --- a/nav2_system_tests/src/gps_navigation/test_case_py.launch.py +++ b/nav2_system_tests/src/gps_navigation/test_case_py.launch.py @@ -33,79 +33,79 @@ def generate_launch_description(): - world = os.getenv("TEST_WORLD") + world = os.getenv('TEST_WORLD') launch_dir = os.path.dirname(os.path.realpath(__file__)) - params_file = os.path.join(launch_dir, "nav2_no_map_params.yaml") - bringup_dir = get_package_share_directory("nav2_bringup") + params_file = os.path.join(launch_dir, 'nav2_no_map_params.yaml') + bringup_dir = get_package_share_directory('nav2_bringup') configured_params = RewrittenYaml( - source_file=params_file, root_key="", param_rewrites="", convert_types=True + source_file=params_file, root_key='', param_rewrites='', convert_types=True ) return LaunchDescription( [ - SetEnvironmentVariable("RCUTILS_LOGGING_BUFFERED_STREAM", "1"), - SetEnvironmentVariable("RCUTILS_LOGGING_USE_STDOUT", "1"), + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), # Launch gazebo server for simulation ExecuteProcess( cmd=[ - "gzserver", - "-s", - "libgazebo_ros_init.so", - "--minimal_comms", + 'gzserver', + '-s', + 'libgazebo_ros_init.so', + '--minimal_comms', world, ], - output="screen", + output='screen', ), # TODO(orduno) Launch the robot state publisher instead # using a local copy of TB3 urdf file Node( - package="tf2_ros", - executable="static_transform_publisher", - output="screen", - arguments=["0", "0", "0", "0", "0", "0", "base_footprint", "base_link"], + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], ), Node( - package="tf2_ros", - executable="static_transform_publisher", - output="screen", - arguments=["0", "0", "0", "0", "0", "0", "base_link", "base_scan"], + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], ), Node( - package="tf2_ros", - executable="static_transform_publisher", - output="screen", + package='tf2_ros', + executable='static_transform_publisher', + output='screen', arguments=[ - "-0.32", - "0", - "0.068", - "0", - "0", - "0", - "base_link", - "imu_link", + '-0.32', + '0', + '0.068', + '0', + '0', + '0', + 'base_link', + 'imu_link', ], ), Node( - package="tf2_ros", - executable="static_transform_publisher", - output="screen", - arguments=["0", "0", "0", "0", "0", "0", "base_link", "gps_link"], + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'gps_link'], ), IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(bringup_dir, "launch", "navigation_launch.py") + os.path.join(bringup_dir, 'launch', 'navigation_launch.py') ), launch_arguments={ - "use_sim_time": "True", - "params_file": configured_params, - "autostart": "True", + 'use_sim_time': 'True', + 'params_file': configured_params, + 'autostart': 'True', }.items(), ), IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(launch_dir, "dual_ekf_navsat.launch.py") + os.path.join(launch_dir, 'dual_ekf_navsat.launch.py') ), ), ] @@ -116,9 +116,9 @@ def main(argv=sys.argv[1:]): ld = generate_launch_description() test1_action = ExecuteProcess( - cmd=[os.path.join(os.getenv("TEST_DIR"), "tester.py")], - name="tester_node", - output="screen", + cmd=[os.path.join(os.getenv('TEST_DIR'), 'tester.py')], + name='tester_node', + output='screen', ) lts = LaunchTestService() @@ -128,5 +128,5 @@ def main(argv=sys.argv[1:]): return lts.run(ls) -if __name__ == "__main__": +if __name__ == '__main__': sys.exit(main()) diff --git a/nav2_system_tests/src/gps_navigation/tester.py b/nav2_system_tests/src/gps_navigation/tester.py index aab8833668..5e765981ab 100755 --- a/nav2_system_tests/src/gps_navigation/tester.py +++ b/nav2_system_tests/src/gps_navigation/tester.py @@ -21,25 +21,27 @@ from nav2_msgs.action import ComputePathToPose, FollowGPSWaypoints from nav2_msgs.srv import ManageLifecycleNodes from rcl_interfaces.srv import SetParameters -from rclpy.parameter import Parameter import rclpy + from rclpy.action import ActionClient from rclpy.node import Node +from rclpy.parameter import Parameter class GpsWaypointFollowerTest(Node): def __init__(self): - super().__init__(node_name="nav2_gps_waypoint_tester", namespace="") + super().__init__(node_name='nav2_gps_waypoint_tester', namespace='') self.waypoints = None self.action_client = ActionClient( - self, FollowGPSWaypoints, "follow_gps_waypoints" + self, FollowGPSWaypoints, 'follow_gps_waypoints' ) self.goal_handle = None self.action_result = None - self.param_cli = self.create_client(SetParameters, - '/waypoint_follower/set_parameters') + self.param_cli = self.create_client( + SetParameters, '/waypoint_follower/set_parameters' + ) def setWaypoints(self, waypoints): self.waypoints = [] @@ -52,29 +54,30 @@ def setWaypoints(self, waypoints): def run(self, block, cancel): # if not self.waypoints: - # rclpy.error_msg("Did not set valid waypoints before running test!") + # rclpy.error_msg('Did not set valid waypoints before running test!') # return False while not self.action_client.wait_for_server(timeout_sec=1.0): self.info_msg( - "'follow_gps_waypoints' action server not available, waiting...") + "'follow_gps_waypoints' action server not available, waiting..." + ) action_request = FollowGPSWaypoints.Goal() action_request.gps_poses = self.waypoints - self.info_msg("Sending goal request...") + self.info_msg('Sending goal request...') send_goal_future = self.action_client.send_goal_async(action_request) try: rclpy.spin_until_future_complete(self, send_goal_future) self.goal_handle = send_goal_future.result() except Exception as e: # noqa: B902 - self.error_msg(f"Service call failed {e!r}") + self.error_msg(f'Service call failed {e!r}') if not self.goal_handle.accepted: - self.error_msg("Goal rejected") + self.error_msg('Goal rejected') return False - self.info_msg("Goal accepted") + self.info_msg('Goal accepted') if not block: return True @@ -90,40 +93,39 @@ def run(self, block, cancel): result = get_result_future.result().result self.action_result = result except Exception as e: # noqa: B902 - self.error_msg(f"Service call failed {e!r}") + self.error_msg(f'Service call failed {e!r}') if status != GoalStatus.STATUS_SUCCEEDED: - self.info_msg(f"Goal failed with status code: {status}") + self.info_msg(f'Goal failed with status code: {status}') return False if len(result.missed_waypoints) > 0: self.info_msg( - "Goal failed to process all waypoints," - " missed {0} wps.".format(len(result.missed_waypoints)) + 'Goal failed to process all waypoints,' + ' missed {0} wps.'.format(len(result.missed_waypoints)) ) return False - self.info_msg("Goal succeeded!") + self.info_msg('Goal succeeded!') return True def setStopFailureParam(self, value): req = SetParameters.Request() - req.parameters = [Parameter('stop_on_failure', - Parameter.Type.BOOL, value).to_parameter_msg()] + req.parameters = [ + Parameter('stop_on_failure', Parameter.Type.BOOL, value).to_parameter_msg() + ] future = self.param_cli.call_async(req) rclpy.spin_until_future_complete(self, future) def shutdown(self): - self.info_msg("Shutting down") + self.info_msg('Shutting down') self.action_client.destroy() - self.info_msg("Destroyed follow_gps_waypoints action client") + self.info_msg('Destroyed follow_gps_waypoints action client') - transition_service = "lifecycle_manager_navigation/manage_nodes" - mgr_client = self.create_client( - ManageLifecycleNodes, transition_service) + transition_service = 'lifecycle_manager_navigation/manage_nodes' + mgr_client = self.create_client(ManageLifecycleNodes, transition_service) while not mgr_client.wait_for_service(timeout_sec=1.0): - self.info_msg( - f"{transition_service} service not available, waiting...") + self.info_msg(f'{transition_service} service not available, waiting...') req = ManageLifecycleNodes.Request() req.command = ManageLifecycleNodes.Request().SHUTDOWN @@ -132,9 +134,9 @@ def shutdown(self): rclpy.spin_until_future_complete(self, future) future.result() except Exception as e: # noqa: B902 - self.error_msg(f"{transition_service} service call failed {e!r}") + self.error_msg(f'{transition_service} service call failed {e!r}') - self.info_msg(f"{transition_service} finished") + self.info_msg(f'{transition_service} finished') def cancel_goal(self): cancel_future = self.goal_handle.cancel_goal_async() @@ -183,8 +185,10 @@ def main(argv=sys.argv[1:]): result = test.run(True, False) assert not result result = not result - assert test.action_result.missed_waypoints[0].error_code == \ - ComputePathToPose.Result().GOAL_OUTSIDE_MAP + assert ( + test.action_result.missed_waypoints[0].error_code + == ComputePathToPose.Result().GOAL_OUTSIDE_MAP + ) # stop on failure test with bogous waypoint test.setStopFailureParam(True) @@ -208,15 +212,15 @@ def main(argv=sys.argv[1:]): result = not result test.shutdown() - test.info_msg("Done Shutting Down.") + test.info_msg('Done Shutting Down.') if not result: - test.info_msg("Exiting failed") + test.info_msg('Exiting failed') exit(1) else: - test.info_msg("Exiting passed") + test.info_msg('Exiting passed') exit(0) -if __name__ == "__main__": +if __name__ == '__main__': main() diff --git a/nav2_system_tests/src/localization/test_localization_launch.py b/nav2_system_tests/src/localization/test_localization_launch.py index 70c30e0842..d638e02c4e 100755 --- a/nav2_system_tests/src/localization/test_localization_launch.py +++ b/nav2_system_tests/src/localization/test_localization_launch.py @@ -32,40 +32,50 @@ def main(argv=sys.argv[1:]): launch_gazebo = launch.actions.ExecuteProcess( cmd=['gzserver', '-s', 'libgazebo_ros_init.so', '--minimal_comms', world], - output='screen') + output='screen', + ) link_footprint = launch_ros.actions.Node( package='tf2_ros', executable='static_transform_publisher', output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']) + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], + ) footprint_scan = launch_ros.actions.Node( package='tf2_ros', executable='static_transform_publisher', output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']) + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + ) run_map_server = launch_ros.actions.Node( package='nav2_map_server', executable='map_server', name='map_server', output='screen', - parameters=[{'yaml_filename': mapFile}]) + parameters=[{'yaml_filename': mapFile}], + ) run_amcl = launch_ros.actions.Node( - package='nav2_amcl', - executable='amcl', - output='screen') + package='nav2_amcl', executable='amcl', output='screen' + ) run_lifecycle_manager = launch_ros.actions.Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager', output='screen', - parameters=[{'node_names': ['map_server', 'amcl']}, {'autostart': True}]) - ld = LaunchDescription([launch_gazebo, link_footprint, footprint_scan, - run_map_server, run_amcl, run_lifecycle_manager]) + parameters=[{'node_names': ['map_server', 'amcl']}, {'autostart': True}], + ) + ld = LaunchDescription( + [ + launch_gazebo, + link_footprint, + footprint_scan, + run_map_server, + run_amcl, + run_lifecycle_manager, + ] + ) test1_action = ExecuteProcess( - cmd=[testExecutable], - name='test_localization_node', - output='screen' + cmd=[testExecutable], name='test_localization_node', output='screen' ) lts = LaunchTestService() diff --git a/nav2_system_tests/src/planning/test_planner_costmaps_launch.py b/nav2_system_tests/src/planning/test_planner_costmaps_launch.py index 4aa3dfa55a..09bd3d34ee 100755 --- a/nav2_system_tests/src/planning/test_planner_costmaps_launch.py +++ b/nav2_system_tests/src/planning/test_planner_costmaps_launch.py @@ -32,7 +32,7 @@ def main(argv=sys.argv[1:]): test1_action = ExecuteProcess( cmd=[testExecutable, '--ros-args -p use_sim_time:=True'], name='test_planner_costmaps_node', - output='screen' + output='screen', ) lts = LaunchTestService() diff --git a/nav2_system_tests/src/planning/test_planner_random_launch.py b/nav2_system_tests/src/planning/test_planner_random_launch.py index 6d6563616c..a7747e513a 100755 --- a/nav2_system_tests/src/planning/test_planner_random_launch.py +++ b/nav2_system_tests/src/planning/test_planner_random_launch.py @@ -32,7 +32,7 @@ def main(argv=sys.argv[1:]): test1_action = ExecuteProcess( cmd=[testExecutable, '--ros-args -p use_sim_time:=True'], name='test_planner_random_node', - output='screen' + output='screen', ) lts = LaunchTestService() diff --git a/nav2_system_tests/src/system/nav_through_poses_tester_node.py b/nav2_system_tests/src/system/nav_through_poses_tester_node.py index de3b823c6d..5e587ad8fa 100755 --- a/nav2_system_tests/src/system/nav_through_poses_tester_node.py +++ b/nav2_system_tests/src/system/nav_through_poses_tester_node.py @@ -37,29 +37,28 @@ class NavTester(Node): - - def __init__( - self, - initial_pose: Pose, - goal_pose: Pose, - namespace: str = '' - ): + def __init__(self, initial_pose: Pose, goal_pose: Pose, namespace: str = ''): super().__init__(node_name='nav2_tester', namespace=namespace) - self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped, - 'initialpose', 10) + self.initial_pose_pub = self.create_publisher( + PoseWithCovarianceStamped, 'initialpose', 10 + ) pose_qos = QoSProfile( - durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1) - - self.model_pose_sub = self.create_subscription(PoseWithCovarianceStamped, - 'amcl_pose', self.poseCallback, pose_qos) + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ) + + self.model_pose_sub = self.create_subscription( + PoseWithCovarianceStamped, 'amcl_pose', self.poseCallback, pose_qos + ) self.initial_pose_received = False self.initial_pose = initial_pose self.goal_pose = goal_pose - self.action_client = ActionClient(self, NavigateThroughPoses, 'navigate_through_poses') + self.action_client = ActionClient( + self, NavigateThroughPoses, 'navigate_through_poses' + ) def info_msg(self, msg: str): self.get_logger().info('\033[1;37;44m' + msg + '\033[0m') @@ -88,12 +87,16 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None): # Sends a `NavToPose` action request and waits for completion self.info_msg("Waiting for 'NavigateThroughPoses' action server") while not self.action_client.wait_for_server(timeout_sec=1.0): - self.info_msg("'NavigateThroughPoses' action server not available, waiting...") + self.info_msg( + "'NavigateThroughPoses' action server not available, waiting..." + ) self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose goal_msg = NavigateThroughPoses.Goal() - goal_msg.poses = [self.getStampedPoseMsg(self.goal_pose), - self.getStampedPoseMsg(self.goal_pose)] + goal_msg.poses = [ + self.getStampedPoseMsg(self.goal_pose), + self.getStampedPoseMsg(self.goal_pose), + ] self.info_msg('Sending goal request...') send_goal_future = self.action_client.send_goal_async(goal_msg) @@ -122,7 +125,9 @@ def runFakeNavigateAction(self): # Sends a `NavToPose` action request and waits for completion self.info_msg("Waiting for 'NavigateThroughPoses' action server") while not self.action_client.wait_for_server(timeout_sec=1.0): - self.info_msg("'NavigateThroughPoses' action server not available, waiting...") + self.info_msg( + "'NavigateThroughPoses' action server not available, waiting..." + ) goal_msg = NavigateThroughPoses.Goal() @@ -153,7 +158,9 @@ def runNavigatePreemptionAction(self, block): # Sends a `NavToPose` action request and waits for completion self.info_msg("Waiting for 'NavigateThroughPoses' action server") while not self.action_client.wait_for_server(timeout_sec=1.0): - self.info_msg("'NavigateThroughPoses' action server not available, waiting...") + self.info_msg( + "'NavigateThroughPoses' action server not available, waiting..." + ) goal_msg = NavigateThroughPoses.Goal() goal_msg.poses = [self.getStampedPoseMsg(self.initial_pose)] @@ -198,7 +205,7 @@ def wait_for_node_active(self, node_name: str): self.info_msg(f'{node_service} service not available, waiting...') req = GetState.Request() # empty request state = 'UNKNOWN' - while (state != 'active'): + while state != 'active': self.info_msg(f'Getting {node_name} state...') future = state_client.call_async(req) rclpy.spin_until_future_complete(self, future) @@ -206,7 +213,9 @@ def wait_for_node_active(self, node_name: str): state = future.result().current_state.label self.info_msg(f'Result of get_state: {state}') else: - self.error_msg(f'Exception while calling service: {future.exception()!r}') + self.error_msg( + f'Exception while calling service: {future.exception()!r}' + ) time.sleep(5) def shutdown(self): @@ -256,7 +265,7 @@ def wait_for_initial_pose(self): def run_all_tests(robot_tester): # set transforms to use_sim_time result = True - if (result): + if result: robot_tester.wait_for_node_active('amcl') robot_tester.wait_for_initial_pose() robot_tester.wait_for_node_active('bt_navigator') @@ -269,7 +278,7 @@ def run_all_tests(robot_tester): # Add more tests here if desired - if (result): + if result: robot_tester.info_msg('Test PASSED') else: robot_tester.error_msg('Test FAILED') @@ -295,10 +304,19 @@ def get_testers(args): init_x, init_y, final_x, final_y = args.robot[0] tester = NavTester( initial_pose=fwd_pose(float(init_x), float(init_y)), - goal_pose=fwd_pose(float(final_x), float(final_y))) + goal_pose=fwd_pose(float(final_x), float(final_y)), + ) tester.info_msg( - 'Starting tester, robot going from ' + init_x + ', ' + init_y + - ' to ' + final_x + ', ' + final_y + '.') + 'Starting tester, robot going from ' + + init_x + + ', ' + + init_y + + ' to ' + + final_x + + ', ' + + final_y + + '.' + ) testers.append(tester) return testers @@ -307,9 +325,14 @@ def main(argv=sys.argv[1:]): # The robot(s) positions from the input arguments parser = argparse.ArgumentParser(description='System-level navigation tester node') group = parser.add_mutually_exclusive_group(required=True) - group.add_argument('-r', '--robot', action='append', nargs=4, - metavar=('init_x', 'init_y', 'final_x', 'final_y'), - help='The robot starting and final positions.') + group.add_argument( + '-r', + '--robot', + action='append', + nargs=4, + metavar=('init_x', 'init_y', 'final_x', 'final_y'), + help='The robot starting and final positions.', + ) args, unknown = parser.parse_known_args() diff --git a/nav2_system_tests/src/system/nav_to_pose_tester_node.py b/nav2_system_tests/src/system/nav_to_pose_tester_node.py index 8fd1bc6034..cbb2ac20e6 100755 --- a/nav2_system_tests/src/system/nav_to_pose_tester_node.py +++ b/nav2_system_tests/src/system/nav_to_pose_tester_node.py @@ -38,7 +38,6 @@ class NavTester(Node): - def __init__(self, initial_pose: Pose, goal_pose: Pose, namespace: str = ''): super().__init__(node_name='nav2_tester', namespace=namespace) self.initial_pose_pub = self.create_publisher( diff --git a/nav2_system_tests/src/system/test_multi_robot_launch.py b/nav2_system_tests/src/system/test_multi_robot_launch.py index ea7e00db93..92f9a912df 100755 --- a/nav2_system_tests/src/system/test_multi_robot_launch.py +++ b/nav2_system_tests/src/system/test_multi_robot_launch.py @@ -20,8 +20,12 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription, LaunchService -from launch.actions import (ExecuteProcess, GroupAction, - IncludeLaunchDescription, SetEnvironmentVariable) +from launch.actions import ( + ExecuteProcess, + GroupAction, + IncludeLaunchDescription, + SetEnvironmentVariable, +) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import TextSubstitution from launch_ros.actions import Node, PushROSNamespace @@ -35,26 +39,39 @@ def generate_launch_description(): urdf = os.getenv('TEST_URDF') sdf = os.getenv('TEST_SDF') - bt_xml_file = os.path.join(get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', - os.getenv('BT_NAVIGATOR_XML')) + bt_xml_file = os.path.join( + get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML'), + ) bringup_dir = get_package_share_directory('nav2_bringup') - robot1_params_file = os.path.join(bringup_dir, # noqa: F841 - 'params/nav2_multirobot_params_1.yaml') - robot2_params_file = os.path.join(bringup_dir, # noqa: F841 - 'params/nav2_multirobot_params_2.yaml') + robot1_params_file = os.path.join( # noqa: F841 + bringup_dir, 'params/nav2_multirobot_params_1.yaml' + ) + robot2_params_file = os.path.join( # noqa: F841 + bringup_dir, 'params/nav2_multirobot_params_2.yaml' + ) # Names and poses of the robots robots = [ {'name': 'robot1', 'x_pose': 0.0, 'y_pose': 0.5, 'z_pose': 0.01}, - {'name': 'robot2', 'x_pose': 0.0, 'y_pose': -0.5, 'z_pose': 0.01}] + {'name': 'robot2', 'x_pose': 0.0, 'y_pose': -0.5, 'z_pose': 0.01}, + ] # Launch Gazebo server for simulation start_gazebo_cmd = ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_init.so', - '-s', 'libgazebo_ros_factory.so', '--minimal_comms', world], - output='screen') + cmd=[ + 'gzserver', + '-s', + 'libgazebo_ros_init.so', + '-s', + 'libgazebo_ros_factory.so', + '--minimal_comms', + world, + ], + output='screen', + ) # Define commands for spawing the robots into Gazebo spawn_robots_cmds = [] @@ -65,13 +82,21 @@ def generate_launch_description(): executable='spawn_entity.py', output='screen', arguments=[ - '-entity', TextSubstitution(text=robot['name']), - '-robot_namespace', TextSubstitution(text=robot['name']), - '-file', TextSubstitution(text=sdf), - '-x', TextSubstitution(text=str(robot['x_pose'])), - '-y', TextSubstitution(text=str(robot['y_pose'])), - '-z', TextSubstitution(text=str(robot['z_pose']))] - )) + '-entity', + TextSubstitution(text=robot['name']), + '-robot_namespace', + TextSubstitution(text=robot['name']), + '-file', + TextSubstitution(text=sdf), + '-x', + TextSubstitution(text=str(robot['x_pose'])), + '-y', + TextSubstitution(text=str(robot['y_pose'])), + '-z', + TextSubstitution(text=str(robot['z_pose'])), + ], + ) + ) with open(urdf, 'r') as infp: robot_description = infp.read() @@ -85,35 +110,48 @@ def generate_launch_description(): executable='robot_state_publisher', namespace=TextSubstitution(text=robot['name']), output='screen', - parameters=[{'use_sim_time': True, 'robot_description': robot_description}], - remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')])) + parameters=[ + {'use_sim_time': True, 'robot_description': robot_description} + ], + remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')], + ) + ) # Define commands for launching the navigation instances nav_instances_cmds = [] for robot in robots: params_file = eval(f"{robot['name']}_params_file") - group = GroupAction([ - # Instances use the robot's name for namespace - PushROSNamespace(robot['name']), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), - launch_arguments={ - 'namespace': robot['name'], - 'map': map_yaml_file, - 'use_sim_time': 'True', - 'params_file': params_file, - 'bt_xml_file': bt_xml_file, - 'autostart': 'True', - 'use_composition': 'False', - 'use_remappings': 'True'}.items()) - ]) + group = GroupAction( + [ + # Instances use the robot's name for namespace + PushROSNamespace(robot['name']), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py') + ), + launch_arguments={ + 'namespace': robot['name'], + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': params_file, + 'bt_xml_file': bt_xml_file, + 'autostart': 'True', + 'use_composition': 'False', + 'use_remappings': 'True', + }.items(), + ), + ] + ) nav_instances_cmds.append(group) ld = LaunchDescription() - ld.add_action(SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),) - ld.add_action(SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'),) + ld.add_action( + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + ) + ld.add_action( + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + ) ld.add_action(start_gazebo_cmd) for spawn_robot in spawn_robots_cmds: ld.add_action(spawn_robot) @@ -130,12 +168,26 @@ def main(argv=sys.argv[1:]): # TODO(orduno) remove duplicated definition of robots on `generate_launch_description` test1_action = ExecuteProcess( - cmd=[os.path.join(os.getenv('TEST_DIR'), os.getenv('TESTER')), - '-rs', 'robot1', '0.0', '0.5', '1.0', '0.5', - '-rs', 'robot2', '0.0', '-0.5', '1.0', '-0.5', - '-e', 'True'], + cmd=[ + os.path.join(os.getenv('TEST_DIR'), os.getenv('TESTER')), + '-rs', + 'robot1', + '0.0', + '0.5', + '1.0', + '0.5', + '-rs', + 'robot2', + '0.0', + '-0.5', + '1.0', + '-0.5', + '-e', + 'True', + ], name='tester_node', - output='screen') + output='screen', + ) lts = LaunchTestService() lts.add_test_action(ld, test1_action) diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py index 3678b242ac..5b0dd2625c 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/nav2_system_tests/src/system/test_system_launch.py @@ -22,7 +22,11 @@ from launch import LaunchDescription from launch import LaunchService -from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.actions import ( + ExecuteProcess, + IncludeLaunchDescription, + SetEnvironmentVariable, +) from launch.launch_context import LaunchContext from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node @@ -35,9 +39,11 @@ def generate_launch_description(): map_yaml_file = os.getenv('TEST_MAP') world = os.getenv('TEST_WORLD') - bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', - os.getenv('BT_NAVIGATOR_XML')) + bt_navigator_xml = os.path.join( + get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML'), + ) bringup_dir = get_package_share_directory('nav2_bringup') params_file = os.path.join(bringup_dir, 'params', 'nav2_params.yaml') @@ -47,69 +53,90 @@ def generate_launch_description(): context = LaunchContext() param_substitutions = {} - if (os.getenv('ASTAR') == 'True'): + if os.getenv('ASTAR') == 'True': param_substitutions.update({'use_astar': 'True'}) param_substitutions.update( - {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER')}) + {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER')} + ) param_substitutions.update( - {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER')}) + {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER')} + ) configured_params = RewrittenYaml( source_file=params_file, root_key='', param_rewrites=param_substitutions, - convert_types=True) + convert_types=True, + ) new_yaml = configured_params.perform(context) - return LaunchDescription([ - SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), - SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), - - # Launch gazebo server for simulation - ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_init.so', - '--minimal_comms', world], - output='screen'), - - # TODO(orduno) Launch the robot state publisher instead - # using a local copy of TB3 urdf file - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), - - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), - launch_arguments={'namespace': '', - 'use_namespace': 'False', - 'map': map_yaml_file, - 'use_sim_time': 'True', - 'params_file': new_yaml, - 'bt_xml_file': bt_navigator_xml, - 'use_composition': 'False', - 'autostart': 'True'}.items()), - ]) + return LaunchDescription( + [ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + # Launch gazebo server for simulation + ExecuteProcess( + cmd=[ + 'gzserver', + '-s', + 'libgazebo_ros_init.so', + '--minimal_comms', + world, + ], + output='screen', + ), + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], + ), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py') + ), + launch_arguments={ + 'namespace': '', + 'use_namespace': 'False', + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': new_yaml, + 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', + 'autostart': 'True', + }.items(), + ), + ] + ) def main(argv=sys.argv[1:]): ld = generate_launch_description() test1_action = ExecuteProcess( - cmd=[os.path.join(os.getenv('TEST_DIR'), os.getenv('TESTER')), - '-r', '-2.0', '-0.5', '0.0', '2.0', - '-e', 'True'], + cmd=[ + os.path.join(os.getenv('TEST_DIR'), os.getenv('TESTER')), + '-r', + '-2.0', + '-0.5', + '0.0', + '2.0', + '-e', + 'True', + ], name='tester_node', - output='screen') + output='screen', + ) lts = LaunchTestService() lts.add_test_action(ld, test1_action) diff --git a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py index 2c10898a22..d09cc54919 100755 --- a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py +++ b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py @@ -22,7 +22,11 @@ from launch import LaunchDescription from launch import LaunchService -from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.actions import ( + ExecuteProcess, + IncludeLaunchDescription, + SetEnvironmentVariable, +) from launch.launch_context import LaunchContext from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node @@ -35,9 +39,11 @@ def generate_launch_description(): map_yaml_file = os.getenv('TEST_MAP') world = os.getenv('TEST_WORLD') - bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', - os.getenv('BT_NAVIGATOR_XML')) + bt_navigator_xml = os.path.join( + get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML'), + ) bringup_dir = get_package_share_directory('nav2_bringup') params_file = os.path.join(bringup_dir, 'params', 'nav2_params.yaml') @@ -47,70 +53,91 @@ def generate_launch_description(): context = LaunchContext() param_substitutions = {} - if (os.getenv('ASTAR') == 'True'): + if os.getenv('ASTAR') == 'True': param_substitutions.update({'use_astar': 'True'}) param_substitutions.update( - {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER')}) + {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER')} + ) param_substitutions.update( - {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER')}) + {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER')} + ) configured_params = RewrittenYaml( source_file=params_file, root_key='', param_rewrites=param_substitutions, - convert_types=True) + convert_types=True, + ) new_yaml = configured_params.perform(context) - return LaunchDescription([ - SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), - SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), - - # Launch gazebo server for simulation - ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_init.so', - '--minimal_comms', world], - output='screen'), - - # TODO(orduno) Launch the robot state publisher instead - # using a local copy of TB3 urdf file - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), - - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), - launch_arguments={'namespace': '', - 'use_namespace': 'False', - 'map': map_yaml_file, - 'use_sim_time': 'True', - 'params_file': new_yaml, - 'bt_xml_file': bt_navigator_xml, - 'use_composition': 'False', - 'autostart': 'True', - 'use_composition': 'False'}.items()), - ]) + return LaunchDescription( + [ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + # Launch gazebo server for simulation + ExecuteProcess( + cmd=[ + 'gzserver', + '-s', + 'libgazebo_ros_init.so', + '--minimal_comms', + world, + ], + output='screen', + ), + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], + ), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py') + ), + launch_arguments={ + 'namespace': '', + 'use_namespace': 'False', + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': new_yaml, + 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', + 'autostart': 'True', + 'use_composition': 'False', + }.items(), + ), + ] + ) def main(argv=sys.argv[1:]): ld = generate_launch_description() test1_action = ExecuteProcess( - cmd=[os.path.join(os.getenv('TEST_DIR'), os.getenv('TESTER')), - '-r', '-200000.0', '-200000.0', '0.0', '2.0', - '-e', 'False'], + cmd=[ + os.path.join(os.getenv('TEST_DIR'), os.getenv('TESTER')), + '-r', + '-200000.0', + '-200000.0', + '0.0', + '2.0', + '-e', + 'False', + ], name='tester_node', - output='screen') + output='screen', + ) lts = LaunchTestService() lts.add_test_action(ld, test1_action) diff --git a/nav2_system_tests/src/system_failure/test_system_failure_launch.py b/nav2_system_tests/src/system_failure/test_system_failure_launch.py index 5bc8022766..01a7d50bc9 100755 --- a/nav2_system_tests/src/system_failure/test_system_failure_launch.py +++ b/nav2_system_tests/src/system_failure/test_system_failure_launch.py @@ -22,7 +22,11 @@ from launch import LaunchDescription from launch import LaunchService -from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.actions import ( + ExecuteProcess, + IncludeLaunchDescription, + SetEnvironmentVariable, +) from launch.launch_context import LaunchContext from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node @@ -35,70 +39,91 @@ def generate_launch_description(): map_yaml_file = os.getenv('TEST_MAP') world = os.getenv('TEST_WORLD') - bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', - os.getenv('BT_NAVIGATOR_XML')) + bt_navigator_xml = os.path.join( + get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML'), + ) bringup_dir = get_package_share_directory('nav2_bringup') params_file = os.path.join(bringup_dir, 'params', 'nav2_params.yaml') # Replace the `use_astar` setting on the params file param_substitutions = { - 'planner_server.ros__parameters.GridBased.use_astar': 'False'} + 'planner_server.ros__parameters.GridBased.use_astar': 'False' + } configured_params = RewrittenYaml( source_file=params_file, root_key='', param_rewrites=param_substitutions, - convert_types=True) + convert_types=True, + ) context = LaunchContext() new_yaml = configured_params.perform(context) - return LaunchDescription([ - SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), - SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), - - # Launch gazebo server for simulation - ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_init.so', - '--minimal_comms', world], - output='screen'), - - # TODO(orduno) Launch the robot state publisher instead - # using a local copy of TB3 urdf file - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), - - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), - launch_arguments={'namespace': '', - 'use_namespace': 'False', - 'map': map_yaml_file, - 'use_sim_time': 'True', - 'params_file': new_yaml, - 'bt_xml_file': bt_navigator_xml, - 'use_composition': 'False', - 'autostart': 'True'}.items()), - ]) + return LaunchDescription( + [ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + # Launch gazebo server for simulation + ExecuteProcess( + cmd=[ + 'gzserver', + '-s', + 'libgazebo_ros_init.so', + '--minimal_comms', + world, + ], + output='screen', + ), + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], + ), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py') + ), + launch_arguments={ + 'namespace': '', + 'use_namespace': 'False', + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': new_yaml, + 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', + 'autostart': 'True', + }.items(), + ), + ] + ) def main(argv=sys.argv[1:]): ld = generate_launch_description() test1_action = ExecuteProcess( - cmd=[os.path.join(os.getenv('TEST_DIR'), 'tester_node.py'), - '-r', '-2.0', '-0.5', '100.0', '100.0'], + cmd=[ + os.path.join(os.getenv('TEST_DIR'), 'tester_node.py'), + '-r', + '-2.0', + '-0.5', + '100.0', + '100.0', + ], name='tester_node', - output='screen') + output='screen', + ) lts = LaunchTestService() lts.add_test_action(ld, test1_action) diff --git a/nav2_system_tests/src/system_failure/tester_node.py b/nav2_system_tests/src/system_failure/tester_node.py index e4b3a9baeb..765535c6e5 100755 --- a/nav2_system_tests/src/system_failure/tester_node.py +++ b/nav2_system_tests/src/system_failure/tester_node.py @@ -37,26 +37,23 @@ class NavTester(Node): - - def __init__( - self, - initial_pose: Pose, - goal_pose: Pose, - namespace: str = '' - ): + def __init__(self, initial_pose: Pose, goal_pose: Pose, namespace: str = ''): super().__init__(node_name='nav2_tester', namespace=namespace) - self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped, - 'initialpose', 10) + self.initial_pose_pub = self.create_publisher( + PoseWithCovarianceStamped, 'initialpose', 10 + ) self.goal_pub = self.create_publisher(PoseStamped, 'goal_pose', 10) pose_qos = QoSProfile( - durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1) - - self.model_pose_sub = self.create_subscription(PoseWithCovarianceStamped, - 'amcl_pose', self.poseCallback, pose_qos) + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ) + + self.model_pose_sub = self.create_subscription( + PoseWithCovarianceStamped, 'amcl_pose', self.poseCallback, pose_qos + ) self.initial_pose_received = False self.initial_pose = initial_pose self.goal_pose = goal_pose @@ -158,7 +155,7 @@ def wait_for_node_active(self, node_name: str): self.info_msg(f'{node_service} service not available, waiting...') req = GetState.Request() # empty request state = 'UNKNOWN' - while (state != 'active'): + while state != 'active': self.info_msg(f'Getting {node_name} state...') future = state_client.call_async(req) rclpy.spin_until_future_complete(self, future) @@ -166,7 +163,9 @@ def wait_for_node_active(self, node_name: str): state = future.result().current_state.label self.info_msg(f'Result of get_state: {state}') else: - self.error_msg(f'Exception while calling service: {future.exception()!r}') + self.error_msg( + f'Exception while calling service: {future.exception()!r}' + ) time.sleep(5) def shutdown(self): @@ -216,7 +215,7 @@ def wait_for_initial_pose(self): def run_all_tests(robot_tester): # set transforms to use_sim_time result = True - if (result): + if result: robot_tester.wait_for_node_active('amcl') robot_tester.wait_for_initial_pose() robot_tester.wait_for_node_active('bt_navigator') @@ -224,7 +223,7 @@ def run_all_tests(robot_tester): # Add more tests here if desired - if (result): + if result: robot_tester.info_msg('Test PASSED') else: robot_tester.error_msg('Test FAILED') @@ -252,10 +251,19 @@ def get_testers(args): init_x, init_y, final_x, final_y = args.robot[0] tester = NavTester( initial_pose=fwd_pose(float(init_x), float(init_y)), - goal_pose=fwd_pose(float(final_x), float(final_y))) + goal_pose=fwd_pose(float(final_x), float(final_y)), + ) tester.info_msg( - 'Starting tester, robot going from ' + init_x + ', ' + init_y + - ' to ' + final_x + ', ' + final_y + '.') + 'Starting tester, robot going from ' + + init_x + + ', ' + + init_y + + ' to ' + + final_x + + ', ' + + final_y + + '.' + ) testers.append(tester) return testers @@ -266,13 +274,23 @@ def main(argv=sys.argv[1:]): # The robot(s) positions from the input arguments parser = argparse.ArgumentParser(description='System-level navigation tester node') group = parser.add_mutually_exclusive_group(required=True) - group.add_argument('-r', '--robot', action='append', nargs=4, - metavar=('init_x', 'init_y', 'final_x', 'final_y'), - help='The robot starting and final positions.') - group.add_argument('-rs', '--robots', action='append', nargs=5, - metavar=('name', 'init_x', 'init_y', 'final_x', 'final_y'), - help="The robot's namespace and starting and final positions. " + - 'Repeating the argument for multiple robots is supported.') + group.add_argument( + '-r', + '--robot', + action='append', + nargs=4, + metavar=('init_x', 'init_y', 'final_x', 'final_y'), + help='The robot starting and final positions.', + ) + group.add_argument( + '-rs', + '--robots', + action='append', + nargs=5, + metavar=('name', 'init_x', 'init_y', 'final_x', 'final_y'), + help="The robot's namespace and starting and final positions. " + + 'Repeating the argument for multiple robots is supported.', + ) args, unknown = parser.parse_known_args() diff --git a/nav2_system_tests/src/updown/test_updown_launch.py b/nav2_system_tests/src/updown/test_updown_launch.py index 28b20bd4c3..458779411b 100755 --- a/nav2_system_tests/src/updown/test_updown_launch.py +++ b/nav2_system_tests/src/updown/test_updown_launch.py @@ -25,56 +25,70 @@ def generate_launch_description(): # Configuration parameters for the launch - launch_dir = os.path.join( - get_package_share_directory('nav2_bringup'), 'launch') + launch_dir = os.path.join(get_package_share_directory('nav2_bringup'), 'launch') map_yaml_file = os.path.join( - get_package_share_directory('nav2_system_tests'), 'maps/map_circular.yaml') + get_package_share_directory('nav2_system_tests'), 'maps/map_circular.yaml' + ) # Specify the actions start_tf_cmd_1 = Node( package='tf2_ros', executable='static_transform_publisher', output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom']) + arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'], + ) start_tf_cmd_2 = Node( package='tf2_ros', executable='static_transform_publisher', output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'odom', 'base_footprint']) + arguments=['0', '0', '0', '0', '0', '0', 'odom', 'base_footprint'], + ) start_tf_cmd_3 = Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']) + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], + ) start_tf_cmd_4 = Node( package='tf2_ros', executable='static_transform_publisher', output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']) + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + ) nav2_bringup = launch.actions.IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(launch_dir, 'bringup_launch.py')), - launch_arguments={'map': map_yaml_file, - 'use_sim_time': 'True', - 'use_composition': 'False', - 'autostart': 'False'}.items()) + PythonLaunchDescriptionSource(os.path.join(launch_dir, 'bringup_launch.py')), + launch_arguments={ + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'use_composition': 'False', + 'autostart': 'False', + }.items(), + ) start_test = launch.actions.ExecuteProcess( cmd=[ os.path.join( get_package_prefix('nav2_system_tests'), - 'lib/nav2_system_tests/test_updown')], - cwd=[launch_dir], output='screen') + 'lib/nav2_system_tests/test_updown', + ) + ], + cwd=[launch_dir], + output='screen', + ) test_exit_event_handler = launch.actions.RegisterEventHandler( event_handler=launch.event_handlers.OnProcessExit( target_action=start_test, - on_exit=launch.actions.EmitEvent(event=launch.events.Shutdown(reason='Done!')))) + on_exit=launch.actions.EmitEvent( + event=launch.events.Shutdown(reason='Done!') + ), + ) + ) # Compose the launch description diff --git a/nav2_system_tests/src/updown/updownresults.py b/nav2_system_tests/src/updown/updownresults.py index 604c3a2532..6025094be1 100755 --- a/nav2_system_tests/src/updown/updownresults.py +++ b/nav2_system_tests/src/updown/updownresults.py @@ -63,7 +63,7 @@ def main(): shutdown_successful = True print('Number of tests: ', test_count) - print('Number of successes: ', test_count-fail_count) + print('Number of successes: ', test_count - fail_count) print('Number of successful bringups', successful_bringup_count) print('Number of successful shutdowns', successful_shutdown_count) diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index 30dfff574b..7eb4645706 100755 --- a/nav2_system_tests/src/waypoint_follower/tester.py +++ b/nav2_system_tests/src/waypoint_follower/tester.py @@ -31,27 +31,30 @@ class WaypointFollowerTest(Node): - def __init__(self): super().__init__(node_name='nav2_waypoint_tester', namespace='') self.waypoints = None self.action_client = ActionClient(self, FollowWaypoints, 'follow_waypoints') - self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped, - 'initialpose', 10) + self.initial_pose_pub = self.create_publisher( + PoseWithCovarianceStamped, 'initialpose', 10 + ) self.initial_pose_received = False self.goal_handle = None self.action_result = None pose_qos = QoSProfile( - durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1) - - self.model_pose_sub = self.create_subscription(PoseWithCovarianceStamped, - 'amcl_pose', self.poseCallback, pose_qos) - self.param_cli = self.create_client(SetParameters, - '/waypoint_follower/set_parameters') + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ) + + self.model_pose_sub = self.create_subscription( + PoseWithCovarianceStamped, 'amcl_pose', self.poseCallback, pose_qos + ) + self.param_cli = self.create_client( + SetParameters, '/waypoint_follower/set_parameters' + ) def setInitialPose(self, pose): self.init_pose = PoseWithCovarianceStamped() @@ -120,8 +123,10 @@ def run(self, block, cancel): self.info_msg(f'Goal failed with status code: {status}') return False if len(self.action_result.missed_waypoints) > 0: - self.info_msg('Goal failed to process all waypoints,' - ' missed {0} wps.'.format(len(self.action_result.missed_waypoints))) + self.info_msg( + 'Goal failed to process all waypoints,' + ' missed {0} wps.'.format(len(self.action_result.missed_waypoints)) + ) return False self.info_msg('Goal succeeded!') @@ -132,8 +137,9 @@ def publishInitialPose(self): def setStopFailureParam(self, value): req = SetParameters.Request() - req.parameters = [Parameter('stop_on_failure', - Parameter.Type.BOOL, value).to_parameter_msg()] + req.parameters = [ + Parameter('stop_on_failure', Parameter.Type.BOOL, value).to_parameter_msg() + ] future = self.param_cli.call_async(req) rclpy.spin_until_future_complete(self, future) @@ -230,8 +236,10 @@ def main(argv=sys.argv[1:]): result = test.run(True, False) assert not result result = not result - assert test.action_result.missed_waypoints[0].error_code == \ - ComputePathToPose.Result().GOAL_OUTSIDE_MAP + assert ( + test.action_result.missed_waypoints[0].error_code + == ComputePathToPose.Result().GOAL_OUTSIDE_MAP + ) # stop on failure test with bogous waypoint test.setStopFailureParam(True) diff --git a/tools/bt2img.py b/tools/bt2img.py index 31480f19be..b3c2b34eaf 100755 --- a/tools/bt2img.py +++ b/tools/bt2img.py @@ -18,64 +18,66 @@ import argparse import xml.etree.ElementTree + import graphviz # pip3 install graphviz control_nodes = [ - "Fallback", - "Parallel", - "ReactiveFallback", - "ReactiveSequence", - "Sequence", - "SequenceStar", - "BlackboardCheckInt", - "BlackboardCheckDouble", - "BlackboardCheckString", - "ForceFailure", - "ForceSuccess", - "Inverter", - "Repeat", - "Subtree", - "Timeout", - "RecoveryNode", - "PipelineSequence", - "RoundRobin", - "Control", - ] + 'Fallback', + 'Parallel', + 'ReactiveFallback', + 'ReactiveSequence', + 'Sequence', + 'SequenceStar', + 'BlackboardCheckInt', + 'BlackboardCheckDouble', + 'BlackboardCheckString', + 'ForceFailure', + 'ForceSuccess', + 'Inverter', + 'Repeat', + 'Subtree', + 'Timeout', + 'RecoveryNode', + 'PipelineSequence', + 'RoundRobin', + 'Control', +] action_nodes = [ - "AlwaysFailure", - "AlwaysSuccess", - "SetBlackboard", - "ComputePathToPose", - "FollowPath", - "BackUp", - "Spin", - "Wait", - "ClearEntireCostmap", - "ReinitializeGlobalLocalization", - "Action", - ] + 'AlwaysFailure', + 'AlwaysSuccess', + 'SetBlackboard', + 'ComputePathToPose', + 'FollowPath', + 'BackUp', + 'Spin', + 'Wait', + 'ClearEntireCostmap', + 'ReinitializeGlobalLocalization', + 'Action', +] condition_nodes = [ - "IsStuck", - "GoalReached", - "initialPoseReceived", - "GoalUpdated", - "DistanceTraveled", - "TimeExpired", - "TransformAvailable", - "Condition", - ] + 'IsStuck', + 'GoalReached', + 'initialPoseReceived', + 'GoalUpdated', + 'DistanceTraveled', + 'TimeExpired', + 'TransformAvailable', + 'Condition', +] decorator_nodes = [ - "Decorator", - "RateController", - "DistanceController", - "SpeedController", + 'Decorator', + 'RateController', + 'DistanceController', + 'SpeedController', ] subtree_nodes = [ - "SubTree", + 'SubTree', ] global xml_tree + def main(): global xml_tree args = parse_command_line() @@ -93,27 +95,43 @@ def main(): args.save_dot.write(dot.source) dot.render(args.image_out, view=args.display) + def parse_command_line(): - parser = argparse.ArgumentParser(description='Convert a behavior tree XML file to an image') - parser.add_argument('--behavior_tree', required=True, - help='the behavior tree XML file to convert to an image') - parser.add_argument('--image_out', required=True, - help='The name of the output image file. Leave off the .png extension') - parser.add_argument('--display', action="store_true", - help='If specified, opens the image in the default viewer') - parser.add_argument('--save_dot', type=argparse.FileType('w'), - help='Saves the intermediate dot source to the specified file') - parser.add_argument('--legend', - help='Generate a legend image as well') + parser = argparse.ArgumentParser( + description='Convert a behavior tree XML file to an image' + ) + parser.add_argument( + '--behavior_tree', + required=True, + help='the behavior tree XML file to convert to an image', + ) + parser.add_argument( + '--image_out', + required=True, + help='The name of the output image file. Leave off the .png extension', + ) + parser.add_argument( + '--display', + action='store_true', + help='If specified, opens the image in the default viewer', + ) + parser.add_argument( + '--save_dot', + type=argparse.FileType('w'), + help='Saves the intermediate dot source to the specified file', + ) + parser.add_argument('--legend', help='Generate a legend image as well') return parser.parse_args() + def find_root_tree_name(xml_tree): return xml_tree.getroot().get('main_tree_to_execute') + def find_behavior_tree(xml_tree, tree_name): trees = xml_tree.findall('BehaviorTree') if len(trees) == 0: - raise RuntimeError("No behavior trees were found in the XML file") + raise RuntimeError('No behavior trees were found in the XML file') for tree in trees: if tree_name == tree.get('ID'): @@ -121,6 +139,7 @@ def find_behavior_tree(xml_tree, tree_name): raise RuntimeError(f'No behavior tree for name {tree_name} found in the XML file') + # Generate a dot description of the root of the behavior tree. def convert2dot(behavior_tree): dot = graphviz.Digraph() @@ -130,65 +149,85 @@ def convert2dot(behavior_tree): convert_subtree(dot, root, parent_dot_name) return dot + # Recursive function. We add the children to the dot file, and then recursively # call this function on the children. Nodes are given an ID that is the hash # of the node to ensure each is unique. def convert_subtree(dot, parent_node, parent_dot_name): - if parent_node.tag == "SubTree": + if parent_node.tag == 'SubTree': add_sub_tree(dot, parent_dot_name, parent_node) else: add_nodes(dot, parent_dot_name, parent_node) + def add_sub_tree(dot, parent_dot_name, parent_node): root_tree_name = parent_node.get('ID') dot.node(parent_dot_name, root_tree_name, shape='box') behavior_tree = find_behavior_tree(xml_tree, root_tree_name) convert_subtree(dot, behavior_tree, parent_dot_name) + def add_nodes(dot, parent_dot_name, parent_node): for node in list(parent_node): label = make_label(node) - dot.node(str(hash(node)), label, color=node_color(node.tag), style='filled', shape='box') + dot.node( + str(hash(node)), + label, + color=node_color(node.tag), + style='filled', + shape='box', + ) dot_name = str(hash(node)) dot.edge(parent_dot_name, dot_name) convert_subtree(dot, node, dot_name) + # The node label contains the: # type, the name if provided, and the parameters. def make_label(node): - label = '< ' - label += '' + label = "<
' + node.tag + '
" + label += "" name = node.get('name') if name: - label += '' + label += "" for (param_name, value) in node.items(): - label += '' + label += ( + "" + ) label += '
' + node.tag + '
' + name + '
' + name + '
' + param_name + '=' + value + '
' + param_name + '=' + value + '
>' return label -def node_color(type): - if type in control_nodes: - return "chartreuse4" - if type in action_nodes: - return "cornflowerblue" - if type in condition_nodes: - return "yellow2" - if type in decorator_nodes: - return "darkorange1" - if type in subtree_nodes: - return "darkorchid1" - #else it's unknown - return "grey" + +def node_color(node_type): + if node_type in control_nodes: + return 'chartreuse4' + if node_type in action_nodes: + return 'cornflowerblue' + if node_type in condition_nodes: + return 'yellow2' + if node_type in decorator_nodes: + return 'darkorange1' + if node_type in subtree_nodes: + return 'darkorchid1' + # else it's unknown + return 'grey' + # creates a legend which can be provided with the other images. def make_legend(): legend = graphviz.Digraph(graph_attr={'rankdir': 'LR'}) legend.attr(label='Legend') - legend.node('Unknown', shape='box', style='filled', color="grey") - legend.node('Action', 'Action Node', shape='box', style='filled', color="cornflowerblue") - legend.node('Condition', 'Condition Node', shape='box', style='filled', color="yellow2") - legend.node('Control', 'Control Node', shape='box', style='filled', color="chartreuse4") + legend.node('Unknown', shape='box', style='filled', color='grey') + legend.node( + 'Action', 'Action Node', shape='box', style='filled', color='cornflowerblue' + ) + legend.node( + 'Condition', 'Condition Node', shape='box', style='filled', color='yellow2' + ) + legend.node( + 'Control', 'Control Node', shape='box', style='filled', color='chartreuse4' + ) return legend diff --git a/tools/planner_benchmarking/metrics.py b/tools/planner_benchmarking/metrics.py index 975416fcbd..0ae0f883ef 100644 --- a/tools/planner_benchmarking/metrics.py +++ b/tools/planner_benchmarking/metrics.py @@ -13,22 +13,17 @@ # See the License for the specific language governing permissions and # limitations under the License. -from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator -import rclpy -from ament_index_python.packages import get_package_share_directory - +import glob import math import os import pickle -import glob +from random import randint, seed, uniform import time -import numpy as np - -from random import seed -from random import randint -from random import uniform +from geometry_msgs.msg import PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator +import numpy as np +import rclpy from transforms3d.euler import euler2quat @@ -39,7 +34,7 @@ def getPlannerResults(navigator, initial_pose, goal_pose, planners): if path is not None and path.error_code == 0: results.append(path) else: - print(planner, "planner failed to produce the path") + print(planner, 'planner failed to produce the path') return results return results @@ -49,14 +44,14 @@ def getRandomStart(costmap, max_cost, side_buffer, time_stamp, res): start.header.frame_id = 'map' start.header.stamp = time_stamp while True: - row = randint(side_buffer, costmap.shape[0]-side_buffer) - col = randint(side_buffer, costmap.shape[1]-side_buffer) + row = randint(side_buffer, costmap.shape[0] - side_buffer) + col = randint(side_buffer, costmap.shape[1] - side_buffer) if costmap[row, col] < max_cost: - start.pose.position.x = col*res - start.pose.position.y = row*res + start.pose.position.x = col * res + start.pose.position.y = row * res - yaw = uniform(0, 1) * 2*math.pi + yaw = uniform(0, 1) * 2 * math.pi quad = euler2quat(0.0, 0.0, yaw) start.pose.orientation.w = quad[0] start.pose.orientation.x = quad[1] @@ -71,13 +66,13 @@ def getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res): goal.header.frame_id = 'map' goal.header.stamp = time_stamp while True: - row = randint(side_buffer, costmap.shape[0]-side_buffer) - col = randint(side_buffer, costmap.shape[1]-side_buffer) + row = randint(side_buffer, costmap.shape[0] - side_buffer) + col = randint(side_buffer, costmap.shape[1] - side_buffer) start_x = start.pose.position.x start_y = start.pose.position.y - goal_x = col*res - goal_y = row*res + goal_x = col * res + goal_y = row * res x_diff = goal_x - start_x y_diff = goal_y - start_y dist = math.sqrt(x_diff ** 2 + y_diff ** 2) @@ -86,7 +81,7 @@ def getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res): goal.pose.position.x = goal_x goal.pose.position.y = goal_y - yaw = uniform(0, 1) * 2*math.pi + yaw = uniform(0, 1) * 2 * math.pi quad = euler2quat(0.0, 0.0, yaw) goal.pose.orientation.w = quad[0] goal.pose.orientation.x = quad[1] @@ -111,7 +106,7 @@ def main(): costmap = np.asarray(costmap_msg.data) costmap.resize(costmap_msg.metadata.size_y, costmap_msg.metadata.size_x) - planners = ['Navfn', 'ThetaStar', 'SmacHybrid', 'Smac2d', 'SmacLattice'] + planners = ['Navfn', 'ThetaStar', 'SmacHybrid', 'Smac2d', 'SmacLattice'] max_cost = 210 side_buffer = 100 time_stamp = navigator.get_clock().now().to_msg() @@ -122,19 +117,19 @@ def main(): res = costmap_msg.metadata.resolution i = 0 while len(results) != random_pairs: - print("Cycle: ", i, "out of: ", random_pairs) + print('Cycle: ', i, 'out of: ', random_pairs) start = getRandomStart(costmap, max_cost, side_buffer, time_stamp, res) goal = getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res) - print("Start", start) - print("Goal", goal) + print('Start', start) + print('Goal', goal) result = getPlannerResults(navigator, start, goal, planners) if len(result) == len(planners): results.append(result) i = i + 1 else: - print("One of the planners was invalid") + print('One of the planners was invalid') - print("Write Results...") + print('Write Results...') with open(os.getcwd() + '/results.pickle', 'wb+') as f: pickle.dump(results, f, pickle.HIGHEST_PROTOCOL) @@ -143,7 +138,7 @@ def main(): with open(os.getcwd() + '/planners.pickle', 'wb+') as f: pickle.dump(planners, f, pickle.HIGHEST_PROTOCOL) - print("Write Complete") + print('Write Complete') exit(0) diff --git a/tools/planner_benchmarking/planning_benchmark_bringup.py b/tools/planner_benchmarking/planning_benchmark_bringup.py index d6589e8996..506ba54418 100644 --- a/tools/planner_benchmarking/planning_benchmark_bringup.py +++ b/tools/planner_benchmarking/planning_benchmark_bringup.py @@ -14,60 +14,69 @@ import os +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory + def generate_launch_description(): nav2_bringup_dir = get_package_share_directory('nav2_bringup') - config = os.path.join(get_package_share_directory('nav2_bringup'), 'params', 'nav2_params.yaml') + config = os.path.join( + get_package_share_directory('nav2_bringup'), 'params', 'nav2_params.yaml' + ) map_file = os.path.join(nav2_bringup_dir, 'maps', 'turtlebot3_world.yaml') lifecycle_nodes = ['map_server', 'planner_server'] - return LaunchDescription([ - Node( - package='nav2_map_server', - executable='map_server', - name='map_server', - output='screen', - parameters=[{'use_sim_time': True}, - {'yaml_filename': map_file}, - {'topic_name': "map"}]), - - Node( - package='nav2_planner', - executable='planner_server', - name='planner_server', - output='screen', - parameters=[config]), - - Node( - package = 'tf2_ros', - executable = 'static_transform_publisher', - output = 'screen', - arguments = ["0", "0", "0", "0", "0", "0", "base_link", "map"]), - - Node( - package = 'tf2_ros', - executable = 'static_transform_publisher', - output = 'screen', - arguments = ["0", "0", "0", "0", "0", "0", "base_link", "odom"]), - - Node( - package='nav2_lifecycle_manager', - executable='lifecycle_manager', - name='lifecycle_manager', - output='screen', - parameters=[{'use_sim_time': True}, - {'autostart': True}, - {'node_names': lifecycle_nodes}]), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), - launch_arguments={'namespace': '', - 'use_namespace': 'False'}.items()) - - ]) + return LaunchDescription( + [ + Node( + package='nav2_map_server', + executable='map_server', + name='map_server', + output='screen', + parameters=[ + {'use_sim_time': True}, + {'yaml_filename': map_file}, + {'topic_name': 'map'}, + ], + ), + Node( + package='nav2_planner', + executable='planner_server', + name='planner_server', + output='screen', + parameters=[config], + ), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'map'], + ), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'odom'], + ), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager', + output='screen', + parameters=[ + {'use_sim_time': True}, + {'autostart': True}, + {'node_names': lifecycle_nodes}, + ], + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py') + ), + launch_arguments={'namespace': '', 'use_namespace': 'False'}.items(), + ), + ] + ) diff --git a/tools/planner_benchmarking/process_data.py b/tools/planner_benchmarking/process_data.py index 50be77cc48..299997774d 100644 --- a/tools/planner_benchmarking/process_data.py +++ b/tools/planner_benchmarking/process_data.py @@ -13,15 +13,13 @@ # See the License for the specific language governing permissions and # limitations under the License. -import numpy as np import math - import os -from ament_index_python.packages import get_package_share_directory import pickle -import seaborn as sns import matplotlib.pylab as plt +import numpy as np +import seaborn as sns from tabulate import tabulate @@ -37,7 +35,7 @@ def getTimes(results): times = [] for result in results: for time in result: - times.append(time.planning_time.nanosec/1e09 + time.planning_time.sec) + times.append(time.planning_time.nanosec / 1e09 + time.planning_time.sec) return times @@ -47,8 +45,8 @@ def getMapCoordsFromPaths(paths, resolution): x = [] y = [] for pose in path.poses: - x.append(pose.pose.position.x/resolution) - y.append(pose.pose.position.y/resolution) + x.append(pose.pose.position.x / resolution) + y.append(pose.pose.position.y / resolution) coords.append(x) coords.append(y) return coords @@ -61,7 +59,9 @@ def getPathLength(path): for i in range(1, len(path.poses)): x_curr = path.poses[i].pose.position.x y_curr = path.poses[i].pose.position.y - path_length = path_length + math.sqrt((x_curr-x_prev)**2 + (y_curr-y_prev)**2) + path_length = path_length + math.sqrt( + (x_curr - x_prev) ** 2 + (y_curr - y_prev) ** 2 + ) x_prev = x_curr y_prev = y_curr return path_length @@ -76,7 +76,7 @@ def plotResults(costmap, paths): plt.figure(3) ax = sns.heatmap(data, cmap='Greys', cbar=False) for i in range(0, len(coords), 2): - ax.plot(coords[i], coords[i+1], linewidth=0.7) + ax.plot(coords[i], coords[i + 1], linewidth=0.7) plt.axis('off') ax.set_aspect('equal', 'box') plt.show() @@ -95,8 +95,8 @@ def averagePathCost(paths, costmap, num_of_planners): for i in range(0, len(coords), 2): costs = [] for j in range(len(coords[i])): - costs.append(data[math.floor(coords[i+1][j])][math.floor(coords[i][j])]) - average_path_costs[k % num_of_planners].append(sum(costs)/len(costs)) + costs.append(data[math.floor(coords[i + 1][j])][math.floor(coords[i][j])]) + average_path_costs[k % num_of_planners].append(sum(costs) / len(costs)) k += 1 return average_path_costs @@ -115,7 +115,7 @@ def maxPathCost(paths, costmap, num_of_planners): for i in range(0, len(coords), 2): max_cost = 0 for j in range(len(coords[i])): - cost = data[math.floor(coords[i+1][j])][math.floor(coords[i][j])] + cost = data[math.floor(coords[i + 1][j])][math.floor(coords[i][j])] if max_cost < cost: max_cost = cost max_path_costs[k % num_of_planners].append(max_cost) @@ -126,7 +126,7 @@ def maxPathCost(paths, costmap, num_of_planners): def main(): - print("Read data") + print('Read data') with open(os.getcwd() + '/results.pickle', 'rb') as f: results = pickle.load(f) @@ -144,12 +144,12 @@ def main(): path_lengths = np.asarray(path_lengths) total_paths = len(paths) - path_lengths.resize((int(total_paths/len(planners)), len(planners))) + path_lengths.resize((int(total_paths / len(planners)), len(planners))) path_lengths = path_lengths.transpose() times = getTimes(results) times = np.asarray(times) - times.resize((int(total_paths/len(planners)), len(planners))) + times.resize((int(total_paths / len(planners)), len(planners))) times = np.transpose(times) # Costs @@ -157,12 +157,26 @@ def main(): max_path_costs = np.asarray(maxPathCost(paths, costmap, len(planners))) # Generate table - planner_table = [['Planner', 'Average path length (m)', 'Average Time (s)', - 'Average cost', 'Max cost']] + planner_table = [ + [ + 'Planner', + 'Average path length (m)', + 'Average Time (s)', + 'Average cost', + 'Max cost', + ] + ] for i in range(0, len(planners)): - planner_table.append([planners[i], np.average(path_lengths[i]), np.average(times[i]), - np.average(average_path_costs[i]), np.average(max_path_costs[i])]) + planner_table.append( + [ + planners[i], + np.average(path_lengths[i]), + np.average(times[i]), + np.average(average_path_costs[i]), + np.average(max_path_costs[i]), + ] + ) # Visualize results print(tabulate(planner_table)) diff --git a/tools/smoother_benchmarking/metrics.py b/tools/smoother_benchmarking/metrics.py index fac2b19a95..fa01be9754 100644 --- a/tools/smoother_benchmarking/metrics.py +++ b/tools/smoother_benchmarking/metrics.py @@ -14,31 +14,29 @@ # See the License for the specific language governing permissions and # limitations under the License. -from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator -import rclpy - import math import os import pickle -import numpy as np - -from random import seed -from random import randint -from random import uniform +from random import randint, seed, uniform +from geometry_msgs.msg import PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator +import numpy as np +import rclpy from transforms3d.euler import euler2quat # Note: Map origin is assumed to be (0,0) + def getPlannerResults(navigator, initial_pose, goal_pose, planner): result = navigator._getPathImpl(initial_pose, goal_pose, planner, use_start=True) if result is None or result.error_code != 0: - print(planner, "planner failed to produce the path") + print(planner, 'planner failed to produce the path') return None return result + def getSmootherResults(navigator, path, smoothers): smoothed_results = [] for smoother in smoothers: @@ -46,23 +44,24 @@ def getSmootherResults(navigator, path, smoothers): if smoothed_result is not None: smoothed_results.append(smoothed_result) else: - print(smoother, "failed to smooth the path") + print(smoother, 'failed to smooth the path') return None return smoothed_results + def getRandomStart(costmap, max_cost, side_buffer, time_stamp, res): start = PoseStamped() start.header.frame_id = 'map' start.header.stamp = time_stamp while True: - row = randint(side_buffer, costmap.shape[0]-side_buffer) - col = randint(side_buffer, costmap.shape[1]-side_buffer) + row = randint(side_buffer, costmap.shape[0] - side_buffer) + col = randint(side_buffer, costmap.shape[1] - side_buffer) if costmap[row, col] < max_cost: - start.pose.position.x = col*res - start.pose.position.y = row*res + start.pose.position.x = col * res + start.pose.position.y = row * res - yaw = uniform(0, 1) * 2*math.pi + yaw = uniform(0, 1) * 2 * math.pi quad = euler2quat(0.0, 0.0, yaw) start.pose.orientation.w = quad[0] start.pose.orientation.x = quad[1] @@ -71,18 +70,19 @@ def getRandomStart(costmap, max_cost, side_buffer, time_stamp, res): break return start + def getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res): goal = PoseStamped() goal.header.frame_id = 'map' goal.header.stamp = time_stamp while True: - row = randint(side_buffer, costmap.shape[0]-side_buffer) - col = randint(side_buffer, costmap.shape[1]-side_buffer) + row = randint(side_buffer, costmap.shape[0] - side_buffer) + col = randint(side_buffer, costmap.shape[1] - side_buffer) start_x = start.pose.position.x start_y = start.pose.position.y - goal_x = col*res - goal_y = row*res + goal_x = col * res + goal_y = row * res x_diff = goal_x - start_x y_diff = goal_y - start_y dist = math.sqrt(x_diff ** 2 + y_diff ** 2) @@ -91,7 +91,7 @@ def getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res): goal.pose.position.x = goal_x goal.pose.position.y = goal_y - yaw = uniform(0, 1) * 2*math.pi + yaw = uniform(0, 1) * 2 * math.pi quad = euler2quat(0.0, 0.0, yaw) goal.pose.orientation.w = quad[0] goal.pose.orientation.x = quad[1] @@ -100,13 +100,14 @@ def getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res): break return goal + def main(): rclpy.init() navigator = BasicNavigator() # Wait for planner and smoother to fully activate - print("Waiting for planner and smoother servers to activate") + print('Waiting for planner and smoother servers to activate') navigator.waitUntilNav2Active('smoother_server', 'planner_server') # Get the costmap for start/goal validation @@ -126,20 +127,20 @@ def main(): i = 0 res = costmap_msg.metadata.resolution while i < random_pairs: - print("Cycle: ", i, "out of: ", random_pairs) + print('Cycle: ', i, 'out of: ', random_pairs) start = getRandomStart(costmap, max_cost, side_buffer, time_stamp, res) goal = getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res) - print("Start", start) - print("Goal", goal) + print('Start', start) + print('Goal', goal) result = getPlannerResults(navigator, start, goal, planner) if result is not None: smoothed_results = getSmootherResults(navigator, result.path, smoothers) if smoothed_results is not None: - results.append(result) - results.append(smoothed_results) - i += 1 + results.append(result) + results.append(smoothed_results) + i += 1 - print("Write Results...") + print('Write Results...') benchmark_dir = os.getcwd() with open(os.path.join(benchmark_dir, 'results.pickle'), 'wb') as f: pickle.dump(results, f, pickle.HIGHEST_PROTOCOL) @@ -150,7 +151,7 @@ def main(): smoothers.insert(0, planner) with open(os.path.join(benchmark_dir, 'methods.pickle'), 'wb') as f: pickle.dump(smoothers, f, pickle.HIGHEST_PROTOCOL) - print("Write Complete") + print('Write Complete') exit(0) diff --git a/tools/smoother_benchmarking/process_data.py b/tools/smoother_benchmarking/process_data.py index 43392833bd..4a422bc003 100644 --- a/tools/smoother_benchmarking/process_data.py +++ b/tools/smoother_benchmarking/process_data.py @@ -15,14 +15,13 @@ # See the License for the specific language governing permissions and # limitations under the License. -import numpy as np import math - import os import pickle -import seaborn as sns import matplotlib.pylab as plt +import numpy as np +import seaborn as sns from tabulate import tabulate @@ -44,11 +43,16 @@ def getTimes(results): for i in range(len(results)): if (i % 2) == 0: # Append non-smoothed time - times.append(results[i].planning_time.nanosec/1e09 + results[i].planning_time.sec) + times.append( + results[i].planning_time.nanosec / 1e09 + results[i].planning_time.sec + ) else: # Append smoothed times array for result in results[i]: - times.append(result.smoothing_duration.nanosec/1e09 + result.smoothing_duration.sec) + times.append( + result.smoothing_duration.nanosec / 1e09 + + result.smoothing_duration.sec + ) return times @@ -58,8 +62,8 @@ def getMapCoordsFromPaths(paths, resolution): x = [] y = [] for pose in path.poses: - x.append(pose.pose.position.x/resolution) - y.append(pose.pose.position.y/resolution) + x.append(pose.pose.position.x / resolution) + y.append(pose.pose.position.y / resolution) coords.append(x) coords.append(y) return coords @@ -72,11 +76,14 @@ def getPathLength(path): for i in range(1, len(path.poses)): x_curr = path.poses[i].pose.position.x y_curr = path.poses[i].pose.position.y - path_length = path_length + math.sqrt((x_curr-x_prev)**2 + (y_curr-y_prev)**2) + path_length = path_length + math.sqrt( + (x_curr - x_prev) ** 2 + (y_curr - y_prev) ** 2 + ) x_prev = x_curr y_prev = y_curr return path_length + # Path smoothness calculations def getSmoothness(pt_prev, pt, pt_next): d1 = pt - pt_prev @@ -84,6 +91,7 @@ def getSmoothness(pt_prev, pt, pt_next): delta = d2 - d1 return np.linalg.norm(delta) + def getPathSmoothnesses(paths): smoothnesses = [] pm0 = np.array([0.0, 0.0]) @@ -94,14 +102,15 @@ def getPathSmoothnesses(paths): for i in range(2, len(path.poses)): pm0[0] = path.poses[i].pose.position.x pm0[1] = path.poses[i].pose.position.y - pm1[0] = path.poses[i-1].pose.position.x - pm1[1] = path.poses[i-1].pose.position.y - pm2[0] = path.poses[i-2].pose.position.x - pm2[1] = path.poses[i-2].pose.position.y + pm1[0] = path.poses[i - 1].pose.position.x + pm1[1] = path.poses[i - 1].pose.position.y + pm2[0] = path.poses[i - 2].pose.position.x + pm2[1] = path.poses[i - 2].pose.position.y smoothness += getSmoothness(pm2, pm1, pm0) smoothnesses.append(smoothness) return smoothnesses + # Curvature calculations def arcCenter(pt_prev, pt, pt_next): cusp_thresh = -0.7 @@ -132,9 +141,12 @@ def arcCenter(pt_prev, pt, pt_next): n2 = (-d2[1], d2[0]) det1 = (mid1[0] + n1[0]) * mid1[1] - (mid1[1] + n1[1]) * mid1[0] det2 = (mid2[0] + n2[0]) * mid2[1] - (mid2[1] + n2[1]) * mid2[0] - center = np.array([(det1 * n2[0] - det2 * n1[0]) / det, (det1 * n2[1] - det2 * n1[1]) / det]) + center = np.array( + [(det1 * n2[0] - det2 * n1[0]) / det, (det1 * n2[1] - det2 * n1[1]) / det] + ) return center + def getPathCurvatures(paths): curvatures = [] pm0 = np.array([0.0, 0.0]) @@ -145,17 +157,18 @@ def getPathCurvatures(paths): for i in range(2, len(path.poses)): pm0[0] = path.poses[i].pose.position.x pm0[1] = path.poses[i].pose.position.y - pm1[0] = path.poses[i-1].pose.position.x - pm1[1] = path.poses[i-1].pose.position.y - pm2[0] = path.poses[i-2].pose.position.x - pm2[1] = path.poses[i-2].pose.position.y + pm1[0] = path.poses[i - 1].pose.position.x + pm1[1] = path.poses[i - 1].pose.position.y + pm2[0] = path.poses[i - 2].pose.position.x + pm2[1] = path.poses[i - 2].pose.position.y center = arcCenter(pm2, pm1, pm0) if center[0] != float('inf'): - turning_rad = np.linalg.norm(pm1 - center); - radiuses.append(turning_rad) + turning_rad = np.linalg.norm(pm1 - center) + radiuses.append(turning_rad) curvatures.append(np.average(radiuses)) return curvatures + def plotResults(costmap, paths): coords = getMapCoordsFromPaths(paths, costmap.metadata.resolution) data = np.asarray(costmap.data) @@ -165,7 +178,7 @@ def plotResults(costmap, paths): plt.figure(3) ax = sns.heatmap(data, cmap='Greys', cbar=False) for i in range(0, len(coords), 2): - ax.plot(coords[i], coords[i+1], linewidth=0.7) + ax.plot(coords[i], coords[i + 1], linewidth=0.7) plt.axis('off') ax.set_aspect('equal', 'box') plt.show() @@ -184,8 +197,8 @@ def averagePathCost(paths, costmap, num_of_planners): for i in range(0, len(coords), 2): costs = [] for j in range(len(coords[i])): - costs.append(data[math.floor(coords[i+1][j])][math.floor(coords[i][j])]) - average_path_costs[k % num_of_planners].append(sum(costs)/len(costs)) + costs.append(data[math.floor(coords[i + 1][j])][math.floor(coords[i][j])]) + average_path_costs[k % num_of_planners].append(sum(costs) / len(costs)) k += 1 return average_path_costs @@ -204,7 +217,7 @@ def maxPathCost(paths, costmap, num_of_planners): for i in range(0, len(coords), 2): max_cost = 0 for j in range(len(coords[i])): - cost = data[math.floor(coords[i+1][j])][math.floor(coords[i][j])] + cost = data[math.floor(coords[i + 1][j])][math.floor(coords[i][j])] if max_cost < cost: max_cost = cost max_path_costs[k % num_of_planners].append(max_cost) @@ -216,7 +229,7 @@ def maxPathCost(paths, costmap, num_of_planners): def main(): # Read the data benchmark_dir = os.getcwd() - print("Read data") + print('Read data') with open(os.path.join(benchmark_dir, 'results.pickle'), 'rb') as f: results = pickle.load(f) @@ -239,14 +252,14 @@ def main(): total_paths = len(paths) # [planner, smoothers] path lenghth in a row - path_lengths.resize((int(total_paths/methods_num), methods_num)) + path_lengths.resize((int(total_paths / methods_num), methods_num)) # [planner, smoothers] path length in a column path_lengths = path_lengths.transpose() # Times times = getTimes(results) times = np.asarray(times) - times.resize((int(total_paths/methods_num), methods_num)) + times.resize((int(total_paths / methods_num), methods_num)) times = np.transpose(times) # Costs @@ -256,40 +269,52 @@ def main(): # Smoothness smoothnesses = getPathSmoothnesses(paths) smoothnesses = np.asarray(smoothnesses) - smoothnesses.resize((int(total_paths/methods_num), methods_num)) + smoothnesses.resize((int(total_paths / methods_num), methods_num)) smoothnesses = np.transpose(smoothnesses) # Curvatures curvatures = getPathCurvatures(paths) curvatures = np.asarray(curvatures) - curvatures.resize((int(total_paths/methods_num), methods_num)) + curvatures.resize((int(total_paths / methods_num), methods_num)) curvatures = np.transpose(curvatures) # Generate table - planner_table = [['Planner', - 'Time (s)', - 'Path length (m)', - 'Average cost', - 'Max cost', - 'Path smoothness (x100)', - 'Average turning rad (m)']] + planner_table = [ + [ + 'Planner', + 'Time (s)', + 'Path length (m)', + 'Average cost', + 'Max cost', + 'Path smoothness (x100)', + 'Average turning rad (m)', + ] + ] # for path planner - planner_table.append([planner, - np.average(times[0]), - np.average(path_lengths[0]), - np.average(average_path_costs[0]), - np.average(max_path_costs[0]), - np.average(smoothnesses[0]) * 100, - np.average(curvatures[0])]) + planner_table.append( + [ + planner, + np.average(times[0]), + np.average(path_lengths[0]), + np.average(average_path_costs[0]), + np.average(max_path_costs[0]), + np.average(smoothnesses[0]) * 100, + np.average(curvatures[0]), + ] + ) # for path smoothers for i in range(1, methods_num): - planner_table.append([smoothers[i-1], - np.average(times[i]), - np.average(path_lengths[i]), - np.average(average_path_costs[i]), - np.average(max_path_costs[i]), - np.average(smoothnesses[i]) * 100, - np.average(curvatures[i])]) + planner_table.append( + [ + smoothers[i - 1], + np.average(times[i]), + np.average(path_lengths[i]), + np.average(average_path_costs[i]), + np.average(max_path_costs[i]), + np.average(smoothnesses[i]) * 100, + np.average(curvatures[i]), + ] + ) # Visualize results print(tabulate(planner_table)) diff --git a/tools/smoother_benchmarking/smoother_benchmark_bringup.py b/tools/smoother_benchmarking/smoother_benchmark_bringup.py index 9b0ed744de..4d60e17e64 100644 --- a/tools/smoother_benchmarking/smoother_benchmark_bringup.py +++ b/tools/smoother_benchmarking/smoother_benchmark_bringup.py @@ -14,73 +14,87 @@ import os +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import ExecuteProcess, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory + def generate_launch_description(): nav2_bringup_dir = get_package_share_directory('nav2_bringup') benchmark_dir = os.getcwd() metrics_py = os.path.join(benchmark_dir, 'metrics.py') - config = os.path.join(get_package_share_directory('nav2_bringup'), 'params', 'nav2_params.yaml') + config = os.path.join( + get_package_share_directory('nav2_bringup'), 'params', 'nav2_params.yaml' + ) map_file = os.path.join(benchmark_dir, 'maps', 'smoothers_world.yaml') lifecycle_nodes = ['map_server', 'planner_server', 'smoother_server'] static_transform_one = Node( - package = 'tf2_ros', - executable = 'static_transform_publisher', - output = 'screen', - arguments = ["0", "0", "0", "0", "0", "0", "base_link", "map"]) + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'map'], + ) static_transform_two = Node( - package = 'tf2_ros', - executable = 'static_transform_publisher', - output = 'screen', - arguments = ["0", "0", "0", "0", "0", "0", "base_link", "odom"]) + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'odom'], + ) start_map_server_cmd = Node( package='nav2_map_server', executable='map_server', name='map_server', output='screen', - parameters=[{'use_sim_time': True}, - {'yaml_filename': map_file}, - {'topic_name': "map"}]) + parameters=[ + {'use_sim_time': True}, + {'yaml_filename': map_file}, + {'topic_name': 'map'}, + ], + ) start_planner_server_cmd = Node( package='nav2_planner', executable='planner_server', name='planner_server', output='screen', - parameters=[config]) + parameters=[config], + ) start_smoother_server_cmd = Node( package='nav2_smoother', executable='smoother_server', name='smoother_server', output='screen', - parameters=[config]) + parameters=[config], + ) start_lifecycle_manager_cmd = Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager', output='screen', - parameters=[{'use_sim_time': True}, - {'autostart': True}, - {'node_names': lifecycle_nodes}]) + parameters=[ + {'use_sim_time': True}, + {'autostart': True}, + {'node_names': lifecycle_nodes}, + ], + ) rviz_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), - launch_arguments={'namespace': '', - 'use_namespace': 'False'}.items()) + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py') + ), + launch_arguments={'namespace': '', 'use_namespace': 'False'}.items(), + ) metrics_cmd = ExecuteProcess( - cmd=['python3', '-u', metrics_py], - cwd=[benchmark_dir], output='screen') + cmd=['python3', '-u', metrics_py], cwd=[benchmark_dir], output='screen' + ) ld = LaunchDescription() ld.add_action(static_transform_one)