diff --git a/src/hangar_sim/CMakeLists.txt b/src/hangar_sim/CMakeLists.txt index 694e4a4dc..cac8f97ce 100644 --- a/src/hangar_sim/CMakeLists.txt +++ b/src/hangar_sim/CMakeLists.txt @@ -35,6 +35,9 @@ if(BUILD_TESTING) find_package(ament_cmake_pytest REQUIRED) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() + ament_add_pytest_test( + nav2_params_distro_parity_test test/nav2_params_distro_parity_test.py + TIMEOUT 60) # Redirect ROS node logs into the test_results tree so the test-results CI # artifact ships them back too. Default would be ~/.ros/log/, which lives # on the doomed container filesystem and never gets uploaded -- making diff --git a/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py b/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py index cb8725353..f46829a3d 100644 --- a/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py +++ b/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py @@ -192,9 +192,39 @@ def generate_launch_description(): description="Use simulation clock if true", ) + # Nav2's pluginlib lookup names differ by distro: Humble exports `pkg/Class`, + # Jazzy exports `pkg::Class`, and the two are mutually exclusive. Jazzy's + # bt_navigator also loads its default BT plugin libraries implicitly, so + # listing them again double-registers every node. Each distro therefore gets + # its own parameter file. This is the only place the choice is made: every + # nav2 consumer below -- the slam, localization and navigation includes, and + # the nav2_container node -- takes it from the `params_file` argument. + # Anything that is not Jazzy gets the Humble file, which is correct for the + # two distros the 9.4 line publishes; a future distro must be added here + # explicitly rather than inheriting the Humble form by default. + ros_distro = os.environ.get("ROS_DISTRO", "") + nav2_params_file = ( + "nav2_params_jazzy.yaml" if ros_distro == "jazzy" else "nav2_params.yaml" + ) + # The wrong branch reproduces the bug this selection exists to avoid, and the + # only symptom is nav2_container aborting, so name the choice in the log. + nav2_params_log = LogInfo( + msg=( + [f"nav2 params (ROS_DISTRO={ros_distro}): ", params_file] + if ros_distro in ("humble", "jazzy") + else [ + f"WARNING: ROS_DISTRO={ros_distro or 'unset'} is not a distro " + f"hangar_sim ships nav2 parameters for; defaulting to " + f"{nav2_params_file}, which is only correct for Humble. " + "nav2 params: ", + params_file, + ] + ) + ) + declare_params_file_cmd = DeclareLaunchArgument( "params_file", - default_value=os.path.join(config_dir, "params", "nav2_params.yaml"), + default_value=os.path.join(config_dir, "params", nav2_params_file), description="Full path to the ROS2 parameters file to use for all launched nodes", ) @@ -414,6 +444,7 @@ def generate_launch_description(): ld.add_action(declare_map_yaml_cmd) ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_params_file_cmd) + ld.add_action(nav2_params_log) ld.add_action(declare_autostart_cmd) ld.add_action(declare_use_composition_cmd) ld.add_action(declare_use_respawn_cmd) diff --git a/src/hangar_sim/objectives/navigate_to_clicked_point_with_replanning.xml b/src/hangar_sim/objectives/navigate_to_clicked_point_with_replanning.xml index bc2a45169..05a0632ca 100644 --- a/src/hangar_sim/objectives/navigate_to_clicked_point_with_replanning.xml +++ b/src/hangar_sim/objectives/navigate_to_clicked_point_with_replanning.xml @@ -27,10 +27,15 @@ user_pose="{goal}" view_name="Visualization" /> + moveit_pro_sam2 ament_lint_auto + ament_cmake_pytest + python3-pytest + python3-yaml ament_clang_format ament_clang_tidy diff --git a/src/hangar_sim/params/nav2_params_jazzy.yaml b/src/hangar_sim/params/nav2_params_jazzy.yaml new file mode 100644 index 000000000..0bc072e5e --- /dev/null +++ b/src/hangar_sim/params/nav2_params_jazzy.yaml @@ -0,0 +1,483 @@ +# Nav2 parameters for hangar_sim on ROS 2 Jazzy. +# +# Jazzy-specific counterpart to nav2_params.yaml, which serves Humble. The two +# files must stay in sync; nav2_params_distro_parity_test.py enforces that only +# the differences below may exist between them. +# +# 1. pluginlib lookup names use the `pkg::Class` form. Jazzy's nav2 exports +# these plugins with no `name` attribute, so the lookup name is the type; +# Humble's exports declare `name="pkg/Class"`. The two forms are mutually +# exclusive, which is why this file exists rather than a shared one. +# 2. `bt_navigator` carries no `plugin_lib_names` list -- see the comment on +# that parameter below. +# +# robot_drivers_to_persist_sim.launch.py selects between the two by $ROS_DISTRO. +# amcl is not used because odom is received directly from MuJoCo +# amcl: +# ros__parameters: +# use_sim_time: True +# alpha1: 0.2 +# alpha2: 0.2 +# alpha3: 0.2 +# alpha4: 0.2 +# alpha5: 0.2 +# base_frame_id: "base_footprint" +# beam_skip_distance: 0.5 +# beam_skip_error_threshold: 0.9 +# beam_skip_threshold: 0.3 +# do_beamskip: false +# global_frame_id: "map" +# lambda_short: 0.1 +# laser_likelihood_max_dist: 2.0 +# laser_max_range: 100.0 +# laser_min_range: -1.0 +# laser_model_type: "likelihood_field" +# max_beams: 60 +# max_particles: 2000 +# min_particles: 500 +# odom_frame_id: "odom" +# pf_err: 0.05 +# pf_z: 0.99 +# recovery_alpha_fast: 0.0 +# recovery_alpha_slow: 0.0 +# resample_interval: 1 +# robot_model_type: "nav2_amcl::DifferentialMotionModel" +# save_pose_rate: 0.5 +# sigma_hit: 0.2 +# tf_broadcast: true +# transform_tolerance: 1.0 +# update_min_a: 0.2 +# update_min_d: 0.25 +# z_hit: 0.5 +# z_max: 0.05 +# z_rand: 0.5 +# z_short: 0.05 +# scan_topic: scan + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: ridgeback_base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + # + # Do NOT list the stock nav2 BT plugin libraries in `plugin_lib_names` here. Since + # Jazzy, bt_navigator always loads the default nav2 set implicitly and the parameter + # is additive, so re-listing them double-registers every node and configure fails + # with "ID [ComputePathToPose] already registered". Only custom user libraries + # belong in `plugin_lib_names`. + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + # Jazzy takes a list here; the singular Humble key is undeclared and would be + # silently ignored, leaving nav2 on its own default. + progress_checker_plugins: ["progress_checker"] + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.1 + movement_time_allowance: 20.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.4 + yaw_goal_tolerance: 0.25 + FollowPath: + plugin: "nav2_mppi_controller::MPPIController" + # Integrate from MPPI's last command instead of lagging odometry feedback; + # this requires Nav2's Jazzy backport of ros-navigation/navigation2#5617. + open_loop: true + time_steps: 56 + model_dt: 0.05 + batch_size: 500 + vx_std: 0.2 + vy_std: 0.2 + wz_std: 0.4 + vx_max: 0.5 + vx_min: -0.35 + vy_max: 0.5 + wz_max: 1.9 + iteration_count: 1 + prune_distance: 1.7 + transform_tolerance: 0.1 + temperature: 0.3 + gamma: 0.015 + motion_model: "DiffDrive" + visualize: false + TrajectoryVisualizer: + trajectory_step: 5 + time_step: 3 + AckermannConstraints: + min_turning_r: 0.2 + critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"] + ConstraintCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 + GoalCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 1.4 + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + threshold_to_consider: 0.5 + PreferForwardCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 0.5 + # ObstaclesCritic: + # enabled: true + # cost_power: 1 + # repulsion_weight: 1.5 + # critical_weight: 20.0 + # consider_footprint: false + # collision_cost: 10000.0 + # collision_margin_distance: 0.1 + # near_goal_distance: 0.5 + CostCritic: + enabled: true + cost_power: 1 + cost_weight: 3.81 + critical_cost: 300.0 + consider_footprint: true + collision_cost: 1000000.0 + near_goal_distance: 1.0 + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 14.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 3 + threshold_to_consider: 0.5 + offset_from_furthest: 20 + use_path_orientations: false + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + offset_from_furthest: 5 + threshold_to_consider: 1.4 + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + offset_from_furthest: 4 + threshold_to_consider: 0.5 + max_angle_to_furthest: 1.0 + forward_preference: true + # VelocityDeadbandCritic: + # enabled: true + # cost_power: 1 + # cost_weight: 35.0 + # deadband_velocities: [0.05, 0.05, 0.05] + # TwirlingCritic: + # enabled: true + # twirling_cost_power: 1 + # twirling_cost_weight: 10.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 10.0 + publish_frequency: 5.0 + global_frame: odom + robot_base_frame: ridgeback_base_link + use_sim_time: True + rolling_window: true + width: 5 + height: 5 + resolution: 0.05 + footprint: "[[0.45, 0.375], [0.45, -0.375], [-0.45, -0.375], [-0.45, 0.375]]" + footprint_padding: 0.025 + plugins: ["obstacle_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.75 + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan_front scan_rear + scan_front: + topic: /scan_front_filtered + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 4.5 + raytrace_min_range: 0.0 + obstacle_max_range: 3.5 + obstacle_min_range: 0.0 + transform_tolerance: 0.3 + inf_is_valid: false + qos_overriding_options: + policy_kinds: [reliability] + reliability: best_effort + scan_rear: + topic: /scan_rear_filtered + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 4.5 + raytrace_min_range: 0.0 + obstacle_max_range: 3.5 + obstacle_min_range: 0.0 + transform_tolerance: 0.3 + inf_is_valid: false + qos_overriding_options: + policy_kinds: [reliability] + reliability: best_effort + always_send_full_costmap: True + transform_tolerance: 0.3 + + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: ridgeback_base_link + use_sim_time: True + footprint: "[[0.45, 0.375], [0.45, -0.375], [-0.45, -0.375], [-0.45, 0.375]]" + footprint_padding: 0.025 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan_front scan_rear + scan_front: + topic: /scan_front_filtered + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 10.0 + raytrace_min_range: 0.0 + obstacle_max_range: 8.0 + obstacle_min_range: 0.0 + transform_tolerance: 0.3 + inf_is_valid: false + qos_overriding_options: + policy_kinds: [reliability] + reliability: best_effort + scan_rear: + topic: /scan_rear_filtered + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 10.0 + raytrace_min_range: 0.0 + obstacle_max_range: 8.0 + obstacle_min_range: 0.0 + transform_tolerance: 0.3 + inf_is_valid: false + qos_overriding_options: + policy_kinds: [reliability] + reliability: best_effort + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 4.0 + inflation_radius: 0.7 + always_send_full_costmap: True + transform_tolerance: 0.3 + +map_server: + ros__parameters: + use_sim_time: True + # Overridden in launch by the "map" launch configuration or provided default value. + # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. + yaml_filename: "" + +map_saver: + ros__parameters: + use_sim_time: True + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner::NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + # Jazzy split these into local_/global_ pairs; the unprefixed Humble keys are + # undeclared here. This topic feeds the collision check guarding Spin/BackUp. + local_costmap_topic: local_costmap/costmap_raw + local_footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors::Spin" + backup: + plugin: "nav2_behaviors::BackUp" + drive_on_heading: + plugin: "nav2_behaviors::DriveOnHeading" + wait: + plugin: "nav2_behaviors::Wait" + assisted_teleop: + plugin: "nav2_behaviors::AssistedTeleop" + global_frame: odom + robot_base_frame: ridgeback_base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 3.6 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [1.0, 1.0, 1.0] + min_velocity: [-1.0, -1.0, -1.0] + max_accel: [2.5, 2.5, 3.2] + max_decel: [-2.5, -2.5, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 + +slam_toolbox: + ros__parameters: + use_sim_time: True + + # Plugin params + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS Parameters + odom_frame: odom + map_frame: map + base_frame: ridgeback_base_link + scan_topic: /scan_front_filtered + mode: mapping + + debug_logging: false + throttle_scans: 1 + transform_publish_period: 0.02 + map_update_interval: 5.0 + resolution: 0.05 + max_laser_range: 24.9 + minimum_time_interval: 0.5 + tf_buffer_duration: 30.0 + stack_size_to_use: 40000000 + enable_interactive_mode: true + + # General Parameters + use_scan_matching: false + use_scan_barycenter: true + minimum_travel_distance: 0.3 + minimum_travel_heading: 0.3 + scan_buffer_size: 10 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + loop_search_maximum_distance: 3.0 + do_loop_closing: false + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + + # Correlation Parameters + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + # Loop Closure Parameters + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # Scan Matcher Parameters + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true diff --git a/src/hangar_sim/test/nav2_params_distro_parity_test.py b/src/hangar_sim/test/nav2_params_distro_parity_test.py new file mode 100644 index 000000000..2489eb03d --- /dev/null +++ b/src/hangar_sim/test/nav2_params_distro_parity_test.py @@ -0,0 +1,162 @@ +# Copyright 2026 PickNik Inc. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the PickNik Inc. nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +"""hangar_sim ships one nav2 parameter file per ROS distro. + +`nav2_params.yaml` serves Humble and `nav2_params_jazzy.yaml` serves Jazzy, +because nav2's pluginlib lookup names differ between the two and the forms are +mutually exclusive. Everything else in the two files must stay identical -- any +tuning applied to one and forgotten on the other is a silent, distro-specific +config bug that only shows up on a running robot. + +This test pins the complete set of allowed differences. +""" + +import re +from pathlib import Path + +import pytest +import yaml + +PARAMS_DIR = Path(__file__).resolve().parent.parent / "params" +HUMBLE_PARAMS = PARAMS_DIR / "nav2_params.yaml" +JAZZY_PARAMS = PARAMS_DIR / "nav2_params_jazzy.yaml" + +# Jazzy exports these plugins with no `name` attribute, so the pluginlib lookup +# name is the `pkg::Class` type. Humble's exports declare `name="pkg/Class"`. +EXPECTED_PLUGIN_RENAMES = { + "planner_server.ros__parameters.GridBased.plugin": ( + "nav2_navfn_planner/NavfnPlanner", + "nav2_navfn_planner::NavfnPlanner", + ), + "behavior_server.ros__parameters.spin.plugin": ( + "nav2_behaviors/Spin", + "nav2_behaviors::Spin", + ), + "behavior_server.ros__parameters.backup.plugin": ( + "nav2_behaviors/BackUp", + "nav2_behaviors::BackUp", + ), + "behavior_server.ros__parameters.drive_on_heading.plugin": ( + "nav2_behaviors/DriveOnHeading", + "nav2_behaviors::DriveOnHeading", + ), + "behavior_server.ros__parameters.wait.plugin": ( + "nav2_behaviors/Wait", + "nav2_behaviors::Wait", + ), + "behavior_server.ros__parameters.assisted_teleop.plugin": ( + "nav2_behaviors/AssistedTeleop", + "nav2_behaviors::AssistedTeleop", + ), +} + +# Jazzy's bt_navigator always loads the default nav2 BT plugin libraries and the +# parameter is additive, so re-listing them double-registers every node. +# +# The rest are parameters Jazzy's nav2 renamed. Their Humble spellings are +# undeclared on Jazzy, so leaving them would be silently ignored rather than +# rejected -- the values happen to match Jazzy's defaults today, which is exactly +# what would make a future divergence invisible. +HUMBLE_ONLY_KEYS = { + "bt_navigator.ros__parameters.plugin_lib_names", + "controller_server.ros__parameters.progress_checker_plugin", + "behavior_server.ros__parameters.costmap_topic", + "behavior_server.ros__parameters.footprint_topic", +} + +JAZZY_ONLY_KEYS = { + "controller_server.ros__parameters.progress_checker_plugins", + "controller_server.ros__parameters.FollowPath.open_loop", + "behavior_server.ros__parameters.local_costmap_topic", + "behavior_server.ros__parameters.local_footprint_topic", +} + + +def _flatten(node, prefix=""): + if isinstance(node, dict) and node: + for key, value in node.items(): + yield from _flatten(value, f"{prefix}.{key}" if prefix else str(key)) + else: + yield prefix, node + + +@pytest.fixture(scope="module") +def flattened(): + humble = dict(_flatten(yaml.safe_load(HUMBLE_PARAMS.read_text()))) + jazzy = dict(_flatten(yaml.safe_load(JAZZY_PARAMS.read_text()))) + return humble, jazzy + + +def test_only_expected_keys_are_distro_specific(flattened): + humble, jazzy = flattened + assert set(humble) - set(jazzy) == HUMBLE_ONLY_KEYS + assert set(jazzy) - set(humble) == JAZZY_ONLY_KEYS + + +def test_shared_keys_hold_identical_values(flattened): + humble, jazzy = flattened + mismatches = { + key: (humble[key], jazzy[key]) + for key in set(humble) & set(jazzy) + if humble[key] != jazzy[key] + } + assert mismatches == EXPECTED_PLUGIN_RENAMES + + +def test_jazzy_enables_mppi_open_loop(flattened): + """The Jazzy backport only restores MPPI acceleration when explicitly enabled.""" + humble, jazzy = flattened + key = "controller_server.ros__parameters.FollowPath.open_loop" + assert key not in humble + assert jazzy[key] is True + + +def test_jazzy_uses_no_slash_form_plugin_names(flattened): + """Reject the pre-Jazzy `pkg/Class` form generally, not just today's six -- + a plugin added later to both files would otherwise slip through.""" + _, jazzy = flattened + slash_form = { + key: value + for key, value in jazzy.items() + if key.endswith(".plugin") + and isinstance(value, str) + and re.fullmatch(r"\w+/\w+", value) + } + assert slash_form == {} + + +def test_launch_file_references_both_params_files(): + """A typo in either filename would only surface as a bringup failure, and the + integration suite does not observe nav2 bringup -- so pin it here.""" + launch_source = ( + PARAMS_DIR.parent / "launch" / "sim" / "robot_drivers_to_persist_sim.launch.py" + ).read_text() + for params in (HUMBLE_PARAMS, JAZZY_PARAMS): + assert params.name in launch_source + assert params.is_file()