diff --git a/jetson/ros2_ws/src/saltybot_bringup/launch/nav2.launch.py b/jetson/ros2_ws/src/saltybot_bringup/launch/nav2.launch.py index 0505fdc..c73862a 100644 --- a/jetson/ros2_ws/src/saltybot_bringup/launch/nav2.launch.py +++ b/jetson/ros2_ws/src/saltybot_bringup/launch/nav2.launch.py @@ -1,76 +1,139 @@ """ nav2.launch.py — Nav2 autonomous navigation stack for SaltyBot -Starts the full Nav2 navigation stack (controllers, planners, behavior server, -BT navigator, velocity smoother, lifecycle managers). +Supports two localization modes via the 'nav_mode' argument: -Localization is provided by RTAB-Map (slam_rtabmap.launch.py must be running): - /map — OccupancyGrid (static global costmap layer) - /rtabmap/odom — Odometry (velocity smoother + controller feedback) - TF map→odom — published by RTAB-Map + nav_mode:=slam (default) + Uses RTAB-Map for odometry and live map building. + Requires slam_rtabmap.launch.py to be running. + Sources: /rtabmap/odom (pose), /map (from RTAB-Map) -Output: - /cmd_vel — consumed by saltybot_bridge → STM32 over UART + nav_mode:=amcl + Uses VESC CAN odometry (/odom) + AMCL on a pre-built static map. + Launches: sensors, VESC odometry bridge, map_server, AMCL, Nav2 stack. + Sources: /odom (VESC CAN IDs 61/79), /scan (RPLIDAR) + Map: saltybot_nav2_slam maps/saltybot_map.yaml (override with map:=...) -Profile Support (Issue #506) -──────────────────────────── - Supports profile-based parameter overrides via 'profile' launch argument: - profile:=indoor — conservative (0.2 m/s, tight geofence, aggressive inflation) - profile:=outdoor — moderate (0.5 m/s, wide geofence, standard inflation) - profile:=demo — agile (0.3 m/s, tricks enabled, enhanced obstacle avoidance) + nav_mode:=slam profile support (Issue #506) + profile:=indoor — conservative (0.2 m/s, tight geofence) + profile:=outdoor — moderate (0.5 m/s) + profile:=demo — agile (0.3 m/s, tricks enabled) Run sequence on Orin: - 1. docker compose up saltybot-ros2 # RTAB-Map + sensors - 2. docker compose up saltybot-nav2 # this launch file + # SLAM mode (existing behaviour): + ros2 launch saltybot_bringup nav2.launch.py + + # AMCL mode with saved map: + ros2 launch saltybot_bringup nav2.launch.py nav_mode:=amcl \ + map:=/mnt/nvme/saltybot/maps/my_map.yaml """ import os + from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, LogInfo +from launch.actions import ( + DeclareLaunchArgument, + GroupAction, + IncludeLaunchDescription, + LogInfo, +) +from launch.conditions import LaunchConfigurationEquals from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration -def generate_launch_description(): - nav2_bringup_dir = get_package_share_directory('nav2_bringup') - bringup_dir = get_package_share_directory('saltybot_bringup') +def generate_launch_description() -> LaunchDescription: + nav2_bringup_dir = get_package_share_directory("nav2_bringup") + bringup_dir = get_package_share_directory("saltybot_bringup") + nav2_slam_dir = get_package_share_directory("saltybot_nav2_slam") + + # ── Arguments ───────────────────────────────────────────────────────────── + + nav_mode_arg = DeclareLaunchArgument( + "nav_mode", + default_value="slam", + choices=["slam", "amcl"], + description=( + "Localization mode. " + "slam: RTAB-Map live map (requires slam_rtabmap.launch.py running); " + "amcl: static map + AMCL particle filter + VESC odometry (Issue #655)." + ), + ) - # Profile argument (Issue #506) profile_arg = DeclareLaunchArgument( "profile", default_value="indoor", choices=["indoor", "outdoor", "demo"], - description="Launch profile for parameter overrides (Issue #506)" + description="Nav2 parameter profile for slam mode (Issue #506)", ) - profile = LaunchConfiguration('profile') - - nav2_params_file = os.path.join(bringup_dir, 'config', 'nav2_params.yaml') - bt_xml_file = os.path.join( - bringup_dir, 'behavior_trees', 'navigate_to_pose_with_recovery.xml' + map_arg = DeclareLaunchArgument( + "map", + default_value=os.path.join(nav2_slam_dir, "maps", "saltybot_map.yaml"), + description="Map YAML path for AMCL mode", ) - nav2_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, 'launch', 'navigation_launch.py') - ), - launch_arguments={ - 'use_sim_time': 'false', - 'autostart': 'true', - 'params_file': nav2_params_file, - 'default_bt_xml_filename': bt_xml_file, - # RTAB-Map publishes /map with transient_local QoS — must match - 'map_subscribe_transient_local': 'true', - }.items(), + use_sim_time_arg = DeclareLaunchArgument( + "use_sim_time", + default_value="false", ) - profile_log = LogInfo( - msg=['[nav2] Loaded profile: ', profile] + # ── Mode: SLAM (original behaviour — RTAB-Map + navigation_launch.py) ──── + + slam_nav2_params = os.path.join(bringup_dir, "config", "nav2_params.yaml") + slam_bt_xml = os.path.join( + bringup_dir, "behavior_trees", "navigate_to_pose_with_recovery.xml" + ) + + slam_mode = GroupAction( + condition=LaunchConfigurationEquals("nav_mode", "slam"), + actions=[ + LogInfo(msg="[nav2] Mode: SLAM (RTAB-Map odometry + live map)"), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, "launch", "navigation_launch.py") + ), + launch_arguments={ + "use_sim_time": "false", + "autostart": "true", + "params_file": slam_nav2_params, + "default_bt_xml_filename": slam_bt_xml, + "map_subscribe_transient_local": "true", + }.items(), + ), + ], + ) + + # ── Mode: AMCL (static map + AMCL + VESC odometry, Issue #655) ─────────── + + amcl_mode = GroupAction( + condition=LaunchConfigurationEquals("nav_mode", "amcl"), + actions=[ + LogInfo(msg="[nav2] Mode: AMCL (static map + VESC odometry, Issue #655)"), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + nav2_slam_dir, "launch", "nav2_amcl_bringup.launch.py" + ) + ), + launch_arguments={ + "map": LaunchConfiguration("map"), + "use_sim_time": LaunchConfiguration("use_sim_time"), + # Sensors are already started by saltybot_bringup GROUP A + "include_sensors": "false", + }.items(), + ), + ], ) return LaunchDescription([ + nav_mode_arg, profile_arg, - profile_log, - nav2_launch, + map_arg, + use_sim_time_arg, + LogInfo(msg=["[nav2] Loaded nav_mode=", LaunchConfiguration("nav_mode"), + " profile=", LaunchConfiguration("profile")]), + slam_mode, + amcl_mode, ]) diff --git a/jetson/ros2_ws/src/saltybot_nav2_slam/config/amcl_nav2_params.yaml b/jetson/ros2_ws/src/saltybot_nav2_slam/config/amcl_nav2_params.yaml new file mode 100644 index 0000000..28c4a30 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_nav2_slam/config/amcl_nav2_params.yaml @@ -0,0 +1,331 @@ +# amcl_nav2_params.yaml — Complete Nav2 + AMCL parameter file (Issue #655) +# +# AMCL localises on a pre-built static map using /scan (RPLIDAR A1M8). +# Odometry source: /odom from vesc_can_odometry (VESC CAN IDs 61/79, Issue #646). +# +# Launch with: +# ros2 launch saltybot_nav2_slam nav2_amcl_bringup.launch.py +# ros2 launch saltybot_nav2_slam nav2_amcl_bringup.launch.py \ +# map:=/mnt/nvme/saltybot/maps/my_map.yaml +# +# Costmaps (inline — required by nav2_bringup navigation_launch.py): +# global: static_layer + obstacle_layer (LiDAR) + inflation_layer +# local: obstacle_layer (LiDAR) + inflation_layer, 4m rolling window + +# ── AMCL ───────────────────────────────────────────────────────────────────── +amcl: + ros__parameters: + use_sim_time: false + + # Particle filter bounds + min_particles: 500 + max_particles: 3000 + + # Motion model noise (alpha1–5: low = trust odometry more) + # Tuned for VESC differential drive (Issue #646): encoder slip ~5% + alpha1: 0.10 # rot noise from rot + alpha2: 0.10 # rot noise from trans + alpha3: 0.05 # trans noise from trans + alpha4: 0.05 # trans noise from rot + alpha5: 0.10 # (omni only) + + # Sensor model — likelihood field + laser_model_type: "likelihood_field" + laser_likelihood_max_dist: 2.0 + max_beams: 60 + sigma_hit: 0.2 + z_hit: 0.50 # beam hit weight + z_rand: 0.45 # random noise weight + z_max: 0.025 # max-range weight (z_hit+z_rand+z_max+z_short = 1.0) + z_short: 0.025 # short-read weight + sigma_short: 0.05 + do_beamskip: false + beam_search_increment: 0.1 + + # Frame IDs + global_frame_id: "map" + odom_frame_id: "odom" + base_frame_id: "base_link" + + # Update triggers + update_min_d: 0.20 # m — update after moving this far + update_min_a: 0.15 # rad — update after rotating this much + resample_interval: 1 + + # Particle filter convergence + pf_err: 0.05 + pf_z: 0.99 + + # Transform + tf_broadcast: true + transform_tolerance: 0.5 # s — relaxed for slower Nav2 loop + save_pose_rate: 0.5 # Hz — save last pose for fast re-init + + # Recovery particle injection (0 = disabled) + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + + # Robot model + robot_model_type: "nav2_amcl::DifferentialMotionModel" + + # Scan topic + scan_topic: /scan + +# ── Map Server ──────────────────────────────────────────────────────────────── +map_server: + ros__parameters: + use_sim_time: false + yaml_filename: "" # overridden by launch argument --map + +# ── BT Navigator ───────────────────────────────────────────────────────────── +bt_navigator: + ros__parameters: + use_sim_time: false + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + enable_groot_monitoring: false + default_nav_to_pose_bt_xml: >- + $(find-pkg-share nav2_bt_navigator)/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml + default_nav_through_poses_bt_xml: >- + $(find-pkg-share nav2_bt_navigator)/behavior_trees/navigate_through_poses_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_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_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_updated_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_planner_selector_bt_node + +# ── Controller Server (DWB local planner) ───────────────────────────────────── +controller_server: + ros__parameters: + use_sim_time: 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 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] + controller_plugins: ["FollowPath"] + + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.3 + movement_time_allowance: 15.0 + + general_goal_checker: + stateful: true + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.20 + yaw_goal_tolerance: 0.20 + + # DWB — tuned for VESC differential drive on Saltybot + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: false + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 1.0 # m/s — conservative indoor nav + max_vel_y: 0.0 # diff drive: no lateral + max_vel_theta: 1.5 # rad/s + min_speed_xy: 0.0 + max_speed_xy: 1.0 + min_speed_theta: 0.0 + acc_lim_x: 2.0 + acc_lim_y: 0.0 + acc_lim_theta: 2.0 + decel_lim_x: -2.0 + decel_lim_theta: -2.0 + vx_samples: 20 + vy_samples: 1 # diff drive: no lateral + vtheta_samples: 40 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: true + stateful: true + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", + "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +# ── Planner Server (NavFn A*) ───────────────────────────────────────────────── +planner_server: + ros__parameters: + use_sim_time: false + expected_planner_frequency: 20.0 + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: true + allow_unknown: true + +# ── Behavior Server (recovery behaviors) ────────────────────────────────────── +behavior_server: + ros__parameters: + use_sim_time: false + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + global_frame: odom + robot_base_frame: base_link + transform_timeout: 0.1 + simulate_ahead_time: 2.0 + behavior_plugins: ["spin", "backup", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + wait: + plugin: "nav2_behaviors/Wait" + # Spin recovery + max_rotations: 2.0 # full rotations before giving up + # Backup recovery + max_backup_dist: 0.30 # m + backup_speed: 0.15 # m/s — slow for a balancing robot + +# ── Global Costmap ──────────────────────────────────────────────────────────── +# Uses /scan (RPLIDAR) for obstacle detection. +# Layers: static (from /map) + obstacle (LiDAR) + inflation. +global_costmap: + global_costmap: + ros__parameters: + use_sim_time: false + global_frame: map + robot_base_frame: base_link + update_frequency: 5.0 # Hz + publish_frequency: 2.0 # Hz + rolling_window: false + resolution: 0.05 # m/cell + robot_radius: 0.35 # m — inscribed circle + track_unknown_space: true + unknown_cost_value: -1 + lethal_cost_threshold: 100 + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: true + enabled: true + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: true + observation_sources: scan + scan: + topic: /scan + data_type: "LaserScan" + max_obstacle_height: 2.0 + min_obstacle_height: 0.0 + obstacle_max_range: 9.5 + obstacle_min_range: 0.0 + raytrace_max_range: 10.0 + raytrace_min_range: 0.0 + clearing: true + marking: true + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + enabled: true + inflation_radius: 0.55 # m — clears obstacles wider than robot + cost_scaling_factor: 10.0 + inflate_unknown: false + inflate_around_unknown: true + +# ── Local Costmap ───────────────────────────────────────────────────────────── +# Rolling 4m window in odom frame. LiDAR obstacle layer + inflation. +local_costmap: + local_costmap: + ros__parameters: + use_sim_time: false + global_frame: odom + robot_base_frame: base_link + update_frequency: 10.0 # Hz + publish_frequency: 5.0 # Hz + rolling_window: true + width: 4 # m + height: 4 # m + resolution: 0.05 # m/cell + robot_radius: 0.35 + track_unknown_space: true + unknown_cost_value: 0 # treat unknown as free locally + lethal_cost_threshold: 100 + plugins: ["obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: true + observation_sources: scan + scan: + topic: /scan + data_type: "LaserScan" + max_obstacle_height: 2.0 + min_obstacle_height: 0.0 + obstacle_max_range: 5.0 + obstacle_min_range: 0.0 + raytrace_max_range: 6.0 + raytrace_min_range: 0.0 + clearing: true + marking: true + observation_persistence: 0.0 + expected_update_rate: 5.5 # RPLIDAR A1M8 scan rate + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + enabled: true + inflation_radius: 0.55 + cost_scaling_factor: 10.0 + inflate_unknown: false + inflate_around_unknown: false + +# ── Velocity Smoother ───────────────────────────────────────────────────────── +velocity_smoother: + ros__parameters: + use_sim_time: false + smoothing_frequency: 20.0 + scale_velocities: false + feedback: "OPEN_LOOP" + max_velocity: [1.0, 0.0, 1.5] + min_velocity: [-1.0, 0.0, -1.5] + max_accel: [2.0, 0.0, 2.0] + max_decel: [-2.0, 0.0, -2.0] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 + +# ── Lifecycle Managers ──────────────────────────────────────────────────────── +lifecycle_manager_localization: + ros__parameters: + use_sim_time: false + autostart: true + node_names: ["map_server", "amcl"] + +lifecycle_manager_navigation: + ros__parameters: + use_sim_time: false + autostart: true + node_names: ["controller_server", "planner_server", "behavior_server", + "bt_navigator", "velocity_smoother"] diff --git a/jetson/ros2_ws/src/saltybot_nav2_slam/launch/nav2_amcl_bringup.launch.py b/jetson/ros2_ws/src/saltybot_nav2_slam/launch/nav2_amcl_bringup.launch.py new file mode 100644 index 0000000..d225d85 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_nav2_slam/launch/nav2_amcl_bringup.launch.py @@ -0,0 +1,188 @@ +""" +nav2_amcl_bringup.launch.py — Nav2 with AMCL localization (Issue #655). + +Full autonomous navigation stack for SaltyBot using a pre-built static map +and AMCL particle-filter localization. + +Odometry: /odom from vesc_can_odometry (VESC CAN IDs 61/79, Issue #646) +LiDAR: /scan from RPLIDAR A1M8 (via saltybot_bringup sensors.launch.py) +Map: static OccupancyGrid loaded by map_server (defaults to placeholder) + +Startup sequence +──────────────── + 1. Sensors — RealSense D435i + RPLIDAR A1M8 drivers + static TF + 2. VESC odometry — /odom (differential drive from dual CAN motors) + 3. Localization — map_server + AMCL (nav2_bringup localization_launch.py) + 4. Navigation — controller + planner + behaviors + BT navigator + (nav2_bringup navigation_launch.py) + +Arguments +───────── + map path to map YAML (default: maps/saltybot_map.yaml placeholder) + use_sim_time (default: false) + autostart lifecycle manager autostart (default: true) + params_file Nav2 params override (default: config/amcl_nav2_params.yaml) + include_sensors launch sensors.launch.py (default: true — set false if + sensors are already running) + +Usage +───── + # Minimal — sensors included, placeholder map: + ros2 launch saltybot_nav2_slam nav2_amcl_bringup.launch.py + + # With a real saved map: + ros2 launch saltybot_nav2_slam nav2_amcl_bringup.launch.py \ + map:=/mnt/nvme/saltybot/maps/my_map.yaml + + # Without sensor bringup (sensors already running): + ros2 launch saltybot_nav2_slam nav2_amcl_bringup.launch.py \ + include_sensors:=false + +Integration with saltybot_bringup +────────────────────────────────── + The saltybot_bringup orchestrator (saltybot_bringup.launch.py) calls + nav2.launch.py at t=22 s in GROUP C. To use AMCL instead of RTAB-Map: + ros2 launch saltybot_bringup nav2.launch.py nav_mode:=amcl +""" + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + GroupAction, + IncludeLaunchDescription, + LogInfo, +) +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description() -> LaunchDescription: + nav2_slam_dir = get_package_share_directory("saltybot_nav2_slam") + bringup_dir = get_package_share_directory("saltybot_bringup") + nav2_bringup_dir = get_package_share_directory("nav2_bringup") + + default_map = os.path.join(nav2_slam_dir, "maps", "saltybot_map.yaml") + default_params = os.path.join(nav2_slam_dir, "config", "amcl_nav2_params.yaml") + + # ── Launch arguments ────────────────────────────────────────────────────── + + map_arg = DeclareLaunchArgument( + "map", + default_value=default_map, + description="Path to map YAML file (Nav2 map_server format)", + ) + + use_sim_time_arg = DeclareLaunchArgument( + "use_sim_time", + default_value="false", + description="Use simulated clock from /clock topic", + ) + + autostart_arg = DeclareLaunchArgument( + "autostart", + default_value="true", + description="Automatically start lifecycle nodes after launch", + ) + + params_file_arg = DeclareLaunchArgument( + "params_file", + default_value=default_params, + description="Path to Nav2 parameter YAML file", + ) + + include_sensors_arg = DeclareLaunchArgument( + "include_sensors", + default_value="true", + description="Launch sensors.launch.py (set false if sensors already running)", + ) + + # ── Substitutions ───────────────────────────────────────────────────────── + + map_file = LaunchConfiguration("map") + use_sim_time = LaunchConfiguration("use_sim_time") + autostart = LaunchConfiguration("autostart") + params_file = LaunchConfiguration("params_file") + include_sensors = LaunchConfiguration("include_sensors") + + # ── Sensor bringup (conditional) ───────────────────────────────────────── + # RealSense D435i + RPLIDAR A1M8 drivers + base_link→laser/camera_link TF + + sensors_launch = GroupAction( + condition=IfCondition(include_sensors), + actions=[ + LogInfo(msg="[nav2_amcl] Starting sensors (RealSense + RPLIDAR)"), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, "launch", "sensors.launch.py") + ), + ), + ], + ) + + # ── VESC CAN odometry ──────────────────────────────────────────────────── + # Differential drive from CAN IDs 61 (left) + 79 (right) → /odom + TF + + odom_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_slam_dir, "launch", "odometry_bridge.launch.py") + ), + launch_arguments={"use_sim_time": use_sim_time}.items(), + ) + + # ── Localization: map_server + AMCL ────────────────────────────────────── + # Publishes: /map, TF map→odom (AMCL), /amcl_pose, /particlecloud + + localization_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, "launch", "localization_launch.py") + ), + launch_arguments={ + "map": map_file, + "use_sim_time": use_sim_time, + "autostart": autostart, + "params_file": params_file, + "use_lifecycle_mgr": "true", + }.items(), + ) + + # ── Navigation: controller + planner + behaviors + BT navigator ────────── + # Subscribes: /odom, /scan, /map, /cmd_vel_nav (→ velocity_smoother → /cmd_vel) + # Provides: action servers for NavigateToPose + NavigateThroughPoses + + navigation_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, "launch", "navigation_launch.py") + ), + launch_arguments={ + "use_sim_time": use_sim_time, + "autostart": autostart, + "params_file": params_file, + "map_subscribe_transient_local": "true", + "use_lifecycle_mgr": "true", + }.items(), + ) + + return LaunchDescription([ + # Arguments + map_arg, + use_sim_time_arg, + autostart_arg, + params_file_arg, + include_sensors_arg, + + # Banner + LogInfo(msg="[nav2_amcl] Nav2 AMCL bringup starting (Issue #655)"), + + # Startup sequence + sensors_launch, # step 1 — sensors + odom_launch, # step 2 — /odom from VESC + LogInfo(msg="[nav2_amcl] Starting AMCL localization"), + localization_launch, # step 3 — map_server + AMCL + LogInfo(msg="[nav2_amcl] Starting Nav2 navigation stack"), + navigation_launch, # step 4 — full nav stack + ]) diff --git a/jetson/ros2_ws/src/saltybot_nav2_slam/maps/saltybot_map.pgm b/jetson/ros2_ws/src/saltybot_nav2_slam/maps/saltybot_map.pgm new file mode 100644 index 0000000..5f71f78 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_nav2_slam/maps/saltybot_map.pgm @@ -0,0 +1,4 @@ +P5 +200 200 +255 +þþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþ \ No newline at end of file diff --git a/jetson/ros2_ws/src/saltybot_nav2_slam/maps/saltybot_map.yaml b/jetson/ros2_ws/src/saltybot_nav2_slam/maps/saltybot_map.yaml new file mode 100644 index 0000000..3d875be --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_nav2_slam/maps/saltybot_map.yaml @@ -0,0 +1,16 @@ +# SaltyBot placeholder map — 10m × 10m all free space at 0.05 m/cell. +# +# Replace with a real map saved by slam_toolbox or RTAB-Map: +# ros2 run nav2_map_server map_saver_cli -f /mnt/nvme/saltybot/maps/my_map +# +# Then launch with: +# ros2 launch saltybot_nav2_slam nav2_amcl_bringup.launch.py \ +# map:=/mnt/nvme/saltybot/maps/my_map.yaml + +image: saltybot_map.pgm +mode: trinary +resolution: 0.05 # m/cell — matches costmap resolution +origin: [-5.0, -5.0, 0.0] # lower-left corner (x, y, yaw) +negate: 0 # 0: white=free, black=occupied +occupied_thresh: 0.65 # p ≥ 0.65 → lethal +free_thresh: 0.25 # p ≤ 0.25 → free diff --git a/jetson/ros2_ws/src/saltybot_nav2_slam/package.xml b/jetson/ros2_ws/src/saltybot_nav2_slam/package.xml index 2e1ce41..88f91e9 100644 --- a/jetson/ros2_ws/src/saltybot_nav2_slam/package.xml +++ b/jetson/ros2_ws/src/saltybot_nav2_slam/package.xml @@ -4,9 +4,10 @@ saltybot_nav2_slam 0.1.0 - Nav2 SLAM integration for SaltyBot autonomous navigation. - Combines SLAM Toolbox (RPLIDAR 2D SLAM), RealSense depth for obstacle avoidance, - VESC odometry, and DWB planner for autonomous navigation up to 20km/h. + Nav2 SLAM + AMCL integration for SaltyBot autonomous navigation (Issues #422, #655). + Combines SLAM Toolbox (RPLIDAR 2D SLAM), VESC CAN differential-drive odometry + (Issue #646), AMCL particle-filter localization on static maps, DWB local planner, + NavFn A* global planner, RealSense depth costmap, and spin/backup/wait recovery. sl-controls MIT @@ -20,6 +21,14 @@ tf2_ros nav2_bringup + nav2_amcl + nav2_map_server + nav2_bt_navigator + nav2_controller + nav2_planner + nav2_behaviors + dwb_core + nav2_navfn_planner slam_toolbox rplidar_ros realsense2_camera diff --git a/jetson/ros2_ws/src/saltybot_nav2_slam/setup.py b/jetson/ros2_ws/src/saltybot_nav2_slam/setup.py index ac86fea..0a472e9 100644 --- a/jetson/ros2_ws/src/saltybot_nav2_slam/setup.py +++ b/jetson/ros2_ws/src/saltybot_nav2_slam/setup.py @@ -14,6 +14,7 @@ setup( "launch/nav2_slam_integrated.launch.py", "launch/depth_to_costmap.launch.py", "launch/odometry_bridge.launch.py", + "launch/nav2_amcl_bringup.launch.py", ]), (f"share/{package_name}/config", [ "config/nav2_params.yaml", @@ -23,6 +24,11 @@ setup( "config/local_costmap_params.yaml", "config/dwb_local_planner_params.yaml", "config/vesc_odometry_params.yaml", + "config/amcl_nav2_params.yaml", + ]), + (f"share/{package_name}/maps", [ + "maps/saltybot_map.yaml", + "maps/saltybot_map.pgm", ]), ], install_requires=["setuptools"], diff --git a/jetson/ros2_ws/src/saltybot_nav2_slam/test/test_nav2_amcl.py b/jetson/ros2_ws/src/saltybot_nav2_slam/test/test_nav2_amcl.py new file mode 100644 index 0000000..c3d0471 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_nav2_slam/test/test_nav2_amcl.py @@ -0,0 +1,338 @@ +""" +test_nav2_amcl.py — Unit tests for Nav2 AMCL integration (Issue #655). + +Tests run without a ROS2 installation: + - amcl_nav2_params.yaml: parse, required sections, value ranges + - saltybot_map.yaml: parse, required keys, valid geometry + - saltybot_map.pgm: valid PGM header, correct dimensions + - nav2_amcl_bringup.launch.py: import (syntax) test +""" + +import os +import struct + +import pytest +import yaml + +# ── Paths ───────────────────────────────────────────────────────────────────── +_PKG_ROOT = os.path.join(os.path.dirname(__file__), "..") +_CFG = os.path.join(_PKG_ROOT, "config", "amcl_nav2_params.yaml") +_MAP_YAML = os.path.join(_PKG_ROOT, "maps", "saltybot_map.yaml") +_MAP_PGM = os.path.join(_PKG_ROOT, "maps", "saltybot_map.pgm") +_LAUNCH = os.path.join(_PKG_ROOT, "launch", "nav2_amcl_bringup.launch.py") + + +# ── amcl_nav2_params.yaml ───────────────────────────────────────────────────── + +def _load_params(): + with open(_CFG) as f: + return yaml.safe_load(f) + + +def test_params_file_exists(): + assert os.path.isfile(_CFG) + + +def test_params_top_level_sections(): + params = _load_params() + required = [ + "amcl", + "map_server", + "bt_navigator", + "controller_server", + "planner_server", + "behavior_server", + "global_costmap", + "local_costmap", + "velocity_smoother", + "lifecycle_manager_localization", + "lifecycle_manager_navigation", + ] + for section in required: + assert section in params, f"Missing section: {section}" + + +def test_amcl_params_present(): + amcl = _load_params()["amcl"]["ros__parameters"] + for key in ("min_particles", "max_particles", "alpha1", "alpha2", + "alpha3", "alpha4", "laser_model_type", "global_frame_id", + "odom_frame_id", "base_frame_id", "robot_model_type"): + assert key in amcl, f"Missing amcl param: {key}" + + +def test_amcl_particle_bounds(): + amcl = _load_params()["amcl"]["ros__parameters"] + assert amcl["min_particles"] >= 100 + assert amcl["max_particles"] >= amcl["min_particles"] + assert amcl["max_particles"] <= 10000 + + +def test_amcl_frames(): + amcl = _load_params()["amcl"]["ros__parameters"] + assert amcl["global_frame_id"] == "map" + assert amcl["odom_frame_id"] == "odom" + assert amcl["base_frame_id"] == "base_link" + + +def test_amcl_motion_model(): + amcl = _load_params()["amcl"]["ros__parameters"] + for key in ("alpha1", "alpha2", "alpha3", "alpha4"): + assert 0.0 <= amcl[key] <= 1.0, f"{key} out of [0,1]" + + +def test_amcl_laser_model(): + amcl = _load_params()["amcl"]["ros__parameters"] + assert amcl["laser_model_type"] in ("likelihood_field", "beam", "likelihood_field_prob") + z_sum = amcl["z_hit"] + amcl["z_rand"] + amcl["z_max"] + amcl["z_short"] + assert abs(z_sum - 1.0) < 1e-3, f"z weights sum {z_sum:.4f} ≠ 1.0" + + +def test_controller_server_params(): + ctrl = _load_params()["controller_server"]["ros__parameters"] + assert ctrl["controller_frequency"] > 0 + follow = ctrl["FollowPath"] + assert follow["max_vel_x"] > 0 + assert follow["max_vel_x"] <= 10.0 # sanity cap + assert follow["max_vel_theta"] > 0 + assert follow["min_vel_y"] == 0.0 # differential drive: no lateral + assert follow["vy_samples"] == 1 # differential drive + + +def test_planner_server_params(): + planner = _load_params()["planner_server"]["ros__parameters"] + assert "GridBased" in planner["planner_plugins"] + grid = planner["GridBased"] + assert "NavfnPlanner" in grid["plugin"] + assert grid["use_astar"] is True + + +def test_behavior_server_recoveries(): + behaviors = _load_params()["behavior_server"]["ros__parameters"] + plugins = behaviors["behavior_plugins"] + # Issue #655 requires: spin, backup, wait + for required in ("spin", "backup", "wait"): + assert required in plugins, f"Recovery behavior '{required}' missing" + # Check each has a plugin entry + for name in plugins: + assert name in behaviors, f"behavior_plugins entry '{name}' has no config block" + assert "plugin" in behaviors[name] + + +def test_global_costmap_layers(): + gcm = _load_params()["global_costmap"]["global_costmap"]["ros__parameters"] + plugins = gcm["plugins"] + for layer in ("static_layer", "obstacle_layer", "inflation_layer"): + assert layer in plugins, f"Global costmap missing layer: {layer}" + # Each layer has plugin field + for layer in plugins: + assert layer in gcm, f"Layer {layer} has no config block" + assert "plugin" in gcm[layer] + + +def test_global_costmap_scan_source(): + gcm = _load_params()["global_costmap"]["global_costmap"]["ros__parameters"] + obs = gcm["obstacle_layer"] + assert "scan" in obs["observation_sources"] + assert obs["scan"]["topic"] == "/scan" + assert obs["scan"]["data_type"] == "LaserScan" + + +def test_local_costmap_layers(): + lcm = _load_params()["local_costmap"]["local_costmap"]["ros__parameters"] + plugins = lcm["plugins"] + # Local costmap: obstacle + inflation (no static — rolling window) + assert "obstacle_layer" in plugins + assert "inflation_layer" in plugins + assert lcm["rolling_window"] is True + + +def test_local_costmap_size(): + lcm = _load_params()["local_costmap"]["local_costmap"]["ros__parameters"] + assert lcm["width"] >= 2 + assert lcm["height"] >= 2 + assert lcm["resolution"] == pytest.approx(0.05, abs=1e-6) + + +def test_costmap_frames(): + gcm = _load_params()["global_costmap"]["global_costmap"]["ros__parameters"] + lcm = _load_params()["local_costmap"]["local_costmap"]["ros__parameters"] + assert gcm["global_frame"] == "map" + assert gcm["robot_base_frame"] == "base_link" + assert lcm["global_frame"] == "odom" # local uses odom frame + assert lcm["robot_base_frame"] == "base_link" + + +def test_inflation_radius_larger_than_robot(): + gcm = _load_params()["global_costmap"]["global_costmap"]["ros__parameters"] + lcm = _load_params()["local_costmap"]["local_costmap"]["ros__parameters"] + robot_r = gcm["robot_radius"] + g_infl = gcm["inflation_layer"]["inflation_radius"] + l_infl = lcm["inflation_layer"]["inflation_radius"] + assert g_infl > robot_r, "Global inflation_radius must exceed robot_radius" + assert l_infl > robot_r, "Local inflation_radius must exceed robot_radius" + + +def test_lifecycle_manager_localization_nodes(): + lm = _load_params()["lifecycle_manager_localization"]["ros__parameters"] + assert "map_server" in lm["node_names"] + assert "amcl" in lm["node_names"] + + +def test_lifecycle_manager_navigation_nodes(): + lm = _load_params()["lifecycle_manager_navigation"]["ros__parameters"] + for node in ("controller_server", "planner_server", "behavior_server", "bt_navigator"): + assert node in lm["node_names"], f"lifecycle nav missing: {node}" + + +def test_velocity_smoother_limits_consistent(): + vs = _load_params()["velocity_smoother"]["ros__parameters"] + ctrl = _load_params()["controller_server"]["ros__parameters"] + max_vx_ctrl = ctrl["FollowPath"]["max_vel_x"] + max_vx_vs = vs["max_velocity"][0] + # Smoother limit should be >= planner limit (or equal) + assert max_vx_vs >= max_vx_ctrl - 1e-3, ( + f"velocity_smoother max_velocity[0]={max_vx_vs} < " + f"controller max_vel_x={max_vx_ctrl}" + ) + + +# ── saltybot_map.yaml ───────────────────────────────────────────────────────── + +def _load_map_yaml(): + with open(_MAP_YAML) as f: + return yaml.safe_load(f) + + +def test_map_yaml_exists(): + assert os.path.isfile(_MAP_YAML) + + +def test_map_yaml_required_keys(): + m = _load_map_yaml() + for key in ("image", "resolution", "origin", "negate", + "occupied_thresh", "free_thresh"): + assert key in m, f"Map YAML missing key: {key}" + + +def test_map_yaml_thresholds(): + m = _load_map_yaml() + assert 0.0 < m["free_thresh"] < m["occupied_thresh"] < 1.0 + + +def test_map_yaml_resolution(): + m = _load_map_yaml() + assert m["resolution"] == pytest.approx(0.05, abs=1e-6) + + +def test_map_yaml_origin_3d(): + m = _load_map_yaml() + assert len(m["origin"]) == 3 # [x, y, yaw] + + +def test_map_yaml_image_name(): + m = _load_map_yaml() + assert m["image"].endswith(".pgm") + + +# ── saltybot_map.pgm ───────────────────────────────────────────────────────── + +def test_pgm_exists(): + assert os.path.isfile(_MAP_PGM) + + +def test_pgm_valid_header(): + """P5 binary PGM: header 'P5\\nW H\\nMAXVAL\\n'.""" + with open(_MAP_PGM, "rb") as f: + magic = f.readline().strip() + assert magic == b"P5", f"Expected P5 PGM, got {magic}" + + +def test_pgm_dimensions(): + """Parse width and height from PGM header; verify > 0.""" + with open(_MAP_PGM, "rb") as f: + f.readline() # P5 + wh_line = f.readline() + w_str, h_str = wh_line.decode().split() + w, h = int(w_str), int(h_str) + assert w > 0 and h > 0 + assert w == 200 and h == 200 + + +def test_pgm_pixel_count(): + """File size should match header dimensions + header bytes.""" + with open(_MAP_PGM, "rb") as f: + lines = [] + for _ in range(3): + lines.append(f.readline()) + payload = f.read() + w, h = map(int, lines[1].decode().split()) + assert len(payload) == w * h, ( + f"Expected {w*h} pixels, got {len(payload)}" + ) + + +def test_pgm_all_free_space(): + """All pixels should be 254 (near-white = free).""" + with open(_MAP_PGM, "rb") as f: + for _ in range(3): + f.readline() + data = f.read() + assert all(b == 254 for b in data), "Not all pixels are free (254)" + + +# ── Launch file syntax check ────────────────────────────────────────────────── + +def test_launch_file_exists(): + assert os.path.isfile(_LAUNCH) + + +def test_launch_file_imports_cleanly(): + """Compile launch file to check for Python syntax errors.""" + import py_compile + py_compile.compile(_LAUNCH, doraise=True) + + +def test_launch_file_has_generate_function(): + """Launch file must define generate_launch_description().""" + with open(_LAUNCH) as f: + src = f.read() + assert "def generate_launch_description" in src + + +def test_launch_file_references_amcl_params(): + """Launch file must reference amcl_nav2_params.yaml.""" + with open(_LAUNCH) as f: + src = f.read() + assert "amcl_nav2_params.yaml" in src + + +def test_launch_file_references_odometry_bridge(): + """Launch file must wire in VESC odometry bridge.""" + with open(_LAUNCH) as f: + src = f.read() + assert "odometry_bridge.launch.py" in src + + +def test_launch_file_references_localization(): + """Launch file must include Nav2 localization_launch.py.""" + with open(_LAUNCH) as f: + src = f.read() + assert "localization_launch.py" in src + + +def test_launch_file_references_navigation(): + """Launch file must include Nav2 navigation_launch.py.""" + with open(_LAUNCH) as f: + src = f.read() + assert "navigation_launch.py" in src + + +def test_launch_file_has_map_argument(): + """Launch file must expose 'map' argument for map file path.""" + with open(_LAUNCH) as f: + src = f.read() + assert '"map"' in src or "'map'" in src + + +if __name__ == "__main__": + pytest.main([__file__, "-v"])