From 7791388f2f13656b7eec616af65d715a76a848bc Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Thu, 15 Jan 2026 22:15:09 -0500 Subject: [PATCH 1/2] integrated localizer' --- .../launch/ground_truth_tf.launch.py | 3 +- src/simulation/launch/spawn.launch.py | 6 +- .../athena_planner/config/nav2_params.yaml | 228 +++++++++++------- .../launch/navigation.launch.py | 44 ++-- .../localizer/config/localizer.yaml | 83 +++---- 5 files changed, 199 insertions(+), 165 deletions(-) diff --git a/src/simulation/launch/ground_truth_tf.launch.py b/src/simulation/launch/ground_truth_tf.launch.py index f64fdcc..8c7c2ae 100644 --- a/src/simulation/launch/ground_truth_tf.launch.py +++ b/src/simulation/launch/ground_truth_tf.launch.py @@ -22,7 +22,8 @@ def generate_launch_description(): name='ground_truth_tf_publisher', output='screen', parameters=[{ - 'child_frame_id': LaunchConfiguration('child_frame_id') + 'child_frame_id': LaunchConfiguration('child_frame_id'), + 'use_sim_time': True }] ), ]) diff --git a/src/simulation/launch/spawn.launch.py b/src/simulation/launch/spawn.launch.py index 4b5bdf0..4216151 100644 --- a/src/simulation/launch/spawn.launch.py +++ b/src/simulation/launch/spawn.launch.py @@ -21,9 +21,11 @@ def generate_launch_description(): pkg_share = get_package_share_directory('description') - + pkg_sim = get_package_share_directory('simulation') + urdf_file = os.path.join(pkg_share, 'urdf', 'athena_drive.urdf.xacro') controllers_file = os.path.join(pkg_share, 'config', 'athena_drive_sim_controllers.yaml') + rviz_config_file = os.path.join(pkg_sim, 'sim_rviz.rviz') namespace = LaunchConfiguration('namespace') @@ -70,6 +72,8 @@ def generate_launch_description(): executable='rviz2', name='rviz2', output='screen', + arguments=['-d', rviz_config_file], + parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')}], condition=conditions.IfCondition(LaunchConfiguration('rviz')) ), ]) diff --git a/src/subsystems/navigation/athena_planner/config/nav2_params.yaml b/src/subsystems/navigation/athena_planner/config/nav2_params.yaml index 729302d..a157bea 100644 --- a/src/subsystems/navigation/athena_planner/config/nav2_params.yaml +++ b/src/subsystems/navigation/athena_planner/config/nav2_params.yaml @@ -2,9 +2,45 @@ bt_navigator: ros__parameters: use_sim_time: true global_frame: map - robot_base_frame: base_link - # Note the bt xml file is set up in navigation.launch.py - + robot_base_frame: base_footprint + odom_topic: /localization/odom + bt_loop_duration: 10 + default_server_timeout: 20 + default_bt_xml_filename: "$(find-pkg-share nav2_bt_navigator)/behavior_trees/navigate_w_replanning_and_recovery.xml" + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_distance_traveled_condition_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_truncate_path_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_reinitialize_global_localization_service_bt_node + planner_server: ros__parameters: use_sim_time: true @@ -12,127 +48,151 @@ planner_server: planner_plugins: ["GridBased"] GridBased: - plugin: "nav2_smac_planner/SmacPlannerHybrid" - # Search / map handling + plugin: "nav2_smac_planner/SmacPlannerHybrid" + tolerance: 0.5 downsample_costmap: false downsampling_factor: 1 allow_unknown: true - tolerance: 0.25 max_iterations: 1000000 - max_on_approach_iterations: 1000 max_planning_time: 5.0 - motion_model_for_search: "DUBIN" # "DUBIN" or "REEDS_SHEPP" if reversing allowed + + # Ackermann vehicle parameters + motion_model_for_search: "REEDS_SHEPP" angle_quantization_bins: 72 - - # Analytic expansion (goal approach shortcuts) analytic_expansion_ratio: 3.5 analytic_expansion_max_length: 3.0 - analytic_expansion_max_cost: 200.0 - analytic_expansion_max_cost_override: false - - # Vehicle / penalties (tunes path quality) - minimum_turning_radius: 0.40 + minimum_turning_radius: 1.0 reverse_penalty: 2.0 change_penalty: 0.0 non_straight_penalty: 1.2 cost_penalty: 2.0 retrospective_penalty: 0.015 - - # Heuristics / caching + lookup_table_size: 20.0 cache_obstacle_heuristic: false - downsample_obstacle_heuristic: true - use_quadratic_cost_penalty: false - allow_primitive_interpolation: false - coarse_search_resolution: 4 # only used with ALL_DIRECTION goal mode - goal_heading_mode: "DEFAULT" # DEFAULT, BIDIRECTIONAL, ALL_DIRECTION - - # Debug + post-smoothing - debug_visualizations: false smooth_path: true - smoother: - max_iterations: 1000 - w_smooth: 0.3 - w_data: 0.2 - tolerance: 1.0e-10 - do_refinement: true - refinement_num: 2 - - -global_costmap: - ros__parameters: - use_sim_time: true - global_frame: map - robot_base_frame: base_link - update_frequency: 2.0 - publish_frequency: 2.0 - resolution: 1.0 - size_x: 799 - size_y: 799 - origin_x: 0.0 - origin_y: 0.0 - rolling_window: false - track_unknown_space: true - plugins: - - {name: static_layer, type: "nav2_costmap_2d::StaticLayer"} - - {name: inflation_layer, type: "nav2_costmap_2d::InflationLayer"} controller_server: ros__parameters: - odom_topic: /ackermann_steering_controller/odometry use_sim_time: true - enable_stamped_cmd_vel: false - + 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 + odom_topic: /localization/odom progress_checker_plugins: ["progress_checker"] - goal_checker_plugins: ["simple_goal_checker"] - current_goal_checker: "simple_goal_checker" + goal_checker_plugins: ["general_goal_checker"] + controller_plugins: ["FollowPath"] progress_checker: plugin: "nav2_controller::SimpleProgressChecker" required_movement_radius: 0.5 movement_time_allowance: 10.0 - simple_goal_checker: + general_goal_checker: + stateful: true plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 2.0 - yaw_goal_tolerance: 6.283185 - stateful: true - - controller_plugins: ["FollowPath"] + xy_goal_tolerance: 1.0 + yaw_goal_tolerance: 0.5 + FollowPath: plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" desired_linear_vel: 1.0 + lookahead_dist: 2.5 + min_lookahead_dist: 1.5 + max_lookahead_dist: 4.0 + lookahead_time: 1.5 + rotate_to_heading_angular_vel: 0.3 + transform_tolerance: 0.5 + use_velocity_scaled_lookahead_dist: true + min_approach_linear_velocity: 0.2 + approach_velocity_scaling_dist: 1.0 + use_collision_detection: false + max_allowed_time_to_collision_up_to_carrot: 1.0 + use_regulated_linear_velocity_scaling: true + use_cost_regulated_linear_velocity_scaling: false + regulated_linear_scaling_min_radius: 1.0 + regulated_linear_scaling_min_speed: 0.25 + use_rotate_to_heading: true + allow_reversing: false + rotate_to_heading_min_angle: 0.785 max_linear_accel: 1.0 max_linear_decel: 1.0 - lookahead_dist: 2.0 - min_lookahead_dist: 1.0 - max_lookahead_dist: 3.0 - transform_tolerance: 1.0 - use_velocity_scaled_lookahead_dist: false - use_final_approach_orientation: false - - + max_angular_accel: 1.5 + max_robot_pose_search_dist: 10.0 + use_interpolation: false -local_costmap: +behavior_server: ros__parameters: use_sim_time: true - global_frame: odom - robot_base_frame: base_link - rolling_window: true - width: 6.0 - height: 6.0 - resolution: 1 - plugins: ["static_layer"] - static_layer: - plugin: "nav2_costmap_2d::StaticLayer" - map_subscribe_transient_local: true - + global_frame: map + robot_base_frame: base_footprint + transform_tolerance: 0.5 + simulate_ahead_time: 2.0 + max_rotational_vel: 0.3 + min_rotational_vel: 0.05 + rotational_acc_lim: 0.8 + + 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" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + wait: + plugin: "nav2_behaviors/Wait" + +global_costmap: + global_costmap: + ros__parameters: + use_sim_time: true + robot_base_frame: base_footprint + global_frame: map + update_frequency: 1.0 + publish_frequency: 1.0 + resolution: 0.5 + width: 100 + height: 100 + rolling_window: true + track_unknown_space: false + always_send_full_costmap: true + footprint: "[[0.45, 0.33], [0.45, -0.33], [-0.35, -0.33], [-0.35, 0.33]]" + plugins: ["inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 1.5 + +local_costmap: + local_costmap: + ros__parameters: + use_sim_time: true + global_frame: map + robot_base_frame: base_footprint + update_frequency: 5.0 + publish_frequency: 2.0 + rolling_window: true + width: 10 + height: 10 + resolution: 0.25 + always_send_full_costmap: true + footprint: "[[0.45, 0.33], [0.45, -0.33], [-0.35, -0.33], [-0.35, 0.33]]" + plugins: ["inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 1.0 lifecycle_manager: ros__parameters: use_sim_time: true autostart: true node_names: - - planner_server - controller_server - - bt_navigator + - planner_server + - behavior_server + - bt_navigator \ No newline at end of file diff --git a/src/subsystems/navigation/athena_planner/launch/navigation.launch.py b/src/subsystems/navigation/athena_planner/launch/navigation.launch.py index c1c163b..be5f1de 100644 --- a/src/subsystems/navigation/athena_planner/launch/navigation.launch.py +++ b/src/subsystems/navigation/athena_planner/launch/navigation.launch.py @@ -9,51 +9,47 @@ import os def generate_launch_description(): - # --- Packages / paths --- - athena_map_share = get_package_share_directory('athena_map') - dem_launch = os.path.join(athena_map_share, 'launch', 'dem_costmap.launch.py') + #athena_map_share = get_package_share_directory('athena_map') + #dem_launch = os.path.join(athena_map_share, 'launch', 'dem_costmap.launch.py') athena_planner_share = get_package_share_directory('athena_planner') nav2_nav = os.path.join(athena_planner_share, 'launch', 'nav2_nodes.launch.py') - + + localizer_share = get_package_share_directory('localizer') + localizer_launch_file = os.path.join(localizer_share, 'launch', 'localizer.launch.py') + default_params = PathJoinSubstitution([ FindPackageShare('athena_planner'), 'config', 'nav2_params.yaml' ]) params_file = LaunchConfiguration('params_file') - map_frame = LaunchConfiguration('map_frame') - odom_frame = LaunchConfiguration('odom_frame') - base_frame = LaunchConfiguration('base_frame') - use_respawn = LaunchConfiguration('use_respawn') log_level = LaunchConfiguration('log_level') - - static_map_to_odom = Node( - package='tf2_ros', - executable='static_transform_publisher', - name='static_map_to_odom', - arguments=['400', '400', '0', '0', '0', '0', map_frame, odom_frame], - output='screen', - ) - twist_stamper_node = Node( package='twist_stamper', executable='twist_stamper', name='cmd_vel_stamper', + parameters=[{'use_sim_time': True}], remappings=[ - ('cmd_vel_in', '/cmd_vel_nav2'), + ('cmd_vel_in', '/cmd_vel_nav'), ('cmd_vel_out', '/ackermann_steering_controller/reference'), ], ) + localizer_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(localizer_launch_file), + launch_arguments={ + 'use_sim_time': 'true', + }.items() + ) + return LaunchDescription([ - # Params DeclareLaunchArgument( 'params_file', default_value=default_params, description='Full path to the Nav2 params YAML' ), - DeclareLaunchArgument('map_frame', default_value='map'), + DeclareLaunchArgument('map_frame', default_value='map'), DeclareLaunchArgument('odom_frame', default_value='odom'), DeclareLaunchArgument('base_frame', default_value='base_link'), DeclareLaunchArgument('use_respawn', default_value='False', @@ -61,12 +57,10 @@ def generate_launch_description(): DeclareLaunchArgument('log_level', default_value='info', description='Log level for nav2 nodes'), - SetRemap(src='cmd_vel', dst='/cmd_vel_nav2'), - - static_map_to_odom, twist_stamper_node, + localizer_launch, - IncludeLaunchDescription(PythonLaunchDescriptionSource(dem_launch)), + #IncludeLaunchDescription(PythonLaunchDescriptionSource(dem_launch)), IncludeLaunchDescription( PythonLaunchDescriptionSource(nav2_nav), @@ -78,4 +72,4 @@ def generate_launch_description(): 'log_level': log_level }.items() ), - ]) + ]) \ No newline at end of file diff --git a/src/subsystems/navigation/localizer/config/localizer.yaml b/src/subsystems/navigation/localizer/config/localizer.yaml index 33bcd15..4c5ed65 100644 --- a/src/subsystems/navigation/localizer/config/localizer.yaml +++ b/src/subsystems/navigation/localizer/config/localizer.yaml @@ -1,16 +1,11 @@ state_estimator: ros__parameters: - # ========================================================================== - # Topic Configuration - # ========================================================================== imu_topic: "/imu" gnss_topic: "/gps/fix" odom_topic: "/odom/ground_truth" output_odom_topic: "/localization/odom" - # ========================================================================== # Frame Names - # ========================================================================== tf_prefix: "" map_frame: "map" odom_frame: "odom" @@ -18,71 +13,51 @@ state_estimator: imu_frame: "imu_link" gnss_frame: "gnss_link" - # ========================================================================== # Publish Rates - # ========================================================================== - publish_rate: 100.0 # Hz - high rate for smooth output - tf_publish_rate: 100.0 # Hz - increased from 50 for smoother transforms + publish_rate: 100.0 + tf_publish_rate: 100.0 - # ========================================================================== # IMU Noise Parameters (continuous-time spectral densities) - # ========================================================================== - accel_noise_sigma: 3.0 # m/s^2/sqrt(Hz) - realistic for raw IMU data - gyro_noise_sigma: 10.0 # rad/s/sqrt(Hz) - realistic for raw gyro data - accel_bias_rw_sigma: 5.0e-2 # m/s^3/sqrt(Hz) - allow bias adaptation for raw data - gyro_bias_rw_sigma: 5.0e-2 # rad/s^2/sqrt(Hz) - allow gyro bias changes + accel_noise_sigma: 3.0 + gyro_noise_sigma: 10.0 + accel_bias_rw_sigma: 5.0e-2 + gyro_bias_rw_sigma: 5.0e-2 - # ========================================================================== # GNSS Parameters - # ========================================================================== - gnss_position_sigma: 0.0000001 # meters - realistic consumer GPS uncertainty + gnss_position_sigma: 0.0000001 gnss_altitude_sigma: 0.0000001 - gnss_max_age: 0.5 # seconds - moderate timing constraint - gnss_fix_quality_multiplier: 1.0 # neutral GPS weight + gnss_max_age: 0.5 + gnss_fix_quality_multiplier: 1.0 + use_robust_gnss_noise: false + gnss_huber_k: 1.0 - # GNSS outlier rejection (Huber robust noise model) - use_robust_gnss_noise: false # reject GPS jumps/outliers - gnss_huber_k: 1.0 # aggressive outlier rejection - - # ========================================================================== # Odometry Parameters - # ========================================================================== - odom_position_sigma: 0.00001 # meters - realistic wheel odometry with slip - odom_rotation_sigma: 0.00001 # radians - account for steering uncertainty + odom_position_sigma: 0.00001 + odom_rotation_sigma: 0.00001 - # ========================================================================== - # Graph Management - MAXIMUM STABILITY FOR RAW DATA - # ========================================================================== - state_creation_rate: 50.0 # Hz - high rate for dense optimization - max_graph_states: 500 # massive window: 10 seconds @ 50Hz + # Graph Management + state_creation_rate: 50.0 + max_graph_states: 500 - # ISAM2 optimizer parameters - AGGRESSIVE OPTIMIZATION - isam2_relinearize_threshold: 0.01 # tight but realistic convergence - isam2_relinearize_skip: 1 # relinearize every update for stability + # ISAM2 optimizer parameters + isam2_relinearize_threshold: 0.01 + isam2_relinearize_skip: 1 - # ========================================================================== - # IMU Integration Parameters - REALISTIC FOR RAW DATA - # ========================================================================== - integration_covariance: 1.0e-6 # account for discretization errors in raw data - bias_acc_omega_int: 1.0e-5 # realistic bias integration uncertainty + # IMU Integration Parameters + integration_covariance: 1.0e-6 + bias_acc_omega_int: 1.0e-5 - # ========================================================================== # Initialization Parameters - # ========================================================================== - min_imu_samples_for_init: 100 # more samples for noisy raw data (1 second @ 100Hz) - initial_velocity_sigma: 0.5 # m/s - less confident with raw data - initial_rotation_sigma: 0.1 # rad - ~6° uncertainty for raw IMU - initial_accel_bias_sigma: 0.1 # m/s^2 - significant initial bias uncertainty - initial_gyro_bias_sigma: 0.01 # rad/s - significant gyro bias uncertainty + min_imu_samples_for_init: 100 + initial_velocity_sigma: 0.5 + initial_rotation_sigma: 0.1 + initial_accel_bias_sigma: 0.1 + initial_gyro_bias_sigma: 0.01 - # ========================================================================== # Buffer Configuration - # ========================================================================== - imu_buffer_max_size: 5000 # large buffer for dense processing and delays + imu_buffer_max_size: 5000 - # ========================================================================== - # ENU Origin (optional - if not set, uses first GNSS fix) - # ========================================================================== + # ENU Origin origin_lat: 38.42391162772634 origin_lon: -110.78490558433397 origin_alt: 0.0 \ No newline at end of file From ac6f98e32265a97b99bd62674fb2cacefc9d0f09 Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Thu, 15 Jan 2026 22:35:03 -0500 Subject: [PATCH 2/2] fix rviz file --- src/simulation/CMakeLists.txt | 2 +- src/simulation/launch/spawn.launch.py | 2 +- src/simulation/{sim_rviz.rviz => rviz/sim.rviz} | 0 3 files changed, 2 insertions(+), 2 deletions(-) rename src/simulation/{sim_rviz.rviz => rviz/sim.rviz} (100%) diff --git a/src/simulation/CMakeLists.txt b/src/simulation/CMakeLists.txt index c61a7d5..3e08eda 100644 --- a/src/simulation/CMakeLists.txt +++ b/src/simulation/CMakeLists.txt @@ -17,7 +17,7 @@ find_package(ros_gz_bridge REQUIRED) # --- Install data directories so package:// URIs work --- # Guard each one so CMake doesn't error if a folder is missing. -foreach(dir urdf meshes config launch worlds textures models) +foreach(dir urdf meshes config launch worlds textures models rviz) if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/${dir}) install(DIRECTORY ${dir}/ DESTINATION share/${PROJECT_NAME}/${dir} diff --git a/src/simulation/launch/spawn.launch.py b/src/simulation/launch/spawn.launch.py index 4216151..8c8618c 100644 --- a/src/simulation/launch/spawn.launch.py +++ b/src/simulation/launch/spawn.launch.py @@ -25,7 +25,7 @@ def generate_launch_description(): urdf_file = os.path.join(pkg_share, 'urdf', 'athena_drive.urdf.xacro') controllers_file = os.path.join(pkg_share, 'config', 'athena_drive_sim_controllers.yaml') - rviz_config_file = os.path.join(pkg_sim, 'sim_rviz.rviz') + rviz_config_file = os.path.join(pkg_sim, 'rviz', 'sim.rviz') namespace = LaunchConfiguration('namespace') diff --git a/src/simulation/sim_rviz.rviz b/src/simulation/rviz/sim.rviz similarity index 100% rename from src/simulation/sim_rviz.rviz rename to src/simulation/rviz/sim.rviz