From 1d2eeecd5b82721c60c45fba3f69af062d7c9f05 Mon Sep 17 00:00:00 2001 From: Orange <2314753575@qq.com> Date: Fri, 31 Jul 2026 16:54:19 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E4=BA=86=E4=B8=80=E4=B8=8B=E9=9A=9C?= =?UTF-8?q?=E7=A2=8D=E7=89=A9=E7=9B=B8=E5=85=B3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../obstacle_nav2/config/nav2_params.yaml | 22 +- .../nav2_params.yaml.bak.20260724-205824 | 334 + .../nav2_params.yaml.bak.20260724-210811 | 334 + src/origincar_base/CMakeLists.txt | 42 +- src/origincar_base/config/wall_fit.json | 5437 +++++++++++++++++ .../config/wall_map_calibration (2).json | 5437 +++++++++++++++++ .../include/origincar_base/origincar_base.h | 47 +- .../include/origincar_base/wall_fit_core.hpp | 128 + .../origincar_base/wall_kalman_filter.hpp | 36 + .../launch/base_serial.launch.py | 7 +- .../launch/origincar_bringup.launch.py | 130 +- .../launch/wall_localization.launch.py | 33 + src/origincar_base/preview/map_reference.png | Bin 0 -> 53197 bytes .../preview/wall_localization.html | 275 + src/origincar_base/src/origincar_base.cpp | 129 +- .../src/wall_fit_calibrator.cpp | 246 + src/origincar_base/src/wall_fit_core.cpp | 605 ++ src/origincar_base/src/wall_kalman_filter.cpp | 66 + .../src/wall_localization_node.cpp | 256 + .../src/wall_preview_server.cpp | 332 + .../test_wall_fit_core.cpython-310.pyc | Bin 0 -> 4644 bytes src/origincar_base/test/test_wall_fit_core.py | 104 + .../test/wall_fit_core_test.cpp | 164 + .../test/wall_kalman_filter_test.cpp | 79 + 24 files changed, 14131 insertions(+), 112 deletions(-) create mode 100755 src/navigation/obstacle_nav2/config/nav2_params.yaml.bak.20260724-205824 create mode 100755 src/navigation/obstacle_nav2/config/nav2_params.yaml.bak.20260724-210811 create mode 100644 src/origincar_base/config/wall_fit.json create mode 100644 src/origincar_base/config/wall_map_calibration (2).json create mode 100644 src/origincar_base/include/origincar_base/wall_fit_core.hpp create mode 100644 src/origincar_base/include/origincar_base/wall_kalman_filter.hpp create mode 100644 src/origincar_base/launch/wall_localization.launch.py create mode 100644 src/origincar_base/preview/map_reference.png create mode 100644 src/origincar_base/preview/wall_localization.html create mode 100644 src/origincar_base/src/wall_fit_calibrator.cpp create mode 100644 src/origincar_base/src/wall_fit_core.cpp create mode 100644 src/origincar_base/src/wall_kalman_filter.cpp create mode 100644 src/origincar_base/src/wall_localization_node.cpp create mode 100644 src/origincar_base/src/wall_preview_server.cpp create mode 100644 src/origincar_base/test/__pycache__/test_wall_fit_core.cpython-310.pyc create mode 100644 src/origincar_base/test/test_wall_fit_core.py create mode 100644 src/origincar_base/test/wall_fit_core_test.cpp create mode 100644 src/origincar_base/test/wall_kalman_filter_test.cpp diff --git a/src/navigation/obstacle_nav2/config/nav2_params.yaml b/src/navigation/obstacle_nav2/config/nav2_params.yaml index 359391a..6033640 100755 --- a/src/navigation/obstacle_nav2/config/nav2_params.yaml +++ b/src/navigation/obstacle_nav2/config/nav2_params.yaml @@ -76,13 +76,13 @@ controller_server: time_steps: 36 model_dt: 0.05 batch_size: 1000 - vx_std: 0.75 + vx_std: 0.22 vy_std: 0.0 wz_std: 0.4 - vx_max: 1.75 + vx_max: 0.75 vx_min: -0.75 vy_max: 0.0 - wz_max: 1.5 + wz_max: 2.0 iteration_count: 1 temperature: 0.3 gamma: 0.015 @@ -109,9 +109,9 @@ controller_server: cost_weight: 3.0 threshold_to_consider: 0.5 PreferForwardCritic: - enabled: false + enabled: true cost_power: 1 - cost_weight: 0.0 + cost_weight: 3.0 threshold_to_consider: 0.5 CostCritic: enabled: true @@ -246,10 +246,10 @@ planner_server: analytic_expansion_ratio: 3.5 analytic_expansion_max_length: 3.0 minimum_turning_radius: 0.40 - reverse_penalty: 1.0 - change_penalty: 0.0 + reverse_penalty: 1.9 + change_penalty: 1.0 non_straight_penalty: 1.2 - cost_penalty: 2.0 + cost_penalty: 4.0 retrospective_penalty: 0.015 # 5 m covers the rolling planning horizon without the startup and memory # cost of the previous 20 m (401-cell) Hybrid-A* lookup table. @@ -324,10 +324,10 @@ velocity_smoother: smoothing_frequency: 20.0 scale_velocities: False feedback: "OPEN_LOOP" - max_velocity: [1.75, 0.0, 1.5] - min_velocity: [-0.75, 0.0, -1.5] + max_velocity: [0.75, 0.0, 2.5] + min_velocity: [-0.75, 0.0, -2.5] max_accel: [2.5, 0.0, 3.2] - max_decel: [-2.5, 0.0, -3.2] + max_decel: [-0.5, 0.0, -0.5] odom_topic: /odom_combined odom_duration: 0.1 deadband_velocity: [0.03, 0.0, 0.03] diff --git a/src/navigation/obstacle_nav2/config/nav2_params.yaml.bak.20260724-205824 b/src/navigation/obstacle_nav2/config/nav2_params.yaml.bak.20260724-205824 new file mode 100755 index 0000000..359391a --- /dev/null +++ b/src/navigation/obstacle_nav2/config/nav2_params.yaml.bak.20260724-205824 @@ -0,0 +1,334 @@ +# ============================================================================ +# nav2_params.yaml — Odometry-only obstacle navigation +# +# No static map, no AMCL, no SLAM. +# Both costmaps are rolling windows in odom. The launch file can rewrite every +# global_frame leaf when a different connected odometry frame is required. +# Global planner: Smac Hybrid A* (Reeds-Shepp) +# Local controller: MPPI (Ackermann) +# ============================================================================ + +bt_navigator: + ros__parameters: + use_sim_time: False + global_frame: odom + robot_base_frame: base_footprint + odom_topic: /odom_combined + bt_loop_duration: 50 + default_server_timeout: 20 + # Injected by obstacle_nav2.launch.py from this package's share directory. + default_nav_to_pose_bt_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_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_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_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_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: False + +controller_server: + ros__parameters: + use_sim_time: False + controller_frequency: 20.0 + FollowPath: + plugin: "nav2_mppi_controller::MPPIController" + time_steps: 36 + model_dt: 0.05 + batch_size: 1000 + vx_std: 0.75 + vy_std: 0.0 + wz_std: 0.4 + vx_max: 1.75 + vx_min: -0.75 + vy_max: 0.0 + wz_max: 1.5 + iteration_count: 1 + temperature: 0.3 + gamma: 0.015 + motion_model: "Ackermann" + visualize: false + TrajectoryVisualizer: + trajectory_step: 5 + time_step: 3 + AckermannConstraints: + min_turning_r: 0.4 + critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"] + ConstraintCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 + GoalCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 1.4 + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + threshold_to_consider: 0.5 + PreferForwardCritic: + enabled: false + cost_power: 1 + cost_weight: 0.0 + threshold_to_consider: 0.5 + CostCritic: + enabled: true + cost_power: 1 + cost_weight: 3.81 + critical_cost: 300.0 + consider_footprint: true + collision_cost: 1000000.0 + near_goal_distance: 1.0 + trajectory_point_step: 2 + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 14.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 4 + threshold_to_consider: 0.5 + offset_from_furthest: 20 + use_path_orientations: false + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + offset_from_furthest: 5 + threshold_to_consider: 1.4 + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + offset_from_furthest: 4 + threshold_to_consider: 0.5 + max_angle_to_furthest: 1.0 + forward_preference: false + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: False + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + transform_tolerance: 0.5 + global_frame: odom + robot_base_frame: base_footprint + use_sim_time: False + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]" + footprint_padding: 0.02 + track_unknown_space: false + plugins: ["obstacle_array_layer", "inflation_layer"] + obstacle_array_layer: + plugin: "obstacle_nav2::ObstacleArrayLayer" + enabled: true + topic: /obstacles + obstacle_timeout: 0.5 + transform_tolerance: 0.2 + default_obstacle_radius: 0.05 + minimum_obstacle_radius: 0.02 + maximum_obstacle_radius: 0.50 + extra_inflation: 0.02 + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + local_costmap_client: + ros__parameters: + use_sim_time: False + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: False + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + transform_tolerance: 0.5 + global_frame: odom + robot_base_frame: base_footprint + use_sim_time: False + rolling_window: true + width: 10 + height: 10 + resolution: 0.05 + footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]" + footprint_padding: 0.02 + track_unknown_space: false + plugins: ["obstacle_array_layer", "inflation_layer"] + obstacle_array_layer: + plugin: "obstacle_nav2::ObstacleArrayLayer" + enabled: true + topic: /obstacles + obstacle_timeout: 0.5 + transform_tolerance: 0.2 + default_obstacle_radius: 0.05 + minimum_obstacle_radius: 0.02 + maximum_obstacle_radius: 0.50 + extra_inflation: 0.02 + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + global_costmap_client: + ros__parameters: + use_sim_time: False + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: False + +planner_server: + ros__parameters: + planner_plugins: ["GridBased"] + use_sim_time: False + GridBased: + plugin: "nav2_smac_planner/SmacPlannerHybrid" + downsample_costmap: false + downsampling_factor: 1 + tolerance: 0.25 + allow_unknown: false + max_iterations: 1000000 + max_on_approach_iterations: 1000 + max_planning_time: 5.0 + motion_model_for_search: "REEDS_SHEPP" + angle_quantization_bins: 72 + analytic_expansion_ratio: 3.5 + analytic_expansion_max_length: 3.0 + minimum_turning_radius: 0.40 + reverse_penalty: 1.0 + change_penalty: 0.0 + non_straight_penalty: 1.2 + cost_penalty: 2.0 + retrospective_penalty: 0.015 + # 5 m covers the rolling planning horizon without the startup and memory + # cost of the previous 20 m (401-cell) Hybrid-A* lookup table. + lookup_table_size: 5.0 + cache_obstacle_heuristic: false + viz_expansions: 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 + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: False + +smoother_server: + ros__parameters: + use_sim_time: False + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + backup_dist: 0.8 + backup_speed: 0.18 + wait: + plugin: "nav2_behaviors/Wait" + wait_duration: 0.5 + global_frame: odom + robot_base_frame: base_footprint + transform_tolerance: 0.5 + use_sim_time: False + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: False + +waypoint_follower: + ros__parameters: + loop_rate: 20 + use_sim_time: False + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: False + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [1.75, 0.0, 1.5] + min_velocity: [-0.75, 0.0, -1.5] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: /odom_combined + odom_duration: 0.1 + deadband_velocity: [0.03, 0.0, 0.03] + velocity_timeout: 1.0 diff --git a/src/navigation/obstacle_nav2/config/nav2_params.yaml.bak.20260724-210811 b/src/navigation/obstacle_nav2/config/nav2_params.yaml.bak.20260724-210811 new file mode 100755 index 0000000..cda8b34 --- /dev/null +++ b/src/navigation/obstacle_nav2/config/nav2_params.yaml.bak.20260724-210811 @@ -0,0 +1,334 @@ +# ============================================================================ +# nav2_params.yaml — Odometry-only obstacle navigation +# +# No static map, no AMCL, no SLAM. +# Both costmaps are rolling windows in odom. The launch file can rewrite every +# global_frame leaf when a different connected odometry frame is required. +# Global planner: Smac Hybrid A* (Reeds-Shepp) +# Local controller: MPPI (Ackermann) +# ============================================================================ + +bt_navigator: + ros__parameters: + use_sim_time: False + global_frame: odom + robot_base_frame: base_footprint + odom_topic: /odom_combined + bt_loop_duration: 50 + default_server_timeout: 20 + # Injected by obstacle_nav2.launch.py from this package's share directory. + default_nav_to_pose_bt_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_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_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_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_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: False + +controller_server: + ros__parameters: + use_sim_time: False + controller_frequency: 20.0 + FollowPath: + plugin: "nav2_mppi_controller::MPPIController" + time_steps: 36 + model_dt: 0.05 + batch_size: 1000 + vx_std: 0.55 + vy_std: 0.0 + wz_std: 0.4 + vx_max: 1.75 + vx_min: -0.75 + vy_max: 0.0 + wz_max: 1.5 + iteration_count: 1 + temperature: 0.3 + gamma: 0.015 + motion_model: "Ackermann" + visualize: false + TrajectoryVisualizer: + trajectory_step: 5 + time_step: 3 + AckermannConstraints: + min_turning_r: 0.4 + critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"] + ConstraintCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 + GoalCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 1.4 + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + threshold_to_consider: 0.5 + PreferForwardCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + threshold_to_consider: 0.5 + CostCritic: + enabled: true + cost_power: 1 + cost_weight: 3.81 + critical_cost: 300.0 + consider_footprint: true + collision_cost: 1000000.0 + near_goal_distance: 1.0 + trajectory_point_step: 2 + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 14.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 4 + threshold_to_consider: 0.5 + offset_from_furthest: 20 + use_path_orientations: false + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + offset_from_furthest: 5 + threshold_to_consider: 1.4 + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + offset_from_furthest: 4 + threshold_to_consider: 0.5 + max_angle_to_furthest: 1.0 + forward_preference: false + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: False + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + transform_tolerance: 0.5 + global_frame: odom + robot_base_frame: base_footprint + use_sim_time: False + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]" + footprint_padding: 0.02 + track_unknown_space: false + plugins: ["obstacle_array_layer", "inflation_layer"] + obstacle_array_layer: + plugin: "obstacle_nav2::ObstacleArrayLayer" + enabled: true + topic: /obstacles + obstacle_timeout: 0.5 + transform_tolerance: 0.2 + default_obstacle_radius: 0.05 + minimum_obstacle_radius: 0.02 + maximum_obstacle_radius: 0.50 + extra_inflation: 0.02 + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + local_costmap_client: + ros__parameters: + use_sim_time: False + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: False + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + transform_tolerance: 0.5 + global_frame: odom + robot_base_frame: base_footprint + use_sim_time: False + rolling_window: true + width: 10 + height: 10 + resolution: 0.05 + footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]" + footprint_padding: 0.02 + track_unknown_space: false + plugins: ["obstacle_array_layer", "inflation_layer"] + obstacle_array_layer: + plugin: "obstacle_nav2::ObstacleArrayLayer" + enabled: true + topic: /obstacles + obstacle_timeout: 0.5 + transform_tolerance: 0.2 + default_obstacle_radius: 0.05 + minimum_obstacle_radius: 0.02 + maximum_obstacle_radius: 0.50 + extra_inflation: 0.02 + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + global_costmap_client: + ros__parameters: + use_sim_time: False + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: False + +planner_server: + ros__parameters: + planner_plugins: ["GridBased"] + use_sim_time: False + GridBased: + plugin: "nav2_smac_planner/SmacPlannerHybrid" + downsample_costmap: false + downsampling_factor: 1 + tolerance: 0.25 + allow_unknown: false + max_iterations: 1000000 + max_on_approach_iterations: 1000 + max_planning_time: 5.0 + motion_model_for_search: "REEDS_SHEPP" + angle_quantization_bins: 72 + analytic_expansion_ratio: 3.5 + analytic_expansion_max_length: 3.0 + minimum_turning_radius: 0.40 + reverse_penalty: 1.25 + change_penalty: 0.0 + non_straight_penalty: 1.2 + cost_penalty: 2.0 + retrospective_penalty: 0.015 + # 5 m covers the rolling planning horizon without the startup and memory + # cost of the previous 20 m (401-cell) Hybrid-A* lookup table. + lookup_table_size: 5.0 + cache_obstacle_heuristic: false + viz_expansions: 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 + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: False + +smoother_server: + ros__parameters: + use_sim_time: False + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + backup_dist: 0.8 + backup_speed: 0.18 + wait: + plugin: "nav2_behaviors/Wait" + wait_duration: 0.5 + global_frame: odom + robot_base_frame: base_footprint + transform_tolerance: 0.5 + use_sim_time: False + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: False + +waypoint_follower: + ros__parameters: + loop_rate: 20 + use_sim_time: False + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: False + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [1.75, 0.0, 1.5] + min_velocity: [-0.75, 0.0, -1.5] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: /odom_combined + odom_duration: 0.1 + deadband_velocity: [0.03, 0.0, 0.03] + velocity_timeout: 1.0 diff --git a/src/origincar_base/CMakeLists.txt b/src/origincar_base/CMakeLists.txt index 5bcfd79..bc4474c 100644 --- a/src/origincar_base/CMakeLists.txt +++ b/src/origincar_base/CMakeLists.txt @@ -67,6 +67,43 @@ set(origincar_base_node_SRCS add_executable(origincar_base_node src/origincar_base.cpp) ament_target_dependencies(origincar_base_node tf2_ros tf2 tf2_geometry_msgs rclcpp std_msgs geometry_msgs robot_localization nav_msgs std_srvs sensor_msgs ackermann_msgs serial origincar_msg origincar_description) +add_library(wall_fit_core STATIC src/wall_fit_core.cpp) +target_include_directories(wall_fit_core PUBLIC + $ + $ +) + +add_library(wall_kalman_filter STATIC src/wall_kalman_filter.cpp) +target_include_directories(wall_kalman_filter PUBLIC + $ + $ +) +target_link_libraries(wall_kalman_filter wall_fit_core) + +target_link_libraries(origincar_base_node wall_kalman_filter wall_fit_core) + +add_executable(wall_fit_calibrator src/wall_fit_calibrator.cpp) +target_link_libraries(wall_fit_calibrator wall_fit_core) +ament_target_dependencies(wall_fit_calibrator rclcpp sensor_msgs nav_msgs) + +add_executable(wall_localization_node src/wall_localization_node.cpp) +target_link_libraries(wall_localization_node wall_fit_core) +ament_target_dependencies(wall_localization_node rclcpp sensor_msgs nav_msgs geometry_msgs tf2_ros) + +add_executable(wall_preview_server src/wall_preview_server.cpp) +target_link_libraries(wall_preview_server wall_fit_core pthread) +ament_target_dependencies(wall_preview_server rclcpp sensor_msgs nav_msgs geometry_msgs) + +if(BUILD_TESTING) + add_executable(wall_fit_core_test test/wall_fit_core_test.cpp) + target_link_libraries(wall_fit_core_test wall_fit_core) + add_test(NAME wall_fit_core_test COMMAND wall_fit_core_test) + + add_executable(wall_kalman_filter_test test/wall_kalman_filter_test.cpp) + target_link_libraries(wall_kalman_filter_test wall_kalman_filter) + add_test(NAME wall_kalman_filter_test COMMAND wall_kalman_filter_test) +endif() + #add_executable(testNode src/test.cpp src/Quaternion_Solution.cpp) #ament_target_dependencies(testNode rclcpp std_msgs nav_msgs std_srvs sensor_msgs ackermann_msgs serial origincar_msg) @@ -77,6 +114,9 @@ install(PROGRAMS install(TARGETS origincar_base_node +wall_fit_calibrator +wall_localization_node +wall_preview_server DESTINATION lib/${PROJECT_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} @@ -85,7 +125,7 @@ DESTINATION lib/${PROJECT_NAME} ) install( - DIRECTORY launch config + DIRECTORY launch config preview DESTINATION share/${PROJECT_NAME} ) ament_package() diff --git a/src/origincar_base/config/wall_fit.json b/src/origincar_base/config/wall_fit.json new file mode 100644 index 0000000..21d76b4 --- /dev/null +++ b/src/origincar_base/config/wall_fit.json @@ -0,0 +1,5437 @@ +{ + "coordinate_frame": { + "origin": "map_lower_left", + "origin_note": "map (0,0) is the lower-left map corner; odom zero is placed at the P point", + "width_m": 5, + "height_m": 5, + "image_width_px": 500, + "image_height_px": 500, + "pixels_per_meter": 100, + "p_image_px": { + "x": 56, + "y": 482 + }, + "odom_zero_reference": "P", + "odom_zero_map": { + "x": 0.56, + "y": 0.18 + } + }, + "walls": [ + { + "name": "top", + "a": 0, + "b": -1, + "c": 5.517135597708222, + "color": "#b36cff" + }, + { + "name": "bottom", + "a": 0, + "b": 1, + "c": 0.24605343179761974, + "color": "#24c7d9" + }, + { + "name": "left", + "a": 1, + "b": 0, + "c": 0.1353930847310167, + "color": "#35c56f" + }, + { + "name": "right", + "a": -1, + "b": 0, + "c": 5.102447144358728, + "color": "#ff5b5b" + } + ], + "reference_points": { + "P": { + "label": "P", + "image": { + "x": 56, + "y": 482 + }, + "map": { + "x": 0.56, + "y": 0.18 + } + }, + "A": { + "label": "任务发布点左中点", + "image": { + "x": 450, + "y": 319 + }, + "map": { + "x": 4.5, + "y": 1.81 + } + }, + "B": { + "label": "通道", + "image": { + "x": 250, + "y": 240 + }, + "map": { + "x": 2.5, + "y": 2.6 + } + }, + "C": { + "label": "诊疗室", + "image": { + "x": 280, + "y": 123 + }, + "map": { + "x": 2.8, + "y": 3.77 + } + } + }, + "fit_quality": { + "left": { + "point_count": 313 + }, + "right": { + "point_count": 411 + }, + "bottom": { + "point_count": 283 + }, + "top": { + "point_count": 65 + } + }, + "calibration_captures": [ + { + "key": "P", + "label": "P", + "image": { + "x": 56, + "y": 482 + }, + "map": { + "x": 0.56, + "y": 0.18 + }, + "odom_pose": { + "frame_id": "odom", + "stamp": { + "sec": 1784882167, + "nanosec": 287620295 + }, + "x": 0.016151936730415286, + "y": -0.0026330311461741354, + "theta": -0.07343995175124807 + }, + "scan": { + "frame_id": "pose", + "points": [ + { + "x": 4.438370352446971, + "y": 0.07477142498150946 + }, + { + "x": 4.421489368074988, + "y": 0.14901636776935934 + }, + { + "x": 4.41835201299862, + "y": 0.22347169640554312 + }, + { + "x": 4.413960962265093, + "y": 0.29786361554097696 + }, + { + "x": 4.4083174618233985, + "y": 0.3721710166660462 + }, + { + "x": 4.401423113001808, + "y": 0.446372815252897 + }, + { + "x": 4.392286887952563, + "y": 0.5203303233158304 + }, + { + "x": 4.382899187820247, + "y": 0.5942410797366631 + }, + { + "x": 4.387096140386448, + "y": 0.670248647553117 + }, + { + "x": 4.390957251751719, + "y": 0.7467331379466586 + }, + { + "x": 4.377756165000672, + "y": 0.8205893771048233 + }, + { + "x": 4.393681850652673, + "y": 0.9004365568264803 + }, + { + "x": 4.378867413265467, + "y": 0.974534106457655 + }, + { + "x": 4.391972992278944, + "y": 1.0553975682722996 + }, + { + "x": 4.43457185902176, + "y": 1.1449767370212158 + }, + { + "x": 4.414656487970359, + "y": 1.219511129251669 + }, + { + "x": 4.42322618780224, + "y": 1.302456008626271 + }, + { + "x": 4.5638593004597405, + "y": 1.4278350415335561 + }, + { + "x": 4.5543483580888315, + "y": 1.5095408834336665 + }, + { + "x": 4.528275200950408, + "y": 1.5860410934278057 + }, + { + "x": 4.515926723329014, + "y": 1.6676339761085675 + }, + { + "x": 4.516091276913968, + "y": 1.7546915813152753 + }, + { + "x": 4.5145966231036185, + "y": 1.8422249113080043 + }, + { + "x": 4.497635730475038, + "y": 1.9243021531844242 + }, + { + "x": 4.492875504272423, + "y": 2.012460512260303 + }, + { + "x": 4.486413959728238, + "y": 2.1010011290238695 + }, + { + "x": 4.478236498240625, + "y": 2.1898913664755564 + }, + { + "x": 4.482582432781556, + "y": 2.2863681650091086 + }, + { + "x": 4.470808640000275, + "y": 2.3760978834557793 + }, + { + "x": 4.4572757320751455, + "y": 2.466075353501005 + }, + { + "x": 4.4827090055600705, + "y": 2.579709918034299 + }, + { + "x": 4.438619866327283, + "y": 2.654851591328348 + }, + { + "x": 4.354197324891375, + "y": 2.7049659725866335 + }, + { + "x": 4.320622836715916, + "y": 2.7860540527459654 + }, + { + "x": 4.312146650621412, + "y": 2.884568834564953 + }, + { + "x": 4.301566146979226, + "y": 2.9835809157886315 + }, + { + "x": 4.339205097101707, + "y": 3.119236069539007 + }, + { + "x": 4.323743785438062, + "y": 3.219956281824681 + }, + { + "x": 4.367874694318318, + "y": 3.3686680427450444 + }, + { + "x": 4.346459465387926, + "y": 3.4704656556002753 + }, + { + "x": 4.347510788323729, + "y": 3.5928746762677832 + }, + { + "x": 4.357814716984588, + "y": 3.7266887788984713 + }, + { + "x": 4.340857492675963, + "y": 3.840647376745088 + }, + { + "x": 4.402428585239526, + "y": 4.029348058658877 + }, + { + "x": 4.345552154405537, + "y": 4.113931810163297 + }, + { + "x": 4.286357311188622, + "y": 4.197039774701435 + }, + { + "x": 4.236834896933111, + "y": 4.290703041043336 + }, + { + "x": 4.239919558570812, + "y": 4.441022460795691 + }, + { + "x": 4.217416814707337, + "y": 4.569126678540555 + }, + { + "x": 4.201773194052299, + "y": 4.708908725880537 + }, + { + "x": 4.223746532319315, + "y": 4.897147570719015 + }, + { + "x": 4.180996197286304, + "y": 5.015991780134269 + }, + { + "x": 4.754506401752304, + "y": 6.793521166455235 + }, + { + "x": 4.587366909948152, + "y": 6.795562648782882 + }, + { + "x": 4.429704096865123, + "y": 6.806494495924331 + }, + { + "x": 4.289456106391127, + "y": 6.840325145906352 + }, + { + "x": 3.1623703525327826, + "y": 5.659390442568065 + }, + { + "x": 3.059025531327694, + "y": 5.697758497634303 + }, + { + "x": 2.926884475937348, + "y": 5.679143199868245 + }, + { + "x": 2.8170731822479795, + "y": 5.69984738071638 + }, + { + "x": 2.6941336654384482, + "y": 5.690453395010441 + }, + { + "x": 2.578506833584503, + "y": 5.6922143683283455 + }, + { + "x": 2.4512765982722153, + "y": 5.663257374148202 + }, + { + "x": 2.3257623139928953, + "y": 5.6316496578311845 + }, + { + "x": 2.2133656004583413, + "y": 5.626289075832756 + }, + { + "x": 2.112675519466481, + "y": 5.647787591591576 + }, + { + "x": 1.9704083491688513, + "y": 5.550638644762062 + }, + { + "x": 1.851780844119308, + "y": 5.509106039569819 + }, + { + "x": 1.7493410644852143, + "y": 5.509969911705946 + }, + { + "x": 1.6474001453645855, + "y": 5.5089538717484965 + }, + { + "x": 1.5500473235839225, + "y": 5.5205173359917605 + }, + { + "x": 1.4410862675493534, + "y": 5.485877649979994 + }, + { + "x": 1.3484765715587474, + "y": 5.509373299843324 + }, + { + "x": 1.2484011075465142, + "y": 5.500099378068925 + }, + { + "x": 1.152505873174772, + "y": 5.505665737428822 + }, + { + "x": 1.0507502390361347, + "y": 5.478139164094614 + }, + { + "x": 0.9530003027861944, + "y": 5.464521814498036 + }, + { + "x": 0.8560087721936243, + "y": 5.449174497977252 + }, + { + "x": 0.7618839575268103, + "y": 5.446974649772412 + }, + { + "x": 0.6662493861264833, + "y": 5.428265952712744 + }, + { + "x": 0.5730386741183917, + "y": 5.4228070615521835 + }, + { + "x": 0.47887654148551695, + "y": 5.400811097734903 + }, + { + "x": 0.38669176912648967, + "y": 5.392152349525381 + }, + { + "x": 0.2966858832244066, + "y": 5.413876683310858 + }, + { + "x": 0.20605763150337683, + "y": 5.434094814354785 + }, + { + "x": 0.1144953942230397, + "y": 5.436794736202579 + }, + { + "x": 0.022900669178468336, + "y": 5.437951981973502 + }, + { + "x": -0.0687005538702029, + "y": 5.437566223302139 + }, + { + "x": -0.1602822833190482, + "y": 5.435637569646489 + }, + { + "x": -0.251818533095399, + "y": 5.432166568256909 + }, + { + "x": -0.3422732847844765, + "y": 5.411185845443165 + }, + { + "x": -0.43337184499153353, + "y": 5.404652819503789 + }, + { + "x": -0.525894781446996, + "y": 5.412511512943429 + }, + { + "x": -0.6169896204120656, + "y": 5.40288534092684 + }, + { + "x": -0.7077791593138293, + "y": 5.390734223011471 + }, + { + "x": -0.7961316322644182, + "y": 5.362221131720226 + }, + { + "x": -0.8863410302802703, + "y": 5.348050169865711 + }, + { + "x": -0.9734174368636083, + "y": 5.316623530177606 + }, + { + "x": -0.7770588278607213, + "y": 1.4770716559219574 + }, + { + "x": -0.786935508910348, + "y": 1.4365850278722998 + }, + { + "x": -0.7877508874010696, + "y": 1.3822913639969578 + }, + { + "x": -0.8109227243982758, + "y": 1.368826216375264 + }, + { + "x": -0.698058527000409, + "y": 0.9797566898324197 + }, + { + "x": -0.6877371991839903, + "y": 0.9316552619874384 + }, + { + "x": -0.6838968626964522, + "y": 0.8945172924153035 + }, + { + "x": -0.6796266842032345, + "y": 0.8585642857557728 + }, + { + "x": -0.674344819516951, + "y": 0.8230158483979877 + }, + { + "x": -0.6777646221466184, + "y": 0.7993366443443718 + }, + { + "x": -0.6812404982802301, + "y": 0.7765309831169486 + }, + { + "x": -0.6834711309924748, + "y": 0.7530976906281014 + }, + { + "x": -0.6864775972111706, + "y": 0.7312711881475524 + }, + { + "x": -0.6875521340563108, + "y": 0.7081250174679438 + }, + { + "x": -0.6880449042561416, + "y": 0.685153442876662 + }, + { + "x": -0.6886824336653506, + "y": 0.663062929920371 + }, + { + "x": -0.6880421591648114, + "y": 0.6404670036227901 + }, + { + "x": -0.6987327011398482, + "y": 0.628786615534668 + }, + { + "x": -0.6979075704431539, + "y": 0.6070832275494465 + }, + { + "x": -0.6957872836364909, + "y": 0.5849453125322791 + }, + { + "x": -0.7055415080755978, + "y": 0.5731423410516305 + }, + { + "x": -0.715095536984331, + "y": 0.5611767419528572 + }, + { + "x": -0.7116951444465156, + "y": 0.539387646590177 + }, + { + "x": -0.720679729347353, + "y": 0.5273231832857932 + }, + { + "x": -0.71720687050774, + "y": 0.5064566533661499 + }, + { + "x": -0.7256359770627634, + "y": 0.4943040214630215 + }, + { + "x": -0.7204858714193273, + "y": 0.4732273125629146 + }, + { + "x": -0.728354790965884, + "y": 0.461024162867229 + }, + { + "x": -0.7360170417735945, + "y": 0.4486901987889113 + }, + { + "x": -0.7434704496991282, + "y": 0.4362289200575336 + }, + { + "x": -0.7376494626091232, + "y": 0.41627187649304925 + }, + { + "x": -0.7445565780939661, + "y": 0.4037877002381036 + }, + { + "x": -0.7512524276258655, + "y": 0.3911889502948883 + }, + { + "x": -0.6584333425014968, + "y": 0.32887921124330666 + }, + { + "x": -0.663879636762259, + "y": 0.31774176621609546 + }, + { + "x": -0.6545911130478019, + "y": 0.29985082277287656 + }, + { + "x": -0.8603999335743518, + "y": 0.29324548935829076 + }, + { + "x": -0.8499880295922988, + "y": 0.27380533499666826 + }, + { + "x": -0.8544794716640641, + "y": 0.2594491172225067 + }, + { + "x": -5.648567730810799, + "y": 1.6116945870508923 + }, + { + "x": -5.64496460317401, + "y": 1.5083178945690134 + }, + { + "x": -5.639490143466505, + "y": 1.4055221704462744 + }, + { + "x": -5.632163402812239, + "y": 1.3033410880585135 + }, + { + "x": -5.623002637771807, + "y": 1.2018075285216403 + }, + { + "x": -0.6260664568184171, + "y": 0.12282021922099895 + }, + { + "x": -0.6122960306723176, + "y": 0.10944196965865516 + }, + { + "x": -0.5992442954583053, + "y": 0.09672262535909042 + }, + { + "x": -0.6007884941291002, + "y": 0.08661510963892993 + }, + { + "x": -0.6170426568951662, + "y": 0.07837304046415401 + }, + { + "x": -1.0796553004950444, + "y": 0.027285981933966995 + }, + { + "x": -1.0799617364856797, + "y": 0.009096176338476482 + }, + { + "x": -1.0799617361998601, + "y": -0.009096210272908363 + }, + { + "x": -1.063660374530705, + "y": -0.026881777783641023 + }, + { + "x": -0.8922082719374149, + "y": -0.037595364199434364 + }, + { + "x": -0.8764745037256014, + "y": -0.05173471910919518 + }, + { + "x": -0.8754787248914699, + "y": -0.06649089490112607 + }, + { + "x": -0.8891701863610557, + "y": -0.0826159283036993 + }, + { + "x": -0.9035565968120592, + "y": -0.0993299505745489 + }, + { + "x": -0.9017552719403759, + "y": -0.11453555016444117 + }, + { + "x": -0.8838618331762748, + "y": -0.12742555386229032 + }, + { + "x": -0.8815900543919115, + "y": -0.1422954248038316 + }, + { + "x": -0.8643021589891111, + "y": -0.1544856459646263 + }, + { + "x": -0.8458765918585531, + "y": -0.16594207307414433 + }, + { + "x": -0.8429614220621536, + "y": -0.18016664837127194 + }, + { + "x": -0.8398070641995062, + "y": -0.1943401018600956 + }, + { + "x": -0.8364144133112825, + "y": -0.20845841186080488 + }, + { + "x": -0.8327844320531679, + "y": -0.22251757234040778 + }, + { + "x": -0.8289181504227099, + "y": -0.23651359404943204 + }, + { + "x": -0.8401265187933946, + "y": -0.2550910999279248 + }, + { + "x": -0.8357105203613346, + "y": -0.2692061719198388 + }, + { + "x": -0.8443088742155634, + "y": -0.28776128961106884 + }, + { + "x": -0.8543974146712835, + "y": -0.3073581688324656 + }, + { + "x": -1.6446600963798073, + "y": -0.7533752203133983 + }, + { + "x": -1.6596990938275384, + "y": -0.7943544805572009 + }, + { + "x": -1.646083357080201, + "y": -0.8221980931483777 + }, + { + "x": -1.617454664193721, + "y": -0.8771775943964998 + }, + { + "x": -1.5885154570244788, + "y": -0.8964344071226373 + }, + { + "x": -1.560252955811662, + "y": -0.9154735217838199 + }, + { + "x": -1.5309495771541286, + "y": -0.9332964859324314 + }, + { + "x": -1.5023373639093272, + "y": -0.950929248066425 + }, + { + "x": -1.459360045638734, + "y": -0.9585324392141216 + }, + { + "x": -0.6012121109261549, + "y": -0.4245468169488846 + }, + { + "x": -0.5810631690255987, + "y": -0.4251654205137804 + }, + { + "x": -0.5738191582638535, + "y": -0.4348926474523269 + }, + { + "x": -0.566412327838896, + "y": -0.4444964747563858 + }, + { + "x": -0.5456498185883514, + "y": -0.4432553307211626 + }, + { + "x": -0.5381061219057736, + "y": -0.45238348146871665 + }, + { + "x": -0.5432361626767386, + "y": -0.4725404879573637 + }, + { + "x": -0.5351995232575455, + "y": -0.48162382779886137 + }, + { + "x": -2.771233791904495, + "y": -2.952060727811548 + }, + { + "x": -2.711034907375421, + "y": -2.987213598769834 + }, + { + "x": -2.2080988872827905, + "y": -2.694912814553305 + }, + { + "x": -2.133220807745767, + "y": -2.6948725822405173 + }, + { + "x": -0.5332694584353413, + "y": -0.6975010548683237 + }, + { + "x": -0.5214449512852002, + "y": -0.7063845975256835 + }, + { + "x": -0.7526034438185167, + "y": -1.0563130158554157 + }, + { + "x": -0.7347039169814568, + "y": -1.0688401589972 + }, + { + "x": -1.7329528734403687, + "y": -3.040867366136637 + }, + { + "x": -1.6737992131280415, + "y": -3.0555935541552417 + }, + { + "x": -1.6146433528673216, + "y": -3.0691937692564792 + }, + { + "x": -1.534778301819219, + "y": -3.040606991316603 + }, + { + "x": -1.4554714168122058, + "y": -3.0084160293811664 + }, + { + "x": -0.3174275539953937, + "y": -0.7506428871828992 + }, + { + "x": -0.30473853724972216, + "y": -0.7558832052828511 + }, + { + "x": -1.1585380400787237, + "y": -3.01936175374302 + }, + { + "x": -1.1236105141547665, + "y": -3.0826059996586848 + }, + { + "x": -1.0509521823054824, + "y": -3.041549455973664 + }, + { + "x": -0.9855928133471479, + "y": -3.0160464889626986 + }, + { + "x": -0.934944622669189, + "y": -3.0331756806924335 + }, + { + "x": -0.9007044288873782, + "y": -3.1070815652397448 + }, + { + "x": -0.6531581660907555, + "y": -2.4038438169917375 + }, + { + "x": -0.6125746638332117, + "y": -2.414504704416637 + }, + { + "x": -0.14093816870782183, + "y": -0.8504001486780628 + }, + { + "x": -0.12894363427496874, + "y": -0.8684800377799767 + }, + { + "x": -0.4743694634909472, + "y": -3.6129918352147583 + }, + { + "x": -0.41162884454871423, + "y": -3.6045729076511175 + }, + { + "x": -0.29041218651704137, + "y": -2.988924537008803 + }, + { + "x": -0.23874607251164875, + "y": -2.9774434074375384 + }, + { + "x": -0.017389929876423727, + "y": -0.5897436387042782 + }, + { + "x": -0.0076684682749231705, + "y": -0.6069515520121784 + }, + { + "x": 0.042972678728891205, + "y": -2.040547450393311 + }, + { + "x": 0.07790636713898542, + "y": -2.0545234421709946 + }, + { + "x": 0.11250212683353686, + "y": -2.052919688621276 + }, + { + "x": 0.14821044888355664, + "y": -2.0666924719218565 + }, + { + "x": 0.184414351519855, + "y": -2.079840280856018 + }, + { + "x": 0.21942144253798354, + "y": -2.076438893267095 + }, + { + "x": 0.2581427716448462, + "y": -2.1032173286690945 + }, + { + "x": 0.29561105064730486, + "y": -2.1134262171587186 + }, + { + "x": 0.333651083885392, + "y": -2.123953239669383 + }, + { + "x": 0.48655081909659287, + "y": -2.7898911085677587 + }, + { + "x": 0.5334752860951505, + "y": -2.7812997350797524 + }, + { + "x": 0.5835266269609171, + "y": -2.7875797671274483 + }, + { + "x": 0.6370388291048241, + "y": -2.8066110615791495 + }, + { + "x": 0.7312141845117431, + "y": -2.7835606580663037 + }, + { + "x": 0.7823225284422262, + "y": -2.786253357217235 + }, + { + "x": 0.13265153855032194, + "y": -0.44359054191752245 + }, + { + "x": 0.14010464849384696, + "y": -0.44129319848470855 + }, + { + "x": 0.14751800411520669, + "y": -0.43887063927803904 + }, + { + "x": 0.1548895018944189, + "y": -0.4363235516922592 + }, + { + "x": 0.15661127647122913, + "y": -0.4186668189756044 + }, + { + "x": 0.16364116687809177, + "y": -0.4159694288428848 + }, + { + "x": 0.1706246245381647, + "y": -0.4131540084887349 + }, + { + "x": 0.1775596679138713, + "y": -0.41022135678120786 + }, + { + "x": 0.19063374266284863, + "y": -0.4208358202599808 + }, + { + "x": 0.19812324775161422, + "y": -0.41846884980700927 + }, + { + "x": 0.20514391406952204, + "y": -0.41507225163661005 + }, + { + "x": 0.21210637134627497, + "y": -0.41155787781643166 + }, + { + "x": 0.21900864400309936, + "y": -0.40792672554071824 + }, + { + "x": 0.234141272110168, + "y": -0.41902011216044244 + }, + { + "x": 0.24116611399435964, + "y": -0.4150167408203879 + }, + { + "x": 0.2481225256337724, + "y": -0.410895609581601 + }, + { + "x": 0.2550085331650663, + "y": -0.406657887804133 + }, + { + "x": 0.2618221827018801, + "y": -0.40230477793029107 + }, + { + "x": 0.26856154088924156, + "y": -0.3978375151434487 + }, + { + "x": 0.28382547626313676, + "y": -0.4055466727081995 + }, + { + "x": 0.290616319900315, + "y": -0.4007083220102693 + }, + { + "x": 0.29732470192803184, + "y": -0.39575627138946884 + }, + { + "x": 0.3137733071307587, + "y": -0.4033203302932187 + }, + { + "x": 0.32052240099273993, + "y": -0.3979778480738013 + }, + { + "x": 0.3271805474741222, + "y": -0.39252244069624626 + }, + { + "x": 0.33374585734351225, + "y": -0.386955656117918 + }, + { + "x": 0.3508690601844962, + "y": -0.39321738664454065 + }, + { + "x": 0.35744271508909703, + "y": -0.3872514893245065 + }, + { + "x": 0.3639149465719624, + "y": -0.38117571042216153 + }, + { + "x": 0.3815259153342607, + "y": -0.38637670386027323 + }, + { + "x": 0.3994122098900264, + "y": -0.3910893806409117 + }, + { + "x": 0.4059431332318476, + "y": -0.3843061144988938 + }, + { + "x": 0.4123588712870078, + "y": -0.3774138025168546 + }, + { + "x": 0.42989169644002656, + "y": -0.3803539541357946 + }, + { + "x": 0.44839737451245265, + "y": -0.38345764248950426 + }, + { + "x": 0.4678970124978589, + "y": -0.3866799420596498 + }, + { + "x": 0.48065628599629034, + "y": -0.3707000494037302 + }, + { + "x": 0.48683224088214494, + "y": -0.36255118415658905 + }, + { + "x": 0.5050497024099491, + "y": -0.36305477822976484 + }, + { + "x": 0.5242405426588211, + "y": -0.36361499973934786 + }, + { + "x": 0.5427586160314681, + "y": -0.36307310694557 + }, + { + "x": 0.5630845258419624, + "y": -0.36309205323305455 + }, + { + "x": 0.5827115528168717, + "y": -0.36199893897758517 + }, + { + "x": 0.6033159138268642, + "y": -0.3608585884546517 + }, + { + "x": 0.6240430613818542, + "y": -0.3591243499679389 + }, + { + "x": 0.6440037476748436, + "y": -0.35630769720351163 + }, + { + "x": 0.6790542635928064, + "y": -0.360896519188741 + }, + { + "x": 0.6992900115940054, + "y": -0.35667705401349437 + }, + { + "x": 0.7330473164511299, + "y": -0.3584656286751265 + }, + { + "x": 0.7534712453187414, + "y": -0.35285281781824906 + }, + { + "x": 0.7729973040091479, + "y": -0.3462429390488156 + }, + { + "x": 0.9466153199572828, + "y": -0.3315546381966728 + }, + { + "x": 0.9947805688820732, + "y": -0.32972044795542765 + }, + { + "x": 1.6203232562289498, + "y": -0.33206729806219804 + }, + { + "x": 1.6256867844202922, + "y": -0.3047271305281581 + } + ], + "stamp": { + "sec": 1784882167, + "nanosec": 307027762 + } + }, + "captured_at": "2026-07-24T08:36:07.326Z" + }, + { + "key": "A", + "label": "任务发布点左中点", + "image": { + "x": 450, + "y": 319 + }, + "map": { + "x": 4.5, + "y": 1.81 + }, + "odom_pose": { + "frame_id": "odom", + "stamp": { + "sec": 1784882182, + "nanosec": 687383993 + }, + "x": 0.011512968195960083, + "y": -0.0023025743030921047, + "theta": -0.06749248869060631 + }, + "scan": { + "frame_id": "pose", + "points": [ + { + "x": 0.6529078483824241, + "y": 0.01096985798164591 + }, + { + "x": 0.6526314281200959, + "y": 0.021936619918033008 + }, + { + "x": 0.6521708143666431, + "y": 0.03289719063770564 + }, + { + "x": 0.6515261371220196, + "y": 0.043848476716567754 + }, + { + "x": 0.6506975783347909, + "y": 0.05478738735094634 + }, + { + "x": 0.6496853718507837, + "y": 0.06571083522991515 + }, + { + "x": 0.6484898033470857, + "y": 0.07661573740663276 + }, + { + "x": 0.64711121025142, + "y": 0.08749901616844885 + }, + { + "x": 0.6455499816469109, + "y": 0.09835759990553343 + }, + { + "x": 0.6438065581622728, + "y": 0.10918842397778326 + }, + { + "x": 0.6418814318474502, + "y": 0.11998843157976173 + }, + { + "x": 0.6397751460347463, + "y": 0.13075457460342702 + }, + { + "x": 0.6540844874193774, + "y": 0.1451671646102978 + }, + { + "x": 0.6515534977427915, + "y": 0.15613475571696794 + }, + { + "x": 0.6488386188015569, + "y": 0.16705828059497946 + }, + { + "x": 0.6449765387848908, + "y": 0.17766908558769143 + }, + { + "x": 0.6419008290889026, + "y": 0.18847908417509823 + }, + { + "x": 0.6386439544205099, + "y": 0.19923588792844973 + }, + { + "x": 0.635206833974054, + "y": 0.2099364609332139 + }, + { + "x": 0.6466957134383536, + "y": 0.22585317683573394 + }, + { + "x": 0.6428103083976657, + "y": 0.23668525658357437 + }, + { + "x": 0.6396759419279876, + "y": 0.24781177317813022 + }, + { + "x": 0.6354226412958924, + "y": 0.25852282788121234 + }, + { + "x": 0.6309900040434678, + "y": 0.26916091916244406 + }, + { + "x": 0.6263792812029126, + "y": 0.279723044611755 + }, + { + "x": 0.6369956698012165, + "y": 0.29739793104984363 + }, + { + "x": 0.631909741101209, + "y": 0.3080569620048066 + }, + { + "x": 0.6266454672338164, + "y": 0.31862904941881837 + }, + { + "x": 0.6212043339457252, + "y": 0.3291112095102138 + }, + { + "x": 0.6304740871513016, + "y": 0.34771031999275515 + }, + { + "x": 0.6245438723745643, + "y": 0.3582526938881482 + }, + { + "x": 0.6184373913145063, + "y": 0.36869395739941213 + }, + { + "x": 0.6257598185979011, + "y": 0.3874540637727716 + }, + { + "x": 0.619162607170553, + "y": 0.3979116333904757 + }, + { + "x": 0.6123906482252731, + "y": 0.4082568996011491 + }, + { + "x": 0.619430352099263, + "y": 0.42815309235753635 + }, + { + "x": 0.6251574796999522, + "y": 0.44781593991429314 + }, + { + "x": 0.6175463238516261, + "y": 0.45825487271109333 + }, + { + "x": 0.6097608766522862, + "y": 0.4685644713060875 + }, + { + "x": 0.6268458712234176, + "y": 0.4986634658789114 + }, + { + "x": 0.6183802808817166, + "y": 0.5091235868518204 + }, + { + "x": 0.609740163819618, + "y": 0.5194400168425984 + }, + { + "x": 0.6121812803934694, + "y": 0.539527622122787 + }, + { + "x": 0.6030312755514883, + "y": 0.5497356235540877 + }, + { + "x": 0.605352493426121, + "y": 0.570764738983788 + }, + { + "x": 0.5956786945979345, + "y": 0.5808536144043128 + }, + { + "x": 0.5963987269822116, + "y": 0.6014295996212443 + }, + { + "x": 0.5965925791029748, + "y": 0.6221907061582057 + }, + { + "x": 0.5969341965769444, + "y": 0.6438583709166051 + }, + { + "x": 0.5960456283736237, + "y": 0.6649651275213825 + }, + { + "x": 0.5952684087579106, + "y": 0.6869763345695086 + }, + { + "x": 0.6035480298232715, + "y": 0.7206453852028597 + }, + { + "x": 0.6014222426140171, + "y": 0.7431199294615527 + }, + { + "x": 0.6079482102780631, + "y": 0.7775396781059234 + }, + { + "x": 0.6044425718160598, + "y": 0.8004112814446975 + }, + { + "x": 0.5991590043376687, + "y": 0.8217648118839147 + }, + { + "x": 1.8001410237241204, + "y": 2.651700738330269 + }, + { + "x": 1.7805342824890178, + "y": 2.7200548151868653 + }, + { + "x": 0.4917402234696982, + "y": 0.8741856751912186 + }, + { + "x": 0.4769852372540917, + "y": 0.8823231412182349 + }, + { + "x": 0.4685455911340875, + "y": 0.9026372181326896 + }, + { + "x": 0.46044774429599145, + "y": 0.9247036601043624 + }, + { + "x": 0.45819828342056923, + "y": 0.9602866006531864 + }, + { + "x": 0.45487948017607427, + "y": 0.9960470476695104 + }, + { + "x": 0.4504848689540917, + "y": 1.0319590534897547 + }, + { + "x": 0.4453931500479093, + "y": 1.068919512508179 + }, + { + "x": 0.43844517493349666, + "y": 1.1041330275253023 + }, + { + "x": 0.4604158261520157, + "y": 2.3533850675519297 + }, + { + "x": 0.4234482498082942, + "y": 2.3755549025873335 + }, + { + "x": 0.459957971616614, + "y": 4.963734984412943 + }, + { + "x": 0.3800562408421118, + "y": 5.017627119111771 + }, + { + "x": 0.28101913164279035, + "y": 4.773735735027101 + }, + { + "x": 0.20015486874779173, + "y": 4.762796334058535 + }, + { + "x": 0.13818207219770173, + "y": 5.482259021084427 + }, + { + "x": 0.04606517104981081, + "y": 5.4838067306711835 + }, + { + "x": -0.0460647311646995, + "y": 5.483806734366297 + }, + { + "x": -0.13780366528692412, + "y": 5.467263451465257 + }, + { + "x": -0.2296297073039668, + "y": 5.464176945837114 + }, + { + "x": -0.4141944095686657, + "y": 5.468336241529089 + }, + { + "x": -0.5059994704083364, + "y": 5.46060644940513 + }, + { + "x": -0.597661721991594, + "y": 5.451335499251946 + }, + { + "x": -0.6891552942953354, + "y": 5.440526007628594 + }, + { + "x": -0.780454364903152, + "y": 5.4281810253198115 + }, + { + "x": -0.8740758998101299, + "y": 5.430100489067501 + }, + { + "x": -0.6749196623371744, + "y": 3.7863173548911857 + }, + { + "x": -0.7445754170051287, + "y": 3.8058496569253752 + }, + { + "x": -0.811532323780442, + "y": 3.80747467637976 + }, + { + "x": -0.8823509110249319, + "y": 3.8235105681047634 + }, + { + "x": -0.9503173659080869, + "y": 3.8236758956468497 + }, + { + "x": -1.0185373140289038, + "y": 3.822632397864734 + }, + { + "x": -1.0867155413503116, + "y": 3.8194096381220954 + }, + { + "x": -1.1553616248941678, + "y": 3.815928252007199 + }, + { + "x": -1.2238902665827929, + "y": 3.8102618617512523 + }, + { + "x": -1.2877267929422231, + "y": 3.7891638320994985 + }, + { + "x": -1.3620041007987662, + "y": 3.7971173517671035 + }, + { + "x": -1.4309012163318546, + "y": 3.7877329603841705 + }, + { + "x": -1.4943300632866017, + "y": 3.7631604977782267 + }, + { + "x": -1.5692605184395134, + "y": 3.7661413678602615 + }, + { + "x": -1.6387084327161037, + "y": 3.7539115954288493 + }, + { + "x": -1.7144175068112186, + "y": 3.7540512004835254 + }, + { + "x": -1.777240519465743, + "y": 3.7247206525431853 + }, + { + "x": -1.846693871599574, + "y": 3.708661724622479 + }, + { + "x": -1.9230176716882967, + "y": 3.704629165118875 + }, + { + "x": -1.9925900628426114, + "y": 3.685876465776943 + }, + { + "x": -2.054228541934805, + "y": 3.651882470316899 + }, + { + "x": -2.138509977556465, + "y": 3.6565655461076165 + }, + { + "x": -2.22404120582501, + "y": 3.660290924153303 + }, + { + "x": -2.3017574941625907, + "y": 3.6486311895168906 + }, + { + "x": -2.388467959457911, + "y": 3.648772716817478 + }, + { + "x": -2.466838997590005, + "y": 3.6337819282946544 + }, + { + "x": -2.554583218416839, + "y": 3.63026538033476 + }, + { + "x": -2.6334714203626723, + "y": 3.6118868202708687 + }, + { + "x": -2.722100567962124, + "y": 3.604644128055226 + }, + { + "x": -2.8112213354330104, + "y": 3.595432072380896 + }, + { + "x": -2.8907268725765425, + "y": 3.5717952376956257 + }, + { + "x": -3.0004038246803737, + "y": 3.5825276788481983 + }, + { + "x": -3.0909423187254195, + "y": 3.567138012710126 + }, + { + "x": -3.181802187208114, + "y": 3.5497078376512246 + }, + { + "x": -3.2729396155405057, + "y": 3.530221455061193 + }, + { + "x": -3.374692769986698, + "y": 3.519492008995316 + }, + { + "x": -3.4664351845731285, + "y": 3.495676575636201 + }, + { + "x": -3.891620588234179, + "y": 3.5476813593820347 + }, + { + "x": -4.021190654449121, + "y": 3.543956426346899 + }, + { + "x": -4.1273544374281546, + "y": 3.516109867914217 + }, + { + "x": -4.269989009572173, + "y": 3.515559184150851 + }, + { + "x": -4.389486143123747, + "y": 3.491889847942149 + }, + { + "x": -4.522062594402508, + "y": 3.4749330461885926 + }, + { + "x": -4.680181836919367, + "y": 3.4729645511758505 + }, + { + "x": -4.788266957176046, + "y": 3.4299559726061184 + }, + { + "x": -4.845211669610288, + "y": 3.349033040128541 + }, + { + "x": -4.900788908131679, + "y": 3.2671649032661096 + }, + { + "x": -4.875905273841331, + "y": 3.1335545000440415 + }, + { + "x": -4.8751450320077865, + "y": 3.0185624524308 + }, + { + "x": -4.871911852688682, + "y": 2.904489304628419 + }, + { + "x": -4.852358384321503, + "y": 2.7834246125352213 + }, + { + "x": -4.857276978408434, + "y": 2.678818506965134 + }, + { + "x": -4.832668888870524, + "y": 2.5603263285052646 + }, + { + "x": -4.805469990678903, + "y": 2.443427295349912 + }, + { + "x": -4.789210482116189, + "y": 2.334747885677997 + }, + { + "x": -4.786075390940322, + "y": 2.2345037563873764 + }, + { + "x": -4.765412996304386, + "y": 2.1280973993715864 + }, + { + "x": -4.757259760676508, + "y": 2.029301076524128 + }, + { + "x": -4.776785051803565, + "y": 1.9434439116494728 + }, + { + "x": -4.779852449280101, + "y": 1.851725093002137 + }, + { + "x": -4.780256225679683, + "y": 1.7601093766416247 + }, + { + "x": -4.735511625148862, + "y": 1.6538390434234227 + }, + { + "x": -4.7626264919184385, + "y": 1.5740529952795086 + }, + { + "x": -4.744484454084078, + "y": 1.4801233662974027 + }, + { + "x": -4.768679775446855, + "y": 1.4002110961384298 + }, + { + "x": -4.746217038752426, + "y": 1.307421673123989 + }, + { + "x": -4.7374903093467555, + "y": 1.2197751881562109 + }, + { + "x": -4.741753235170516, + "y": 1.1362884976678775 + }, + { + "x": -4.744553075241361, + "y": 1.0530039952335253 + }, + { + "x": -4.777248894671991, + "y": 0.976354593135925 + }, + { + "x": -4.76250439452032, + "y": 0.8902667348226461 + }, + { + "x": -4.730449841812449, + "y": 0.8022760994076154 + }, + { + "x": -4.727442618523308, + "y": 0.7202852968050177 + }, + { + "x": -4.708155119642224, + "y": 0.6366126614267957 + }, + { + "x": -4.687399265149254, + "y": 0.5537925139322986 + }, + { + "x": -4.696041052653251, + "y": 0.47496994768571493 + }, + { + "x": -4.672467108679931, + "y": 0.3934124230728863 + }, + { + "x": -4.662452593744672, + "y": 0.31378892898515914 + }, + { + "x": -4.667066040592113, + "y": 0.23541931446237357 + }, + { + "x": -4.655370891771634, + "y": 0.15647935636991228 + }, + { + "x": -4.641345131545252, + "y": 0.0779821229192743 + }, + { + "x": -4.656342893840352, + "y": -0.07823336241011568 + }, + { + "x": -4.638380313789405, + "y": -0.15590751302571462 + }, + { + "x": -4.6510866009106495, + "y": -0.23461252063980453 + }, + { + "x": -4.69504627695203, + "y": -0.47486857251963877 + }, + { + "x": -4.687399353994441, + "y": -0.5537917619301702 + }, + { + "x": -4.677434644912931, + "y": -0.6324580317914148 + }, + { + "x": -4.650332559912294, + "y": -0.7085358468919565 + }, + { + "x": -4.622984822388759, + "y": -0.7840494552558833 + }, + { + "x": -4.609161065088238, + "y": -0.86160112140046 + }, + { + "x": -4.594036454915555, + "y": -0.9389096161137057 + }, + { + "x": -4.577615260520104, + "y": -1.01595312046242 + }, + { + "x": -4.590048291276181, + "y": -1.0999339539998876 + }, + { + "x": -4.5709225844433385, + "y": -1.176887737396842 + }, + { + "x": -4.550506816862251, + "y": -1.253509365381611 + }, + { + "x": -4.544158898361135, + "y": -1.3342850087482496 + }, + { + "x": -4.5211027640410215, + "y": -1.4104348177443862 + }, + { + "x": -4.511012808394069, + "y": -1.490893609150848 + }, + { + "x": -4.48533045701169, + "y": -1.5664644913643895 + }, + { + "x": -4.473397003623409, + "y": -1.647121774240743 + }, + { + "x": -4.459082416352983, + "y": -1.7274572713661098 + }, + { + "x": -4.444253483049892, + "y": -1.808151991394259 + }, + { + "x": -4.398534095692743, + "y": -1.8762788783260411 + }, + { + "x": -4.566359912553981, + "y": -2.0392050961299995 + }, + { + "x": -4.559548123019054, + "y": -2.1287427196639688 + }, + { + "x": -4.578873710928859, + "y": -2.232207459053865 + }, + { + "x": -4.202659652241099, + "y": -2.4107415486488515 + }, + { + "x": -4.161568132489393, + "y": -2.4810025484883895 + }, + { + "x": -3.5488063502732623, + "y": -2.197327400487924 + }, + { + "x": -3.4979323962024433, + "y": -2.24798418325205 + }, + { + "x": -3.4729871828741454, + "y": -2.3153043524052634 + }, + { + "x": -3.4204401996590468, + "y": -2.3642235040516653 + }, + { + "x": -2.8894262964374327, + "y": -2.2203495370497537 + }, + { + "x": -2.8000683097020853, + "y": -2.2274878119454744 + }, + { + "x": -2.725968609345226, + "y": -2.2443385277115784 + }, + { + "x": -2.6521034440373663, + "y": -2.2593368708955848 + }, + { + "x": -2.5665102325716305, + "y": -2.2619164998304595 + }, + { + "x": -2.469768030088651, + "y": -2.2514906014786957 + }, + { + "x": -2.3872134362675186, + "y": -2.2508159290254466 + }, + { + "x": -2.3039592679972904, + "y": -2.2466186311957217 + }, + { + "x": -2.168018678209201, + "y": -2.18630647152798 + }, + { + "x": -2.088766254206594, + "y": -2.178389050659946 + }, + { + "x": -2.019922076204554, + "y": -2.1787050156897743 + }, + { + "x": -1.9309743306078102, + "y": -2.154248472998066 + }, + { + "x": -1.8650436224920386, + "y": -2.1523746348329973 + }, + { + "x": -1.8183491477278242, + "y": -2.1711357567380487 + }, + { + "x": -1.7621171461766785, + "y": -2.17727922545964 + }, + { + "x": -1.696341976675826, + "y": -2.169548251252849 + }, + { + "x": -1.6409743703209119, + "y": -2.1730007791123374 + }, + { + "x": -1.5141045034420513, + "y": -2.1516618631789726 + }, + { + "x": -1.4597713396736431, + "y": -2.150318236190783 + }, + { + "x": -1.4064635001301566, + "y": -2.1486006665835977 + }, + { + "x": -1.3376233800671042, + "y": -2.1203331200739943 + }, + { + "x": -1.3018147833831066, + "y": -2.142504869714433 + }, + { + "x": -1.249988723214931, + "y": -2.1373124575755313 + }, + { + "x": -1.198218599416561, + "y": -2.1301192961854447 + }, + { + "x": -1.1551318461221898, + "y": -2.1367524274579797 + }, + { + "x": -1.1117018492576973, + "y": -2.1416555544320004 + }, + { + "x": -1.0688808109655954, + "y": -2.1466014583748834 + }, + { + "x": -1.0257786370756545, + "y": -2.149814420440395 + }, + { + "x": -0.9770563438111969, + "y": -2.139454344464057 + }, + { + "x": -0.9345761883741383, + "y": -2.140902423681116 + }, + { + "x": -0.886555651983336, + "y": -2.1276852172459884 + }, + { + "x": -0.850687199048571, + "y": -2.1422783659285747 + }, + { + "x": -0.8145786549409076, + "y": -2.1562668950446207 + }, + { + "x": -0.6815680931080467, + "y": -2.251081723413189 + }, + { + "x": -0.5893410762683263, + "y": -2.211831425085567 + }, + { + "x": -0.544623824444709, + "y": -2.191334849974682 + }, + { + "x": -0.5043614892263732, + "y": -2.185559110407955 + }, + { + "x": -0.4642393773655069, + "y": -2.178075019830115 + }, + { + "x": -0.42163203237946, + "y": -2.1551433513526392 + }, + { + "x": -0.38273560096582837, + "y": -2.1471549629204136 + }, + { + "x": -0.34422730712957944, + "y": -2.1384721372497566 + }, + { + "x": -0.3059771641614979, + "y": -2.1281161587403195 + }, + { + "x": -0.27018340068914726, + "y": -2.132956009877375 + }, + { + "x": -0.23256965185145803, + "y": -2.121289142402871 + }, + { + "x": -0.19551689872966846, + "y": -2.1099606547226366 + }, + { + "x": -0.15883527073860623, + "y": -2.0969930958158574 + }, + { + "x": -0.12358511296963416, + "y": -2.0993654772293135 + }, + { + "x": -0.08767026601802275, + "y": -2.0861587119330247 + }, + { + "x": -0.05220900031245389, + "y": -2.0713421568980612 + }, + { + "x": -0.017404803702035638, + "y": -2.0719269252231927 + }, + { + "x": 0.017404305101418494, + "y": -2.0719269294115263 + }, + { + "x": 0.05220850185255781, + "y": -2.0713421694618805 + }, + { + "x": 0.08632616339215698, + "y": -2.054186887794034 + }, + { + "x": 0.12082261618992164, + "y": -2.052446801232122 + }, + { + "x": 0.15528496897575933, + "y": -2.050127448448302 + }, + { + "x": 0.18970349536997327, + "y": -2.0472294840382115 + }, + { + "x": 0.222433729058817, + "y": -2.028842966964365 + }, + { + "x": 0.25837022805240406, + "y": -2.0397011550045994 + }, + { + "x": 0.29259905438943884, + "y": -2.035072915116644 + }, + { + "x": 0.3267452999021286, + "y": -2.029870312473882 + }, + { + "x": 0.3607993274263729, + "y": -2.0240948154174063 + }, + { + "x": 0.3947515258249415, + "y": -2.0177480539774604 + }, + { + "x": 0.42859231270004455, + "y": -2.0108318194133927 + }, + { + "x": 0.46590990422064665, + "y": -2.0189383526511766 + }, + { + "x": 0.49951946580100554, + "y": -2.009856142637643 + }, + { + "x": 0.5332128906815932, + "y": -2.0011810073112746 + }, + { + "x": 0.5711343753147337, + "y": -2.007330087330572 + }, + { + "x": 0.6091220207879117, + "y": -2.011808727432297 + }, + { + "x": 0.6431385782227587, + "y": -2.0022441050809934 + }, + { + "x": 0.6766838602335176, + "y": -1.9911573620458642 + }, + { + "x": 0.5848055186941019, + "y": -1.2256290687204106 + }, + { + "x": 0.5852542896948747, + "y": -1.1753494464322074 + }, + { + "x": 0.590173794365138, + "y": -1.1369502798573097 + }, + { + "x": 0.5824398692214083, + "y": -1.0354263434141528 + }, + { + "x": 0.584101877424787, + "y": -0.9987361368254916 + }, + { + "x": 0.5841807216197502, + "y": -0.961435325171604 + }, + { + "x": 0.5837093790013897, + "y": -0.9252671620778852 + }, + { + "x": 0.5821923888295661, + "y": -0.8893935829223663 + }, + { + "x": 0.5802013318522149, + "y": -0.8546668349448147 + }, + { + "x": 0.5852693531753446, + "y": -0.8317143155935371 + }, + { + "x": 0.5909109000573837, + "y": -0.8104525589329261 + }, + { + "x": 0.5948002714303512, + "y": -0.7876430755842037 + }, + { + "x": 0.5980928140500772, + "y": -0.7649353020617143 + }, + { + "x": 0.6014221233948579, + "y": -0.7431200259480545 + }, + { + "x": 0.6035479142097161, + "y": -0.720645482030403 + }, + { + "x": 0.6057460898474426, + "y": -0.6990684492152773 + }, + { + "x": 0.6067249305119701, + "y": -0.6768794733325701 + }, + { + "x": 0.6071322739726448, + "y": -0.6548583158391146 + }, + { + "x": 0.6076661441798592, + "y": -0.6337396106130098 + }, + { + "x": 0.6069605796038972, + "y": -0.6120807422859236 + }, + { + "x": 0.6171573762082817, + "y": -0.6017979339131162 + }, + { + "x": 0.6162661965465684, + "y": -0.5810550574950701 + }, + { + "x": 0.6148553524690654, + "y": -0.5605148741153928 + }, + { + "x": 0.6121811938367392, + "y": -0.5395277203353699 + }, + { + "x": 0.6211584231769033, + "y": -0.5291674483696437 + }, + { + "x": 0.7207705598321891, + "y": -0.5538689039862459 + }, + { + "x": 0.7299733854564769, + "y": -0.5416823970270238 + }, + { + "x": 0.7259630349667893, + "y": -0.5200256568522793 + }, + { + "x": 0.7090954612663254, + "y": -0.49013019401163527 + }, + { + "x": 0.7172291770076147, + "y": -0.47814881357101113 + }, + { + "x": 0.7125416779286574, + "y": -0.4579228778530139 + }, + { + "x": 0.7201338511732245, + "y": -0.4458881488007873 + }, + { + "x": 0.7146386812934012, + "y": -0.42604645728333657 + }, + { + "x": 0.7078162821912892, + "y": -0.4060198095691364 + }, + { + "x": 0.7145371937956769, + "y": -0.3940717875974576 + }, + { + "x": 0.721056440119184, + "y": -0.3820125459462113 + }, + { + "x": 0.7140013815682652, + "y": -0.3630468641798498 + }, + { + "x": 0.719999510539792, + "y": -0.3510010018428346 + }, + { + "x": 0.7112966907565338, + "y": -0.3320874265817562 + }, + { + "x": 0.7021656504642149, + "y": -0.3135672013688357 + }, + { + "x": 0.7073342323783364, + "y": -0.3017271518638448 + }, + { + "x": 0.7123031819179813, + "y": -0.28980194529305203 + }, + { + "x": 0.7021515672330305, + "y": -0.27201508823838594 + }, + { + "x": 0.7066221101209296, + "y": -0.2601811389885568 + }, + { + "x": 0.6948438214467864, + "y": -0.24266863394954732 + }, + { + "x": 0.6988223994992588, + "y": -0.23096159001836458 + }, + { + "x": 0.6873297789545888, + "y": -0.21442438331721358 + }, + { + "x": 0.6908349373900371, + "y": -0.2028475547795309 + }, + { + "x": 0.6941451200804848, + "y": -0.19121347616520312 + }, + { + "x": 0.680796305511357, + "y": -0.17528663137933562 + }, + { + "x": 0.6836449041751298, + "y": -0.16382508273645274 + }, + { + "x": 0.6697043647085743, + "y": -0.14863394463389656 + }, + { + "x": 0.6721067856064198, + "y": -0.1373624925114327 + }, + { + "x": 0.6743195164435126, + "y": -0.12605227229421417 + }, + { + "x": 0.6763419327163424, + "y": -0.11470647608862361 + }, + { + "x": 0.6781734636342641, + "y": -0.10332830604172694 + }, + { + "x": 0.6639579083656074, + "y": -0.08977704763708046 + }, + { + "x": 0.6653723931603666, + "y": -0.07861043686247698 + }, + { + "x": 0.6665990885556067, + "y": -0.06742163970490049 + }, + { + "x": 0.6676376483385992, + "y": -0.05621381400125728 + }, + { + "x": 0.668487779394511, + "y": -0.04499012295891879 + }, + { + "x": 0.6521708090889198, + "y": -0.03289729526583865 + }, + { + "x": 0.652631424600783, + "y": -0.02193672462006269 + } + ], + "stamp": { + "sec": 1784882182, + "nanosec": 657254188 + } + }, + "captured_at": "2026-07-24T08:36:22.707Z" + }, + { + "key": "B", + "label": "通道", + "image": { + "x": 250, + "y": 240 + }, + "map": { + "x": 2.5, + "y": 2.6 + }, + "odom_pose": { + "frame_id": "odom", + "stamp": { + "sec": 1784882202, + "nanosec": 87103294 + }, + "x": 0.008717998261200446, + "y": -0.0021310477980812778, + "theta": -0.05600910099161461 + }, + "scan": { + "frame_id": "pose", + "points": [ + { + "x": 2.6766222782011893, + "y": 0.04497137893980167 + }, + { + "x": 2.6754890820937645, + "y": 0.08993006551614872 + }, + { + "x": 2.673600777278327, + "y": 0.13486337094777806 + }, + { + "x": 2.543998326786614, + "y": 0.21419938599818422 + }, + { + "x": 2.5400409559780934, + "y": 0.25690621948287806 + }, + { + "x": 2.535366704260806, + "y": 0.29954054580438727 + }, + { + "x": 2.5299768908582907, + "y": 0.34209033219035595 + }, + { + "x": 2.523873036948512, + "y": 0.38454356972830556 + }, + { + "x": 2.517056865234533, + "y": 0.4268882767549347 + }, + { + "x": 2.540002600566208, + "y": 0.47480876238041314 + }, + { + "x": 2.531667772393339, + "y": 0.5174116948246041 + }, + { + "x": 2.5372619783388486, + "y": 0.5631185792559642 + }, + { + "x": 2.5430035229405195, + "y": 0.6093916082366695 + }, + { + "x": 2.5479020757769324, + "y": 0.6560154213537266 + }, + { + "x": 2.550983239399749, + "y": 0.7027090634142269 + }, + { + "x": 2.538818325775903, + "y": 0.745464301095216 + }, + { + "x": 2.5555303731943964, + "y": 0.7972413416071581 + }, + { + "x": 2.5569685769389396, + "y": 0.8450805392026429 + }, + { + "x": 2.5565721899634557, + "y": 0.892861880656609 + }, + { + "x": 2.555288314674758, + "y": 0.9408670995202115 + }, + { + "x": 2.55404152215361, + "y": 0.9894409292114801 + }, + { + "x": 2.5805940440315926, + "y": 1.0499192608495642 + }, + { + "x": 2.562592108706105, + "y": 1.0931229385533106 + }, + { + "x": 2.5575633637786193, + "y": 1.1421345379268435 + }, + { + "x": 2.5525131192258153, + "y": 1.1917068774929538 + }, + { + "x": 2.559998507539081, + "y": 1.2480031746227174 + }, + { + "x": 2.552934063190977, + "y": 1.2980847964546898 + }, + { + "x": 2.5572764615165506, + "y": 1.3548333508169712 + }, + { + "x": 2.561300835520563, + "y": 1.4125730958120115 + }, + { + "x": 2.5779781142723093, + "y": 1.4787873920070136 + }, + { + "x": 2.565656121354119, + "y": 1.5295677816269322 + }, + { + "x": 2.5532020988126023, + "y": 1.5808757600234808 + }, + { + "x": 2.564982066503052, + "y": 1.6484138284183643 + }, + { + "x": 2.511966570793331, + "y": 1.6746298903581012 + }, + { + "x": 2.470317806800694, + "y": 1.70749496614611 + }, + { + "x": 2.4664860320137794, + "y": 1.766805639503589 + }, + { + "x": 2.4485029543744883, + "y": 1.816929947362462 + }, + { + "x": 2.454902031968163, + "y": 1.886444205854396 + }, + { + "x": 2.4228649090127488, + "y": 1.9274183150104398 + }, + { + "x": 2.4140764025134116, + "y": 1.987552441403062 + }, + { + "x": 2.403944363274639, + "y": 2.0479295520336716 + }, + { + "x": 2.392458422947468, + "y": 2.108521520179189 + }, + { + "x": 2.390693810818209, + "y": 2.179405291400732 + }, + { + "x": 2.3762995461133847, + "y": 2.240525982652499 + }, + { + "x": 2.3705434878391842, + "y": 2.311546082646768 + }, + { + "x": 2.375032873171785, + "y": 2.3950672685483525 + }, + { + "x": 2.3787572427145376, + "y": 2.4808230951328043 + }, + { + "x": 2.3578219880964393, + "y": 2.543167124404209 + }, + { + "x": 2.3568163917131884, + "y": 2.6293301013486587 + }, + { + "x": 2.3542244167067845, + "y": 2.7169196899226065 + }, + { + "x": 2.3692470959913745, + "y": 2.8289165099775264 + }, + { + "x": 2.3610227604077374, + "y": 2.917289955132807 + }, + { + "x": 3.192155727250048, + "y": 4.227097122796266 + }, + { + "x": 3.0841667563798025, + "y": 4.23002858344214 + }, + { + "x": 3.0126705616557325, + "y": 4.281243100412398 + }, + { + "x": 2.9139258088443882, + "y": 4.292363274276708 + }, + { + "x": 2.8074496482524633, + "y": 4.288834542095308 + }, + { + "x": 2.717930806106284, + "y": 4.308327602923155 + }, + { + "x": 2.6046676915493747, + "y": 4.286719042586133 + }, + { + "x": 2.5166368820113476, + "y": 4.30311109956957 + }, + { + "x": 2.436638839830985, + "y": 4.331707409178529 + }, + { + "x": 2.3340413677306446, + "y": 4.317489411548328 + }, + { + "x": 2.290206800801934, + "y": 4.412005863977556 + }, + { + "x": 2.301757241167972, + "y": 4.823995891362221 + }, + { + "x": 2.2137467440979632, + "y": 4.847428835201051 + }, + { + "x": 1.3562554970108691, + "y": 3.1068749151000783 + }, + { + "x": 1.3034865010989094, + "y": 3.1282972249702614 + }, + { + "x": 1.2393088888050454, + "y": 3.1209418045092776 + }, + { + "x": 1.1867047704466405, + "y": 3.141320750385737 + }, + { + "x": 1.1283636151664957, + "y": 3.1457526290163487 + }, + { + "x": 1.0653834863175804, + "y": 3.134913036595781 + }, + { + "x": 1.0073702101990665, + "y": 3.1361824993165968 + }, + { + "x": 0.951065448537267, + "y": 3.141177953932736 + }, + { + "x": 0.8937834382062521, + "y": 3.1413225522680466 + }, + { + "x": 0.8329042015490727, + "y": 3.125939044881058 + }, + { + "x": 0.7764143755891538, + "y": 3.123962606721241 + }, + { + "x": 0.7206767501614544, + "y": 3.122923235164858 + }, + { + "x": 0.6647771746965262, + "y": 3.1189407840793564 + }, + { + "x": 0.6063357691071217, + "y": 3.09924521245319 + }, + { + "x": 0.5513776730390546, + "y": 3.093242054862937 + }, + { + "x": 0.49695214226308165, + "y": 3.087259003193529 + }, + { + "x": 0.44274158283153225, + "y": 3.0793345499624714 + }, + { + "x": 0.3890638278440507, + "y": 3.0714565056706196 + }, + { + "x": 0.33392354526735324, + "y": 3.0457495628640983 + }, + { + "x": 0.2827103585318532, + "y": 3.0509293972410365 + }, + { + "x": 0.2302844739303222, + "y": 3.040291139392705 + }, + { + "x": 0.17917760931142723, + "y": 3.0437306936594437 + }, + { + "x": 0.1273903567184753, + "y": 3.0313243328479222 + }, + { + "x": 0.07607069067733423, + "y": 3.018041512717124 + }, + { + "x": 0.025090565322777838, + "y": 2.9868946073078604 + }, + { + "x": -0.024964327228198826, + "y": 2.971895033609357 + }, + { + "x": -0.12478688618161307, + "y": 2.969378982457951 + }, + { + "x": -0.17465237916403045, + "y": 2.966863639414232 + }, + { + "x": -0.22446857965138042, + "y": 2.963510952430446 + }, + { + "x": -0.2742214279164006, + "y": 2.959321867742238 + }, + { + "x": -0.3238968821118545, + "y": 2.954297567643472 + }, + { + "x": -0.3753659334154651, + "y": 2.96332066230844 + }, + { + "x": -0.4250942909313271, + "y": 2.956596654170228 + }, + { + "x": -0.47470267322360965, + "y": 2.94903819976401 + }, + { + "x": -0.5241770792180008, + "y": 2.9406474323276974 + }, + { + "x": -0.5735035456525908, + "y": 2.931426720004596 + }, + { + "x": -0.6226681510187515, + "y": 2.9213786651750375 + }, + { + "x": -0.6752547878636666, + "y": 2.926096392376384 + }, + { + "x": -0.7243154894199929, + "y": 2.914339753322727 + }, + { + "x": -0.7731717659472397, + "y": 2.9017605942518627 + }, + { + "x": -0.826188444627937, + "y": 2.9037517070898575 + }, + { + "x": -0.8748524748608859, + "y": 2.8894626610658554 + }, + { + "x": -0.9278568478932381, + "y": 2.888639330847378 + }, + { + "x": -0.9810792518307052, + "y": 2.8868468357068164 + }, + { + "x": -1.0294374366816783, + "y": 2.869958138224565 + }, + { + "x": -1.082805977423351, + "y": 2.8662914277908005 + }, + { + "x": -1.1367094896867724, + "y": 2.862567215994297 + }, + { + "x": -1.190791810562995, + "y": 2.8578366979691374 + }, + { + "x": -1.2386330553771996, + "y": 2.8374290973502725 + }, + { + "x": -1.299002639285135, + "y": 2.8444194007968693 + }, + { + "x": -1.3466032143620068, + "y": 2.8221958414626864 + }, + { + "x": -1.3938237348835423, + "y": 2.7991757680743556 + }, + { + "x": -1.4475615133330046, + "y": 2.788678793517767 + }, + { + "x": -1.5165559443901948, + "y": 2.805312526996914 + }, + { + "x": -1.5781770391038217, + "y": 2.805587083670646 + }, + { + "x": -1.6412407180984632, + "y": 2.8063017360924634 + }, + { + "x": -1.710481332164952, + "y": 2.815082418283951 + }, + { + "x": -1.7751383238157312, + "y": 2.813860743543344 + }, + { + "x": -1.8473520044191014, + "y": 2.8221302133826174 + }, + { + "x": -1.9130357461161602, + "y": 2.818000983935292 + }, + { + "x": -1.9779460143583674, + "y": 2.8108181750862196 + }, + { + "x": -2.052576012036022, + "y": 2.815170951980965 + }, + { + "x": -2.127902680584171, + "y": 2.817798869342668 + }, + { + "x": -2.184794390968985, + "y": 2.7942587536024908 + }, + { + "x": -2.2616243012240895, + "y": 2.7944732465744235 + }, + { + "x": -2.3596157170286958, + "y": 2.8174169583992787 + }, + { + "x": -2.406613003994874, + "y": 2.777379790112879 + }, + { + "x": -2.484301841366633, + "y": 2.7715568720280435 + }, + { + "x": -2.551587477119314, + "y": 2.752164694216181 + }, + { + "x": -2.629990269821837, + "y": 2.7428362725919717 + }, + { + "x": -2.697524656426661, + "y": 2.720279841272434 + }, + { + "x": -2.776492502161062, + "y": 2.7073923890287426 + }, + { + "x": -2.844137765043274, + "y": 2.681633969647105 + }, + { + "x": -2.876961470697589, + "y": 2.6226972413786553 + }, + { + "x": -2.8741010026010985, + "y": 2.533003180754086 + }, + { + "x": -2.856872277098683, + "y": 2.433780998739848 + }, + { + "x": -2.873422202534319, + "y": 2.3657404717944743 + }, + { + "x": -2.839196868335088, + "y": 2.2586157918232064 + }, + { + "x": -2.8370927719568417, + "y": 2.180135109270472 + }, + { + "x": -2.8235276761301966, + "y": 2.095220200871235 + }, + { + "x": -2.819305581220038, + "y": 2.0195394499496575 + }, + { + "x": -2.774685718034317, + "y": 1.9178757873373016 + }, + { + "x": -2.771088944851634, + "y": 1.7808709861012573 + }, + { + "x": -2.7768090792829803, + "y": 1.7193276444619947 + }, + { + "x": -2.764930348405605, + "y": 1.6483694425946287 + }, + { + "x": -2.753197356140039, + "y": 1.579297462654056 + }, + { + "x": -2.751318597534239, + "y": 1.517369343027885 + }, + { + "x": -2.735773092638501, + "y": 1.4494003290872446 + }, + { + "x": -2.717840601111496, + "y": 1.3819347372987842 + }, + { + "x": -2.7137061777298785, + "y": 1.3229361675510594 + }, + { + "x": -2.7210496635649326, + "y": 1.2703927953290912 + }, + { + "x": -2.742007224457138, + "y": 1.2245021466031643 + }, + { + "x": -2.733676651766886, + "y": 1.1661025992641063 + }, + { + "x": -2.724166101959387, + "y": 1.1083320617861794 + }, + { + "x": -2.7125615662607774, + "y": 1.0508521699900524 + }, + { + "x": -2.715756212281623, + "y": 0.9999522511431858 + }, + { + "x": -2.6887435999057288, + "y": 0.9390219041304023 + }, + { + "x": -2.704138963756557, + "y": 0.8937207322001199 + }, + { + "x": -2.70349710716838, + "y": 0.8434023291177257 + }, + { + "x": -2.702891564613361, + "y": 0.7936407850065688 + }, + { + "x": -2.7004174015761797, + "y": 0.7438733223691558 + }, + { + "x": -2.698006445197125, + "y": 0.6946634408611123 + }, + { + "x": -2.693735965180583, + "y": 0.6455125438173301 + }, + { + "x": -2.704199910879375, + "y": 0.6001689231648583 + }, + { + "x": -2.69822465247564, + "y": 0.5514521204233922 + }, + { + "x": -2.6923633626569634, + "y": 0.503290147634636 + }, + { + "x": -2.70043828383638, + "y": 0.4579896554229529 + }, + { + "x": -2.6919335654760315, + "y": 0.4101499105649553 + }, + { + "x": -2.6984438686275274, + "y": 0.36486978216816496 + }, + { + "x": -2.6892960836835424, + "y": 0.3177267296098302 + }, + { + "x": -2.69425412629436, + "y": 0.2725039511089947 + }, + { + "x": -2.6835047858857415, + "y": 0.2259457574739912 + }, + { + "x": -2.670957882275577, + "y": 0.17975882786844258 + }, + { + "x": -2.642639993391038, + "y": 0.13330184107187817 + }, + { + "x": -2.6445064348629868, + "y": 0.08888887146991054 + }, + { + "x": -2.6310000419616615, + "y": 2.1104641406256523e-7 + }, + { + "x": -2.5986332200976725, + "y": -0.043660834073839895 + }, + { + "x": -2.597533045561276, + "y": -0.08730955414147953 + }, + { + "x": -2.5956997640071084, + "y": -0.1309336326566396 + }, + { + "x": -2.5931338928459398, + "y": -0.17452075750719592 + }, + { + "x": -2.5898361562487895, + "y": -0.21805862701053197 + }, + { + "x": -2.585807484942544, + "y": -0.2615349533854677 + }, + { + "x": -2.581049015947273, + "y": -0.30493746622026513 + }, + { + "x": -2.5755620922553297, + "y": -0.34825391593573113 + }, + { + "x": -2.569348262452311, + "y": -0.39147207724244126 + }, + { + "x": -2.562409280280001, + "y": -0.4345797525911082 + }, + { + "x": -2.570474705858885, + "y": -0.48050477252244467 + }, + { + "x": -2.562039889341117, + "y": -0.5236188072463797 + }, + { + "x": -2.5685019427124813, + "y": -0.5700517442167141 + }, + { + "x": -2.5731499784647576, + "y": -0.6166155235069548 + }, + { + "x": -2.5624282367744597, + "y": -0.6597552931836701 + }, + { + "x": -2.566408774550807, + "y": -0.7069580518759089 + }, + { + "x": -2.5685628002861516, + "y": -0.7541978427924835 + }, + { + "x": -2.5555304371453285, + "y": -0.7972411366145622 + }, + { + "x": -2.556968644727306, + "y": -0.8450803340946811 + }, + { + "x": -2.5707335809724228, + "y": -0.8978073990192127 + }, + { + "x": -2.57030296831516, + "y": -0.9463953192793612 + }, + { + "x": -2.5680285609502658, + "y": -0.9948592998461896 + }, + { + "x": -2.5805941282511555, + "y": -1.0499190538464784 + }, + { + "x": -2.5763894510891365, + "y": -1.0990082159768366 + }, + { + "x": -2.5721726973505805, + "y": -1.1486584003471814 + }, + { + "x": -2.5806027649283387, + "y": -1.2048210041806706 + }, + { + "x": -2.6013469024748703, + "y": -1.2681603219218578 + }, + { + "x": -2.59304646492402, + "y": -1.3184803998061432 + }, + { + "x": -2.5846695604158048, + "y": -1.369345823563294 + }, + { + "x": -2.575311512556783, + "y": -1.4202998005844745 + }, + { + "x": -2.5510882370182015, + "y": -1.4633624695184602 + }, + { + "x": -2.5390291652040338, + "y": -1.51369330718836 + }, + { + "x": -2.5268453761157343, + "y": -1.5645560883613117 + }, + { + "x": -2.500205554018842, + "y": -1.606784209556234 + }, + { + "x": -2.485340969436032, + "y": -1.6568793550756067 + }, + { + "x": -2.48347983751547, + "y": -1.7165923275833803 + }, + { + "x": -2.4664861737385864, + "y": -1.766805441653707 + }, + { + "x": -2.460548760760467, + "y": -1.8258683221116725 + }, + { + "x": -2.454902183289812, + "y": -1.8864440089337278 + }, + { + "x": -2.45886370824269, + "y": -1.9560554727782276 + }, + { + "x": -2.474293241320181, + "y": -2.037129712834479 + }, + { + "x": -2.4861567206755875, + "y": -2.1179662348675983 + }, + { + "x": -2.495989267637431, + "y": -2.199764971212239 + }, + { + "x": -2.5738762589442388, + "y": -2.50981796338913 + }, + { + "x": -2.519379989967269, + "y": -2.5406316060216314 + }, + { + "x": -2.3787574417144577, + "y": -2.4808229043201098 + }, + { + "x": -2.3367459951622425, + "y": -2.5204339712050383 + }, + { + "x": -2.2445426177166046, + "y": -2.87067323575413 + }, + { + "x": -2.19600094902125, + "y": -2.9079745908654293 + }, + { + "x": -3.8559452458047736, + "y": -5.288545382242957 + }, + { + "x": -3.7665579370939457, + "y": -5.352575763611604 + }, + { + "x": -1.6116681415615997, + "y": -2.9812491321221537 + }, + { + "x": -1.5470761247783893, + "y": -2.9803891915562817 + }, + { + "x": -1.4758400660966058, + "y": -2.963885594829927 + }, + { + "x": -1.413352475455914, + "y": -2.9620869679665183 + }, + { + "x": -1.3505145006334158, + "y": -2.957213403242791 + }, + { + "x": -1.2822417193745224, + "y": -2.9373254303960405 + }, + { + "x": -1.2207928763041263, + "y": -2.9298363282898823 + }, + { + "x": -1.1654967853795464, + "y": -2.935060679966036 + }, + { + "x": -1.1050704799823832, + "y": -2.9252262850540083 + }, + { + "x": -1.0453065458757924, + "y": -2.9141979949056753 + }, + { + "x": -0.991054582709369, + "y": -2.9161979260880186 + }, + { + "x": -0.9278573113192715, + "y": -2.8886391819907407 + }, + { + "x": -0.8791996350200931, + "y": -2.9038187814809135 + }, + { + "x": -0.8218102920210468, + "y": -2.8883623335619055 + }, + { + "x": -0.7690527573899897, + "y": -2.8862998437417846 + }, + { + "x": -0.7168388027970588, + "y": -2.884254745897178 + }, + { + "x": -0.664686842212308, + "y": -2.880299972532958 + }, + { + "x": -0.6130794804299872, + "y": -2.876389135865226 + }, + { + "x": -0.5616000086084946, + "y": -2.870580107117933 + }, + { + "x": -0.5132973945492451, + "y": -2.879609436329729 + }, + { + "x": -0.46230713758432856, + "y": -2.8720293599590008 + }, + { + "x": -0.41185948378899406, + "y": -2.8645432576111705 + }, + { + "x": -0.36367941457700964, + "y": -2.8710579221822528 + }, + { + "x": -0.3170314332944235, + "y": -2.891672803799076 + }, + { + "x": -0.268408991979661, + "y": -2.896590607412857 + }, + { + "x": -0.22091924668093113, + "y": -2.9166452316824882 + }, + { + "x": -0.16912884940582287, + "y": -2.8730262012491896 + }, + { + "x": -0.1195808979389053, + "y": -2.8454884802646503 + }, + { + "x": -0.07176217836038208, + "y": -2.847095796110078 + }, + { + "x": -0.02392320519676725, + "y": -2.847899570336675 + }, + { + "x": 0.02392251986155294, + "y": -2.8478995760936128 + }, + { + "x": 0.07176149321859132, + "y": -2.847095813379266 + }, + { + "x": 0.11958021318390705, + "y": -2.845488509041215 + }, + { + "x": 0.1673651837832151, + "y": -2.8430781167122223 + }, + { + "x": 0.21510291856738242, + "y": -2.8398653166822014 + }, + { + "x": 0.2642562392073978, + "y": -2.851782794994662 + }, + { + "x": 0.3121265324825837, + "y": -2.8469410772806665 + }, + { + "x": 0.3616680563351767, + "y": -2.85518481674872 + }, + { + "x": 0.4075893570532098, + "y": -2.834848741256675 + }, + { + "x": 0.4573798553457509, + "y": -2.8414235496823745 + }, + { + "x": 0.5050488690626873, + "y": -2.833338977359431 + }, + { + "x": 0.5525753418133559, + "y": -2.8244547460325045 + }, + { + "x": 0.5999458601049954, + "y": -2.814773363116214 + }, + { + "x": 0.6507448215830421, + "y": -2.81988784994996 + }, + { + "x": 0.698024766525227, + "y": -2.808557945713414 + }, + { + "x": 0.7451077065223576, + "y": -2.7964353764736893 + }, + { + "x": 0.8004638744899628, + "y": -2.8133400623967915 + }, + { + "x": 0.847612694279797, + "y": -2.799495926331383 + }, + { + "x": 0.8994154140009838, + "y": -2.8000951454022487 + }, + { + "x": 0.9563026272083299, + "y": -2.8139418248464843 + }, + { + "x": 1.00850406655627, + "y": -2.81159891865331 + }, + { + "x": 1.0612486285402534, + "y": -2.8092277363062483 + }, + { + "x": 1.1082915637599433, + "y": -2.7910032131313347 + }, + { + "x": 1.1669449656096165, + "y": -2.800606177444293 + }, + { + "x": 1.2198293204464319, + "y": -2.794354590989188 + }, + { + "x": 1.3931117347766822, + "y": -2.9196684770188384 + }, + { + "x": 1.4419631515792106, + "y": -2.8958533441384335 + }, + { + "x": 1.4904076001979865, + "y": -2.8712209086975666 + }, + { + "x": 1.5460403580458986, + "y": -2.8598530976316328 + }, + { + "x": 1.5938653454419827, + "y": -2.8334773317143447 + }, + { + "x": 1.648813172201702, + "y": -2.8192501369998695 + }, + { + "x": 1.7042499255467152, + "y": -2.804827377388573 + }, + { + "x": 1.7575307805757339, + "y": -2.7859506393231483 + }, + { + "x": 1.8221581251530152, + "y": -2.7836429583681292 + }, + { + "x": 1.8770889349445683, + "y": -2.7650499419185075 + }, + { + "x": 1.932482388210073, + "y": -2.7462112242620225 + }, + { + "x": 1.9966071409149047, + "y": -2.738408390133094 + }, + { + "x": 2.0616127197826746, + "y": -2.7300172196765433 + }, + { + "x": 2.117039124873159, + "y": -2.707603108446189 + }, + { + "x": 2.191793643519449, + "y": -2.708190613359933 + }, + { + "x": 2.257526054589064, + "y": -2.6955207921408473 + }, + { + "x": 2.322790606925161, + "y": -2.6806440101725415 + }, + { + "x": 2.3995337643004477, + "y": -2.6769876577395113 + }, + { + "x": 2.477480428042232, + "y": -2.672232609240609 + }, + { + "x": 2.597461272618962, + "y": -2.708911976186534 + }, + { + "x": 2.6426022021231415, + "y": -2.664894511761408 + }, + { + "x": 2.7206473680956513, + "y": -2.652937529622205 + }, + { + "x": 2.6521029003372467, + "y": -2.2593375091132075 + }, + { + "x": 2.6410470241574875, + "y": -2.174422139854412 + }, + { + "x": 2.6396389484718443, + "y": -2.0998653745487457 + }, + { + "x": 2.6380678069745125, + "y": -2.0271967340488466 + }, + { + "x": 2.635613589051089, + "y": -1.9557774502441942 + }, + { + "x": 2.6298883811921736, + "y": -1.8838554678478938 + }, + { + "x": 2.636485997019803, + "y": -1.8223518042557842 + }, + { + "x": 2.6409344035143203, + "y": -1.760608313548616 + }, + { + "x": 2.630599629221854, + "y": -1.6905842703743614 + }, + { + "x": 2.632271976516821, + "y": -1.629834338209644 + }, + { + "x": 2.606026030120244, + "y": -1.5536356858145621 + }, + { + "x": 2.63175806816619, + "y": -1.5096373685567428 + }, + { + "x": 2.629602019711455, + "y": -1.4502421673432104 + }, + { + "x": 2.639455415023358, + "y": -1.3983719261115304 + }, + { + "x": 2.6492035274338805, + "y": -1.347035255165135 + }, + { + "x": 2.6570769291313923, + "y": -1.2953295807095653 + }, + { + "x": 2.6648706837865284, + "y": -1.2441644380639263 + }, + { + "x": 2.7977054538704107, + "y": -1.2493756549391075 + }, + { + "x": 2.7907048396248895, + "y": -1.1904293393540764 + }, + { + "x": 2.7815949122070154, + "y": -1.1316973404558928 + }, + { + "x": 2.7852943756331654, + "y": -1.0790292733282312 + }, + { + "x": 2.773937533417469, + "y": -1.0213750979348075 + }, + { + "x": 2.761437691098976, + "y": -0.9644099746623427 + }, + { + "x": 2.747815295572305, + "y": -0.9081560496585586 + }, + { + "x": 2.747409777288938, + "y": -0.8571018821720302 + }, + { + "x": 2.7479878315611335, + "y": -0.806882486722537 + }, + { + "x": 2.715842589809605, + "y": -0.7481226724676677 + }, + { + "x": 2.728027180907669, + "y": -0.7023931989369375 + }, + { + "x": 2.723882550158275, + "y": -0.6527369419691391 + }, + { + "x": 2.7198198215038647, + "y": -0.6036358280858672 + }, + { + "x": 2.729576592284952, + "y": -0.5578599297711694 + }, + { + "x": 2.7071077871104072, + "y": -0.5060465841332562 + }, + { + "x": 2.715226933460838, + "y": -0.46049800885085174 + }, + { + "x": 2.7077510217503744, + "y": -0.4125601210652936 + }, + { + "x": 2.6984438393593857, + "y": -0.36486999862460356 + }, + { + "x": 2.704192558867345, + "y": -0.31948689275057807 + }, + { + "x": 2.6942541044353696, + "y": -0.2725041672293522 + }, + { + "x": 2.698451745875432, + "y": -0.2272044797226268 + }, + { + "x": 2.6869217870793767, + "y": -0.1808334352666725 + }, + { + "x": 2.6895804710138034, + "y": -0.1356698608141947 + }, + { + "x": 2.6754890676661987, + "y": -0.08993049474637764 + }, + { + "x": 2.676622270986371, + "y": -0.044971808351830435 + } + ], + "stamp": { + "sec": 1784882202, + "nanosec": 93357838 + } + }, + "captured_at": "2026-07-24T08:36:42.100Z" + }, + { + "key": "C", + "label": "诊疗室", + "image": { + "x": 280, + "y": 123 + }, + "map": { + "x": 2.8, + "y": 3.77 + }, + "odom_pose": { + "frame_id": "odom", + "stamp": { + "sec": 1784882222, + "nanosec": 736759483 + }, + "x": 0.007021478047595366, + "y": -0.00203423478568464, + "theta": -0.05373038737536273 + }, + "scan": { + "frame_id": "pose", + "points": [ + { + "x": 2.2746790564227695, + "y": 0.03821811342075731 + }, + { + "x": 2.272716428774812, + "y": 0.07639184129258338 + }, + { + "x": 2.2711123925612835, + "y": 0.11456081089783537 + }, + { + "x": 2.2838336244636723, + "y": 0.15370469395006772 + }, + { + "x": 2.280929227678493, + "y": 0.19204951313437965 + }, + { + "x": 2.2932998959393838, + "y": 0.23195019947203796 + }, + { + "x": 2.304969225852611, + "y": 0.2723202678389297 + }, + { + "x": 2.3010601134255997, + "y": 0.31113739474699126 + }, + { + "x": 2.2955085469979695, + "y": 0.34974938837323266 + }, + { + "x": 2.30409780130375, + "y": 0.3907708059594209 + }, + { + "x": 2.236263897496991, + "y": 0.4180301600045028 + }, + { + "x": 2.243621847523947, + "y": 0.458542070698095 + }, + { + "x": 2.205338599170942, + "y": 0.48945168033319625 + }, + { + "x": 2.1968050085794566, + "y": 0.5264304689647565 + }, + { + "x": 2.187651409256527, + "y": 0.5632606820577588 + }, + { + "x": 2.177880384641048, + "y": 0.5999319249464911 + }, + { + "x": 2.1674946924291065, + "y": 0.6364338478317242 + }, + { + "x": 2.1555424806830503, + "y": 0.6724582878046087 + }, + { + "x": 2.1591333808622335, + "y": 0.7135956296709134 + }, + { + "x": 2.1468408782508086, + "y": 0.7497665786831949 + }, + { + "x": 2.1480187049198856, + "y": 0.7909088446132431 + }, + { + "x": 2.134428972923827, + "y": 0.8268821661618385 + }, + { + "x": 2.1350572201220284, + "y": 0.8686518143396141 + }, + { + "x": 2.1210830436803643, + "y": 0.9047887573470214 + }, + { + "x": 2.1201934838498677, + "y": 0.946817677828599 + }, + { + "x": 2.1175800755361145, + "y": 0.9886471182658726 + }, + { + "x": 2.1267402395496613, + "y": 1.0367891085637038 + }, + { + "x": 2.109022925924644, + "y": 1.0723702719118633 + }, + { + "x": 2.104848770172594, + "y": 1.115139233153052 + }, + { + "x": 2.0998288918260677, + "y": 1.1580684928794809 + }, + { + "x": 2.1069679774630394, + "y": 1.2086051713105217 + }, + { + "x": 2.099251288682388, + "y": 1.2515111085942041 + }, + { + "x": 2.105137706519074, + "y": 1.303446042636083 + }, + { + "x": 2.0955625169734726, + "y": 1.3467362116896717 + }, + { + "x": 2.0867876082738017, + "y": 1.3911797013049163 + }, + { + "x": 2.0878011367681513, + "y": 1.4430976943661575 + }, + { + "x": 2.0884649959330392, + "y": 1.4960197158334119 + }, + { + "x": 2.08793289475068, + "y": 1.5493662353063622 + }, + { + "x": 2.086983903351575, + "y": 1.6037213057470094 + }, + { + "x": 2.095746851930564, + "y": 1.6671919474381172 + }, + { + "x": 2.0913760940226407, + "y": 1.7218674840775288 + }, + { + "x": 2.633827745948096, + "y": 2.4010511510015498 + }, + { + "x": 2.616403239051169, + "y": 2.4669109783647585 + }, + { + "x": 2.574591965680929, + "y": 2.510516260601638 + }, + { + "x": 2.5426160947574648, + "y": 2.5640641246810114 + }, + { + "x": 2.8174205961079113, + "y": 3.038894145450594 + }, + { + "x": 2.7659721134912783, + "y": 3.085795636463236 + }, + { + "x": 2.682964574650452, + "y": 3.096306039689048 + }, + { + "x": 2.580488904515995, + "y": 3.0811423925137498 + }, + { + "x": 2.5182983063417823, + "y": 3.111620301301169 + }, + { + "x": 2.3183354484645293, + "y": 3.069972125803298 + }, + { + "x": 2.257598235333095, + "y": 3.0963646974124637 + }, + { + "x": 2.19605549648587, + "y": 3.120768517512813 + }, + { + "x": 2.1169209909710975, + "y": 3.118333997595284 + }, + { + "x": 1.4754720006628157, + "y": 2.2540227164095863 + }, + { + "x": 1.8008348796534024, + "y": 2.9637842847321285 + }, + { + "x": 1.742714150294971, + "y": 2.9798071613405797 + }, + { + "x": 1.0766316619368577, + "y": 1.9139698796279423 + }, + { + "x": 1.044326626527968, + "y": 1.931786306176911 + }, + { + "x": 1.0048160981608971, + "y": 1.9357441938224034 + }, + { + "x": 0.972155406671647, + "y": 1.9523511058002658 + }, + { + "x": 0.9258706234385788, + "y": 1.9404288182598768 + }, + { + "x": 0.89314237461757, + "y": 1.9557088507092393 + }, + { + "x": 0.8541608801117707, + "y": 1.9566895896295846 + }, + { + "x": 0.8154002015124151, + "y": 1.9569164586522425 + }, + { + "x": 0.7765056534858624, + "y": 1.955468065542981 + }, + { + "x": 0.7382448540943768, + "y": 1.9542045644253456 + }, + { + "x": 0.6999095503434687, + "y": 1.9512702097734151 + }, + { + "x": 0.6618827170286178, + "y": 1.9476036422174552 + }, + { + "x": 0.6195908088839223, + "y": 1.9289332083536452 + }, + { + "x": 0.5870988977548404, + "y": 1.9390696163361365 + }, + { + "x": 0.550610022910833, + "y": 1.935193256596782 + }, + { + "x": 0.5180226824604571, + "y": 1.9441699612338421 + }, + { + "x": 0.48142997359197104, + "y": 1.937070310058182 + }, + { + "x": 0.44522309299011675, + "y": 1.9292942940358582 + }, + { + "x": 0.41274972199825166, + "y": 1.936501418156303 + }, + { + "x": 0.37420785858866573, + "y": 1.9127387386383714 + }, + { + "x": 0.34202262857365895, + "y": 1.9187552020153555 + }, + { + "x": 0.3073570265411176, + "y": 1.909420780565876 + }, + { + "x": 0.272959926586488, + "y": 1.8984774985836927 + }, + { + "x": 0.24102856127256217, + "y": 1.9027950932251152 + }, + { + "x": 0.2072854402985382, + "y": 1.8906709278972462 + }, + { + "x": 0.1754944866146344, + "y": 1.8938863473090088 + }, + { + "x": 0.14252108581398013, + "y": 1.8816101102325187 + }, + { + "x": 0.11089148563417436, + "y": 1.8837388208678698 + }, + { + "x": 0.07851680090997618, + "y": 1.8683509118494237 + }, + { + "x": 0.04711897628781832, + "y": 1.8694062747052331 + }, + { + "x": 0.015581854479624244, + "y": 1.8549345747420878 + }, + { + "x": -0.015581705685521542, + "y": 1.8549345759919849 + }, + { + "x": -0.04674086819428085, + "y": 1.8544110552957453 + }, + { + "x": -0.0772570268264135, + "y": 1.8383774026676885 + }, + { + "x": -0.1081293391898147, + "y": 1.8368201242472346 + }, + { + "x": -0.13897113399059144, + "y": 1.8347444363590864 + }, + { + "x": -0.16977370668646743, + "y": 1.8321509248288446 + }, + { + "x": -0.19878463540012198, + "y": 1.8131355912363103 + }, + { + "x": -0.22934142919540365, + "y": 1.8105324307783537 + }, + { + "x": -0.2575897707430508, + "y": 1.7915767644369816 + }, + { + "x": -0.2876504233893293, + "y": 1.7869966499089698 + }, + { + "x": -0.34751971502915996, + "y": 1.77632481278847 + }, + { + "x": -0.3804383694858133, + "y": 1.7849066701283125 + }, + { + "x": -0.41036963417913797, + "y": 1.7782637423591714 + }, + { + "x": -0.4440442152462818, + "y": 1.7866464650114005 + }, + { + "x": -0.4778577243690961, + "y": 1.7934290610499777 + }, + { + "x": -0.5079184107255575, + "y": 1.7851483663280874 + }, + { + "x": -0.5378357462391897, + "y": 1.776363845563501 + }, + { + "x": -0.5721886143799133, + "y": 1.7813594197356857 + }, + { + "x": -0.6071815344476347, + "y": 1.7866447467408406 + }, + { + "x": -0.637110000293922, + "y": 1.7761924767198813 + }, + { + "x": -0.6721595915268098, + "y": 1.7792710009647505 + }, + { + "x": -0.7019550151196511, + "y": 1.7677281940682972 + }, + { + "x": -0.7377062999292571, + "y": 1.7704556897012085 + }, + { + "x": -0.7737456014027793, + "y": 1.7724767426769943 + }, + { + "x": -0.8034125516826526, + "y": 1.7592283339065924 + }, + { + "x": -0.8393123081655178, + "y": 1.7590212770399605 + }, + { + "x": -0.8758757926800841, + "y": 1.7589959428534614 + }, + { + "x": -0.9117518241155481, + "y": 1.7564593652451304 + }, + { + "x": -0.9411302208604887, + "y": 1.740894826782607 + }, + { + "x": -0.9780873257729488, + "y": 1.7387841160385968 + }, + { + "x": -1.0152368572967176, + "y": 1.735919005517819 + }, + { + "x": -1.05152548725642, + "y": 1.7305835824623599 + }, + { + "x": -1.0889862423991468, + "y": 1.7262066829580884 + }, + { + "x": -1.1260467879500342, + "y": 1.7202193487513031 + }, + { + "x": -1.155347767748847, + "y": 1.7018872506243294 + }, + { + "x": -1.2021906179864024, + "y": 1.708408224706014 + }, + { + "x": -1.2395580245221915, + "y": 1.700091847253129 + }, + { + "x": -1.2775852643641272, + "y": 1.6917965028484256 + }, + { + "x": -1.3249204612222525, + "y": 1.6945167068812217 + }, + { + "x": -1.3632656319462404, + "y": 1.6844571993616344 + }, + { + "x": -1.410632887233959, + "y": 1.6843170648029555 + }, + { + "x": -1.4485517760149635, + "y": 1.6717180622550343 + }, + { + "x": -1.4977897372505402, + "y": 1.6709762759126727 + }, + { + "x": -1.5569238456174603, + "y": 1.6793117531401387 + }, + { + "x": -1.5849151824053909, + "y": 1.6529197469531764 + }, + { + "x": -1.6230212392707615, + "y": 1.6367123646587105 + }, + { + "x": -1.6724822987712378, + "y": 1.6308583015996476 + }, + { + "x": -1.711284767215584, + "y": 1.6135081147994692 + }, + { + "x": -2.6859077932146413, + "y": 1.6012585396644174 + }, + { + "x": -2.766208569885463, + "y": 1.586760994757329 + }, + { + "x": -2.8747864576262354, + "y": 1.5854626368836942 + }, + { + "x": -2.9407794474033166, + "y": 1.5580117774784303 + }, + { + "x": -3.0503280664971224, + "y": 1.550993944797898 + }, + { + "x": -3.1182003439358144, + "y": 1.5201277303034024 + }, + { + "x": -3.128799403757756, + "y": 1.4607613649198499 + }, + { + "x": -3.124591564521176, + "y": 1.3953533907161035 + }, + { + "x": -3.10344064006603, + "y": 1.3238325735063794 + }, + { + "x": -3.096527422454171, + "y": 1.2598279598434121 + }, + { + "x": -3.1032675583950637, + "y": 1.2022125095190095 + }, + { + "x": -3.0920583142812625, + "y": 1.1385081834845205 + }, + { + "x": -3.0994189130173946, + "y": 1.0824469278146933 + }, + { + "x": -3.0877317449934947, + "y": 1.0204985442536498 + }, + { + "x": -3.0738914935250747, + "y": 0.9589532158995366 + }, + { + "x": -3.0607825751950206, + "y": 0.8987270956464308 + }, + { + "x": -3.075448519049653, + "y": 0.8471816639551557 + }, + { + "x": -3.059225449891966, + "y": 0.787667531771433 + }, + { + "x": -3.056466387626108, + "y": 0.7324353308830805 + }, + { + "x": -3.053695825347178, + "y": 0.6777358906781618 + }, + { + "x": -3.034277992390631, + "y": 0.6201333277874471 + }, + { + "x": -3.044267532390722, + "y": 0.5690724651312483 + }, + { + "x": -3.037623346275293, + "y": 0.5151756579635396 + }, + { + "x": -3.0152027531448353, + "y": 0.4594040341106525 + }, + { + "x": -3.0076302694609818, + "y": 0.40667638634957415 + }, + { + "x": -2.999141169616644, + "y": 0.3543333592912892 + }, + { + "x": -2.988751632923041, + "y": 0.30229020377345295 + }, + { + "x": -2.9934080967382637, + "y": 0.25203899892545334 + }, + { + "x": -2.9972197249174117, + "y": 0.20171666059979498 + }, + { + "x": -2.98420572626056, + "y": 0.15053133171473596 + }, + { + "x": -2.9863134084807066, + "y": 0.10037790993277167 + }, + { + "x": -3.0345717946271233, + "y": 0.05098572590252286 + }, + { + "x": -3.019999980926504, + "y": 2.422501544197397e-7 + }, + { + "x": -3.0035760400516818, + "y": -0.050464464972832364 + }, + { + "x": -3.0023044262464555, + "y": -0.10091492822410944 + }, + { + "x": -2.9991868081643838, + "y": -0.151286535235729 + }, + { + "x": -2.9802581671671624, + "y": -0.20057464612064332 + }, + { + "x": -2.992411735299311, + "y": -0.25195462379933176 + }, + { + "x": -2.9877568295187324, + "y": -0.30218910250878833 + }, + { + "x": -2.9822586830706843, + "y": -0.352338293775176 + }, + { + "x": -2.975918847708186, + "y": -0.4023880438905694 + }, + { + "x": -2.9697276321897097, + "y": -0.4524748404126318 + }, + { + "x": -2.9774821260949147, + "y": -0.5049753197745916 + }, + { + "x": -2.96857878942843, + "y": -0.5549232881685356 + }, + { + "x": -2.973533940992011, + "y": -0.6077182099960506 + }, + { + "x": -2.9629051661954477, + "y": -0.6575853534900348 + }, + { + "x": -2.9660270533514907, + "y": -0.7107624271980107 + }, + { + "x": -2.983689304753172, + "y": -0.7682185138984924 + }, + { + "x": -2.9703628249770033, + "y": -0.8182336099119316 + }, + { + "x": -2.956198013678532, + "y": -0.868017774194732 + }, + { + "x": -2.956472667602154, + "y": -0.9223218771449869 + }, + { + "x": -2.9699954482350033, + "y": -0.9815860475370793 + }, + { + "x": -2.953086503404764, + "y": -1.0313409885506972 + }, + { + "x": -2.9353441029834855, + "y": -1.0808048521061426 + }, + { + "x": -2.93076021387319, + "y": -1.1353822534247968 + }, + { + "x": -2.9112731571501165, + "y": -1.184456371957697 + }, + { + "x": -2.905681444479248, + "y": -1.2394740162998956 + }, + { + "x": -2.899058731201656, + "y": -1.2946363081004075 + }, + { + "x": -2.8904925647619635, + "y": -1.3495010552505464 + }, + { + "x": -2.894380423572088, + "y": -1.4110145810347032 + }, + { + "x": -2.870268106282668, + "y": -1.4594386531493493 + }, + { + "x": -2.873622491986539, + "y": -1.5224317329238322 + }, + { + "x": -2.885294686970887, + "y": -1.5912573871359332 + }, + { + "x": -2.8867806433235907, + "y": -1.6559232996620017 + }, + { + "x": -2.85855513527877, + "y": -1.7041851412339644 + }, + { + "x": -2.868632845194751, + "y": -1.776181884988088 + }, + { + "x": -2.851849738295306, + "y": -1.8327721575349487 + }, + { + "x": -2.8605966506830223, + "y": -1.9070476011147235 + }, + { + "x": -2.866819085826218, + "y": -1.9815581237905788 + }, + { + "x": -2.8591404097847843, + "y": -2.0480734448240865 + }, + { + "x": -2.8492257024451964, + "y": -2.114288908069928 + }, + { + "x": -2.851365719800591, + "y": -2.191102283427243 + }, + { + "x": -2.87676110677953, + "y": -2.288497848794251 + }, + { + "x": -2.8857747733450942, + "y": -2.3759097899781856 + }, + { + "x": -2.8934114805484525, + "y": -2.4649080922439306 + }, + { + "x": -2.874851576821066, + "y": -2.5336638575017627 + }, + { + "x": -2.8547916430822697, + "y": -2.6024859319880904 + }, + { + "x": -2.83322448482958, + "y": -2.6713433763719396 + }, + { + "x": -2.8101430078512704, + "y": -2.740204536363563 + }, + { + "x": -2.808073503560531, + "y": -2.8317603234081683 + }, + { + "x": -2.76010601626383, + "y": -2.878534020923091 + }, + { + "x": -2.7535122567121793, + "y": -2.9699615817529073 + }, + { + "x": -2.755292750413158, + "y": -3.0738809450521547 + }, + { + "x": -2.744521649333594, + "y": -3.1673461743926943 + }, + { + "x": -2.7077043381680284, + "y": -3.713698292183364 + }, + { + "x": -2.617887097247951, + "y": -3.7202239452108605 + }, + { + "x": -1.915479109099363, + "y": -4.194312976575559 + }, + { + "x": -1.8195429945569341, + "y": -4.168160986228196 + }, + { + "x": -1.7319565260259129, + "y": -4.156601211772904 + }, + { + "x": -1.6445387950612462, + "y": -4.141428114270708 + }, + { + "x": -1.5581245641490824, + "y": -4.124503380553498 + }, + { + "x": -1.4781498370155377, + "y": -4.120916785794892 + }, + { + "x": -1.3987384736140411, + "y": -4.115815926849884 + }, + { + "x": -1.31961254355552, + "y": -4.108265842019415 + }, + { + "x": -1.2414275759537468, + "y": -4.100184494299099 + }, + { + "x": -1.1638891005173397, + "y": -4.090644119472083 + }, + { + "x": -1.0870240535718279, + "y": -4.079664659958953 + }, + { + "x": -1.0069994130439124, + "y": -4.0517377475307255 + }, + { + "x": -0.9387914779624547, + "y": -4.068082736811189 + }, + { + "x": -0.8669831768869313, + "y": -4.067630822069693 + }, + { + "x": -0.7925759833216391, + "y": -4.051198034593689 + }, + { + "x": -0.7244073735492279, + "y": -4.063940964381931 + }, + { + "x": -0.6560343128150931, + "y": -4.075536919872431 + }, + { + "x": -0.5874760981867871, + "y": -4.0859826283151595 + }, + { + "x": -0.518752078988222, + "y": -4.0952751415966455 + }, + { + "x": -0.4447594595179925, + "y": -4.056691855303097 + }, + { + "x": -0.37516362576402695, + "y": -4.048655101366269 + }, + { + "x": -0.30588820710449904, + "y": -4.038432115277763 + }, + { + "x": -0.23800273362017182, + "y": -4.043000895837367 + }, + { + "x": -0.16942025861487184, + "y": -4.031441497105795 + }, + { + "x": -0.1016714795267224, + "y": -4.033718715883222 + }, + { + "x": -0.0338940054907741, + "y": -4.034857490049375 + }, + { + "x": 0.034019033818506975, + "y": -4.049857312337125 + }, + { + "x": 0.10167050882904928, + "y": -4.033718740349926 + }, + { + "x": 0.1694192884652, + "y": -4.031441537875961 + }, + { + "x": 0.23618001083236956, + "y": -4.012054390428766 + }, + { + "x": 0.30347034369120723, + "y": -4.006523525715517 + }, + { + "x": 0.37073378910444293, + "y": -4.000860091183922 + }, + { + "x": 0.43789260156874205, + "y": -3.994067485797318 + }, + { + "x": 0.5049278267163028, + "y": -3.9861476266464977 + }, + { + "x": 0.5740975511927509, + "y": -3.992939688424803 + }, + { + "x": 0.6410946095809519, + "y": -3.9827318583165656 + }, + { + "x": 0.7107185549419187, + "y": -3.987151951031425 + }, + { + "x": 0.7835510324156888, + "y": -4.005072729091636 + }, + { + "x": 0.8507223585767058, + "y": -3.991344542172588 + }, + { + "x": 0.9212514043753333, + "y": -3.9920803912472294 + }, + { + "x": 0.9920441441454099, + "y": -3.9915682038158153 + }, + { + "x": 1.0950044420682452, + "y": -4.109619498484672 + }, + { + "x": 1.1595095666130308, + "y": -4.075255386838428 + }, + { + "x": 1.2278068494050376, + "y": -4.055201504682207 + }, + { + "x": 1.295757605780642, + "y": -4.034003114783674 + }, + { + "x": 1.3633426578601056, + "y": -4.0116662000073875 + }, + { + "x": 1.4305429309771567, + "y": -3.9881970645460902 + }, + { + "x": 1.496986088532125, + "y": -3.9626669261787746 + }, + { + "x": 1.5692492625797008, + "y": -3.95182989501902 + }, + { + "x": 1.6354152787160183, + "y": -3.9249101433555365 + }, + { + "x": 1.7135221146860662, + "y": -3.925293733866091 + }, + { + "x": 1.785868538854257, + "y": -3.910507988126306 + }, + { + "x": 1.8577695206802378, + "y": -3.8934932293610847 + }, + { + "x": 1.937624172859532, + "y": -3.891275192790883 + }, + { + "x": 2.017002893721157, + "y": -3.8856893111564696 + }, + { + "x": 2.0891281571631426, + "y": -3.8644525678255963 + }, + { + "x": 2.1694415917643672, + "y": -3.8567019418055195 + }, + { + "x": 2.2571475629215687, + "y": -3.8594206325379243 + }, + { + "x": 2.338280588827375, + "y": -3.8483049423959153 + }, + { + "x": 2.4271424902768843, + "y": -3.847385916222869 + }, + { + "x": 2.339711316161115, + "y": -2.793651257091482 + }, + { + "x": 2.3430913763628185, + "y": -2.70407235357668 + }, + { + "x": 2.3468040962287837, + "y": -2.6181609503496706 + }, + { + "x": 2.35850157227446, + "y": -2.5439009483346053 + }, + { + "x": 2.368375232309642, + "y": -2.469996414783242 + }, + { + "x": 2.3757367363848543, + "y": -2.3957778378809276 + }, + { + "x": 2.5123579832385223, + "y": -2.368811271134756 + }, + { + "x": 2.505239983612657, + "y": -2.283828657268047 + }, + { + "x": 2.484735507459009, + "y": -2.1898478709772715 + }, + { + "x": 2.475499127448439, + "y": -2.108888019280873 + }, + { + "x": 2.4750648894372875, + "y": -2.037766023831247 + }, + { + "x": 2.4596457598214383, + "y": -1.9566785706411811 + }, + { + "x": 2.4556945987804633, + "y": -1.8870538722726786 + }, + { + "x": 2.4493056557260386, + "y": -1.8175262072272238 + }, + { + "x": 2.4551045188840614, + "y": -1.7586533729394216 + }, + { + "x": 2.445639059742114, + "y": -1.6904374831185516 + }, + { + "x": 2.4345854809771206, + "y": -1.6230435076877259 + }, + { + "x": 2.435428733229034, + "y": -1.565155511419532 + }, + { + "x": 2.4222683808498173, + "y": -1.4998055743056613 + }, + { + "x": 2.4204949738220245, + "y": -1.443027554291607 + }, + { + "x": 2.417504869489822, + "y": -1.3867367725760367 + }, + { + "x": 2.3805466790824257, + "y": -1.2612032110409563 + }, + { + "x": 2.3586112266044914, + "y": -1.1992783652005874 + }, + { + "x": 2.37842526996081, + "y": -1.1594864167875296 + }, + { + "x": 2.3839765104526944, + "y": -1.1130216612501769 + }, + { + "x": 2.387728475242581, + "y": -1.0662916010139558 + }, + { + "x": 2.2627337485823, + "y": -0.9652130183072539 + }, + { + "x": 2.2638088357821378, + "y": -0.9210350606812326 + }, + { + "x": 2.2509878621240262, + "y": -0.8720377344625243 + }, + { + "x": 2.2512434861613246, + "y": -0.828917020824302 + }, + { + "x": 2.249745586579751, + "y": -0.7857056094887381 + }, + { + "x": 2.247435717902517, + "y": -0.7427800357326417 + }, + { + "x": 2.246231742342804, + "y": -0.7007507471478663 + }, + { + "x": 2.257686794023556, + "y": -0.6629170310290119 + }, + { + "x": 2.2675406336534243, + "y": -0.6246306634791949 + }, + { + "x": 2.262219240121977, + "y": -0.5824602554866343 + }, + { + "x": 2.2716848536284022, + "y": -0.5443746553568742 + }, + { + "x": 2.265865773587731, + "y": -0.5028854307763602 + }, + { + "x": 2.2592977659818585, + "y": -0.46174622709807583 + }, + { + "x": 2.2667358962671407, + "y": -0.42372673999162014 + }, + { + "x": 2.2735342810847663, + "y": -0.3855876636282593 + }, + { + "x": 2.2796910017128043, + "y": -0.3473397620768398 + }, + { + "x": 2.2693485775453976, + "y": -0.3068498963329575 + }, + { + "x": 2.274183159702643, + "y": -0.2686834222868414 + }, + { + "x": 2.277381042659329, + "y": -0.23034049515676686 + }, + { + "x": 2.2809291968678513, + "y": -0.19204987906514612 + }, + { + "x": 2.2838335998047117, + "y": -0.15370506034678874 + }, + { + "x": 2.2860934317543813, + "y": -0.11531686113762306 + }, + { + "x": 2.287708054919907, + "y": -0.07689611581694138 + } + ], + "stamp": { + "sec": 1784882222, + "nanosec": 694827303 + } + }, + "captured_at": "2026-07-24T08:37:02.757Z" + } + ], + "generated_at": "2026-07-24T08:37:03.963Z" +} \ No newline at end of file diff --git a/src/origincar_base/config/wall_map_calibration (2).json b/src/origincar_base/config/wall_map_calibration (2).json new file mode 100644 index 0000000..21d76b4 --- /dev/null +++ b/src/origincar_base/config/wall_map_calibration (2).json @@ -0,0 +1,5437 @@ +{ + "coordinate_frame": { + "origin": "map_lower_left", + "origin_note": "map (0,0) is the lower-left map corner; odom zero is placed at the P point", + "width_m": 5, + "height_m": 5, + "image_width_px": 500, + "image_height_px": 500, + "pixels_per_meter": 100, + "p_image_px": { + "x": 56, + "y": 482 + }, + "odom_zero_reference": "P", + "odom_zero_map": { + "x": 0.56, + "y": 0.18 + } + }, + "walls": [ + { + "name": "top", + "a": 0, + "b": -1, + "c": 5.517135597708222, + "color": "#b36cff" + }, + { + "name": "bottom", + "a": 0, + "b": 1, + "c": 0.24605343179761974, + "color": "#24c7d9" + }, + { + "name": "left", + "a": 1, + "b": 0, + "c": 0.1353930847310167, + "color": "#35c56f" + }, + { + "name": "right", + "a": -1, + "b": 0, + "c": 5.102447144358728, + "color": "#ff5b5b" + } + ], + "reference_points": { + "P": { + "label": "P", + "image": { + "x": 56, + "y": 482 + }, + "map": { + "x": 0.56, + "y": 0.18 + } + }, + "A": { + "label": "任务发布点左中点", + "image": { + "x": 450, + "y": 319 + }, + "map": { + "x": 4.5, + "y": 1.81 + } + }, + "B": { + "label": "通道", + "image": { + "x": 250, + "y": 240 + }, + "map": { + "x": 2.5, + "y": 2.6 + } + }, + "C": { + "label": "诊疗室", + "image": { + "x": 280, + "y": 123 + }, + "map": { + "x": 2.8, + "y": 3.77 + } + } + }, + "fit_quality": { + "left": { + "point_count": 313 + }, + "right": { + "point_count": 411 + }, + "bottom": { + "point_count": 283 + }, + "top": { + "point_count": 65 + } + }, + "calibration_captures": [ + { + "key": "P", + "label": "P", + "image": { + "x": 56, + "y": 482 + }, + "map": { + "x": 0.56, + "y": 0.18 + }, + "odom_pose": { + "frame_id": "odom", + "stamp": { + "sec": 1784882167, + "nanosec": 287620295 + }, + "x": 0.016151936730415286, + "y": -0.0026330311461741354, + "theta": -0.07343995175124807 + }, + "scan": { + "frame_id": "pose", + "points": [ + { + "x": 4.438370352446971, + "y": 0.07477142498150946 + }, + { + "x": 4.421489368074988, + "y": 0.14901636776935934 + }, + { + "x": 4.41835201299862, + "y": 0.22347169640554312 + }, + { + "x": 4.413960962265093, + "y": 0.29786361554097696 + }, + { + "x": 4.4083174618233985, + "y": 0.3721710166660462 + }, + { + "x": 4.401423113001808, + "y": 0.446372815252897 + }, + { + "x": 4.392286887952563, + "y": 0.5203303233158304 + }, + { + "x": 4.382899187820247, + "y": 0.5942410797366631 + }, + { + "x": 4.387096140386448, + "y": 0.670248647553117 + }, + { + "x": 4.390957251751719, + "y": 0.7467331379466586 + }, + { + "x": 4.377756165000672, + "y": 0.8205893771048233 + }, + { + "x": 4.393681850652673, + "y": 0.9004365568264803 + }, + { + "x": 4.378867413265467, + "y": 0.974534106457655 + }, + { + "x": 4.391972992278944, + "y": 1.0553975682722996 + }, + { + "x": 4.43457185902176, + "y": 1.1449767370212158 + }, + { + "x": 4.414656487970359, + "y": 1.219511129251669 + }, + { + "x": 4.42322618780224, + "y": 1.302456008626271 + }, + { + "x": 4.5638593004597405, + "y": 1.4278350415335561 + }, + { + "x": 4.5543483580888315, + "y": 1.5095408834336665 + }, + { + "x": 4.528275200950408, + "y": 1.5860410934278057 + }, + { + "x": 4.515926723329014, + "y": 1.6676339761085675 + }, + { + "x": 4.516091276913968, + "y": 1.7546915813152753 + }, + { + "x": 4.5145966231036185, + "y": 1.8422249113080043 + }, + { + "x": 4.497635730475038, + "y": 1.9243021531844242 + }, + { + "x": 4.492875504272423, + "y": 2.012460512260303 + }, + { + "x": 4.486413959728238, + "y": 2.1010011290238695 + }, + { + "x": 4.478236498240625, + "y": 2.1898913664755564 + }, + { + "x": 4.482582432781556, + "y": 2.2863681650091086 + }, + { + "x": 4.470808640000275, + "y": 2.3760978834557793 + }, + { + "x": 4.4572757320751455, + "y": 2.466075353501005 + }, + { + "x": 4.4827090055600705, + "y": 2.579709918034299 + }, + { + "x": 4.438619866327283, + "y": 2.654851591328348 + }, + { + "x": 4.354197324891375, + "y": 2.7049659725866335 + }, + { + "x": 4.320622836715916, + "y": 2.7860540527459654 + }, + { + "x": 4.312146650621412, + "y": 2.884568834564953 + }, + { + "x": 4.301566146979226, + "y": 2.9835809157886315 + }, + { + "x": 4.339205097101707, + "y": 3.119236069539007 + }, + { + "x": 4.323743785438062, + "y": 3.219956281824681 + }, + { + "x": 4.367874694318318, + "y": 3.3686680427450444 + }, + { + "x": 4.346459465387926, + "y": 3.4704656556002753 + }, + { + "x": 4.347510788323729, + "y": 3.5928746762677832 + }, + { + "x": 4.357814716984588, + "y": 3.7266887788984713 + }, + { + "x": 4.340857492675963, + "y": 3.840647376745088 + }, + { + "x": 4.402428585239526, + "y": 4.029348058658877 + }, + { + "x": 4.345552154405537, + "y": 4.113931810163297 + }, + { + "x": 4.286357311188622, + "y": 4.197039774701435 + }, + { + "x": 4.236834896933111, + "y": 4.290703041043336 + }, + { + "x": 4.239919558570812, + "y": 4.441022460795691 + }, + { + "x": 4.217416814707337, + "y": 4.569126678540555 + }, + { + "x": 4.201773194052299, + "y": 4.708908725880537 + }, + { + "x": 4.223746532319315, + "y": 4.897147570719015 + }, + { + "x": 4.180996197286304, + "y": 5.015991780134269 + }, + { + "x": 4.754506401752304, + "y": 6.793521166455235 + }, + { + "x": 4.587366909948152, + "y": 6.795562648782882 + }, + { + "x": 4.429704096865123, + "y": 6.806494495924331 + }, + { + "x": 4.289456106391127, + "y": 6.840325145906352 + }, + { + "x": 3.1623703525327826, + "y": 5.659390442568065 + }, + { + "x": 3.059025531327694, + "y": 5.697758497634303 + }, + { + "x": 2.926884475937348, + "y": 5.679143199868245 + }, + { + "x": 2.8170731822479795, + "y": 5.69984738071638 + }, + { + "x": 2.6941336654384482, + "y": 5.690453395010441 + }, + { + "x": 2.578506833584503, + "y": 5.6922143683283455 + }, + { + "x": 2.4512765982722153, + "y": 5.663257374148202 + }, + { + "x": 2.3257623139928953, + "y": 5.6316496578311845 + }, + { + "x": 2.2133656004583413, + "y": 5.626289075832756 + }, + { + "x": 2.112675519466481, + "y": 5.647787591591576 + }, + { + "x": 1.9704083491688513, + "y": 5.550638644762062 + }, + { + "x": 1.851780844119308, + "y": 5.509106039569819 + }, + { + "x": 1.7493410644852143, + "y": 5.509969911705946 + }, + { + "x": 1.6474001453645855, + "y": 5.5089538717484965 + }, + { + "x": 1.5500473235839225, + "y": 5.5205173359917605 + }, + { + "x": 1.4410862675493534, + "y": 5.485877649979994 + }, + { + "x": 1.3484765715587474, + "y": 5.509373299843324 + }, + { + "x": 1.2484011075465142, + "y": 5.500099378068925 + }, + { + "x": 1.152505873174772, + "y": 5.505665737428822 + }, + { + "x": 1.0507502390361347, + "y": 5.478139164094614 + }, + { + "x": 0.9530003027861944, + "y": 5.464521814498036 + }, + { + "x": 0.8560087721936243, + "y": 5.449174497977252 + }, + { + "x": 0.7618839575268103, + "y": 5.446974649772412 + }, + { + "x": 0.6662493861264833, + "y": 5.428265952712744 + }, + { + "x": 0.5730386741183917, + "y": 5.4228070615521835 + }, + { + "x": 0.47887654148551695, + "y": 5.400811097734903 + }, + { + "x": 0.38669176912648967, + "y": 5.392152349525381 + }, + { + "x": 0.2966858832244066, + "y": 5.413876683310858 + }, + { + "x": 0.20605763150337683, + "y": 5.434094814354785 + }, + { + "x": 0.1144953942230397, + "y": 5.436794736202579 + }, + { + "x": 0.022900669178468336, + "y": 5.437951981973502 + }, + { + "x": -0.0687005538702029, + "y": 5.437566223302139 + }, + { + "x": -0.1602822833190482, + "y": 5.435637569646489 + }, + { + "x": -0.251818533095399, + "y": 5.432166568256909 + }, + { + "x": -0.3422732847844765, + "y": 5.411185845443165 + }, + { + "x": -0.43337184499153353, + "y": 5.404652819503789 + }, + { + "x": -0.525894781446996, + "y": 5.412511512943429 + }, + { + "x": -0.6169896204120656, + "y": 5.40288534092684 + }, + { + "x": -0.7077791593138293, + "y": 5.390734223011471 + }, + { + "x": -0.7961316322644182, + "y": 5.362221131720226 + }, + { + "x": -0.8863410302802703, + "y": 5.348050169865711 + }, + { + "x": -0.9734174368636083, + "y": 5.316623530177606 + }, + { + "x": -0.7770588278607213, + "y": 1.4770716559219574 + }, + { + "x": -0.786935508910348, + "y": 1.4365850278722998 + }, + { + "x": -0.7877508874010696, + "y": 1.3822913639969578 + }, + { + "x": -0.8109227243982758, + "y": 1.368826216375264 + }, + { + "x": -0.698058527000409, + "y": 0.9797566898324197 + }, + { + "x": -0.6877371991839903, + "y": 0.9316552619874384 + }, + { + "x": -0.6838968626964522, + "y": 0.8945172924153035 + }, + { + "x": -0.6796266842032345, + "y": 0.8585642857557728 + }, + { + "x": -0.674344819516951, + "y": 0.8230158483979877 + }, + { + "x": -0.6777646221466184, + "y": 0.7993366443443718 + }, + { + "x": -0.6812404982802301, + "y": 0.7765309831169486 + }, + { + "x": -0.6834711309924748, + "y": 0.7530976906281014 + }, + { + "x": -0.6864775972111706, + "y": 0.7312711881475524 + }, + { + "x": -0.6875521340563108, + "y": 0.7081250174679438 + }, + { + "x": -0.6880449042561416, + "y": 0.685153442876662 + }, + { + "x": -0.6886824336653506, + "y": 0.663062929920371 + }, + { + "x": -0.6880421591648114, + "y": 0.6404670036227901 + }, + { + "x": -0.6987327011398482, + "y": 0.628786615534668 + }, + { + "x": -0.6979075704431539, + "y": 0.6070832275494465 + }, + { + "x": -0.6957872836364909, + "y": 0.5849453125322791 + }, + { + "x": -0.7055415080755978, + "y": 0.5731423410516305 + }, + { + "x": -0.715095536984331, + "y": 0.5611767419528572 + }, + { + "x": -0.7116951444465156, + "y": 0.539387646590177 + }, + { + "x": -0.720679729347353, + "y": 0.5273231832857932 + }, + { + "x": -0.71720687050774, + "y": 0.5064566533661499 + }, + { + "x": -0.7256359770627634, + "y": 0.4943040214630215 + }, + { + "x": -0.7204858714193273, + "y": 0.4732273125629146 + }, + { + "x": -0.728354790965884, + "y": 0.461024162867229 + }, + { + "x": -0.7360170417735945, + "y": 0.4486901987889113 + }, + { + "x": -0.7434704496991282, + "y": 0.4362289200575336 + }, + { + "x": -0.7376494626091232, + "y": 0.41627187649304925 + }, + { + "x": -0.7445565780939661, + "y": 0.4037877002381036 + }, + { + "x": -0.7512524276258655, + "y": 0.3911889502948883 + }, + { + "x": -0.6584333425014968, + "y": 0.32887921124330666 + }, + { + "x": -0.663879636762259, + "y": 0.31774176621609546 + }, + { + "x": -0.6545911130478019, + "y": 0.29985082277287656 + }, + { + "x": -0.8603999335743518, + "y": 0.29324548935829076 + }, + { + "x": -0.8499880295922988, + "y": 0.27380533499666826 + }, + { + "x": -0.8544794716640641, + "y": 0.2594491172225067 + }, + { + "x": -5.648567730810799, + "y": 1.6116945870508923 + }, + { + "x": -5.64496460317401, + "y": 1.5083178945690134 + }, + { + "x": -5.639490143466505, + "y": 1.4055221704462744 + }, + { + "x": -5.632163402812239, + "y": 1.3033410880585135 + }, + { + "x": -5.623002637771807, + "y": 1.2018075285216403 + }, + { + "x": -0.6260664568184171, + "y": 0.12282021922099895 + }, + { + "x": -0.6122960306723176, + "y": 0.10944196965865516 + }, + { + "x": -0.5992442954583053, + "y": 0.09672262535909042 + }, + { + "x": -0.6007884941291002, + "y": 0.08661510963892993 + }, + { + "x": -0.6170426568951662, + "y": 0.07837304046415401 + }, + { + "x": -1.0796553004950444, + "y": 0.027285981933966995 + }, + { + "x": -1.0799617364856797, + "y": 0.009096176338476482 + }, + { + "x": -1.0799617361998601, + "y": -0.009096210272908363 + }, + { + "x": -1.063660374530705, + "y": -0.026881777783641023 + }, + { + "x": -0.8922082719374149, + "y": -0.037595364199434364 + }, + { + "x": -0.8764745037256014, + "y": -0.05173471910919518 + }, + { + "x": -0.8754787248914699, + "y": -0.06649089490112607 + }, + { + "x": -0.8891701863610557, + "y": -0.0826159283036993 + }, + { + "x": -0.9035565968120592, + "y": -0.0993299505745489 + }, + { + "x": -0.9017552719403759, + "y": -0.11453555016444117 + }, + { + "x": -0.8838618331762748, + "y": -0.12742555386229032 + }, + { + "x": -0.8815900543919115, + "y": -0.1422954248038316 + }, + { + "x": -0.8643021589891111, + "y": -0.1544856459646263 + }, + { + "x": -0.8458765918585531, + "y": -0.16594207307414433 + }, + { + "x": -0.8429614220621536, + "y": -0.18016664837127194 + }, + { + "x": -0.8398070641995062, + "y": -0.1943401018600956 + }, + { + "x": -0.8364144133112825, + "y": -0.20845841186080488 + }, + { + "x": -0.8327844320531679, + "y": -0.22251757234040778 + }, + { + "x": -0.8289181504227099, + "y": -0.23651359404943204 + }, + { + "x": -0.8401265187933946, + "y": -0.2550910999279248 + }, + { + "x": -0.8357105203613346, + "y": -0.2692061719198388 + }, + { + "x": -0.8443088742155634, + "y": -0.28776128961106884 + }, + { + "x": -0.8543974146712835, + "y": -0.3073581688324656 + }, + { + "x": -1.6446600963798073, + "y": -0.7533752203133983 + }, + { + "x": -1.6596990938275384, + "y": -0.7943544805572009 + }, + { + "x": -1.646083357080201, + "y": -0.8221980931483777 + }, + { + "x": -1.617454664193721, + "y": -0.8771775943964998 + }, + { + "x": -1.5885154570244788, + "y": -0.8964344071226373 + }, + { + "x": -1.560252955811662, + "y": -0.9154735217838199 + }, + { + "x": -1.5309495771541286, + "y": -0.9332964859324314 + }, + { + "x": -1.5023373639093272, + "y": -0.950929248066425 + }, + { + "x": -1.459360045638734, + "y": -0.9585324392141216 + }, + { + "x": -0.6012121109261549, + "y": -0.4245468169488846 + }, + { + "x": -0.5810631690255987, + "y": -0.4251654205137804 + }, + { + "x": -0.5738191582638535, + "y": -0.4348926474523269 + }, + { + "x": -0.566412327838896, + "y": -0.4444964747563858 + }, + { + "x": -0.5456498185883514, + "y": -0.4432553307211626 + }, + { + "x": -0.5381061219057736, + "y": -0.45238348146871665 + }, + { + "x": -0.5432361626767386, + "y": -0.4725404879573637 + }, + { + "x": -0.5351995232575455, + "y": -0.48162382779886137 + }, + { + "x": -2.771233791904495, + "y": -2.952060727811548 + }, + { + "x": -2.711034907375421, + "y": -2.987213598769834 + }, + { + "x": -2.2080988872827905, + "y": -2.694912814553305 + }, + { + "x": -2.133220807745767, + "y": -2.6948725822405173 + }, + { + "x": -0.5332694584353413, + "y": -0.6975010548683237 + }, + { + "x": -0.5214449512852002, + "y": -0.7063845975256835 + }, + { + "x": -0.7526034438185167, + "y": -1.0563130158554157 + }, + { + "x": -0.7347039169814568, + "y": -1.0688401589972 + }, + { + "x": -1.7329528734403687, + "y": -3.040867366136637 + }, + { + "x": -1.6737992131280415, + "y": -3.0555935541552417 + }, + { + "x": -1.6146433528673216, + "y": -3.0691937692564792 + }, + { + "x": -1.534778301819219, + "y": -3.040606991316603 + }, + { + "x": -1.4554714168122058, + "y": -3.0084160293811664 + }, + { + "x": -0.3174275539953937, + "y": -0.7506428871828992 + }, + { + "x": -0.30473853724972216, + "y": -0.7558832052828511 + }, + { + "x": -1.1585380400787237, + "y": -3.01936175374302 + }, + { + "x": -1.1236105141547665, + "y": -3.0826059996586848 + }, + { + "x": -1.0509521823054824, + "y": -3.041549455973664 + }, + { + "x": -0.9855928133471479, + "y": -3.0160464889626986 + }, + { + "x": -0.934944622669189, + "y": -3.0331756806924335 + }, + { + "x": -0.9007044288873782, + "y": -3.1070815652397448 + }, + { + "x": -0.6531581660907555, + "y": -2.4038438169917375 + }, + { + "x": -0.6125746638332117, + "y": -2.414504704416637 + }, + { + "x": -0.14093816870782183, + "y": -0.8504001486780628 + }, + { + "x": -0.12894363427496874, + "y": -0.8684800377799767 + }, + { + "x": -0.4743694634909472, + "y": -3.6129918352147583 + }, + { + "x": -0.41162884454871423, + "y": -3.6045729076511175 + }, + { + "x": -0.29041218651704137, + "y": -2.988924537008803 + }, + { + "x": -0.23874607251164875, + "y": -2.9774434074375384 + }, + { + "x": -0.017389929876423727, + "y": -0.5897436387042782 + }, + { + "x": -0.0076684682749231705, + "y": -0.6069515520121784 + }, + { + "x": 0.042972678728891205, + "y": -2.040547450393311 + }, + { + "x": 0.07790636713898542, + "y": -2.0545234421709946 + }, + { + "x": 0.11250212683353686, + "y": -2.052919688621276 + }, + { + "x": 0.14821044888355664, + "y": -2.0666924719218565 + }, + { + "x": 0.184414351519855, + "y": -2.079840280856018 + }, + { + "x": 0.21942144253798354, + "y": -2.076438893267095 + }, + { + "x": 0.2581427716448462, + "y": -2.1032173286690945 + }, + { + "x": 0.29561105064730486, + "y": -2.1134262171587186 + }, + { + "x": 0.333651083885392, + "y": -2.123953239669383 + }, + { + "x": 0.48655081909659287, + "y": -2.7898911085677587 + }, + { + "x": 0.5334752860951505, + "y": -2.7812997350797524 + }, + { + "x": 0.5835266269609171, + "y": -2.7875797671274483 + }, + { + "x": 0.6370388291048241, + "y": -2.8066110615791495 + }, + { + "x": 0.7312141845117431, + "y": -2.7835606580663037 + }, + { + "x": 0.7823225284422262, + "y": -2.786253357217235 + }, + { + "x": 0.13265153855032194, + "y": -0.44359054191752245 + }, + { + "x": 0.14010464849384696, + "y": -0.44129319848470855 + }, + { + "x": 0.14751800411520669, + "y": -0.43887063927803904 + }, + { + "x": 0.1548895018944189, + "y": -0.4363235516922592 + }, + { + "x": 0.15661127647122913, + "y": -0.4186668189756044 + }, + { + "x": 0.16364116687809177, + "y": -0.4159694288428848 + }, + { + "x": 0.1706246245381647, + "y": -0.4131540084887349 + }, + { + "x": 0.1775596679138713, + "y": -0.41022135678120786 + }, + { + "x": 0.19063374266284863, + "y": -0.4208358202599808 + }, + { + "x": 0.19812324775161422, + "y": -0.41846884980700927 + }, + { + "x": 0.20514391406952204, + "y": -0.41507225163661005 + }, + { + "x": 0.21210637134627497, + "y": -0.41155787781643166 + }, + { + "x": 0.21900864400309936, + "y": -0.40792672554071824 + }, + { + "x": 0.234141272110168, + "y": -0.41902011216044244 + }, + { + "x": 0.24116611399435964, + "y": -0.4150167408203879 + }, + { + "x": 0.2481225256337724, + "y": -0.410895609581601 + }, + { + "x": 0.2550085331650663, + "y": -0.406657887804133 + }, + { + "x": 0.2618221827018801, + "y": -0.40230477793029107 + }, + { + "x": 0.26856154088924156, + "y": -0.3978375151434487 + }, + { + "x": 0.28382547626313676, + "y": -0.4055466727081995 + }, + { + "x": 0.290616319900315, + "y": -0.4007083220102693 + }, + { + "x": 0.29732470192803184, + "y": -0.39575627138946884 + }, + { + "x": 0.3137733071307587, + "y": -0.4033203302932187 + }, + { + "x": 0.32052240099273993, + "y": -0.3979778480738013 + }, + { + "x": 0.3271805474741222, + "y": -0.39252244069624626 + }, + { + "x": 0.33374585734351225, + "y": -0.386955656117918 + }, + { + "x": 0.3508690601844962, + "y": -0.39321738664454065 + }, + { + "x": 0.35744271508909703, + "y": -0.3872514893245065 + }, + { + "x": 0.3639149465719624, + "y": -0.38117571042216153 + }, + { + "x": 0.3815259153342607, + "y": -0.38637670386027323 + }, + { + "x": 0.3994122098900264, + "y": -0.3910893806409117 + }, + { + "x": 0.4059431332318476, + "y": -0.3843061144988938 + }, + { + "x": 0.4123588712870078, + "y": -0.3774138025168546 + }, + { + "x": 0.42989169644002656, + "y": -0.3803539541357946 + }, + { + "x": 0.44839737451245265, + "y": -0.38345764248950426 + }, + { + "x": 0.4678970124978589, + "y": -0.3866799420596498 + }, + { + "x": 0.48065628599629034, + "y": -0.3707000494037302 + }, + { + "x": 0.48683224088214494, + "y": -0.36255118415658905 + }, + { + "x": 0.5050497024099491, + "y": -0.36305477822976484 + }, + { + "x": 0.5242405426588211, + "y": -0.36361499973934786 + }, + { + "x": 0.5427586160314681, + "y": -0.36307310694557 + }, + { + "x": 0.5630845258419624, + "y": -0.36309205323305455 + }, + { + "x": 0.5827115528168717, + "y": -0.36199893897758517 + }, + { + "x": 0.6033159138268642, + "y": -0.3608585884546517 + }, + { + "x": 0.6240430613818542, + "y": -0.3591243499679389 + }, + { + "x": 0.6440037476748436, + "y": -0.35630769720351163 + }, + { + "x": 0.6790542635928064, + "y": -0.360896519188741 + }, + { + "x": 0.6992900115940054, + "y": -0.35667705401349437 + }, + { + "x": 0.7330473164511299, + "y": -0.3584656286751265 + }, + { + "x": 0.7534712453187414, + "y": -0.35285281781824906 + }, + { + "x": 0.7729973040091479, + "y": -0.3462429390488156 + }, + { + "x": 0.9466153199572828, + "y": -0.3315546381966728 + }, + { + "x": 0.9947805688820732, + "y": -0.32972044795542765 + }, + { + "x": 1.6203232562289498, + "y": -0.33206729806219804 + }, + { + "x": 1.6256867844202922, + "y": -0.3047271305281581 + } + ], + "stamp": { + "sec": 1784882167, + "nanosec": 307027762 + } + }, + "captured_at": "2026-07-24T08:36:07.326Z" + }, + { + "key": "A", + "label": "任务发布点左中点", + "image": { + "x": 450, + "y": 319 + }, + "map": { + "x": 4.5, + "y": 1.81 + }, + "odom_pose": { + "frame_id": "odom", + "stamp": { + "sec": 1784882182, + "nanosec": 687383993 + }, + "x": 0.011512968195960083, + "y": -0.0023025743030921047, + "theta": -0.06749248869060631 + }, + "scan": { + "frame_id": "pose", + "points": [ + { + "x": 0.6529078483824241, + "y": 0.01096985798164591 + }, + { + "x": 0.6526314281200959, + "y": 0.021936619918033008 + }, + { + "x": 0.6521708143666431, + "y": 0.03289719063770564 + }, + { + "x": 0.6515261371220196, + "y": 0.043848476716567754 + }, + { + "x": 0.6506975783347909, + "y": 0.05478738735094634 + }, + { + "x": 0.6496853718507837, + "y": 0.06571083522991515 + }, + { + "x": 0.6484898033470857, + "y": 0.07661573740663276 + }, + { + "x": 0.64711121025142, + "y": 0.08749901616844885 + }, + { + "x": 0.6455499816469109, + "y": 0.09835759990553343 + }, + { + "x": 0.6438065581622728, + "y": 0.10918842397778326 + }, + { + "x": 0.6418814318474502, + "y": 0.11998843157976173 + }, + { + "x": 0.6397751460347463, + "y": 0.13075457460342702 + }, + { + "x": 0.6540844874193774, + "y": 0.1451671646102978 + }, + { + "x": 0.6515534977427915, + "y": 0.15613475571696794 + }, + { + "x": 0.6488386188015569, + "y": 0.16705828059497946 + }, + { + "x": 0.6449765387848908, + "y": 0.17766908558769143 + }, + { + "x": 0.6419008290889026, + "y": 0.18847908417509823 + }, + { + "x": 0.6386439544205099, + "y": 0.19923588792844973 + }, + { + "x": 0.635206833974054, + "y": 0.2099364609332139 + }, + { + "x": 0.6466957134383536, + "y": 0.22585317683573394 + }, + { + "x": 0.6428103083976657, + "y": 0.23668525658357437 + }, + { + "x": 0.6396759419279876, + "y": 0.24781177317813022 + }, + { + "x": 0.6354226412958924, + "y": 0.25852282788121234 + }, + { + "x": 0.6309900040434678, + "y": 0.26916091916244406 + }, + { + "x": 0.6263792812029126, + "y": 0.279723044611755 + }, + { + "x": 0.6369956698012165, + "y": 0.29739793104984363 + }, + { + "x": 0.631909741101209, + "y": 0.3080569620048066 + }, + { + "x": 0.6266454672338164, + "y": 0.31862904941881837 + }, + { + "x": 0.6212043339457252, + "y": 0.3291112095102138 + }, + { + "x": 0.6304740871513016, + "y": 0.34771031999275515 + }, + { + "x": 0.6245438723745643, + "y": 0.3582526938881482 + }, + { + "x": 0.6184373913145063, + "y": 0.36869395739941213 + }, + { + "x": 0.6257598185979011, + "y": 0.3874540637727716 + }, + { + "x": 0.619162607170553, + "y": 0.3979116333904757 + }, + { + "x": 0.6123906482252731, + "y": 0.4082568996011491 + }, + { + "x": 0.619430352099263, + "y": 0.42815309235753635 + }, + { + "x": 0.6251574796999522, + "y": 0.44781593991429314 + }, + { + "x": 0.6175463238516261, + "y": 0.45825487271109333 + }, + { + "x": 0.6097608766522862, + "y": 0.4685644713060875 + }, + { + "x": 0.6268458712234176, + "y": 0.4986634658789114 + }, + { + "x": 0.6183802808817166, + "y": 0.5091235868518204 + }, + { + "x": 0.609740163819618, + "y": 0.5194400168425984 + }, + { + "x": 0.6121812803934694, + "y": 0.539527622122787 + }, + { + "x": 0.6030312755514883, + "y": 0.5497356235540877 + }, + { + "x": 0.605352493426121, + "y": 0.570764738983788 + }, + { + "x": 0.5956786945979345, + "y": 0.5808536144043128 + }, + { + "x": 0.5963987269822116, + "y": 0.6014295996212443 + }, + { + "x": 0.5965925791029748, + "y": 0.6221907061582057 + }, + { + "x": 0.5969341965769444, + "y": 0.6438583709166051 + }, + { + "x": 0.5960456283736237, + "y": 0.6649651275213825 + }, + { + "x": 0.5952684087579106, + "y": 0.6869763345695086 + }, + { + "x": 0.6035480298232715, + "y": 0.7206453852028597 + }, + { + "x": 0.6014222426140171, + "y": 0.7431199294615527 + }, + { + "x": 0.6079482102780631, + "y": 0.7775396781059234 + }, + { + "x": 0.6044425718160598, + "y": 0.8004112814446975 + }, + { + "x": 0.5991590043376687, + "y": 0.8217648118839147 + }, + { + "x": 1.8001410237241204, + "y": 2.651700738330269 + }, + { + "x": 1.7805342824890178, + "y": 2.7200548151868653 + }, + { + "x": 0.4917402234696982, + "y": 0.8741856751912186 + }, + { + "x": 0.4769852372540917, + "y": 0.8823231412182349 + }, + { + "x": 0.4685455911340875, + "y": 0.9026372181326896 + }, + { + "x": 0.46044774429599145, + "y": 0.9247036601043624 + }, + { + "x": 0.45819828342056923, + "y": 0.9602866006531864 + }, + { + "x": 0.45487948017607427, + "y": 0.9960470476695104 + }, + { + "x": 0.4504848689540917, + "y": 1.0319590534897547 + }, + { + "x": 0.4453931500479093, + "y": 1.068919512508179 + }, + { + "x": 0.43844517493349666, + "y": 1.1041330275253023 + }, + { + "x": 0.4604158261520157, + "y": 2.3533850675519297 + }, + { + "x": 0.4234482498082942, + "y": 2.3755549025873335 + }, + { + "x": 0.459957971616614, + "y": 4.963734984412943 + }, + { + "x": 0.3800562408421118, + "y": 5.017627119111771 + }, + { + "x": 0.28101913164279035, + "y": 4.773735735027101 + }, + { + "x": 0.20015486874779173, + "y": 4.762796334058535 + }, + { + "x": 0.13818207219770173, + "y": 5.482259021084427 + }, + { + "x": 0.04606517104981081, + "y": 5.4838067306711835 + }, + { + "x": -0.0460647311646995, + "y": 5.483806734366297 + }, + { + "x": -0.13780366528692412, + "y": 5.467263451465257 + }, + { + "x": -0.2296297073039668, + "y": 5.464176945837114 + }, + { + "x": -0.4141944095686657, + "y": 5.468336241529089 + }, + { + "x": -0.5059994704083364, + "y": 5.46060644940513 + }, + { + "x": -0.597661721991594, + "y": 5.451335499251946 + }, + { + "x": -0.6891552942953354, + "y": 5.440526007628594 + }, + { + "x": -0.780454364903152, + "y": 5.4281810253198115 + }, + { + "x": -0.8740758998101299, + "y": 5.430100489067501 + }, + { + "x": -0.6749196623371744, + "y": 3.7863173548911857 + }, + { + "x": -0.7445754170051287, + "y": 3.8058496569253752 + }, + { + "x": -0.811532323780442, + "y": 3.80747467637976 + }, + { + "x": -0.8823509110249319, + "y": 3.8235105681047634 + }, + { + "x": -0.9503173659080869, + "y": 3.8236758956468497 + }, + { + "x": -1.0185373140289038, + "y": 3.822632397864734 + }, + { + "x": -1.0867155413503116, + "y": 3.8194096381220954 + }, + { + "x": -1.1553616248941678, + "y": 3.815928252007199 + }, + { + "x": -1.2238902665827929, + "y": 3.8102618617512523 + }, + { + "x": -1.2877267929422231, + "y": 3.7891638320994985 + }, + { + "x": -1.3620041007987662, + "y": 3.7971173517671035 + }, + { + "x": -1.4309012163318546, + "y": 3.7877329603841705 + }, + { + "x": -1.4943300632866017, + "y": 3.7631604977782267 + }, + { + "x": -1.5692605184395134, + "y": 3.7661413678602615 + }, + { + "x": -1.6387084327161037, + "y": 3.7539115954288493 + }, + { + "x": -1.7144175068112186, + "y": 3.7540512004835254 + }, + { + "x": -1.777240519465743, + "y": 3.7247206525431853 + }, + { + "x": -1.846693871599574, + "y": 3.708661724622479 + }, + { + "x": -1.9230176716882967, + "y": 3.704629165118875 + }, + { + "x": -1.9925900628426114, + "y": 3.685876465776943 + }, + { + "x": -2.054228541934805, + "y": 3.651882470316899 + }, + { + "x": -2.138509977556465, + "y": 3.6565655461076165 + }, + { + "x": -2.22404120582501, + "y": 3.660290924153303 + }, + { + "x": -2.3017574941625907, + "y": 3.6486311895168906 + }, + { + "x": -2.388467959457911, + "y": 3.648772716817478 + }, + { + "x": -2.466838997590005, + "y": 3.6337819282946544 + }, + { + "x": -2.554583218416839, + "y": 3.63026538033476 + }, + { + "x": -2.6334714203626723, + "y": 3.6118868202708687 + }, + { + "x": -2.722100567962124, + "y": 3.604644128055226 + }, + { + "x": -2.8112213354330104, + "y": 3.595432072380896 + }, + { + "x": -2.8907268725765425, + "y": 3.5717952376956257 + }, + { + "x": -3.0004038246803737, + "y": 3.5825276788481983 + }, + { + "x": -3.0909423187254195, + "y": 3.567138012710126 + }, + { + "x": -3.181802187208114, + "y": 3.5497078376512246 + }, + { + "x": -3.2729396155405057, + "y": 3.530221455061193 + }, + { + "x": -3.374692769986698, + "y": 3.519492008995316 + }, + { + "x": -3.4664351845731285, + "y": 3.495676575636201 + }, + { + "x": -3.891620588234179, + "y": 3.5476813593820347 + }, + { + "x": -4.021190654449121, + "y": 3.543956426346899 + }, + { + "x": -4.1273544374281546, + "y": 3.516109867914217 + }, + { + "x": -4.269989009572173, + "y": 3.515559184150851 + }, + { + "x": -4.389486143123747, + "y": 3.491889847942149 + }, + { + "x": -4.522062594402508, + "y": 3.4749330461885926 + }, + { + "x": -4.680181836919367, + "y": 3.4729645511758505 + }, + { + "x": -4.788266957176046, + "y": 3.4299559726061184 + }, + { + "x": -4.845211669610288, + "y": 3.349033040128541 + }, + { + "x": -4.900788908131679, + "y": 3.2671649032661096 + }, + { + "x": -4.875905273841331, + "y": 3.1335545000440415 + }, + { + "x": -4.8751450320077865, + "y": 3.0185624524308 + }, + { + "x": -4.871911852688682, + "y": 2.904489304628419 + }, + { + "x": -4.852358384321503, + "y": 2.7834246125352213 + }, + { + "x": -4.857276978408434, + "y": 2.678818506965134 + }, + { + "x": -4.832668888870524, + "y": 2.5603263285052646 + }, + { + "x": -4.805469990678903, + "y": 2.443427295349912 + }, + { + "x": -4.789210482116189, + "y": 2.334747885677997 + }, + { + "x": -4.786075390940322, + "y": 2.2345037563873764 + }, + { + "x": -4.765412996304386, + "y": 2.1280973993715864 + }, + { + "x": -4.757259760676508, + "y": 2.029301076524128 + }, + { + "x": -4.776785051803565, + "y": 1.9434439116494728 + }, + { + "x": -4.779852449280101, + "y": 1.851725093002137 + }, + { + "x": -4.780256225679683, + "y": 1.7601093766416247 + }, + { + "x": -4.735511625148862, + "y": 1.6538390434234227 + }, + { + "x": -4.7626264919184385, + "y": 1.5740529952795086 + }, + { + "x": -4.744484454084078, + "y": 1.4801233662974027 + }, + { + "x": -4.768679775446855, + "y": 1.4002110961384298 + }, + { + "x": -4.746217038752426, + "y": 1.307421673123989 + }, + { + "x": -4.7374903093467555, + "y": 1.2197751881562109 + }, + { + "x": -4.741753235170516, + "y": 1.1362884976678775 + }, + { + "x": -4.744553075241361, + "y": 1.0530039952335253 + }, + { + "x": -4.777248894671991, + "y": 0.976354593135925 + }, + { + "x": -4.76250439452032, + "y": 0.8902667348226461 + }, + { + "x": -4.730449841812449, + "y": 0.8022760994076154 + }, + { + "x": -4.727442618523308, + "y": 0.7202852968050177 + }, + { + "x": -4.708155119642224, + "y": 0.6366126614267957 + }, + { + "x": -4.687399265149254, + "y": 0.5537925139322986 + }, + { + "x": -4.696041052653251, + "y": 0.47496994768571493 + }, + { + "x": -4.672467108679931, + "y": 0.3934124230728863 + }, + { + "x": -4.662452593744672, + "y": 0.31378892898515914 + }, + { + "x": -4.667066040592113, + "y": 0.23541931446237357 + }, + { + "x": -4.655370891771634, + "y": 0.15647935636991228 + }, + { + "x": -4.641345131545252, + "y": 0.0779821229192743 + }, + { + "x": -4.656342893840352, + "y": -0.07823336241011568 + }, + { + "x": -4.638380313789405, + "y": -0.15590751302571462 + }, + { + "x": -4.6510866009106495, + "y": -0.23461252063980453 + }, + { + "x": -4.69504627695203, + "y": -0.47486857251963877 + }, + { + "x": -4.687399353994441, + "y": -0.5537917619301702 + }, + { + "x": -4.677434644912931, + "y": -0.6324580317914148 + }, + { + "x": -4.650332559912294, + "y": -0.7085358468919565 + }, + { + "x": -4.622984822388759, + "y": -0.7840494552558833 + }, + { + "x": -4.609161065088238, + "y": -0.86160112140046 + }, + { + "x": -4.594036454915555, + "y": -0.9389096161137057 + }, + { + "x": -4.577615260520104, + "y": -1.01595312046242 + }, + { + "x": -4.590048291276181, + "y": -1.0999339539998876 + }, + { + "x": -4.5709225844433385, + "y": -1.176887737396842 + }, + { + "x": -4.550506816862251, + "y": -1.253509365381611 + }, + { + "x": -4.544158898361135, + "y": -1.3342850087482496 + }, + { + "x": -4.5211027640410215, + "y": -1.4104348177443862 + }, + { + "x": -4.511012808394069, + "y": -1.490893609150848 + }, + { + "x": -4.48533045701169, + "y": -1.5664644913643895 + }, + { + "x": -4.473397003623409, + "y": -1.647121774240743 + }, + { + "x": -4.459082416352983, + "y": -1.7274572713661098 + }, + { + "x": -4.444253483049892, + "y": -1.808151991394259 + }, + { + "x": -4.398534095692743, + "y": -1.8762788783260411 + }, + { + "x": -4.566359912553981, + "y": -2.0392050961299995 + }, + { + "x": -4.559548123019054, + "y": -2.1287427196639688 + }, + { + "x": -4.578873710928859, + "y": -2.232207459053865 + }, + { + "x": -4.202659652241099, + "y": -2.4107415486488515 + }, + { + "x": -4.161568132489393, + "y": -2.4810025484883895 + }, + { + "x": -3.5488063502732623, + "y": -2.197327400487924 + }, + { + "x": -3.4979323962024433, + "y": -2.24798418325205 + }, + { + "x": -3.4729871828741454, + "y": -2.3153043524052634 + }, + { + "x": -3.4204401996590468, + "y": -2.3642235040516653 + }, + { + "x": -2.8894262964374327, + "y": -2.2203495370497537 + }, + { + "x": -2.8000683097020853, + "y": -2.2274878119454744 + }, + { + "x": -2.725968609345226, + "y": -2.2443385277115784 + }, + { + "x": -2.6521034440373663, + "y": -2.2593368708955848 + }, + { + "x": -2.5665102325716305, + "y": -2.2619164998304595 + }, + { + "x": -2.469768030088651, + "y": -2.2514906014786957 + }, + { + "x": -2.3872134362675186, + "y": -2.2508159290254466 + }, + { + "x": -2.3039592679972904, + "y": -2.2466186311957217 + }, + { + "x": -2.168018678209201, + "y": -2.18630647152798 + }, + { + "x": -2.088766254206594, + "y": -2.178389050659946 + }, + { + "x": -2.019922076204554, + "y": -2.1787050156897743 + }, + { + "x": -1.9309743306078102, + "y": -2.154248472998066 + }, + { + "x": -1.8650436224920386, + "y": -2.1523746348329973 + }, + { + "x": -1.8183491477278242, + "y": -2.1711357567380487 + }, + { + "x": -1.7621171461766785, + "y": -2.17727922545964 + }, + { + "x": -1.696341976675826, + "y": -2.169548251252849 + }, + { + "x": -1.6409743703209119, + "y": -2.1730007791123374 + }, + { + "x": -1.5141045034420513, + "y": -2.1516618631789726 + }, + { + "x": -1.4597713396736431, + "y": -2.150318236190783 + }, + { + "x": -1.4064635001301566, + "y": -2.1486006665835977 + }, + { + "x": -1.3376233800671042, + "y": -2.1203331200739943 + }, + { + "x": -1.3018147833831066, + "y": -2.142504869714433 + }, + { + "x": -1.249988723214931, + "y": -2.1373124575755313 + }, + { + "x": -1.198218599416561, + "y": -2.1301192961854447 + }, + { + "x": -1.1551318461221898, + "y": -2.1367524274579797 + }, + { + "x": -1.1117018492576973, + "y": -2.1416555544320004 + }, + { + "x": -1.0688808109655954, + "y": -2.1466014583748834 + }, + { + "x": -1.0257786370756545, + "y": -2.149814420440395 + }, + { + "x": -0.9770563438111969, + "y": -2.139454344464057 + }, + { + "x": -0.9345761883741383, + "y": -2.140902423681116 + }, + { + "x": -0.886555651983336, + "y": -2.1276852172459884 + }, + { + "x": -0.850687199048571, + "y": -2.1422783659285747 + }, + { + "x": -0.8145786549409076, + "y": -2.1562668950446207 + }, + { + "x": -0.6815680931080467, + "y": -2.251081723413189 + }, + { + "x": -0.5893410762683263, + "y": -2.211831425085567 + }, + { + "x": -0.544623824444709, + "y": -2.191334849974682 + }, + { + "x": -0.5043614892263732, + "y": -2.185559110407955 + }, + { + "x": -0.4642393773655069, + "y": -2.178075019830115 + }, + { + "x": -0.42163203237946, + "y": -2.1551433513526392 + }, + { + "x": -0.38273560096582837, + "y": -2.1471549629204136 + }, + { + "x": -0.34422730712957944, + "y": -2.1384721372497566 + }, + { + "x": -0.3059771641614979, + "y": -2.1281161587403195 + }, + { + "x": -0.27018340068914726, + "y": -2.132956009877375 + }, + { + "x": -0.23256965185145803, + "y": -2.121289142402871 + }, + { + "x": -0.19551689872966846, + "y": -2.1099606547226366 + }, + { + "x": -0.15883527073860623, + "y": -2.0969930958158574 + }, + { + "x": -0.12358511296963416, + "y": -2.0993654772293135 + }, + { + "x": -0.08767026601802275, + "y": -2.0861587119330247 + }, + { + "x": -0.05220900031245389, + "y": -2.0713421568980612 + }, + { + "x": -0.017404803702035638, + "y": -2.0719269252231927 + }, + { + "x": 0.017404305101418494, + "y": -2.0719269294115263 + }, + { + "x": 0.05220850185255781, + "y": -2.0713421694618805 + }, + { + "x": 0.08632616339215698, + "y": -2.054186887794034 + }, + { + "x": 0.12082261618992164, + "y": -2.052446801232122 + }, + { + "x": 0.15528496897575933, + "y": -2.050127448448302 + }, + { + "x": 0.18970349536997327, + "y": -2.0472294840382115 + }, + { + "x": 0.222433729058817, + "y": -2.028842966964365 + }, + { + "x": 0.25837022805240406, + "y": -2.0397011550045994 + }, + { + "x": 0.29259905438943884, + "y": -2.035072915116644 + }, + { + "x": 0.3267452999021286, + "y": -2.029870312473882 + }, + { + "x": 0.3607993274263729, + "y": -2.0240948154174063 + }, + { + "x": 0.3947515258249415, + "y": -2.0177480539774604 + }, + { + "x": 0.42859231270004455, + "y": -2.0108318194133927 + }, + { + "x": 0.46590990422064665, + "y": -2.0189383526511766 + }, + { + "x": 0.49951946580100554, + "y": -2.009856142637643 + }, + { + "x": 0.5332128906815932, + "y": -2.0011810073112746 + }, + { + "x": 0.5711343753147337, + "y": -2.007330087330572 + }, + { + "x": 0.6091220207879117, + "y": -2.011808727432297 + }, + { + "x": 0.6431385782227587, + "y": -2.0022441050809934 + }, + { + "x": 0.6766838602335176, + "y": -1.9911573620458642 + }, + { + "x": 0.5848055186941019, + "y": -1.2256290687204106 + }, + { + "x": 0.5852542896948747, + "y": -1.1753494464322074 + }, + { + "x": 0.590173794365138, + "y": -1.1369502798573097 + }, + { + "x": 0.5824398692214083, + "y": -1.0354263434141528 + }, + { + "x": 0.584101877424787, + "y": -0.9987361368254916 + }, + { + "x": 0.5841807216197502, + "y": -0.961435325171604 + }, + { + "x": 0.5837093790013897, + "y": -0.9252671620778852 + }, + { + "x": 0.5821923888295661, + "y": -0.8893935829223663 + }, + { + "x": 0.5802013318522149, + "y": -0.8546668349448147 + }, + { + "x": 0.5852693531753446, + "y": -0.8317143155935371 + }, + { + "x": 0.5909109000573837, + "y": -0.8104525589329261 + }, + { + "x": 0.5948002714303512, + "y": -0.7876430755842037 + }, + { + "x": 0.5980928140500772, + "y": -0.7649353020617143 + }, + { + "x": 0.6014221233948579, + "y": -0.7431200259480545 + }, + { + "x": 0.6035479142097161, + "y": -0.720645482030403 + }, + { + "x": 0.6057460898474426, + "y": -0.6990684492152773 + }, + { + "x": 0.6067249305119701, + "y": -0.6768794733325701 + }, + { + "x": 0.6071322739726448, + "y": -0.6548583158391146 + }, + { + "x": 0.6076661441798592, + "y": -0.6337396106130098 + }, + { + "x": 0.6069605796038972, + "y": -0.6120807422859236 + }, + { + "x": 0.6171573762082817, + "y": -0.6017979339131162 + }, + { + "x": 0.6162661965465684, + "y": -0.5810550574950701 + }, + { + "x": 0.6148553524690654, + "y": -0.5605148741153928 + }, + { + "x": 0.6121811938367392, + "y": -0.5395277203353699 + }, + { + "x": 0.6211584231769033, + "y": -0.5291674483696437 + }, + { + "x": 0.7207705598321891, + "y": -0.5538689039862459 + }, + { + "x": 0.7299733854564769, + "y": -0.5416823970270238 + }, + { + "x": 0.7259630349667893, + "y": -0.5200256568522793 + }, + { + "x": 0.7090954612663254, + "y": -0.49013019401163527 + }, + { + "x": 0.7172291770076147, + "y": -0.47814881357101113 + }, + { + "x": 0.7125416779286574, + "y": -0.4579228778530139 + }, + { + "x": 0.7201338511732245, + "y": -0.4458881488007873 + }, + { + "x": 0.7146386812934012, + "y": -0.42604645728333657 + }, + { + "x": 0.7078162821912892, + "y": -0.4060198095691364 + }, + { + "x": 0.7145371937956769, + "y": -0.3940717875974576 + }, + { + "x": 0.721056440119184, + "y": -0.3820125459462113 + }, + { + "x": 0.7140013815682652, + "y": -0.3630468641798498 + }, + { + "x": 0.719999510539792, + "y": -0.3510010018428346 + }, + { + "x": 0.7112966907565338, + "y": -0.3320874265817562 + }, + { + "x": 0.7021656504642149, + "y": -0.3135672013688357 + }, + { + "x": 0.7073342323783364, + "y": -0.3017271518638448 + }, + { + "x": 0.7123031819179813, + "y": -0.28980194529305203 + }, + { + "x": 0.7021515672330305, + "y": -0.27201508823838594 + }, + { + "x": 0.7066221101209296, + "y": -0.2601811389885568 + }, + { + "x": 0.6948438214467864, + "y": -0.24266863394954732 + }, + { + "x": 0.6988223994992588, + "y": -0.23096159001836458 + }, + { + "x": 0.6873297789545888, + "y": -0.21442438331721358 + }, + { + "x": 0.6908349373900371, + "y": -0.2028475547795309 + }, + { + "x": 0.6941451200804848, + "y": -0.19121347616520312 + }, + { + "x": 0.680796305511357, + "y": -0.17528663137933562 + }, + { + "x": 0.6836449041751298, + "y": -0.16382508273645274 + }, + { + "x": 0.6697043647085743, + "y": -0.14863394463389656 + }, + { + "x": 0.6721067856064198, + "y": -0.1373624925114327 + }, + { + "x": 0.6743195164435126, + "y": -0.12605227229421417 + }, + { + "x": 0.6763419327163424, + "y": -0.11470647608862361 + }, + { + "x": 0.6781734636342641, + "y": -0.10332830604172694 + }, + { + "x": 0.6639579083656074, + "y": -0.08977704763708046 + }, + { + "x": 0.6653723931603666, + "y": -0.07861043686247698 + }, + { + "x": 0.6665990885556067, + "y": -0.06742163970490049 + }, + { + "x": 0.6676376483385992, + "y": -0.05621381400125728 + }, + { + "x": 0.668487779394511, + "y": -0.04499012295891879 + }, + { + "x": 0.6521708090889198, + "y": -0.03289729526583865 + }, + { + "x": 0.652631424600783, + "y": -0.02193672462006269 + } + ], + "stamp": { + "sec": 1784882182, + "nanosec": 657254188 + } + }, + "captured_at": "2026-07-24T08:36:22.707Z" + }, + { + "key": "B", + "label": "通道", + "image": { + "x": 250, + "y": 240 + }, + "map": { + "x": 2.5, + "y": 2.6 + }, + "odom_pose": { + "frame_id": "odom", + "stamp": { + "sec": 1784882202, + "nanosec": 87103294 + }, + "x": 0.008717998261200446, + "y": -0.0021310477980812778, + "theta": -0.05600910099161461 + }, + "scan": { + "frame_id": "pose", + "points": [ + { + "x": 2.6766222782011893, + "y": 0.04497137893980167 + }, + { + "x": 2.6754890820937645, + "y": 0.08993006551614872 + }, + { + "x": 2.673600777278327, + "y": 0.13486337094777806 + }, + { + "x": 2.543998326786614, + "y": 0.21419938599818422 + }, + { + "x": 2.5400409559780934, + "y": 0.25690621948287806 + }, + { + "x": 2.535366704260806, + "y": 0.29954054580438727 + }, + { + "x": 2.5299768908582907, + "y": 0.34209033219035595 + }, + { + "x": 2.523873036948512, + "y": 0.38454356972830556 + }, + { + "x": 2.517056865234533, + "y": 0.4268882767549347 + }, + { + "x": 2.540002600566208, + "y": 0.47480876238041314 + }, + { + "x": 2.531667772393339, + "y": 0.5174116948246041 + }, + { + "x": 2.5372619783388486, + "y": 0.5631185792559642 + }, + { + "x": 2.5430035229405195, + "y": 0.6093916082366695 + }, + { + "x": 2.5479020757769324, + "y": 0.6560154213537266 + }, + { + "x": 2.550983239399749, + "y": 0.7027090634142269 + }, + { + "x": 2.538818325775903, + "y": 0.745464301095216 + }, + { + "x": 2.5555303731943964, + "y": 0.7972413416071581 + }, + { + "x": 2.5569685769389396, + "y": 0.8450805392026429 + }, + { + "x": 2.5565721899634557, + "y": 0.892861880656609 + }, + { + "x": 2.555288314674758, + "y": 0.9408670995202115 + }, + { + "x": 2.55404152215361, + "y": 0.9894409292114801 + }, + { + "x": 2.5805940440315926, + "y": 1.0499192608495642 + }, + { + "x": 2.562592108706105, + "y": 1.0931229385533106 + }, + { + "x": 2.5575633637786193, + "y": 1.1421345379268435 + }, + { + "x": 2.5525131192258153, + "y": 1.1917068774929538 + }, + { + "x": 2.559998507539081, + "y": 1.2480031746227174 + }, + { + "x": 2.552934063190977, + "y": 1.2980847964546898 + }, + { + "x": 2.5572764615165506, + "y": 1.3548333508169712 + }, + { + "x": 2.561300835520563, + "y": 1.4125730958120115 + }, + { + "x": 2.5779781142723093, + "y": 1.4787873920070136 + }, + { + "x": 2.565656121354119, + "y": 1.5295677816269322 + }, + { + "x": 2.5532020988126023, + "y": 1.5808757600234808 + }, + { + "x": 2.564982066503052, + "y": 1.6484138284183643 + }, + { + "x": 2.511966570793331, + "y": 1.6746298903581012 + }, + { + "x": 2.470317806800694, + "y": 1.70749496614611 + }, + { + "x": 2.4664860320137794, + "y": 1.766805639503589 + }, + { + "x": 2.4485029543744883, + "y": 1.816929947362462 + }, + { + "x": 2.454902031968163, + "y": 1.886444205854396 + }, + { + "x": 2.4228649090127488, + "y": 1.9274183150104398 + }, + { + "x": 2.4140764025134116, + "y": 1.987552441403062 + }, + { + "x": 2.403944363274639, + "y": 2.0479295520336716 + }, + { + "x": 2.392458422947468, + "y": 2.108521520179189 + }, + { + "x": 2.390693810818209, + "y": 2.179405291400732 + }, + { + "x": 2.3762995461133847, + "y": 2.240525982652499 + }, + { + "x": 2.3705434878391842, + "y": 2.311546082646768 + }, + { + "x": 2.375032873171785, + "y": 2.3950672685483525 + }, + { + "x": 2.3787572427145376, + "y": 2.4808230951328043 + }, + { + "x": 2.3578219880964393, + "y": 2.543167124404209 + }, + { + "x": 2.3568163917131884, + "y": 2.6293301013486587 + }, + { + "x": 2.3542244167067845, + "y": 2.7169196899226065 + }, + { + "x": 2.3692470959913745, + "y": 2.8289165099775264 + }, + { + "x": 2.3610227604077374, + "y": 2.917289955132807 + }, + { + "x": 3.192155727250048, + "y": 4.227097122796266 + }, + { + "x": 3.0841667563798025, + "y": 4.23002858344214 + }, + { + "x": 3.0126705616557325, + "y": 4.281243100412398 + }, + { + "x": 2.9139258088443882, + "y": 4.292363274276708 + }, + { + "x": 2.8074496482524633, + "y": 4.288834542095308 + }, + { + "x": 2.717930806106284, + "y": 4.308327602923155 + }, + { + "x": 2.6046676915493747, + "y": 4.286719042586133 + }, + { + "x": 2.5166368820113476, + "y": 4.30311109956957 + }, + { + "x": 2.436638839830985, + "y": 4.331707409178529 + }, + { + "x": 2.3340413677306446, + "y": 4.317489411548328 + }, + { + "x": 2.290206800801934, + "y": 4.412005863977556 + }, + { + "x": 2.301757241167972, + "y": 4.823995891362221 + }, + { + "x": 2.2137467440979632, + "y": 4.847428835201051 + }, + { + "x": 1.3562554970108691, + "y": 3.1068749151000783 + }, + { + "x": 1.3034865010989094, + "y": 3.1282972249702614 + }, + { + "x": 1.2393088888050454, + "y": 3.1209418045092776 + }, + { + "x": 1.1867047704466405, + "y": 3.141320750385737 + }, + { + "x": 1.1283636151664957, + "y": 3.1457526290163487 + }, + { + "x": 1.0653834863175804, + "y": 3.134913036595781 + }, + { + "x": 1.0073702101990665, + "y": 3.1361824993165968 + }, + { + "x": 0.951065448537267, + "y": 3.141177953932736 + }, + { + "x": 0.8937834382062521, + "y": 3.1413225522680466 + }, + { + "x": 0.8329042015490727, + "y": 3.125939044881058 + }, + { + "x": 0.7764143755891538, + "y": 3.123962606721241 + }, + { + "x": 0.7206767501614544, + "y": 3.122923235164858 + }, + { + "x": 0.6647771746965262, + "y": 3.1189407840793564 + }, + { + "x": 0.6063357691071217, + "y": 3.09924521245319 + }, + { + "x": 0.5513776730390546, + "y": 3.093242054862937 + }, + { + "x": 0.49695214226308165, + "y": 3.087259003193529 + }, + { + "x": 0.44274158283153225, + "y": 3.0793345499624714 + }, + { + "x": 0.3890638278440507, + "y": 3.0714565056706196 + }, + { + "x": 0.33392354526735324, + "y": 3.0457495628640983 + }, + { + "x": 0.2827103585318532, + "y": 3.0509293972410365 + }, + { + "x": 0.2302844739303222, + "y": 3.040291139392705 + }, + { + "x": 0.17917760931142723, + "y": 3.0437306936594437 + }, + { + "x": 0.1273903567184753, + "y": 3.0313243328479222 + }, + { + "x": 0.07607069067733423, + "y": 3.018041512717124 + }, + { + "x": 0.025090565322777838, + "y": 2.9868946073078604 + }, + { + "x": -0.024964327228198826, + "y": 2.971895033609357 + }, + { + "x": -0.12478688618161307, + "y": 2.969378982457951 + }, + { + "x": -0.17465237916403045, + "y": 2.966863639414232 + }, + { + "x": -0.22446857965138042, + "y": 2.963510952430446 + }, + { + "x": -0.2742214279164006, + "y": 2.959321867742238 + }, + { + "x": -0.3238968821118545, + "y": 2.954297567643472 + }, + { + "x": -0.3753659334154651, + "y": 2.96332066230844 + }, + { + "x": -0.4250942909313271, + "y": 2.956596654170228 + }, + { + "x": -0.47470267322360965, + "y": 2.94903819976401 + }, + { + "x": -0.5241770792180008, + "y": 2.9406474323276974 + }, + { + "x": -0.5735035456525908, + "y": 2.931426720004596 + }, + { + "x": -0.6226681510187515, + "y": 2.9213786651750375 + }, + { + "x": -0.6752547878636666, + "y": 2.926096392376384 + }, + { + "x": -0.7243154894199929, + "y": 2.914339753322727 + }, + { + "x": -0.7731717659472397, + "y": 2.9017605942518627 + }, + { + "x": -0.826188444627937, + "y": 2.9037517070898575 + }, + { + "x": -0.8748524748608859, + "y": 2.8894626610658554 + }, + { + "x": -0.9278568478932381, + "y": 2.888639330847378 + }, + { + "x": -0.9810792518307052, + "y": 2.8868468357068164 + }, + { + "x": -1.0294374366816783, + "y": 2.869958138224565 + }, + { + "x": -1.082805977423351, + "y": 2.8662914277908005 + }, + { + "x": -1.1367094896867724, + "y": 2.862567215994297 + }, + { + "x": -1.190791810562995, + "y": 2.8578366979691374 + }, + { + "x": -1.2386330553771996, + "y": 2.8374290973502725 + }, + { + "x": -1.299002639285135, + "y": 2.8444194007968693 + }, + { + "x": -1.3466032143620068, + "y": 2.8221958414626864 + }, + { + "x": -1.3938237348835423, + "y": 2.7991757680743556 + }, + { + "x": -1.4475615133330046, + "y": 2.788678793517767 + }, + { + "x": -1.5165559443901948, + "y": 2.805312526996914 + }, + { + "x": -1.5781770391038217, + "y": 2.805587083670646 + }, + { + "x": -1.6412407180984632, + "y": 2.8063017360924634 + }, + { + "x": -1.710481332164952, + "y": 2.815082418283951 + }, + { + "x": -1.7751383238157312, + "y": 2.813860743543344 + }, + { + "x": -1.8473520044191014, + "y": 2.8221302133826174 + }, + { + "x": -1.9130357461161602, + "y": 2.818000983935292 + }, + { + "x": -1.9779460143583674, + "y": 2.8108181750862196 + }, + { + "x": -2.052576012036022, + "y": 2.815170951980965 + }, + { + "x": -2.127902680584171, + "y": 2.817798869342668 + }, + { + "x": -2.184794390968985, + "y": 2.7942587536024908 + }, + { + "x": -2.2616243012240895, + "y": 2.7944732465744235 + }, + { + "x": -2.3596157170286958, + "y": 2.8174169583992787 + }, + { + "x": -2.406613003994874, + "y": 2.777379790112879 + }, + { + "x": -2.484301841366633, + "y": 2.7715568720280435 + }, + { + "x": -2.551587477119314, + "y": 2.752164694216181 + }, + { + "x": -2.629990269821837, + "y": 2.7428362725919717 + }, + { + "x": -2.697524656426661, + "y": 2.720279841272434 + }, + { + "x": -2.776492502161062, + "y": 2.7073923890287426 + }, + { + "x": -2.844137765043274, + "y": 2.681633969647105 + }, + { + "x": -2.876961470697589, + "y": 2.6226972413786553 + }, + { + "x": -2.8741010026010985, + "y": 2.533003180754086 + }, + { + "x": -2.856872277098683, + "y": 2.433780998739848 + }, + { + "x": -2.873422202534319, + "y": 2.3657404717944743 + }, + { + "x": -2.839196868335088, + "y": 2.2586157918232064 + }, + { + "x": -2.8370927719568417, + "y": 2.180135109270472 + }, + { + "x": -2.8235276761301966, + "y": 2.095220200871235 + }, + { + "x": -2.819305581220038, + "y": 2.0195394499496575 + }, + { + "x": -2.774685718034317, + "y": 1.9178757873373016 + }, + { + "x": -2.771088944851634, + "y": 1.7808709861012573 + }, + { + "x": -2.7768090792829803, + "y": 1.7193276444619947 + }, + { + "x": -2.764930348405605, + "y": 1.6483694425946287 + }, + { + "x": -2.753197356140039, + "y": 1.579297462654056 + }, + { + "x": -2.751318597534239, + "y": 1.517369343027885 + }, + { + "x": -2.735773092638501, + "y": 1.4494003290872446 + }, + { + "x": -2.717840601111496, + "y": 1.3819347372987842 + }, + { + "x": -2.7137061777298785, + "y": 1.3229361675510594 + }, + { + "x": -2.7210496635649326, + "y": 1.2703927953290912 + }, + { + "x": -2.742007224457138, + "y": 1.2245021466031643 + }, + { + "x": -2.733676651766886, + "y": 1.1661025992641063 + }, + { + "x": -2.724166101959387, + "y": 1.1083320617861794 + }, + { + "x": -2.7125615662607774, + "y": 1.0508521699900524 + }, + { + "x": -2.715756212281623, + "y": 0.9999522511431858 + }, + { + "x": -2.6887435999057288, + "y": 0.9390219041304023 + }, + { + "x": -2.704138963756557, + "y": 0.8937207322001199 + }, + { + "x": -2.70349710716838, + "y": 0.8434023291177257 + }, + { + "x": -2.702891564613361, + "y": 0.7936407850065688 + }, + { + "x": -2.7004174015761797, + "y": 0.7438733223691558 + }, + { + "x": -2.698006445197125, + "y": 0.6946634408611123 + }, + { + "x": -2.693735965180583, + "y": 0.6455125438173301 + }, + { + "x": -2.704199910879375, + "y": 0.6001689231648583 + }, + { + "x": -2.69822465247564, + "y": 0.5514521204233922 + }, + { + "x": -2.6923633626569634, + "y": 0.503290147634636 + }, + { + "x": -2.70043828383638, + "y": 0.4579896554229529 + }, + { + "x": -2.6919335654760315, + "y": 0.4101499105649553 + }, + { + "x": -2.6984438686275274, + "y": 0.36486978216816496 + }, + { + "x": -2.6892960836835424, + "y": 0.3177267296098302 + }, + { + "x": -2.69425412629436, + "y": 0.2725039511089947 + }, + { + "x": -2.6835047858857415, + "y": 0.2259457574739912 + }, + { + "x": -2.670957882275577, + "y": 0.17975882786844258 + }, + { + "x": -2.642639993391038, + "y": 0.13330184107187817 + }, + { + "x": -2.6445064348629868, + "y": 0.08888887146991054 + }, + { + "x": -2.6310000419616615, + "y": 2.1104641406256523e-7 + }, + { + "x": -2.5986332200976725, + "y": -0.043660834073839895 + }, + { + "x": -2.597533045561276, + "y": -0.08730955414147953 + }, + { + "x": -2.5956997640071084, + "y": -0.1309336326566396 + }, + { + "x": -2.5931338928459398, + "y": -0.17452075750719592 + }, + { + "x": -2.5898361562487895, + "y": -0.21805862701053197 + }, + { + "x": -2.585807484942544, + "y": -0.2615349533854677 + }, + { + "x": -2.581049015947273, + "y": -0.30493746622026513 + }, + { + "x": -2.5755620922553297, + "y": -0.34825391593573113 + }, + { + "x": -2.569348262452311, + "y": -0.39147207724244126 + }, + { + "x": -2.562409280280001, + "y": -0.4345797525911082 + }, + { + "x": -2.570474705858885, + "y": -0.48050477252244467 + }, + { + "x": -2.562039889341117, + "y": -0.5236188072463797 + }, + { + "x": -2.5685019427124813, + "y": -0.5700517442167141 + }, + { + "x": -2.5731499784647576, + "y": -0.6166155235069548 + }, + { + "x": -2.5624282367744597, + "y": -0.6597552931836701 + }, + { + "x": -2.566408774550807, + "y": -0.7069580518759089 + }, + { + "x": -2.5685628002861516, + "y": -0.7541978427924835 + }, + { + "x": -2.5555304371453285, + "y": -0.7972411366145622 + }, + { + "x": -2.556968644727306, + "y": -0.8450803340946811 + }, + { + "x": -2.5707335809724228, + "y": -0.8978073990192127 + }, + { + "x": -2.57030296831516, + "y": -0.9463953192793612 + }, + { + "x": -2.5680285609502658, + "y": -0.9948592998461896 + }, + { + "x": -2.5805941282511555, + "y": -1.0499190538464784 + }, + { + "x": -2.5763894510891365, + "y": -1.0990082159768366 + }, + { + "x": -2.5721726973505805, + "y": -1.1486584003471814 + }, + { + "x": -2.5806027649283387, + "y": -1.2048210041806706 + }, + { + "x": -2.6013469024748703, + "y": -1.2681603219218578 + }, + { + "x": -2.59304646492402, + "y": -1.3184803998061432 + }, + { + "x": -2.5846695604158048, + "y": -1.369345823563294 + }, + { + "x": -2.575311512556783, + "y": -1.4202998005844745 + }, + { + "x": -2.5510882370182015, + "y": -1.4633624695184602 + }, + { + "x": -2.5390291652040338, + "y": -1.51369330718836 + }, + { + "x": -2.5268453761157343, + "y": -1.5645560883613117 + }, + { + "x": -2.500205554018842, + "y": -1.606784209556234 + }, + { + "x": -2.485340969436032, + "y": -1.6568793550756067 + }, + { + "x": -2.48347983751547, + "y": -1.7165923275833803 + }, + { + "x": -2.4664861737385864, + "y": -1.766805441653707 + }, + { + "x": -2.460548760760467, + "y": -1.8258683221116725 + }, + { + "x": -2.454902183289812, + "y": -1.8864440089337278 + }, + { + "x": -2.45886370824269, + "y": -1.9560554727782276 + }, + { + "x": -2.474293241320181, + "y": -2.037129712834479 + }, + { + "x": -2.4861567206755875, + "y": -2.1179662348675983 + }, + { + "x": -2.495989267637431, + "y": -2.199764971212239 + }, + { + "x": -2.5738762589442388, + "y": -2.50981796338913 + }, + { + "x": -2.519379989967269, + "y": -2.5406316060216314 + }, + { + "x": -2.3787574417144577, + "y": -2.4808229043201098 + }, + { + "x": -2.3367459951622425, + "y": -2.5204339712050383 + }, + { + "x": -2.2445426177166046, + "y": -2.87067323575413 + }, + { + "x": -2.19600094902125, + "y": -2.9079745908654293 + }, + { + "x": -3.8559452458047736, + "y": -5.288545382242957 + }, + { + "x": -3.7665579370939457, + "y": -5.352575763611604 + }, + { + "x": -1.6116681415615997, + "y": -2.9812491321221537 + }, + { + "x": -1.5470761247783893, + "y": -2.9803891915562817 + }, + { + "x": -1.4758400660966058, + "y": -2.963885594829927 + }, + { + "x": -1.413352475455914, + "y": -2.9620869679665183 + }, + { + "x": -1.3505145006334158, + "y": -2.957213403242791 + }, + { + "x": -1.2822417193745224, + "y": -2.9373254303960405 + }, + { + "x": -1.2207928763041263, + "y": -2.9298363282898823 + }, + { + "x": -1.1654967853795464, + "y": -2.935060679966036 + }, + { + "x": -1.1050704799823832, + "y": -2.9252262850540083 + }, + { + "x": -1.0453065458757924, + "y": -2.9141979949056753 + }, + { + "x": -0.991054582709369, + "y": -2.9161979260880186 + }, + { + "x": -0.9278573113192715, + "y": -2.8886391819907407 + }, + { + "x": -0.8791996350200931, + "y": -2.9038187814809135 + }, + { + "x": -0.8218102920210468, + "y": -2.8883623335619055 + }, + { + "x": -0.7690527573899897, + "y": -2.8862998437417846 + }, + { + "x": -0.7168388027970588, + "y": -2.884254745897178 + }, + { + "x": -0.664686842212308, + "y": -2.880299972532958 + }, + { + "x": -0.6130794804299872, + "y": -2.876389135865226 + }, + { + "x": -0.5616000086084946, + "y": -2.870580107117933 + }, + { + "x": -0.5132973945492451, + "y": -2.879609436329729 + }, + { + "x": -0.46230713758432856, + "y": -2.8720293599590008 + }, + { + "x": -0.41185948378899406, + "y": -2.8645432576111705 + }, + { + "x": -0.36367941457700964, + "y": -2.8710579221822528 + }, + { + "x": -0.3170314332944235, + "y": -2.891672803799076 + }, + { + "x": -0.268408991979661, + "y": -2.896590607412857 + }, + { + "x": -0.22091924668093113, + "y": -2.9166452316824882 + }, + { + "x": -0.16912884940582287, + "y": -2.8730262012491896 + }, + { + "x": -0.1195808979389053, + "y": -2.8454884802646503 + }, + { + "x": -0.07176217836038208, + "y": -2.847095796110078 + }, + { + "x": -0.02392320519676725, + "y": -2.847899570336675 + }, + { + "x": 0.02392251986155294, + "y": -2.8478995760936128 + }, + { + "x": 0.07176149321859132, + "y": -2.847095813379266 + }, + { + "x": 0.11958021318390705, + "y": -2.845488509041215 + }, + { + "x": 0.1673651837832151, + "y": -2.8430781167122223 + }, + { + "x": 0.21510291856738242, + "y": -2.8398653166822014 + }, + { + "x": 0.2642562392073978, + "y": -2.851782794994662 + }, + { + "x": 0.3121265324825837, + "y": -2.8469410772806665 + }, + { + "x": 0.3616680563351767, + "y": -2.85518481674872 + }, + { + "x": 0.4075893570532098, + "y": -2.834848741256675 + }, + { + "x": 0.4573798553457509, + "y": -2.8414235496823745 + }, + { + "x": 0.5050488690626873, + "y": -2.833338977359431 + }, + { + "x": 0.5525753418133559, + "y": -2.8244547460325045 + }, + { + "x": 0.5999458601049954, + "y": -2.814773363116214 + }, + { + "x": 0.6507448215830421, + "y": -2.81988784994996 + }, + { + "x": 0.698024766525227, + "y": -2.808557945713414 + }, + { + "x": 0.7451077065223576, + "y": -2.7964353764736893 + }, + { + "x": 0.8004638744899628, + "y": -2.8133400623967915 + }, + { + "x": 0.847612694279797, + "y": -2.799495926331383 + }, + { + "x": 0.8994154140009838, + "y": -2.8000951454022487 + }, + { + "x": 0.9563026272083299, + "y": -2.8139418248464843 + }, + { + "x": 1.00850406655627, + "y": -2.81159891865331 + }, + { + "x": 1.0612486285402534, + "y": -2.8092277363062483 + }, + { + "x": 1.1082915637599433, + "y": -2.7910032131313347 + }, + { + "x": 1.1669449656096165, + "y": -2.800606177444293 + }, + { + "x": 1.2198293204464319, + "y": -2.794354590989188 + }, + { + "x": 1.3931117347766822, + "y": -2.9196684770188384 + }, + { + "x": 1.4419631515792106, + "y": -2.8958533441384335 + }, + { + "x": 1.4904076001979865, + "y": -2.8712209086975666 + }, + { + "x": 1.5460403580458986, + "y": -2.8598530976316328 + }, + { + "x": 1.5938653454419827, + "y": -2.8334773317143447 + }, + { + "x": 1.648813172201702, + "y": -2.8192501369998695 + }, + { + "x": 1.7042499255467152, + "y": -2.804827377388573 + }, + { + "x": 1.7575307805757339, + "y": -2.7859506393231483 + }, + { + "x": 1.8221581251530152, + "y": -2.7836429583681292 + }, + { + "x": 1.8770889349445683, + "y": -2.7650499419185075 + }, + { + "x": 1.932482388210073, + "y": -2.7462112242620225 + }, + { + "x": 1.9966071409149047, + "y": -2.738408390133094 + }, + { + "x": 2.0616127197826746, + "y": -2.7300172196765433 + }, + { + "x": 2.117039124873159, + "y": -2.707603108446189 + }, + { + "x": 2.191793643519449, + "y": -2.708190613359933 + }, + { + "x": 2.257526054589064, + "y": -2.6955207921408473 + }, + { + "x": 2.322790606925161, + "y": -2.6806440101725415 + }, + { + "x": 2.3995337643004477, + "y": -2.6769876577395113 + }, + { + "x": 2.477480428042232, + "y": -2.672232609240609 + }, + { + "x": 2.597461272618962, + "y": -2.708911976186534 + }, + { + "x": 2.6426022021231415, + "y": -2.664894511761408 + }, + { + "x": 2.7206473680956513, + "y": -2.652937529622205 + }, + { + "x": 2.6521029003372467, + "y": -2.2593375091132075 + }, + { + "x": 2.6410470241574875, + "y": -2.174422139854412 + }, + { + "x": 2.6396389484718443, + "y": -2.0998653745487457 + }, + { + "x": 2.6380678069745125, + "y": -2.0271967340488466 + }, + { + "x": 2.635613589051089, + "y": -1.9557774502441942 + }, + { + "x": 2.6298883811921736, + "y": -1.8838554678478938 + }, + { + "x": 2.636485997019803, + "y": -1.8223518042557842 + }, + { + "x": 2.6409344035143203, + "y": -1.760608313548616 + }, + { + "x": 2.630599629221854, + "y": -1.6905842703743614 + }, + { + "x": 2.632271976516821, + "y": -1.629834338209644 + }, + { + "x": 2.606026030120244, + "y": -1.5536356858145621 + }, + { + "x": 2.63175806816619, + "y": -1.5096373685567428 + }, + { + "x": 2.629602019711455, + "y": -1.4502421673432104 + }, + { + "x": 2.639455415023358, + "y": -1.3983719261115304 + }, + { + "x": 2.6492035274338805, + "y": -1.347035255165135 + }, + { + "x": 2.6570769291313923, + "y": -1.2953295807095653 + }, + { + "x": 2.6648706837865284, + "y": -1.2441644380639263 + }, + { + "x": 2.7977054538704107, + "y": -1.2493756549391075 + }, + { + "x": 2.7907048396248895, + "y": -1.1904293393540764 + }, + { + "x": 2.7815949122070154, + "y": -1.1316973404558928 + }, + { + "x": 2.7852943756331654, + "y": -1.0790292733282312 + }, + { + "x": 2.773937533417469, + "y": -1.0213750979348075 + }, + { + "x": 2.761437691098976, + "y": -0.9644099746623427 + }, + { + "x": 2.747815295572305, + "y": -0.9081560496585586 + }, + { + "x": 2.747409777288938, + "y": -0.8571018821720302 + }, + { + "x": 2.7479878315611335, + "y": -0.806882486722537 + }, + { + "x": 2.715842589809605, + "y": -0.7481226724676677 + }, + { + "x": 2.728027180907669, + "y": -0.7023931989369375 + }, + { + "x": 2.723882550158275, + "y": -0.6527369419691391 + }, + { + "x": 2.7198198215038647, + "y": -0.6036358280858672 + }, + { + "x": 2.729576592284952, + "y": -0.5578599297711694 + }, + { + "x": 2.7071077871104072, + "y": -0.5060465841332562 + }, + { + "x": 2.715226933460838, + "y": -0.46049800885085174 + }, + { + "x": 2.7077510217503744, + "y": -0.4125601210652936 + }, + { + "x": 2.6984438393593857, + "y": -0.36486999862460356 + }, + { + "x": 2.704192558867345, + "y": -0.31948689275057807 + }, + { + "x": 2.6942541044353696, + "y": -0.2725041672293522 + }, + { + "x": 2.698451745875432, + "y": -0.2272044797226268 + }, + { + "x": 2.6869217870793767, + "y": -0.1808334352666725 + }, + { + "x": 2.6895804710138034, + "y": -0.1356698608141947 + }, + { + "x": 2.6754890676661987, + "y": -0.08993049474637764 + }, + { + "x": 2.676622270986371, + "y": -0.044971808351830435 + } + ], + "stamp": { + "sec": 1784882202, + "nanosec": 93357838 + } + }, + "captured_at": "2026-07-24T08:36:42.100Z" + }, + { + "key": "C", + "label": "诊疗室", + "image": { + "x": 280, + "y": 123 + }, + "map": { + "x": 2.8, + "y": 3.77 + }, + "odom_pose": { + "frame_id": "odom", + "stamp": { + "sec": 1784882222, + "nanosec": 736759483 + }, + "x": 0.007021478047595366, + "y": -0.00203423478568464, + "theta": -0.05373038737536273 + }, + "scan": { + "frame_id": "pose", + "points": [ + { + "x": 2.2746790564227695, + "y": 0.03821811342075731 + }, + { + "x": 2.272716428774812, + "y": 0.07639184129258338 + }, + { + "x": 2.2711123925612835, + "y": 0.11456081089783537 + }, + { + "x": 2.2838336244636723, + "y": 0.15370469395006772 + }, + { + "x": 2.280929227678493, + "y": 0.19204951313437965 + }, + { + "x": 2.2932998959393838, + "y": 0.23195019947203796 + }, + { + "x": 2.304969225852611, + "y": 0.2723202678389297 + }, + { + "x": 2.3010601134255997, + "y": 0.31113739474699126 + }, + { + "x": 2.2955085469979695, + "y": 0.34974938837323266 + }, + { + "x": 2.30409780130375, + "y": 0.3907708059594209 + }, + { + "x": 2.236263897496991, + "y": 0.4180301600045028 + }, + { + "x": 2.243621847523947, + "y": 0.458542070698095 + }, + { + "x": 2.205338599170942, + "y": 0.48945168033319625 + }, + { + "x": 2.1968050085794566, + "y": 0.5264304689647565 + }, + { + "x": 2.187651409256527, + "y": 0.5632606820577588 + }, + { + "x": 2.177880384641048, + "y": 0.5999319249464911 + }, + { + "x": 2.1674946924291065, + "y": 0.6364338478317242 + }, + { + "x": 2.1555424806830503, + "y": 0.6724582878046087 + }, + { + "x": 2.1591333808622335, + "y": 0.7135956296709134 + }, + { + "x": 2.1468408782508086, + "y": 0.7497665786831949 + }, + { + "x": 2.1480187049198856, + "y": 0.7909088446132431 + }, + { + "x": 2.134428972923827, + "y": 0.8268821661618385 + }, + { + "x": 2.1350572201220284, + "y": 0.8686518143396141 + }, + { + "x": 2.1210830436803643, + "y": 0.9047887573470214 + }, + { + "x": 2.1201934838498677, + "y": 0.946817677828599 + }, + { + "x": 2.1175800755361145, + "y": 0.9886471182658726 + }, + { + "x": 2.1267402395496613, + "y": 1.0367891085637038 + }, + { + "x": 2.109022925924644, + "y": 1.0723702719118633 + }, + { + "x": 2.104848770172594, + "y": 1.115139233153052 + }, + { + "x": 2.0998288918260677, + "y": 1.1580684928794809 + }, + { + "x": 2.1069679774630394, + "y": 1.2086051713105217 + }, + { + "x": 2.099251288682388, + "y": 1.2515111085942041 + }, + { + "x": 2.105137706519074, + "y": 1.303446042636083 + }, + { + "x": 2.0955625169734726, + "y": 1.3467362116896717 + }, + { + "x": 2.0867876082738017, + "y": 1.3911797013049163 + }, + { + "x": 2.0878011367681513, + "y": 1.4430976943661575 + }, + { + "x": 2.0884649959330392, + "y": 1.4960197158334119 + }, + { + "x": 2.08793289475068, + "y": 1.5493662353063622 + }, + { + "x": 2.086983903351575, + "y": 1.6037213057470094 + }, + { + "x": 2.095746851930564, + "y": 1.6671919474381172 + }, + { + "x": 2.0913760940226407, + "y": 1.7218674840775288 + }, + { + "x": 2.633827745948096, + "y": 2.4010511510015498 + }, + { + "x": 2.616403239051169, + "y": 2.4669109783647585 + }, + { + "x": 2.574591965680929, + "y": 2.510516260601638 + }, + { + "x": 2.5426160947574648, + "y": 2.5640641246810114 + }, + { + "x": 2.8174205961079113, + "y": 3.038894145450594 + }, + { + "x": 2.7659721134912783, + "y": 3.085795636463236 + }, + { + "x": 2.682964574650452, + "y": 3.096306039689048 + }, + { + "x": 2.580488904515995, + "y": 3.0811423925137498 + }, + { + "x": 2.5182983063417823, + "y": 3.111620301301169 + }, + { + "x": 2.3183354484645293, + "y": 3.069972125803298 + }, + { + "x": 2.257598235333095, + "y": 3.0963646974124637 + }, + { + "x": 2.19605549648587, + "y": 3.120768517512813 + }, + { + "x": 2.1169209909710975, + "y": 3.118333997595284 + }, + { + "x": 1.4754720006628157, + "y": 2.2540227164095863 + }, + { + "x": 1.8008348796534024, + "y": 2.9637842847321285 + }, + { + "x": 1.742714150294971, + "y": 2.9798071613405797 + }, + { + "x": 1.0766316619368577, + "y": 1.9139698796279423 + }, + { + "x": 1.044326626527968, + "y": 1.931786306176911 + }, + { + "x": 1.0048160981608971, + "y": 1.9357441938224034 + }, + { + "x": 0.972155406671647, + "y": 1.9523511058002658 + }, + { + "x": 0.9258706234385788, + "y": 1.9404288182598768 + }, + { + "x": 0.89314237461757, + "y": 1.9557088507092393 + }, + { + "x": 0.8541608801117707, + "y": 1.9566895896295846 + }, + { + "x": 0.8154002015124151, + "y": 1.9569164586522425 + }, + { + "x": 0.7765056534858624, + "y": 1.955468065542981 + }, + { + "x": 0.7382448540943768, + "y": 1.9542045644253456 + }, + { + "x": 0.6999095503434687, + "y": 1.9512702097734151 + }, + { + "x": 0.6618827170286178, + "y": 1.9476036422174552 + }, + { + "x": 0.6195908088839223, + "y": 1.9289332083536452 + }, + { + "x": 0.5870988977548404, + "y": 1.9390696163361365 + }, + { + "x": 0.550610022910833, + "y": 1.935193256596782 + }, + { + "x": 0.5180226824604571, + "y": 1.9441699612338421 + }, + { + "x": 0.48142997359197104, + "y": 1.937070310058182 + }, + { + "x": 0.44522309299011675, + "y": 1.9292942940358582 + }, + { + "x": 0.41274972199825166, + "y": 1.936501418156303 + }, + { + "x": 0.37420785858866573, + "y": 1.9127387386383714 + }, + { + "x": 0.34202262857365895, + "y": 1.9187552020153555 + }, + { + "x": 0.3073570265411176, + "y": 1.909420780565876 + }, + { + "x": 0.272959926586488, + "y": 1.8984774985836927 + }, + { + "x": 0.24102856127256217, + "y": 1.9027950932251152 + }, + { + "x": 0.2072854402985382, + "y": 1.8906709278972462 + }, + { + "x": 0.1754944866146344, + "y": 1.8938863473090088 + }, + { + "x": 0.14252108581398013, + "y": 1.8816101102325187 + }, + { + "x": 0.11089148563417436, + "y": 1.8837388208678698 + }, + { + "x": 0.07851680090997618, + "y": 1.8683509118494237 + }, + { + "x": 0.04711897628781832, + "y": 1.8694062747052331 + }, + { + "x": 0.015581854479624244, + "y": 1.8549345747420878 + }, + { + "x": -0.015581705685521542, + "y": 1.8549345759919849 + }, + { + "x": -0.04674086819428085, + "y": 1.8544110552957453 + }, + { + "x": -0.0772570268264135, + "y": 1.8383774026676885 + }, + { + "x": -0.1081293391898147, + "y": 1.8368201242472346 + }, + { + "x": -0.13897113399059144, + "y": 1.8347444363590864 + }, + { + "x": -0.16977370668646743, + "y": 1.8321509248288446 + }, + { + "x": -0.19878463540012198, + "y": 1.8131355912363103 + }, + { + "x": -0.22934142919540365, + "y": 1.8105324307783537 + }, + { + "x": -0.2575897707430508, + "y": 1.7915767644369816 + }, + { + "x": -0.2876504233893293, + "y": 1.7869966499089698 + }, + { + "x": -0.34751971502915996, + "y": 1.77632481278847 + }, + { + "x": -0.3804383694858133, + "y": 1.7849066701283125 + }, + { + "x": -0.41036963417913797, + "y": 1.7782637423591714 + }, + { + "x": -0.4440442152462818, + "y": 1.7866464650114005 + }, + { + "x": -0.4778577243690961, + "y": 1.7934290610499777 + }, + { + "x": -0.5079184107255575, + "y": 1.7851483663280874 + }, + { + "x": -0.5378357462391897, + "y": 1.776363845563501 + }, + { + "x": -0.5721886143799133, + "y": 1.7813594197356857 + }, + { + "x": -0.6071815344476347, + "y": 1.7866447467408406 + }, + { + "x": -0.637110000293922, + "y": 1.7761924767198813 + }, + { + "x": -0.6721595915268098, + "y": 1.7792710009647505 + }, + { + "x": -0.7019550151196511, + "y": 1.7677281940682972 + }, + { + "x": -0.7377062999292571, + "y": 1.7704556897012085 + }, + { + "x": -0.7737456014027793, + "y": 1.7724767426769943 + }, + { + "x": -0.8034125516826526, + "y": 1.7592283339065924 + }, + { + "x": -0.8393123081655178, + "y": 1.7590212770399605 + }, + { + "x": -0.8758757926800841, + "y": 1.7589959428534614 + }, + { + "x": -0.9117518241155481, + "y": 1.7564593652451304 + }, + { + "x": -0.9411302208604887, + "y": 1.740894826782607 + }, + { + "x": -0.9780873257729488, + "y": 1.7387841160385968 + }, + { + "x": -1.0152368572967176, + "y": 1.735919005517819 + }, + { + "x": -1.05152548725642, + "y": 1.7305835824623599 + }, + { + "x": -1.0889862423991468, + "y": 1.7262066829580884 + }, + { + "x": -1.1260467879500342, + "y": 1.7202193487513031 + }, + { + "x": -1.155347767748847, + "y": 1.7018872506243294 + }, + { + "x": -1.2021906179864024, + "y": 1.708408224706014 + }, + { + "x": -1.2395580245221915, + "y": 1.700091847253129 + }, + { + "x": -1.2775852643641272, + "y": 1.6917965028484256 + }, + { + "x": -1.3249204612222525, + "y": 1.6945167068812217 + }, + { + "x": -1.3632656319462404, + "y": 1.6844571993616344 + }, + { + "x": -1.410632887233959, + "y": 1.6843170648029555 + }, + { + "x": -1.4485517760149635, + "y": 1.6717180622550343 + }, + { + "x": -1.4977897372505402, + "y": 1.6709762759126727 + }, + { + "x": -1.5569238456174603, + "y": 1.6793117531401387 + }, + { + "x": -1.5849151824053909, + "y": 1.6529197469531764 + }, + { + "x": -1.6230212392707615, + "y": 1.6367123646587105 + }, + { + "x": -1.6724822987712378, + "y": 1.6308583015996476 + }, + { + "x": -1.711284767215584, + "y": 1.6135081147994692 + }, + { + "x": -2.6859077932146413, + "y": 1.6012585396644174 + }, + { + "x": -2.766208569885463, + "y": 1.586760994757329 + }, + { + "x": -2.8747864576262354, + "y": 1.5854626368836942 + }, + { + "x": -2.9407794474033166, + "y": 1.5580117774784303 + }, + { + "x": -3.0503280664971224, + "y": 1.550993944797898 + }, + { + "x": -3.1182003439358144, + "y": 1.5201277303034024 + }, + { + "x": -3.128799403757756, + "y": 1.4607613649198499 + }, + { + "x": -3.124591564521176, + "y": 1.3953533907161035 + }, + { + "x": -3.10344064006603, + "y": 1.3238325735063794 + }, + { + "x": -3.096527422454171, + "y": 1.2598279598434121 + }, + { + "x": -3.1032675583950637, + "y": 1.2022125095190095 + }, + { + "x": -3.0920583142812625, + "y": 1.1385081834845205 + }, + { + "x": -3.0994189130173946, + "y": 1.0824469278146933 + }, + { + "x": -3.0877317449934947, + "y": 1.0204985442536498 + }, + { + "x": -3.0738914935250747, + "y": 0.9589532158995366 + }, + { + "x": -3.0607825751950206, + "y": 0.8987270956464308 + }, + { + "x": -3.075448519049653, + "y": 0.8471816639551557 + }, + { + "x": -3.059225449891966, + "y": 0.787667531771433 + }, + { + "x": -3.056466387626108, + "y": 0.7324353308830805 + }, + { + "x": -3.053695825347178, + "y": 0.6777358906781618 + }, + { + "x": -3.034277992390631, + "y": 0.6201333277874471 + }, + { + "x": -3.044267532390722, + "y": 0.5690724651312483 + }, + { + "x": -3.037623346275293, + "y": 0.5151756579635396 + }, + { + "x": -3.0152027531448353, + "y": 0.4594040341106525 + }, + { + "x": -3.0076302694609818, + "y": 0.40667638634957415 + }, + { + "x": -2.999141169616644, + "y": 0.3543333592912892 + }, + { + "x": -2.988751632923041, + "y": 0.30229020377345295 + }, + { + "x": -2.9934080967382637, + "y": 0.25203899892545334 + }, + { + "x": -2.9972197249174117, + "y": 0.20171666059979498 + }, + { + "x": -2.98420572626056, + "y": 0.15053133171473596 + }, + { + "x": -2.9863134084807066, + "y": 0.10037790993277167 + }, + { + "x": -3.0345717946271233, + "y": 0.05098572590252286 + }, + { + "x": -3.019999980926504, + "y": 2.422501544197397e-7 + }, + { + "x": -3.0035760400516818, + "y": -0.050464464972832364 + }, + { + "x": -3.0023044262464555, + "y": -0.10091492822410944 + }, + { + "x": -2.9991868081643838, + "y": -0.151286535235729 + }, + { + "x": -2.9802581671671624, + "y": -0.20057464612064332 + }, + { + "x": -2.992411735299311, + "y": -0.25195462379933176 + }, + { + "x": -2.9877568295187324, + "y": -0.30218910250878833 + }, + { + "x": -2.9822586830706843, + "y": -0.352338293775176 + }, + { + "x": -2.975918847708186, + "y": -0.4023880438905694 + }, + { + "x": -2.9697276321897097, + "y": -0.4524748404126318 + }, + { + "x": -2.9774821260949147, + "y": -0.5049753197745916 + }, + { + "x": -2.96857878942843, + "y": -0.5549232881685356 + }, + { + "x": -2.973533940992011, + "y": -0.6077182099960506 + }, + { + "x": -2.9629051661954477, + "y": -0.6575853534900348 + }, + { + "x": -2.9660270533514907, + "y": -0.7107624271980107 + }, + { + "x": -2.983689304753172, + "y": -0.7682185138984924 + }, + { + "x": -2.9703628249770033, + "y": -0.8182336099119316 + }, + { + "x": -2.956198013678532, + "y": -0.868017774194732 + }, + { + "x": -2.956472667602154, + "y": -0.9223218771449869 + }, + { + "x": -2.9699954482350033, + "y": -0.9815860475370793 + }, + { + "x": -2.953086503404764, + "y": -1.0313409885506972 + }, + { + "x": -2.9353441029834855, + "y": -1.0808048521061426 + }, + { + "x": -2.93076021387319, + "y": -1.1353822534247968 + }, + { + "x": -2.9112731571501165, + "y": -1.184456371957697 + }, + { + "x": -2.905681444479248, + "y": -1.2394740162998956 + }, + { + "x": -2.899058731201656, + "y": -1.2946363081004075 + }, + { + "x": -2.8904925647619635, + "y": -1.3495010552505464 + }, + { + "x": -2.894380423572088, + "y": -1.4110145810347032 + }, + { + "x": -2.870268106282668, + "y": -1.4594386531493493 + }, + { + "x": -2.873622491986539, + "y": -1.5224317329238322 + }, + { + "x": -2.885294686970887, + "y": -1.5912573871359332 + }, + { + "x": -2.8867806433235907, + "y": -1.6559232996620017 + }, + { + "x": -2.85855513527877, + "y": -1.7041851412339644 + }, + { + "x": -2.868632845194751, + "y": -1.776181884988088 + }, + { + "x": -2.851849738295306, + "y": -1.8327721575349487 + }, + { + "x": -2.8605966506830223, + "y": -1.9070476011147235 + }, + { + "x": -2.866819085826218, + "y": -1.9815581237905788 + }, + { + "x": -2.8591404097847843, + "y": -2.0480734448240865 + }, + { + "x": -2.8492257024451964, + "y": -2.114288908069928 + }, + { + "x": -2.851365719800591, + "y": -2.191102283427243 + }, + { + "x": -2.87676110677953, + "y": -2.288497848794251 + }, + { + "x": -2.8857747733450942, + "y": -2.3759097899781856 + }, + { + "x": -2.8934114805484525, + "y": -2.4649080922439306 + }, + { + "x": -2.874851576821066, + "y": -2.5336638575017627 + }, + { + "x": -2.8547916430822697, + "y": -2.6024859319880904 + }, + { + "x": -2.83322448482958, + "y": -2.6713433763719396 + }, + { + "x": -2.8101430078512704, + "y": -2.740204536363563 + }, + { + "x": -2.808073503560531, + "y": -2.8317603234081683 + }, + { + "x": -2.76010601626383, + "y": -2.878534020923091 + }, + { + "x": -2.7535122567121793, + "y": -2.9699615817529073 + }, + { + "x": -2.755292750413158, + "y": -3.0738809450521547 + }, + { + "x": -2.744521649333594, + "y": -3.1673461743926943 + }, + { + "x": -2.7077043381680284, + "y": -3.713698292183364 + }, + { + "x": -2.617887097247951, + "y": -3.7202239452108605 + }, + { + "x": -1.915479109099363, + "y": -4.194312976575559 + }, + { + "x": -1.8195429945569341, + "y": -4.168160986228196 + }, + { + "x": -1.7319565260259129, + "y": -4.156601211772904 + }, + { + "x": -1.6445387950612462, + "y": -4.141428114270708 + }, + { + "x": -1.5581245641490824, + "y": -4.124503380553498 + }, + { + "x": -1.4781498370155377, + "y": -4.120916785794892 + }, + { + "x": -1.3987384736140411, + "y": -4.115815926849884 + }, + { + "x": -1.31961254355552, + "y": -4.108265842019415 + }, + { + "x": -1.2414275759537468, + "y": -4.100184494299099 + }, + { + "x": -1.1638891005173397, + "y": -4.090644119472083 + }, + { + "x": -1.0870240535718279, + "y": -4.079664659958953 + }, + { + "x": -1.0069994130439124, + "y": -4.0517377475307255 + }, + { + "x": -0.9387914779624547, + "y": -4.068082736811189 + }, + { + "x": -0.8669831768869313, + "y": -4.067630822069693 + }, + { + "x": -0.7925759833216391, + "y": -4.051198034593689 + }, + { + "x": -0.7244073735492279, + "y": -4.063940964381931 + }, + { + "x": -0.6560343128150931, + "y": -4.075536919872431 + }, + { + "x": -0.5874760981867871, + "y": -4.0859826283151595 + }, + { + "x": -0.518752078988222, + "y": -4.0952751415966455 + }, + { + "x": -0.4447594595179925, + "y": -4.056691855303097 + }, + { + "x": -0.37516362576402695, + "y": -4.048655101366269 + }, + { + "x": -0.30588820710449904, + "y": -4.038432115277763 + }, + { + "x": -0.23800273362017182, + "y": -4.043000895837367 + }, + { + "x": -0.16942025861487184, + "y": -4.031441497105795 + }, + { + "x": -0.1016714795267224, + "y": -4.033718715883222 + }, + { + "x": -0.0338940054907741, + "y": -4.034857490049375 + }, + { + "x": 0.034019033818506975, + "y": -4.049857312337125 + }, + { + "x": 0.10167050882904928, + "y": -4.033718740349926 + }, + { + "x": 0.1694192884652, + "y": -4.031441537875961 + }, + { + "x": 0.23618001083236956, + "y": -4.012054390428766 + }, + { + "x": 0.30347034369120723, + "y": -4.006523525715517 + }, + { + "x": 0.37073378910444293, + "y": -4.000860091183922 + }, + { + "x": 0.43789260156874205, + "y": -3.994067485797318 + }, + { + "x": 0.5049278267163028, + "y": -3.9861476266464977 + }, + { + "x": 0.5740975511927509, + "y": -3.992939688424803 + }, + { + "x": 0.6410946095809519, + "y": -3.9827318583165656 + }, + { + "x": 0.7107185549419187, + "y": -3.987151951031425 + }, + { + "x": 0.7835510324156888, + "y": -4.005072729091636 + }, + { + "x": 0.8507223585767058, + "y": -3.991344542172588 + }, + { + "x": 0.9212514043753333, + "y": -3.9920803912472294 + }, + { + "x": 0.9920441441454099, + "y": -3.9915682038158153 + }, + { + "x": 1.0950044420682452, + "y": -4.109619498484672 + }, + { + "x": 1.1595095666130308, + "y": -4.075255386838428 + }, + { + "x": 1.2278068494050376, + "y": -4.055201504682207 + }, + { + "x": 1.295757605780642, + "y": -4.034003114783674 + }, + { + "x": 1.3633426578601056, + "y": -4.0116662000073875 + }, + { + "x": 1.4305429309771567, + "y": -3.9881970645460902 + }, + { + "x": 1.496986088532125, + "y": -3.9626669261787746 + }, + { + "x": 1.5692492625797008, + "y": -3.95182989501902 + }, + { + "x": 1.6354152787160183, + "y": -3.9249101433555365 + }, + { + "x": 1.7135221146860662, + "y": -3.925293733866091 + }, + { + "x": 1.785868538854257, + "y": -3.910507988126306 + }, + { + "x": 1.8577695206802378, + "y": -3.8934932293610847 + }, + { + "x": 1.937624172859532, + "y": -3.891275192790883 + }, + { + "x": 2.017002893721157, + "y": -3.8856893111564696 + }, + { + "x": 2.0891281571631426, + "y": -3.8644525678255963 + }, + { + "x": 2.1694415917643672, + "y": -3.8567019418055195 + }, + { + "x": 2.2571475629215687, + "y": -3.8594206325379243 + }, + { + "x": 2.338280588827375, + "y": -3.8483049423959153 + }, + { + "x": 2.4271424902768843, + "y": -3.847385916222869 + }, + { + "x": 2.339711316161115, + "y": -2.793651257091482 + }, + { + "x": 2.3430913763628185, + "y": -2.70407235357668 + }, + { + "x": 2.3468040962287837, + "y": -2.6181609503496706 + }, + { + "x": 2.35850157227446, + "y": -2.5439009483346053 + }, + { + "x": 2.368375232309642, + "y": -2.469996414783242 + }, + { + "x": 2.3757367363848543, + "y": -2.3957778378809276 + }, + { + "x": 2.5123579832385223, + "y": -2.368811271134756 + }, + { + "x": 2.505239983612657, + "y": -2.283828657268047 + }, + { + "x": 2.484735507459009, + "y": -2.1898478709772715 + }, + { + "x": 2.475499127448439, + "y": -2.108888019280873 + }, + { + "x": 2.4750648894372875, + "y": -2.037766023831247 + }, + { + "x": 2.4596457598214383, + "y": -1.9566785706411811 + }, + { + "x": 2.4556945987804633, + "y": -1.8870538722726786 + }, + { + "x": 2.4493056557260386, + "y": -1.8175262072272238 + }, + { + "x": 2.4551045188840614, + "y": -1.7586533729394216 + }, + { + "x": 2.445639059742114, + "y": -1.6904374831185516 + }, + { + "x": 2.4345854809771206, + "y": -1.6230435076877259 + }, + { + "x": 2.435428733229034, + "y": -1.565155511419532 + }, + { + "x": 2.4222683808498173, + "y": -1.4998055743056613 + }, + { + "x": 2.4204949738220245, + "y": -1.443027554291607 + }, + { + "x": 2.417504869489822, + "y": -1.3867367725760367 + }, + { + "x": 2.3805466790824257, + "y": -1.2612032110409563 + }, + { + "x": 2.3586112266044914, + "y": -1.1992783652005874 + }, + { + "x": 2.37842526996081, + "y": -1.1594864167875296 + }, + { + "x": 2.3839765104526944, + "y": -1.1130216612501769 + }, + { + "x": 2.387728475242581, + "y": -1.0662916010139558 + }, + { + "x": 2.2627337485823, + "y": -0.9652130183072539 + }, + { + "x": 2.2638088357821378, + "y": -0.9210350606812326 + }, + { + "x": 2.2509878621240262, + "y": -0.8720377344625243 + }, + { + "x": 2.2512434861613246, + "y": -0.828917020824302 + }, + { + "x": 2.249745586579751, + "y": -0.7857056094887381 + }, + { + "x": 2.247435717902517, + "y": -0.7427800357326417 + }, + { + "x": 2.246231742342804, + "y": -0.7007507471478663 + }, + { + "x": 2.257686794023556, + "y": -0.6629170310290119 + }, + { + "x": 2.2675406336534243, + "y": -0.6246306634791949 + }, + { + "x": 2.262219240121977, + "y": -0.5824602554866343 + }, + { + "x": 2.2716848536284022, + "y": -0.5443746553568742 + }, + { + "x": 2.265865773587731, + "y": -0.5028854307763602 + }, + { + "x": 2.2592977659818585, + "y": -0.46174622709807583 + }, + { + "x": 2.2667358962671407, + "y": -0.42372673999162014 + }, + { + "x": 2.2735342810847663, + "y": -0.3855876636282593 + }, + { + "x": 2.2796910017128043, + "y": -0.3473397620768398 + }, + { + "x": 2.2693485775453976, + "y": -0.3068498963329575 + }, + { + "x": 2.274183159702643, + "y": -0.2686834222868414 + }, + { + "x": 2.277381042659329, + "y": -0.23034049515676686 + }, + { + "x": 2.2809291968678513, + "y": -0.19204987906514612 + }, + { + "x": 2.2838335998047117, + "y": -0.15370506034678874 + }, + { + "x": 2.2860934317543813, + "y": -0.11531686113762306 + }, + { + "x": 2.287708054919907, + "y": -0.07689611581694138 + } + ], + "stamp": { + "sec": 1784882222, + "nanosec": 694827303 + } + }, + "captured_at": "2026-07-24T08:37:02.757Z" + } + ], + "generated_at": "2026-07-24T08:37:03.963Z" +} \ No newline at end of file diff --git a/src/origincar_base/include/origincar_base/origincar_base.h b/src/origincar_base/include/origincar_base/origincar_base.h index a282341..c527d53 100644 --- a/src/origincar_base/include/origincar_base/origincar_base.h +++ b/src/origincar_base/include/origincar_base/origincar_base.h @@ -4,8 +4,12 @@ #include #include #include +#include +#include #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" +#include "origincar_base/wall_fit_core.hpp" +#include "origincar_base/wall_kalman_filter.hpp" #include #include @@ -35,19 +39,19 @@ #include #include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" #include "origincar_msg/msg/data.hpp" -#include "origincar_msg/msg/sign.hpp" // 匹配信号发送 +#include "origincar_msg/msg/sign.hpp" #include +#include #include using namespace std; -#define SEND_DATA_CHECK 1 // Send data check flag bits //发送数据校验标志位 -#define READ_DATA_CHECK 0 // Receive data to check flag bits //接收数据校验标志位 -#define FRAME_HEADER 0X7B // Frame head //帧头 -#define FRAME_TAIL 0X7D // Frame tail //帧尾 -#define RECEIVE_DATA_SIZE 24 // The length of the data sent by the lower computer //下位机发送过来的数据的长度 -#define SEND_DATA_SIZE 11 // The length of data sent by ROS to the lower machine //ROS向下位机发送的数据的长度 -#define PI 3.1415926f // PI //圆周率 - +#define SEND_DATA_CHECK 1 +#define READ_DATA_CHECK 0 +#define FRAME_HEADER 0X7B +#define FRAME_TAIL 0X7D +#define RECEIVE_DATA_SIZE 24 +#define SEND_DATA_SIZE 11 +#define PI 3.1415926f #define GYROSCOPE_RATIO 0.00026644f #define ACCEl_RATIO 1671.84f @@ -145,6 +149,8 @@ private: void Publish_Voltage(); void Publish_GyroDebug(); auto createQuaternionMsgFromYaw(double yaw); + void Scan_Callback(const sensor_msgs::msg::LaserScan::SharedPtr scan); + void Apply_Wall_Update(); bool Get_Sensor_Data(); unsigned char Check_Sum(unsigned char Count_Number, unsigned char mode); @@ -158,24 +164,28 @@ private: { auto msg = std::make_shared(); - // 设置头信息 + // Reset pose header msg->header.stamp = this->now(); msg->header.frame_id = "odom_combined"; - // 重置位姿 + // Reset pose msg->pose.pose.position.x = 0.54; msg->pose.pose.position.y = 0.2; msg->pose.pose.orientation.w = 1.0; - // 设置关键协方差 + // Reset covariance std::array cov = {0}; cov[0] = 0.5; // x cov[7] = 0.5; // y cov[35] = 0.5; // yaw msg->pose.covariance = cov; - // 发布重置指令 + // Publish reset command pose_pub_->publish(*msg); + if (wall_filter_) + { + wall_filter_->reset({0.56, 0.18, 0.0}); + } RCLCPP_WARN(this->get_logger(), "FILTER RESET TRIGGERED!"); } @@ -184,6 +194,7 @@ private: float Sampling_Time; rclcpp::Subscription::SharedPtr Cmd_Vel_Sub; rclcpp::Subscription::SharedPtr Akm_Cmd_Vel_Sub; + rclcpp::Subscription::SharedPtr Scan_Sub; rclcpp::Publisher::SharedPtr odom_publisher; rclcpp::Publisher::SharedPtr voltage_publisher; @@ -212,8 +223,10 @@ private: rclcpp::Subscription::SharedPtr Sign_Switch_Sub; string usart_port_name, robot_frame_id, gyro_frame_id, odom_frame_id, akm_cmd_vel, test; + string scan_topic_, wall_config_path_, combined_odom_topic_; bool publish_tf_; double odom_pose_cov_x_, odom_pose_cov_y_, odom_pose_cov_yaw_; + int wall_scan_stride_; std::string cmd_vel; int serial_baud_rate; RECEIVE_DATA Receive_Data; @@ -236,6 +249,14 @@ private: size_t gyro_z_median_count_; bool gyro_z_low_pass_initialized_; size_t count_; + ::origincar_wall::WallFitConfig wall_fit_config_; + std::unique_ptr<::origincar_wall::WallKalmanFilter> wall_filter_; + ::origincar_wall::Pose2D laser_pose_; + std::mutex wall_scan_mutex_; + std::vector<::origincar_wall::WallPoint> latest_scan_points_; + bool has_latest_scan_; + bool latest_scan_consumed_; }; #endif //_ORIGINCAR_BASE_H_ + diff --git a/src/origincar_base/include/origincar_base/wall_fit_core.hpp b/src/origincar_base/include/origincar_base/wall_fit_core.hpp new file mode 100644 index 0000000..290f912 --- /dev/null +++ b/src/origincar_base/include/origincar_base/wall_fit_core.hpp @@ -0,0 +1,128 @@ +#ifndef ORIGINCAR_BASE_WALL_FIT_CORE_HPP_ +#define ORIGINCAR_BASE_WALL_FIT_CORE_HPP_ + +#include +#include +#include + +namespace origincar_wall +{ + +struct WallPoint +{ + double x; + double y; +}; + +struct Pose2D +{ + double x; + double y; + double theta; +}; + +struct WallBounds +{ + double x_min; + double x_max; + double y_min; + double y_max; +}; + +struct WallLine +{ + std::string name; + double a; + double b; + double c; + std::string color; +}; + +struct ReferencePoint +{ + std::string label; + WallPoint image; + WallPoint map; +}; + +struct CoordinateFrame +{ + std::string origin; + std::string origin_note; + double width_m; + double height_m; + int image_width_px; + int image_height_px; + int pixels_per_meter; + WallPoint p_image_px; + std::string odom_zero_reference; + WallPoint odom_zero_map; +}; + +struct WallFitConfig +{ + CoordinateFrame coordinate_frame; + std::vector walls; + std::map reference_points; + std::string generated_at; +}; + +struct MatchedWall +{ + std::string name; + int point_count; + double observed; + double target; + double correction; + double mean_error; +}; + +struct LocalizationQuality +{ + int wall_count; + double mean_error; + bool has_mean_error; +}; + +struct LocalizationResult +{ + bool ok; + Pose2D pose; + std::vector scan_map; + std::vector matched_walls; + LocalizationQuality quality; + std::string message; +}; + +struct WallFitCapture +{ + std::string key; + WallPoint map; + Pose2D odom_pose; + std::vector scan_points; +}; + +WallFitConfig defaultWallFitConfig(); +WallBounds wallBounds(const WallFitConfig &config); +double normalizeAngle(double angle); +Pose2D predictPoseFromOdomDelta(const Pose2D &previous_wall_pose, + const Pose2D &previous_odom_pose, + const Pose2D ¤t_odom_pose); +std::vector transformScanPointsToMap(const std::vector &scan_points, + const Pose2D &pose); +std::vector transformScanPointsToPoseFrame(const std::vector &scan_points, + const Pose2D &laser_pose); +std::vector mapPointsToScan(const std::vector &map_points, + const Pose2D &pose); +LocalizationResult localizeFromScan(const std::vector &scan_points, + const WallFitConfig &config, + const Pose2D &predicted_pose); +WallFitConfig fitWallConfigFromCaptures(const std::vector &captures, + const WallFitConfig &base_config); +WallFitConfig loadWallFitConfig(const std::string &path); +void saveWallFitConfig(const std::string &path, const WallFitConfig &config); +std::string wallFitConfigToJson(const WallFitConfig &config); + +} // namespace origincar_wall + +#endif // ORIGINCAR_BASE_WALL_FIT_CORE_HPP_ diff --git a/src/origincar_base/include/origincar_base/wall_kalman_filter.hpp b/src/origincar_base/include/origincar_base/wall_kalman_filter.hpp new file mode 100644 index 0000000..b088430 --- /dev/null +++ b/src/origincar_base/include/origincar_base/wall_kalman_filter.hpp @@ -0,0 +1,36 @@ +#ifndef ORIGINCAR_BASE_WALL_KALMAN_FILTER_HPP_ +#define ORIGINCAR_BASE_WALL_KALMAN_FILTER_HPP_ + +#include "origincar_base/wall_fit_core.hpp" + +namespace origincar_wall +{ + +struct WallKalmanNoise +{ + double x; + double y; + double theta; +}; + +class WallKalmanFilter +{ +public: + explicit WallKalmanFilter(const Pose2D &initial_pose); + + void reset(const Pose2D &pose); + void predict(double vx, double vy, double yaw_rate, double dt); + void correct(const Pose2D &measurement, const WallKalmanNoise &measurement_noise); + + Pose2D pose() const; + WallKalmanNoise covariance() const; + +private: + Pose2D pose_; + WallKalmanNoise covariance_; + WallKalmanNoise process_noise_per_second_; +}; + +} // namespace origincar_wall + +#endif // ORIGINCAR_BASE_WALL_KALMAN_FILTER_HPP_ diff --git a/src/origincar_base/launch/base_serial.launch.py b/src/origincar_base/launch/base_serial.launch.py index 6ac9d39..eebfcc2 100644 --- a/src/origincar_base/launch/base_serial.launch.py +++ b/src/origincar_base/launch/base_serial.launch.py @@ -6,8 +6,13 @@ def generate_launch_description(): robot_parameters = [ {'usart_port_name': '/dev/ttyACM0', 'serial_baud_rate': 115200, - 'robot_frame_id': 'base_link', + 'robot_frame_id': 'base_footprint', 'odom_frame_id': 'odom', + 'combined_odom_topic': 'odom_combined', + 'scan_topic': '/scan', + 'wall_config_path': '/home/sunrise/yiliao_ws/src/origincar_base/config/wall_fit.json', + 'publish_tf': True, + 'wall_scan_stride': 2, 'cmd_vel': 'cmd_vel', 'akm_cmd_vel': 'none', 'product_number': 0, diff --git a/src/origincar_base/launch/origincar_bringup.launch.py b/src/origincar_base/launch/origincar_bringup.launch.py index 10d485a..ec80048 100644 --- a/src/origincar_base/launch/origincar_bringup.launch.py +++ b/src/origincar_base/launch/origincar_bringup.launch.py @@ -1,99 +1,87 @@ import os from pathlib import Path -import launch -from launch.actions import SetEnvironmentVariable + from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import (DeclareLaunchArgument, GroupAction, - IncludeLaunchDescription, SetEnvironmentVariable) +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PythonExpression -from launch_ros.actions import PushRosNamespace -import launch_ros.actions -from launch.conditions import UnlessCondition +from launch.substitutions import Command, LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue + def generate_launch_description(): - # Get the launch directory bringup_dir = get_package_share_directory('origincar_base') launch_dir = os.path.join(bringup_dir, 'launch') - ekf_config = Path(get_package_share_directory('origincar_base'), 'config', 'ekf.yaml') imu_config = Path(get_package_share_directory('origincar_base'), 'config', 'imu.yaml') - carto_slam = LaunchConfiguration('carto_slam', default='false') - carto_slam_dec = DeclareLaunchArgument('carto_slam',default_value='false') + akmcar = LaunchConfiguration('akmcar', default='true') + wall_preview = LaunchConfiguration('wall_preview', default='false') + wall_preview_port = LaunchConfiguration('wall_preview_port', default='8772') - akmcar = LaunchConfiguration('akmcar', default='false') - akmcar_dec = DeclareLaunchArgument('akmcar', default_value='true', - description='阿克曼底盘模式 (true=阿克曼, false=差速)') - origincar_base = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(launch_dir, 'base_serial.launch.py')), - launch_arguments={'akmcar': akmcar}.items(), + PythonLaunchDescriptionSource(os.path.join(launch_dir, 'base_serial.launch.py')), + launch_arguments={'akmcar': akmcar}.items(), ) - choose_car = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(launch_dir, 'robot_mode_description.launch.py')), + robot_mode_description = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, 'robot_mode_description.launch.py')), ) - - - - base_to_gyro = launch_ros.actions.Node( - package='tf2_ros', - executable='static_transform_publisher', - name='base_to_gyro', - arguments=['0', '0', '0','0', '0','0','base_footprint','gyro_link'], - ) - - link_to_laser = launch_ros.actions.Node( - package='tf2_ros', - executable='static_transform_publisher', - name='link_to_laser', - arguments=['0', '0', '0','0', '0','0','base_link','laser'], + base_to_gyro = Node( + package='tf2_ros', + executable='static_transform_publisher', + name='base_to_gyro', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'gyro_link'], ) - imu_filter_node = launch_ros.actions.Node( + link_to_laser = Node( + package='tf2_ros', + executable='static_transform_publisher', + name='link_to_laser', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'laser'], + ) + + imu_filter_node = Node( package='imu_filter_madgwick', executable='imu_filter_madgwick_node', - parameters=[imu_config] + parameters=[imu_config], + ) + + wall_preview_server = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, 'wall_localization.launch.py')), + condition=IfCondition(wall_preview), + launch_arguments={ + 'port': wall_preview_port, + }.items(), ) - - robot_ekf = launch_ros.actions.Node( - condition=UnlessCondition(carto_slam), - package='robot_localization', - executable='ekf_node', - parameters=[ekf_config], - remappings=[("odometry/filtered", "odom_combined")] - ) - - # 从 URDF 生成 robot_description，供 joint_state_publisher 和 robot_state_publisher 共用 - from launch_ros.parameter_descriptions import ParameterValue - from launch.substitutions import Command robot_description = ParameterValue( - Command(['xacro ', os.path.join( - get_package_share_directory('origincar_description'), - 'urdf', 'origincar.urdf')]), - value_type=str) - - joint_state_publisher_node = launch_ros.actions.Node( - package='joint_state_publisher', - executable='joint_state_publisher', - name='joint_state_publisher', - parameters=[{'robot_description': robot_description}], + Command([ + 'xacro ', + os.path.join(get_package_share_directory('origincar_description'), 'urdf', 'origincar.urdf'), + ]), + value_type=str, ) - ld = LaunchDescription() - - ld.add_action(carto_slam_dec) - ld.add_action(akmcar_dec) - ld.add_action(origincar_base) - ld.add_action(base_to_gyro) - ld.add_action(joint_state_publisher_node) - ld.add_action(choose_car) - ld.add_action(imu_filter_node) - ld.add_action(robot_ekf) - - return ld + joint_state_publisher_node = Node( + package='joint_state_publisher', + executable='joint_state_publisher', + name='joint_state_publisher', + parameters=[{'robot_description': robot_description}], + ) + return LaunchDescription([ + DeclareLaunchArgument('carto_slam', default_value='false'), + DeclareLaunchArgument('akmcar', default_value='true'), + DeclareLaunchArgument('wall_preview', default_value='false'), + DeclareLaunchArgument('wall_preview_port', default_value='8772'), + origincar_base, + base_to_gyro, + joint_state_publisher_node, + robot_mode_description, + imu_filter_node, + wall_preview_server, + ]) diff --git a/src/origincar_base/launch/wall_localization.launch.py b/src/origincar_base/launch/wall_localization.launch.py new file mode 100644 index 0000000..2f7ce7b --- /dev/null +++ b/src/origincar_base/launch/wall_localization.launch.py @@ -0,0 +1,33 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + scan_topic = LaunchConfiguration('scan_topic') + combined_odom_topic = LaunchConfiguration('combined_odom_topic') + wall_config = LaunchConfiguration('wall_config') + port = LaunchConfiguration('port') + + return LaunchDescription([ + DeclareLaunchArgument('scan_topic', default_value='/scan'), + DeclareLaunchArgument('combined_odom_topic', default_value='/odom_combined'), + DeclareLaunchArgument( + 'wall_config', + default_value='/home/sunrise/yiliao_ws/src/origincar_base/config/wall_fit.json'), + DeclareLaunchArgument('port', default_value='8772'), + + Node( + package='origincar_base', + executable='wall_preview_server', + name='wall_preview_server', + output='screen', + parameters=[{ + 'scan_topic': scan_topic, + 'combined_odom_topic': combined_odom_topic, + 'config_path': wall_config, + 'port': port, + }], + ), + ]) diff --git a/src/origincar_base/preview/map_reference.png b/src/origincar_base/preview/map_reference.png new file mode 100644 index 0000000000000000000000000000000000000000..ebaa822c7fcddaa2f16b4653a37929f441d58269 GIT binary patch literal 53197 zcmX_nd03KZ_kU&Ol&O^^l?t|6cRIklmt-+WegvpLf1oY#g8btD7r zl`XpaZ8uI|ir=tdcfvo{#>s1-lnomO!hL`6I-e3MnJ{RPYA%c{J>6BWnTw2q{|SCE(5 zL2ptwdn|ebFpA$JA%glf#`?l~+i6v#yW)|P?yF{fQu`95d}gOoJzr%g(W`K$^?`N8 zdix!MR0tB*_0Mt6lB#@+)7>^28#&}fLkt!pz_$d@@a$r3LdnB2mYz8~8G-a{G zuR`aObE!<;-sB*v&zNfQ2W+qh4q`@lFx*OEb!?T40NK$)f)J6@C2bE&yeqG6Y)IuP zZ(uk=V@vgC?Q!-(JCW`WD`OPz`Sdx&-0X1>g{7q8A<*~n3?PY9tWhRZ`A#kzKoC88 z#N)$r4u}$AY%WVIn<3-_iESgu`cUtZDV>PbesmnJAX%dPXnd6aJA3!8!ix};SNd_Z zf=Ft<+|67mY)ES?Y&c-e6tapUkQZKhw+bVIkkT;| zjjDqVJ%xD!JEe+}(zVZw9bYI~O2z^lmLR+Z$on&Y$$!J=lRNijl}LK~LSfvwja@WQ zd3zri$j_KsVHbrOU8;`bQ0n3a1k+T;vepdbF$J%xE@_;&V8Urut@MsXmz4#dM+tq) z_ks+Q9sHlM?i}L%RKva5A7pnops%I!LM}nWTWbbWBlt&}S<%n}savbi8;hCEs8$?y z?2m8|oS;93O+oI+NlA4X>9+b-xH-;{3<6wLmXuA<$zugFPt{_C<4#ay92J_)Qv?pc z8_09Nk`7L)SkYw*kIBCW_)(w)(TRTT@VMeSBRvjh=TRdD{cN)blemmiPEy~xTT`bjNN3VU0DdH%flVd?dj*8|in4Vr=NEWe!A^(Mt-=IngFg=i?Q5{}Hnh>Xx0^cUP1SWy&tH9?;YCiwFZIwO)1k z)h$wAI;W5mRsA{-YRhpQNLD@o-5*v?u}v1H6~l+W4*Aa2D1y&AunT|FC_gnuGI*&k ztg4YP7DuCuuo#H{SZT#UZpB#IMOeqi=dmY&Gw`uD6s!EDANmWNHEsPT5zxQv*Dv?K zudODe&LFDDu1Ohu&@dVV{@&4Vw|K z=gGC)JpFqFmnI<1d}i-~uMO-oE}HFNg1V)0+Z>FCyi8U-(PNPpKftK{b<@BAXiT-R zVrhGA@ycoHblgXQtEw}!kDn)MRb2cX($$pCSy{|Wjah7wgMFZvm=6hzXb?<&ML`YG z2vRSvD2>{V3F$OUZ#&B2~eJ9_bS)KP$<(+_Q zP18UEVN!{i&nc*veFTo8tyRt{OJZ=NWI?9*;vy0jmvdLYr53M^rBeU#Ab^~~qaexLW z6s~Qlzq`$>E!W#+&?gDyg_2FFeju4tHk6n(w@|EBriYpMA~Hq;+#siTd#Dm1Fy>!%6;Tt^3hfaTDoI{L zu9+bDgIVN46aE_n>}}|WY1SqBYN*?BmEEM=ch0Ln{;9`l*sO9-9_#hEaGY8(QnyxD zA?|E-jKjP}%x#-bmKKbeSVT6pRO*{GCz7CB>c&jcaIPbW{)9T_#KI_#TG3+Lgsq2B zD{equv|t|U^~bAO9uV@GQ@nL+1a6gi!}Xha8=!(E~*_@fUAJTekiW){I74`aNT>rtT<;2FjNpld6_-)N>rU&_ym4Y5ASe#!8n?z?y&a~ z-D*#gQW$BUx1+{PfMG;4egt)L*RaThz(D3$4~~frlSHlL!U7t($6%>Vh4bUo2$Qu; z;@eA-mB5hz*Zpr)Ob>E2EHbN)G~|v578dk2(rGH^8H_DVIkS^3gCvtjsU#cFOf_DP z>jzI?hIJH@f0MqV_gb?m5e}xq--ezQy@G)EBstm|djw1Ne}8cQ>HfzQ%j;yrZ%^Be zS_z1~WXaw@d;f;~n5%2cmVTDW)wWfuoFTLJw`RdDigzKckS-7X|*q+#oWm}77&+1NT3P?rn^Ogsej0DP!cGvgv_aKA*Y8Z96zeCgpidQCstrxfh8 zif9;Ua(&IiWle6-DHTV$BTfhfx9xVuecbYk|Lwrb+F7PI)dnMM;03)Mv^LvWgvPag z?-D%Q5jkvy>f}7)E`b`KIOziJS>5EJug_P18$qsPsJQ0N7oimnRU1N_>tJpPZ zB};R&k1aSoutGiNZ~#VP24Xa_pCNncT4613VH9mdYiX-VV_BKr;CKgzTb9~v&4X4` zC%gNCtY{DjZpZU9=w6=@YRO^JYGX7auLVCV9P-sjlwrdrsB^Agd5O}wMs3YTp6Wer{a-%4q6b_0bA20(zXb*}>OFG;9y?nlAPV-P;!!aLli!^Y(7=jEeFH&^}=U{pNS~y!!SzbZmY;tHEsLcKC$lyqy6+a%g`pMzVes$ z;=J3oh;}yiRw$+p4Z~a-{$?>+bcs+N+6>FCP<$xUnfyEP(DpM`#e4z(S%LDCNg!)8 zZtW=&BCpl$lKBbrxzcfv%ReNw@G-8%#+kEQ{*t2>l)q@@wT=9}2(U~sceo$B+r;Cm z#T>IiEe3eAho-$+XSFIVGr>HN?QUn!Og8rfyt89i^$HpR0im=;)|p7~{60AB3hvwV zC1oW^Kh^GAEAoneg$AS1IgX1h#4@xbtyQ~(v)hgr-b{nu9n;)<-DKJwINwO_dTOkl z4Fd(?pLhe_(M74GOrY_DGD-3B&>TFru?^&fVjZEEsK$n5=~I*nal-F>%Tii&8C9?| zfRJKipHd#(o_H{pp3th=$+o8tD=yOeQ(5X~lfsZmVL)8cAFkPwuz^TkcNpcX>6I}( z@Ia0eJoO0-P(*jAyvuO! z(xsQ0B{4(zrApZIEIFkJy(`H6zq0y6+r(o9I@lAQH|{~C%hRFNAMC6;A6X>x^dC}~2+$}t@<-X8HLRD^tXZH1!*(51Q z%F4P_RIPn^N?mtcrT$_gKwqB6237*{wA>jppUA9gF0lR~jGC0B=`Wx~Rp`{q(-u%$DVgV^K})R0-{>Zed8L zftFpf3WS*}U(@|V28k`$%bx-oyP$zCn1_Tu0OwSDNExbwj%EM39JwM&scZVzUlPBO zMg#az`7!>9bfkLb2ydjk89{JS2HWgG%A`8*S?~v!FXvJmJ3|)Ky<9TqY4GPc{3hNA zpQ57+72%yl=wXwH2Ju}oNNjEQnd*UsrshxSm&h6}JR^s-=21dths(3XwP2VEIv`|T zktI%!Btu5xbRqnR+ny)TOZ}?Ot;1XBd3y5U06eNXuiROU+rb>hHATpnx8;J(YNZbV zxiDIU*72H@ULHh8BgXBMmEq;&o3!x++IXVWjUJd%Mb2p+K(rqP5$bkkB{ImBUOKu@ z&Y>jP7b1IN5q{9CdiHSifRB1u{AOB5=k}VQG0IQ$#A+)J`J-B3(}e9Xf(HdVaGVIF zM7HXUgLflWaKf&$p_ZpfSY3n$W;c&MN6M;`zN_sm09UlgKRJHNTnqA4N1T~xAqwiSq#F&l!*0d)p8u~X^R%K{q z7QrZ_mSHPJ(i3C$9hE<+3kgD>Rv&0e87&+Z<3@#02cW#S@BcFp5+?ldE2I_ zk)b$KePO1vjQ1wjlXVhCpS^-a7>l)sgkP?(@}AjTOmfSuOGmL(%lR z=rNPpWIvm={s8B=LdySNXEx>7Bf1Vgh7S21=ed$AqDz=>nGYZ4gk99esAz#cN+IGSR3~vbSMZ+#LFTl*((thb%j4KcAiaG_|h=X=F)>xQ$ zOtuhhJxa?$QHg)mS=B8<*PImq?^sxhI8vRbCSt!e=HH6@bcIJ2XV5kBA|F4IIvpBA zH^+TvQJF8uwu4QubogJeg$ z%#5g>t42O&Az?QO>MrQ9rdntn+AhD)b@+G_aCxjim!(-und5SrTcq9J#^S;&tcK5m z0iE-yM%;|m^d%&#ljJnd?}dpf0Pxa5_1y-n@<`T%JHdWRbspc&EWn-pXTdYfhtG`q zX~%|-5eES1G_aKpo2?$;rn;K(S7N5f1swYFZJN-ZZu@O+-Ey!-?hT=B z&yYJgm0=~y*(08^-oIrL;P>D14E+Z(%2qi6jn$IpuCrqSG6|DyqDu>LyNf{LS7wGf z%GS-V!iKTnwoe7uK})(=wv&pSQ24xW#nmIf1x)DzE_*Ac9)xp!zmOX0cYK|LvtM!> zKyk}gn3C1&ASZKbnv%a)m$VVstKV=1mqJi7E|FS+y_upzi=4x51Yu0BydZgk^rQ^z zk)06@y|Pb z*o~=iGJo@ZMrZ@TSg4oEh~@!j+F&DW)p^_OrR9`kTwNY0<#RmgiXF`9%;9py{nAMn zCW!=o;(l7g_xdmmj0Q$W!0F2kh&9cp1K*e*1&{iK-F16E3K&X%TXWCIz3Ymxs#f1T z2x|{F9tHR`*R#03?&K0NCkUrthe~?FKfZPEYcHz`OQAoQc7U!z^hTmM#hj4oiPaZa6cElYk zKcFruvKXb7?lF^iyUV0{P@KK;{5a4F1rx>sQy~Mji5aUY?&a7UA7U8xx-9NM^a7-s zMsDtRm=tpXXD<8n57M3AB<7kVAwp$rY>!{g?G^5LLalz&v(ibm;UDRZ0o!@d($xGSD@}ig_)_^rck@~nJ8k~lx2>33^Y$N} zk}l8g9T>D8^s4*uxIOCkKP=A5gd4`*--LcTs|g!25m zbJ{3Y&LcOauY$AD0+Q%3ge>|)ycfilhYDQhiLb1V8#N^*3I_am$n8FD7iiLQXIxT4 zcFff?oWM_tXt@#3K1DqvFb%U%QeGp05h{KPp#ZJDsr0!^PS_Gig1R* zT@2+xqup;zI|5--BYGMX<~+F6#_>VCndg5(t^yf9wj6ghCxtO_*4+QQ%T zWK}kb_JjVWQukQtfolKsE-5cT?$7{5z<_$-Q=p`Gp*Y8o>1oc)XOX?3_i9W0{eF=O zsTF-Z^Jo`J5t(7$l_WVqNA}NGV@DOQ>&En65BRu`G=j^>#ve(Jsk*EXKX(~+!DmccAI2 z^xwjVpCl`p#Jkp=+tj1zn3m+*JAsHRDZ<)_@MYK9aISReX7tXS6ClSs)vl-rgUF`7 zi8sA^b7(GrVp@Ais$MFtYAo;+HSjo#;tgZ+O$be#+oRxwi&q)3lY`Dx z;n`%Ogd~%oq}~ve`kmMxZB|wtyz1HNn@%!G|M&!cvQ98QY7t| z$#~-*Y`!Gl{Hy2RF%jFoCDS|oPf94w&0j0vc!vOdn#4;fW^S^2o~C~YP#7IgN*o1O zq~O*Xhe%1z(*PD7>ZO*HW+{i}z+@yWjL>IO|h4H(hXYcT z`Jp?^ahKg{*_jZIjzCXhc_}I+Ro$_6AX22g&I9t>OxgB#sn^sJ8C$;!XzV9r-nSSU z)s-_c9@o&8<9a+)uU!mO{7-j%)*3~Bq(~HAi64oGPeHBB0tm7bSL zdubGqN_lDGC0C%=6npGA>)RT|Mm}YWvXia@%nQ;MN-H$xgT0gAQ^efe(ug7i`5_$> zp##v#1&F>z?u%JXzNyZ0c+=#+^E5;S44HjOCj*# z3GCF0>wvn@tmkTGoNBblXOsHr5)$t(MZ!Azn^elbo2{~qIhBSHXM)wraDr`Fuu+(? zcIQ&&IO$63+qHByISn`3h&j?Zl*&sP#LkqbE?ijKh_y)h4f%&q?GWBjvokzmLOX+;vwIbk)i2^p@Vdl zTm@kVqOK-P2&GDCB4Mk6tD~uJ z3SnC{Q>*hVPpTv0qUL?WZ~)6pq%+H7M^ zTO&Zse3fz|1vJNXvvW~4LWy_ViW?g)@okfqkVyuWY+$HhX%L{=RZ|w434flJYeVIG zM(W!SZYzSH!>OFhm!rJtQMcvekOU?1sIANg7&cejmRep6HTECy!l$lOnr6?n1TXS$ zWFx6-VqwA7gTFo;a*;Z#YcTc13Fns;D!jj6>cL58*Yy#g32N1I;cc+dr=n%?T}q^E z#KBMV&|)a=zNwiG_(uvJL;v+|WHPM-*Z!i#)4U8iN#^7b+4L0bYVTb7i=t;REKMFF z_&~?|!!UQ^s%o}8cp^y|@@=hE`noLAR2X(q3rg4d@#qg@$WzF-%!500W>8bX--KvP zW~fVvD7D}@6b&=k*69a!!Z!0;i-7&=J6b`F;M3 z1$-^KggfMGAff20@$p}WO-f706+eKTa4b&}1-sbK8gUVi?82NMYD@-BZS3ZyW}fMf zCo4)|uU-k>zNV&HBW@qcT3V!q$00L< z^=no`X7kF<=HjQ*BQ;z17z>2}gdHfv0&7*JMH*%&r&0{o(?%rDh|w4)u)vOVWEC_&Bm?MOw zuAeSThBNqT*BAI+-%T_ecJHnlM(#+OpSsg(<+2uv~%*tKH zMM${YTE|vhS5okhO-gwRFe6H7qZtFg2;{tqpAziH#akqIn@6bElQchqPIv|Tqf$$E zgjE#CVzjbNua|qqmJ3I@Vm;1^v;OI|Hva5b*;NMUcI|8f;M-oWChE1O+>mZ^>=wr^ z#M<&s^BCsj4xnm^p$JcR5 zOBV-vXJxm;Gr)AI(-inU#uNZs^~O{~*Kv8}_|t!h!%roGxqEF>%r1mU!;cQ_a?O5x z*w#J2xi40aRU*F;WOka~=#?fcI>Sb+3Hm(i0nxT^EY;$y9gD(-o{YR#2O^yETH*jI z>;$>GMf`}q)}kur)Dy4j+aH{?wCk*+*-?Y3$8cc@={blzenT?Zkt(KE~!!!J7O-etMaW8V$;-TIHAN~AYt5;vbqr?! zzo;SD`pLMwU}U;he?{IQJ{YP>(RFmk%-r@Mj9SNq(2+*nSr4fxd-+wzYNBG()N>|J zJknRDQIIO2t|O^_qf(j@Rzg?_QVjZP0rJm+z_r6U8zJ2ZPgzfB=1st6T)UZpN=Ft+ ztk<-ucQFH2Mx-oPQ_@r+)I~4dpU;J)m?&JwF6@2U#?E>k!u(|0Ymss~y zb8xK#jdq#LYXeRt#x^(onALlU)|`4|ThyjE-#D1KWY>Y4Od=5ti^u>Wdq*}gYy6mC zLsGD*jniOr(3+UTDmCI(p_c34*}rH((uUpiE$PHva^|l**>5R5){gUeZV85-D4BAL zR0uGsO#2-+0!mvdUy&1Ev7`Q;!>#(3lrJwvsD1hmJC%`hka`$pv$>^*TP=Dm&K&u1 zsXd#K6e2(&*0~KANY%*h<}i4+wG*cZcG+{j(Lh*$3ms$Wu@tDkn@8jcJJj8p5DkCo zb9Z`mQ4|_>us=Kx(`!+&1z+9xCs7Ba=(8o5nBV^_6JV~Lv8x2|Lpcp-F!#o(q@66p zjz`iOMM$tFJWzinE+03!{9!viZ$JO2SO0Y8GWPmalOdqEqNPR7=Bno z_nw}awOpN-VQdViu`UFQ^f{j=Lk_;0o;_M0({Sa<;JCV{MsG%It{ z{bd&Q3v^)p?Py)oKSx)QM~33+4G^)ZR|K7%G=P&0^@8+yN&j8viy=EWtO7ebLKu}m zZ)vzx9vr8+i_@LLo5C{c^0KLr8S*>rY8iDjBLpE6Ju6~lQ+3h!f`Mb9O_E^hw!H9U zs{>bZCC$2;6)z0}MA~xPVDO4i!Oa#b^Gn|~7@{`F4b*K48Z3W5g$~xMqSnS&LYyAqK4kCT`vfb-lW9ucH%lraKwA` zaW=wOqX(HLm5*pwL4XK-c3p~kkr~$nyEZqp5#yU~jc6=yk7%q;IPO!@*scVLG0{vg zxxe4|Ibf@LO3;o!VHEe=JH!8=b==E3Nt~jQlsTXL((U@%;~bx=%lKg%)4(jKJ(K@c zd^KkH$lL|RW1yRIk#iDV^(R7vjE{V;%cab*LY2Ep)YWong4*7x8h9 z^u3kv&{o!3Q|M}g7ds<{^emlD|}Scs2aTu8@C+V zi`seS>EWY?4_f~Ph&32@+qni>&H%NX4jVHYh$^$~d}84=I2>t}VM8Cym_)n~WG=hb zbi8kNaFt;KofwVnYVVQ=2h25nmjqQ_rbp)^T?$|e&6hWi$nXyU@-H9g=6s5Q(8_k{ zzH36AkP}o^O)sA;tT1;>7d_*6F7&z*<)`UTASb<(n|yYb70M55wlrhhHeRNf_2do_ zxotI2a^9~l;>-s=qyk-RegD^=ren9jD11_?R8Un`4L{3v_drn-RM-zZ%uP<*(@CkD zmF09@P1?AR*{4ix-m7IXSU;OOAcVe`0GsH-Q3}W-Kb+%s%zT)5l04iU=at?#11;iY z$t~yOo+^GRRrv#3WfnvC*nfZ-#mgOQDZi^0*Ul!eSR;zJBbnmwzsk`Qg4Qf((>x`P zSs*(iTxuDe3~S{|+>9**@`puz$X9b)<%@gY0K$a7vqQ1Z%$lUwM~_s z{~-U%a*Cqg#}78LUnXYwJ*7E!>CfVOjh~OFWYSWags-XOV!ybxkk9)%JIicd2r8YN z#AKhBfb@^_61nZp0^^xdpX2N}U}x6y)6uu|%g!BiD~b3TtcdM$Q= zrUgih?G4*c;mx1BSfLCHgF!BH_StfHlaU!?i)=K=iJn(&UdOaj6Ta#bzD_3A>$~)t z>C^BA{7_z~CO4T}xltWP+Q=C`_kR5;o9?RG8I%$df@_D4ET2kcjx4BtHs*7>7Z3MX zJDs4D`6>C)1M{Ox`|9*v)ZNbYtSy)Z(O9%GlU?B40f6l;H0j*f*OO3`?x&{=}Vyd ze-N&FXFVGtQ*{i@K(uuf!ZiQE9?v~Zv|tpg{7yS6qhb}arUGx&aH}f1J~ssTS`+dbQ@$oK|f{M|3b%7lOb!wwS`GDH)n zi~U@HDh19wZy*skzaRFl-}lRo2WRB>3fFFlN|9P%K$GwssbSJ)ruD?RZ)8pYhv8lgSufuQ+RYt z_hHfBLB?(17x4|4e8!~QV(y*&^a0~qV&kU}Kdi2^^+Na*U~KlJIz^=k!9^pT{uvdb zVw(92% zY|P@jzEzoa`SNe$&GZw@l_Roif`#Qgb1lC)v~`g$oSPt*A{P8}h1N!aMFOp8;E!!Sl|&NO0s^c50>j{!Xxy zk^Nk}QDs{a#>_n}N89S%&E~a1R`1u$18_ICDX51`b>#~t&vkr_~qdv8MdF3&Mq&j zJKt}NU_Vd82rnw%O=DF*kd2pTEYOKnc_;c8Qap*y&Z^sMj<%5w$|Kqvagws;;FB_Q;&vf z@Ul_j-N^V}BxNfuv4@li0S-+^Ht4&A;F8W5a@uRs?B~vqhUl!*_G&(r-8ff1SwxBN zuzG>d*XIkGI(0p)mkZ1FjXeQYYbo%ZKDgs{Rg-DQk6ki8^72F$Oo}~i>I2S;AiQ(J zzr!tE6A|${Cuj`^xcBQ)RLDyL*T(jev>rl0=4lPz0(#yeCH`08j z7?i-cx$F>zaC276CJ%UOw?Bjqt>LSj6?$g1te=CO_T|eq7ZL zr#h?rbpst3T{*K@Y&Aa*v+wBViSLG!o{hIOY*FElbe)3*8aUeRkBo~ECzHA2T{kY_ z*(%{tb>XvUV~MzaNO6_QJ8ZQ_k6FL*l3^0|JCyd1i~eWMHZIp!Pd1`qqksV0kN+e~ z-O+4vo?^6-cYw)+??8p>3fm{P=3E&IUQu}u=u0em#lD#CMdbzJI7iZv^n>gPq3;?< zGF;=wNt5~1gp0wS=SJmFLF+);ys^FHw)E!GPuV3C1?8mKiLeeUJ80a@p;|A)k$>f@ z9Q1J?6`-H{Rc`=k8G@9O^5Bjjbh!3$sh}~rea!e$o)&u-se!{&eHd2LX@@q|X)S_W zEY8$D;XQ3Lz&sdR<5<;@ch)D?xEjY*wl*!w^R&Xm$viE}lSCjDK}Iqo8k>%N&Ynv# zpA8pQmDy_E=U@m4TJ2(<^m+lY!hP9uye{Z?69mI@uNk(nlcXzKrV`U_gN?CL=9jKQ zMmu&wkgf9t3;k_civY|{tIxd^)1#{5Ftyv#Sp4y&8bO!cHELQD%_we}S@-2<508AD zdHF?pt(sr{*kUYh+*p^^%d1+N%EIRg+UJ}<3iH*1f#fM`0tp84RGIyPUi=B2FuUuk z4L|?PLdgc&T!*nTzz@&uT9g`e7NQgF9OvyO8#ji8LPwf8x3v+o)0peO|byS2Zz@*jCBo~zM!Em zOxsTKZDZ5Lu+>*i3o_sWMj?sx&`ifnPRGPfE}r8p){mbTemeQdVB1))0}9WwNg9~Q zJY~z;!xIQ!xjz`9?W|~IEnS=$|McWQEltQ|07-SenF(lG8#d z7Th4pegD(H+xLOEJ}+FdpJpDvW{_-|6KM^8O&wX` z-(LCdL4%N*z~c1|2YNO_cvh|8Pn19>FEmXgR;2P zJakgXlf8ZSqu(V*KG?ULlh<%$%s%p-#fSF^pU2X}JpLk;F`&_Cwxfpyf z9Vzn*Y$ zu=q0}XiaAyR(_DiE3x?L$n@#Hg|!KWMjpIicqM-0V*^jL`{J?h;cM6WbPYL6mUd%P z-!`{_s$uWO>a{_$@7r#`6Rfi%%UXiAZ1O(Wu&t@0Kph=n*4vbNC#JRZd%D=Gv4_HS zR?8M;shB{Fj>UlY2P{_lot?*k2X$r-7Y=p z20GBADnDq=z+v?ASBv#D6J>OY$_UzT`tBmW(TAUuO$sK5dto)i} z3&WNj%C>zGI-k}Xvh)5o8GBNWoUj9x4j3!c{38lIZu7`&dXv`zb=h1n8-~8%aVeKy zZjR?Epgi4B$J%>div;$MS9g+vwMxbs8_cL2|M0KFn`o&`*UP?-zvc4!-VsS}Xi0EVdVB%Z`E-u;mzO5~(NGmJPjN7{5^C3(&FR9FxsZ> zP>98pZo1_bZFw>L>Dt%1!=*OwnR2J}{~qMs!`d>nvd1}yV=yCH_VXB@50J2*lMmG-uq|vXh&G4hcUG|6{GEeO=^T>wA zeLZ>wS=k+#c;9V5DuHltroer+z>fZpE9jJ#4b!}awWX?i5$TUC`u2YJeSFfS1H1FS zC;FPSst4#Z&Wu4lMVF-6bBDOq>79l}=Ih_@v3+T|7I-8JhtRV_SgNL{Qk~0Gx*8E*PH`W$^mppeLlNn_-58ocQI47y%Tt-nH2yu zdoWOByML5eHzi?NVEbc910WsyC#}t#nf#B7J7WOp3iMiEQSz)C>En{&b*%Gyr_4>;&#V_1b;Syrsoo8>llmU~tkuK2k8|npy5|oUk zhw3dOeW2TS_xj9x9(ZYfhP>JR-Q?J+Afx+N%XdI?qux*n@S^^M&A;H={Xt63Ci}JR zgZgQM`XQ?S>ZKF;<)8P4R(mBkO9_jIA0q~X;bzK(N2tnZ$8x1mu$@t<`T|<}0^t4k zm}D212aQ(;pDe7eEigV5O`kpa^3o8!EK!fw?{J-bRbR$}$ihE3y24vk61O=?Ms-j zDq}J@>e{8a(=xXP$&i2;FBtN+pIw#;?DfGHl8Q%m(1P}AcTqjyQ)*SmjOpjO!5?|vM#+;y$A_!=S}72gXkvY3*G zJbAR00o@b7fNni$lS(-@%5z}ObQ$$~R;r@pByOTTkCx?1`A7545w#Vh!V;tIhufTF(qGq(}%hO1!FOvg< zwi)?!#}?~rN`UT!#w1`-QGENv2U}EJL%?{Y_z??<`b ztq8L7lry=bcPIvMeQh^85;}LOx$S=l^|ns&r`O|Z+M^-K!MS6?Kmg&p1@}Z(=#`r1M^!>Ls1zyEyjxwH&?JQ3Tkrs zuMd;IdF4^>RJqLZSY^{y~cq4?894rx#3y{ z{q8m==O9%$=4ScS+wgLnx_Y_8m-1_>#l=L*mf`;sdi=3h-}m1Y+xOXWS{oXEf1P7; zc;swmI5+16{R^mh|7lauhqso-&HL(9MPTNS=QsQcx<`8+T3+jP{qS7m7SugGg8rC0 zp^vMFP)Bt&bTY3D!)5=2gJ|OPzZ-6Um~8K1w_DR&H~Y|*OOYAegbJQlsYtqu+u-$Z8%>4g6Kc%)o=={t^aV_iB z{#JB&`IQ~Q7e#Dsuz4%-pp3V~66Bi_sgqwsNJy z->{sEwUGH*AY+3I5znz}EBm_MXW>+Yt-cP7{Kg*f(dRo*ekmve@XB`O}Es z%EL|98-MB^?sl$Z+Oeo)CX&&P5gTxrXwv^le%t?ID@w5~>CW5!6ZOLolV-(TLw@(M zGF#xq{V>^cwf;3nn*o}KO@nXfM$c~AciI0cMaSIu{a3`0@6*g;v3FA9a5fdMN^xBH z51%bbY3Drp^#6!D_duri|BqL~=?LAVa+$6wWl3(iEJq!6Ds(E8%S2JRESKCiRw+VA zDl!?}R4&7Ew~d(543W#+#@O8EvW;Er_WP*L_xIoa@tM6p@Avz4c|KmRSF5keBkwE4 zsw~QZx{C6oN4}xuFN&ATvKRX`Ynxp}Z;MZ@rS8s;FO!E$Yj%9xj|}p6aDy-7_QAY9 z``g2yzbzO${QG(X`%EMAV2+PT%vP50o?qWrgLW&>CGu@mM*QqWIUSK%G`vKe_u$~x z2;~LeqA+rNm^fe2U7V`!+vKKlFdU_Wt(Hln~SzbC>4VXGq&NO;U`5 zm3+|QaxNtd`tj&S_5VY8bby^uCw*in`}zq3{Nktm-MC3L2j~4f$QS ze2VJQHs%jQ6UeLbVTP>O{1~s4kQSozz#c;F;q@%)X&pvvY(%Ytoo=e^_kzB;sWO*! zye<2+uOsYen4NV{)j02K4)o%f;!(?C&ga#D$S}~wI2-ujw94^y!Zq+D%`<9;%hzmx zo!6Q@p*^!NKlSi)~ z=SLYn9b8&TPy2rP`|C824LiOu_b&-6 zciTtr&;~bg_O|Nx3dOyfe|he9wdFTeQkh9YPlZ=!DY?w;$Hu{#(nVg zw&6{RQ?u-nQLlXT^t4>>jR5yUDJOCtA1pr4=xWBJ^zH8NSpVm9!hV8lof`t7)>Ck%F;=x_m~laY zZdO5YWN%ICsB`gOR5?g711jYhjjv^^*{rc_OY&V^WEW8W`DJ91{YxJxM z{4Z>N#H9k^>8rh_Q$2VCmDv{NLQ{vR5e21$Z~*&qW`jfUn$m{7qvaPyu#MQ3mNv)w zp>(g7Pp^1SmFfD5(7TSmea7tlt*_#v3o3B0A<+;N80y%D_Hez6H*ueV#wspzEn;e!C;U)qlWa>njtA z8fUUXy^A!=>v*zd!2FJom)ZU z`9O;OW;Ga81=Yu66X&m=h|;oO)u;ZSAF`2U;mch7P#V@LW>K^Zr%Z3=TD#X)dg`cT zbzen>p4E~{xuvI;o~gH;Klxc0b>l*=7~qFaDB|S%H2=WGVik2+%EKed`c`_`dz#}- z8rRV~e;+bZA~Weqr`VUdSD0z(tzNTT(R*u+X*rKhR7 zg>*_Q^9np-&#{JYkhWo#PLW9sLk+uZsHbAK=c&&AUFDmw^+7)}oQ|-JR0CH*m->7) z>89CFJ#z;a|F;niQMYdi?J@Gnw4|eM`FU#>o8-T&K)@rh1(LRZn%#$c{#i5h06yO# z-}^YF9*bSNy+`Ns_UeqNyQ^ngJO~uCd;a=W7|+id7~JK;|9`5cL4GE<-fybL!~0)D z#mtM8BJbDRj1vh6k+-K=$Du@XD7%~B^QpKKddJM&nFE`H~IcpM^#frO`0 z1p}Z9y8|CYWlglaw{Abb`C^Ri79Zk) zt@zmi{48@XIHKhgvU*GnNQP0IqPm@i#AKup40D%q(xGkSm2a=r9}zAASMgO|&VU~``2;T|s%gn7U zgN#MA!-wDRm{72hFO2n}qy`=Azo+K1^E55(LSa(xCCs_T=fS;+t3)c&mAnTGKdONr zeFJf0Pobq#Cg|l)=*aE#l?#G>edJ1eWDi9*nyx#H=+uqsEM!crlapu-k80^Jx23=6 zW%kUS3EnV%jMR$3Uo6j34)diwO58o*09Aegw*>WntTQkb&80(*`Fz>Xu*OtXwf>| zejPgbW>JPNiPp+}VCdaG-zV+(ZKa3?b6F7N^#W1?mGx3= z5skKkb3dc}(RxzUAZbXPhUC}>gxP~wI{s2kpR>VnxQHLFUcQr^l%t0J6WUP`L@1m@ zTQ85`x!<{DU)wjSb!1i+HB<*d0$aP zb%X;Lc!PF-K0mbW>7yNkA5Lh$MnBP|8DBWuT;!gJ^sn<~#7)0*nxDFdk@RD5!fK4D zdSK|+*9e#}DHEkm5Ds^qzrB-h?;83_*v&>0wOIX>D6?-dl(!}(g-lOe zcwV%)#3W+-r2OA?jyfR@wpCTsPQ6X)8qrA$OLiK(-En>!Wc{BN#keEk)gydyHZ3^+ z^-2iUphWmb6!IXVb7T*tz}vpud;693Zmz-LmVk020hyu1+_?Z4lIcKi-2oefaFp}j z6lM8zjozRY3vcQRY4_>v2r@er@Qg3s zXdtu)l4yyCxD%o6xOz^=Dt6jc7WS zbNmc1vN`*blO+7-zG{r`RiJFnH&f3H_A?JF_3^dO^r9S4N_ga16u78ho;=+hv&|V- z4ZLFfqYcZ0&<$Tt5i2^_6RV~w>0s4n|2;+BTIbMf8s%J+?!5ocOH-%pK5{Yi?^ma& znO@$>FC?Zh<1cmNXntoaiYVu!dGu9Ickf5dT&?V`5wCx3{a4cqmgD2vHJ*_bD(o`V z*|EAtB`qsq8Tbze&q``R-Xqvxx~m;SP){N z6I%$T%!;vp8PH8kisK237gue$QbU#nJR!;4{Y{h zt?WbK$_y_yB#Lj@XT~s7%o<=NCdfuGtC0RJ(GXO;q;T2QmG~FC(814Gn0}@bqgl{= z+FB)6KN+o@P%%Hoj-F^+V_e0;b(Z+3+k9|&ZR#uPv)Ese@t-lxiRjWi3L7!?5}dGr zf>N-$s0-!|_bj0n)=uscBjS{S(_#vIr;mI*{`K&1ruFMfla{NJXBy7 z9hcu{+K2Mbm&2L;t#7Z{UFcYHMzi~gOgja6aE}M7P_TDl<2uh>_x2Rq5RLX)HLeaU zR=YUd;~edQX;%<(ekVp!)Xh_xLk#1F4}F6<8s+T}cRV?mm$8)UK%pCBwmU^b+O}St zrBtzi9#|8QP29=cT6nYfhsPy3$5; zIJXHVZZeTALuE5$WV`5_6Q0u}TCNX9iw03Y6D<)jCgCwCn2ZM_2v>|f4I$$rWrh64 zuGJz<$)%O z+AY}*nemYnfcihAw6Ji?AJ;Xj1R)CXu?N?vVOHl~OG>kpBK(BLApZA#bLIrDISenI zAxQt7|D-|+p^uW_1U{UGSaL!x&CYeJwud&OE3yq3fm zKIWx)5q-%9Fk)$Q?BT387-aid;S`P=pwe{NYGg>eFTZ;e+TN?gd)`st+xyLc?p|^l(uH zQiKXakiu#$46Z2fbdelgS|oHcVYWh2j#?393F@@Sys*eTlnESbQj=7jToHk)h`3bv z(tedRPBx0HOo}`&=Fy}B;6ajwYuoqu%@Efk@Yb|D2n#420UwNGI%D;Vm&6^lyDUME zjrhri?SqdPTNH!F)_avCqQa9mS78CZ6H{3Gwr~fdARGOBLZ_KG zP7tWe=!m#z`k98^DX+*RM(=_fZOP39w&V!ATLV{%{$?*cU?N)pNgD=+h5(J!J&@Z2 zlk}`~jS{0=$X*R&^)#8u7HF>^UB-dBEAZ(^Ayk@inqu%q=*c|fRE&7DB^MG^cMI#NcH@q5Wf6v6uL3lG!>=iHZ) zrAwBrqcMO7j{Y*gHOq80<-Ksh zl)B6UmJHJRlBu+qTt)q2sC(UauD;2nebO`2N5lSQ8Vzs1roj7#0po#XW3btpJ!1s-EV4ACYI;#C16&a*PLfwVUyS%!zi}ZNSRR%Ot3u zjBbRKi$?lJi3Qz+WhY1n@;$say&aJ+egsC0GTh2xuLvu?x!mCr_dST^Jmm5t(iqs) zd>HthDmU%!SJBgs@~zB|#bp|?D3Im3M6)LF%an7ZoyG*B0$UxsY>7ta!Z((mE~1TVutX^@#K?bX5@)#V%%SnvQ`$x2z^hQx%j^`eQGN?{p+ z*XtggJb%_(HjWmjk=s`_kb!IgsvI`>l0!p zdRrHyAQ?#($c_+eu~&l@R+~VDFQ5@Ma8JQ22-jy=JWG&Nz``(1Lg#Qn;B0pI!LOJ? z;nB&q($^*<(bvz;9V#`h2sbXhqMK(@EbLX_bdf7QEe(lqid#N!KCBolGBl&-v-~Ie~O>u*DTNhzmf@92Z??l za#B^2i{eM1`Bg%UY(} zAy~O~$R=N9!!!{*B~nvs)Tbj>NQ!2W+~*i6%O6v!*Dr#X-+-%y%>ch)P8cjFw3Rhz zj;~H0NeqJUC!~V|Lz_u3fihv#u_hTTDh0y}{A(e1_CvJ!X`?&MS7grB+uG-JU_*ih(vTghi7H$8Sn>i4B@HANE z;8oX9_{aMvQrbx>5@O|Us2KwHAk}91aR({cUXfL0#$FgLN{MR#02snJCzUVF-4-Kz zO%uKbSEo$SR=QBV0gd*<)0ZzIr|ogT>PGKjTB}4Ko&#d*!{EpTI8DZti6JyZHxQZM zn0KK=#(cz2Q1-&-_JCWl-{QkCpmB3U`IBMl%inwZ-8B|Xp;;-9;y{@E8CioeRJy2w z6aMhKLbdESz^cKF00*@}tfR^r(;LOIqbg+EtZaq#0qTsaE$enZ10xnY8P7|-3C;b?jI$2zG*?x;ck%(7x zO$9QWC;6fzlb-9JFE(i%tq{MY1?A&*$k<3R2#SBKE}j$2(C3eeVX^^PXg=;wchflO ziR6j6a*46%t)&46%toUOc|Gl4=ApS_pN-}Uwa{`6*P8%=1QK032IkUxPD zzaRh^i%h{7Qk(*vo3OgUIb{f+S+N-*C66G>wzA1W-fd0Uh9n4o*+<%nt`dV3G0Ng6 zNH$1fEBju^J1G7PUCMVKQ*A#~n=xf7zB#HdFvF?qC-xFk)&t;%L--S8+d)hS_I*`l zWQm{qJCqCV{*9UJxYtQ7mEXw2iC%C6+mS8vo2O@U&052R&A2s2CHvzC|I*ra#fDfI zrC5&hzMT^Z|Myv7JpW<0^@+iEjNKG2nD{#U=!lXlQi72!4Gi6_jD>Ob!8jQ*VLUi! zS-U;{?2BZpAjXEFf!qgDfWhMG&(o2kVDY1hVW$RjR*EnqrAjlA^f`===9-~{ zg&EQvS%$P${=jO?Y`W0SB4QFB`S~rZs-f*=NF~i`$ZJZF5tJNSoOlD98H^CU9T#`3 z_qT7lz_NtP;dUyKa;y7d?wuw&ZJQi!ffh32d}Bo zkOovoMjOon+f7DtnaLkf3JsI!?zfCa1P^!M6iA`sX31mQY;%2DHPWpv@g3Jqcu{rk zeV$xx1~0?lVrbJUDq_IQCw_?f1t^*#4@^W<*ZQ%W>I3#g8q{J-BCfXX1rzgQP8WD> zEba3P*$}7s&1plzBZMF0vEB#E$6YCxK~q2rvbOlm_6qAmkZYFukIYQ8l2X2n&fw=} zfKUU%BT4&EXbBd{V`xKEf*06dNMlA~0dVJm33O&}r?^&|6qwQA)-Y%?%Mf`E0UwZ< zW#ac>DmPn?D31_zV@0FA)o!-rS+SzlV#=pVd+WKfhjyHqL%)gCC*w$BPL9Yva$f3@u% z2_r?#44U7R>$PN!EkU`y(G-T2^hvwm$=Sh;97$E5w!PAPGXM}0*uZVpE{cH6Gi|KK zZ2~M){!U5sUGSv;%qVO{!&mLRFPWPxR`nN)F&5%m*dsDdxt-a}&a-X`_?^X3P+F{Gi9b~v@ z^FlVEhZD$iL(OO1InT(_Psx@LUF`sY+A-vTfKh&_Br9svrvbSPIDzJhM+C31(2d-v z4YHjH;O>aL+68%YWYS#8h!0m}(x*<~hDqDVNbNCs>y$#`yg-XE#)%8i-Vzz=OwEF;S}9IQl}pxq@Cqh(waWGPSj;|7xc>5(tLP2)L=Hk$76Qxm@|*>~6yfQAwzjEQ@nrW%H=?(T>&2-&Iqeh1=L< zV0JvcuC!>U?RQT1#u)!$Hjw6|3q&Iw=8Fp}=q7&sj7T{cSx1z0GT$QjBK7D|^Gw^7q4#Rbc~l4@vJgr+b*Xn09fg@@50lShz1;eIeT7qpz49^K6) z`5l7X7Uanm(Zhe^_KCM&F(8U!j<&r5U0bYK2E1wNg7Y1)Nk~}Rv9NoG;bugX4P{rk zr$&#TIIzC|nO|<7fU}*Mt(qLcmqHeItd4{_#x!^C2HK$NVsNJ`o$h$vS7vej3!J2x z?d}CpRroDo(rd3a+Z(U91;v1Q?eCZW%>t8#Y(SU92F~Si8Fh*q&>F=`rrSkC-%VvywAkjIjb*F1mX zA`mLkmKyIH%*c1g8zBnL;)PENr(z=_99YbI%XRLM#Ym6c)R{8$ybnQ)oQ&OP zb81R?2q+R?YdoA#S?ds^RU0$eZO^vam}66x;wmT%u^JQrW~cPKgdi}0FiEcO!iHcx zZ&)aIBZ$2*T~dJ{SGdOFL_H@KgT0KAr{ZKgP0@n&@F1-j@x(KtxD2Ut^ z-i+)3);8v=t|ta;(X6}Sr*nj$MyT~s-oCT0=3)dv18EeeNMWE!U(B{>Gg8&QHo7Y1 zf)0pHh}2>3zovTgOk;1x-T;47{@Rhtl`zp2t<&sY#4IEK!l8A-!p*wc#)5wbe2_dJ z=$t(?o10n%ekk0|i4oTdW{2ly1Il_IhK(Zm^U@&{TAYUD6f!QGMyIMs`#>VEbO>e| z*2JB&&2gJ`-Dx*J+#~%JtlNRIbTrI!PQ*z9?N$$@)>M_lmwWE{ zmZd|JGvHw5A~TJ};_-Z_-XWSNCO&$r_UEqEPm3ei0btQaxyKVLe#c2Z(`z(LHb;vR zYRt|#A^%w_B0Mi&Q14ryE!p+uty+PQkTvjQyato$y^< z-ulBhp{#%Y0qre+CK?sfprn~4GMTc^5{#a}=8-CKr1P+=g-pljK7pV2MF_r)_QY3} z&Kf(UaAy{u0jNMV%Y?#HtM*1l|hG7e88a=*|Kj?p2?@058(aOII1U5-xZ~;Jb*Iz`)R!y1?X$8WzPds6xrVp-Zy`!EQbfTu z=}LC3vJujJBo4ZK7P%78SMnyV0@31HRSxY1eucs7c9$*4zM+Bp`|F`w4koHtxTaB} zg*$FcKNcs8d3WVxqkGD_HNb9s3Eb6KJ#1ESa9H6o_dOYn7l2^3Xd%Xs1~HU5X-< ztp*Lu$L)(AgNc(W`tt+DFU4+x!FI`@%WQz)-=VZey4arh&6Mic(i_I9lwx{amhZ>J zG^Zz7%!}+?v+eY)N|ljawPjfK-a)LLp1x~nDmdSuwl+wSmHoKz6cno1 zmJ&0r?qn&lwDvrn`_pg)d-6wCqI4RfKQF4kaOra8j6Hh!S(dE-2QnEiEb+QSbX(>cF77o`IV~?d){Ueo@LtALZ%|fT82zSrJt91Cbj!s6 zgKqib8i#CgfYBj9B_Ss!rW-GU@YlI3;s&lZ#^eaJm&RQP#1mw5XRiYiCoRM`>ilE_ zEr1`sZq5)o&6Tf~##Jg3ML;RT5PUtpXtZR`{_C>6znz6RJZex5qPbwoLGo*4t=n;oK&8%+xZgrXiaMoW ztl&291m4>#xZuhUzv=c2myY#Vm}C6=G(@px;C#gdRXFx2{Qi9Bt?^B!g(?qmPW!38 zcW769Hmgz(mSgTUI$#FxNE>WX90jwxcya$R{;ozVb;-(eHk zpw@T%;~q#$-hrbo=a<&xFbfd$NJi&KO2xi>hP~hMjZ0hNvc`Gv|KQIIy_Z)?4U_Ep zQD=y+&fn4ereh4Z(tMfWoo;zd@jgNGnd)@hbr{~*mTS2SWLX=t){1QV2kz`#jz?lc z$^IzSO$nHIvGN_MXY+6_*ME%I7uNBcl^G+p9qqK3PP&erF-1=5q*JmjAuUeH2%rN( zAF9w36%*8-2vdmVadLL>@mrh>JqT;n3U0Dda*LUT4jySFJ1}^=%;*x>Jrc)&SJ%l- z-d*{hRn5ik8}gJwRlbYDYpbpo|HUo5vX%nE?$7qTvly`4e<%5EHH-e)mpYG{PEstz zYfmx0$2@!cl}Gxj(QcbCuN8G_lCedVMSOwPT~pf9Vs~}u*RVAjfJHhiE z4funjCv`UL>ie)|rxUp7hALz93#@Em_4HF4@7 zt^p`G-*V|Yt7P(oVsO)-LCpFIZ`a_0?G?eN3$nZi{wmr*4ePlajC7;iwtFCE(W&!O z^4H&~U+=!NXU8Y@-r)yI20AHr397&_8KS*+u!A3QHTAQ!Oa5ov$sLOxOsN_ACB-Vr zx+i1A6?OJ%tV1QOIycMv_}-lb=52eNU4jd${#MAUX(vQ12KB0Ns6eTUvP?Y(fzL;9ww2PlnzGb1KpZ$V;LkZx!%Uj2T8nHv?u zk1Ddj43dU%EM_mknid~@d~^uN@WHIc46VjO0eJ&>{i*R;8EY1LnN+PpYQm*S)i zdCFMx>b!ek&^RxCRY5`A?99|-niz%T;$$v>MFU(y1;h`a@QLy>_6p@M-hUO8b*qL( z?e(0P@UbvPH?|IX+2i-4y>cY%C|{=7{-$cbRoz z1+>*$daWK$jXUzE5}@KlB%2|yVsD19e=*ti+E#O8R&yN9#cLqmwThQ{piD#afzYki zhUBU~>T}69y?q*5nQpTvHzKIs5tQGXXQ9cT7RWl(`k3-R7I(Rv6i%7;lv~xR&0cO@ zo*KBM=r$L$1!zUMQlVo45Ibk}MS14G03JO&T`ypT)}fh0f2zIJ4<-gy0A&*(H!W>N z0hNnDflQtW0yg!tzK7draWqf7je6)$^Tg1^~E!J>BKouiA_|2boZ^CUkYpQ;q{AvrWICg zGLUQ-dn{dtY5_pu%HK?8IP#*70^itgzE&>>@&*=fzfkfsOov)#h`2#U*p{hhyVmgh z*EYK)Fk(0wKNya0V@7Alp@H#J@7Q7g{y)As9;)NcOrov@c<7YOIMS8VnWu<0*r^Of zdctMQp!XY7BaOw}?SFJ{CC83kW~JQY3IZb1&aR11;Mo4jtILbWZ*b6loYG6LxQCN- zmcA&C@{{~`@8?w6%%M)7Hi4fGCJo*ed~GX#x4mpbSL-uda1&mAbNGFjuz7d+9K)Wl z^~)YL%fAjmDSoS6JJwO_2d!-sODIicW?PYpwRc;VEq{~S`&&1|_J!@U=G-)lIpL0H zzTEy2c>}BTQt8e;Nl*Xrr!XNak#Ms6lA@gay?0)o9_iR|l|xZp-!Ok;9wZEz7Q@Qs z>pJs)o^~JxGrnmxYjL}6TuE4J(k!}OU8RiScl>jetcM0`YcfjI{gs^M<_tM*+5Dx$j;*kMh?@W1X@#;N8iBvPjp?)4#Ny6e++T8(RIl{y<8t zc&hhLpND_?zDa4S*jX)LDH$N99KP3S+Zs^s#12nk{q63yB>iS9wN+bR`Wh-UdpluE zw{0@krmyB=!*jSUCtecD`blMM_GLMJUe;cIzVG5`j_v8V(>q;~M)zw^ivIcPo+W9r z&JrjGW1O>^>G_w34gN!REp86WS9jO$bOGR!XO|JDX%Q74-9zDzgQF-XT{|=n6)rGP@p$RAE2fc znt7`d-@6pcMP4hq40iYR@i5#Jcdf{KKh&;lPBL+5&(mijO~T0Iid8bBPS1jHE;X^8 z(BK9Fo+>C%dDo|4p(6U<*4*xi&#+eRj@@s*hoWsjDP(GPP{+nq8IP~QvW-Z69243&%(#%Em< zgO@7TXs8%I)u(yZ{Mh{C9_xe->hkWbPsO=??*xu;9rMQ>If^Al6=qLW$1ml_9RIKU zVaGelv)6jPOlQ3X(O(g0Q?kk1XLrVL_Nb)~c~0c@Dpf^$yydsaQYHE8Q`Ohba=};f z@1MiyX!Xk-2!%Y@Pyx->g9R3x30ni;s>361#{O3V5gAh%tT0&zi`McA?uqRv(3o#NM9G}(PVHCt*6M@QI; z3EpF8&ZZGH*SOIXd9I$b%J+;@;u~Lfs(d=RCbSU7(>f)Re{on8X zkUc;qAjr6pCr=L#tueN#bW&7*HV~_~L2)~zpdi=%d$E6Qx6)eQh!b;0qW|z64{d9# zQtQj>hDEQsj!ZCUyPauo!~cAsNwTEY_~I?YLlsv|c?uF- zMm~E7=2o8|G4?pBPrpt3V)ol>^i6zyo|3_|J37>#F?O!sEK|GD*zi8>$yp~apXm}i zJy(TDXWPc+r`~6G`BmM=2Yn|xQ>;Jg`z1|3?cQI6(8CD;qIRRTU7|8|xnPlHbiY`#+^`&D zq5FXKXf>{ye12YsO!<#fz5qb$MwIHeTCJM2v1w&k146PP4wu1n32tpw3W8fZSQXC? z=Bu9d+av?v`ZX0tB&Rh$?KvN!BlbKqb5lv}N!pi%t!LZSAt3WhgdNnz|3IKElzlf< z>e(c`us?6!Z`U=#5I*>5@{$cU8IwUbMo!E$xt9hH8V)}*_E0OBe8$C!o@tu9;PIyF zpj-9-=ncOV*m(vzfTwcP{;R!mKNi-^+*nL?3BeZjc{>Duezi?8!;883c|~BMc&fNs z;C%QATgAWec-!ewB%N2&Tz`k zgaPLb7lY4eZcwMLdt#Vfe7A9JpdUVKVTInU==puI)GttIVhUfXonYpbeJtPcLt-xf zRtQ!(v`9WOzmn++_mC_1Y!R@!W>znLmu^gUA|!M;?~hOnq@>|Q$(r9tSDv`?@FzBm zkI#=B|D-H%h%%$LOw)z2FhOkMe@9`z`*LTx)$QyI$Rf{MbxW6a>oM3-YO-N|l^Uqe zh20D^FYyP@5}(o^;28F8#wqhr2UHHOS#vF3quKNJGxw6D!|3@+Qp1PTkh&^qPl@^l z7fs=_^xE;B;NMZzwx1Ny(G94mGe_W=FmDy8;iv64rL1#@U_M^ODPA*Q+RM%EpO?Og z_+$)JpH4O^F!mkXW%vC|ZSx;p6)gy@>*vQFr?n1iKkI%3$nM!Gr|}#;Kp_18=hFe# zim6OnP@V_s9R9(^1!$$7Z*8mR96}#AF(me-d7d^fAEWcjb^HExYN7*yF;HoA&8%|1K*}@!UypasqT+T2KV(zkR-6Tv>i?P;7w8p>|o^ zCKx2Xz9x>+g#BHj6=(RA$0oTvaXmin1qKi7?S;tOwAF!AU#lp0ZiuU-W69>rrRohm z_g}gs*@aj-R%j}o#kZhpTPQzsQ@@)m=xQr@H)-I$YgpdKf2V9W+*f0Ez3r(+jogc2 zi$6^Zc7wEl`H!5QqOErMQQUTX^&+qR$Es&q8ST#}Ve7I7IEzlq>gs zHFys$oYH|@S-&MF-Q7Qs$HL#lTck(t$Z;Ik)Y-Tz{HRMkzqgRxd8OoEtpTzsm4La3WEg&>KLMv{bLL)3&AV4=`mq6(0FLTM zIVqyw^K(o!uW7D?U+^FpAKx>^FoXnt89DkwUTIINHVPb`3c5X_J+(WM@ZrNkzwi3b zih+m0ezRq9f;*xjO|WqUnY=OMKLebvQB+459kzxkr1%ymn)!DjA=R zMl8Mg+o({$aq4{o^$x4gU42OBfxOG3T@Uw?zUSy`f4;2J?8RNPs_~mDG10}(74zIR ziy#0PECyKivo1Y;bFW7z0-)-L8DqRhW}ev@E)>%ol+B*&KIUyUox?1kIsMVcuw=Hs z3Y=})9ghO$vJjzohDz#|j6u*3U2uHLhLe@0`;H$eO&J!2zka<|x})G4KB-F%T}ki0 z7DESyoRBMOh35%VCT^_XMqncVN1^Y)GEB@vV_!K@wv~IH4lM3Giqg4aC!&Y8eOf&w z4(SuddLd&~7(j1GvgM^Co+pym9w?p%CPmrMn4IqYeLi~j$<7{KE|B~_XWtoXIKf^3 zRNycGfe$2k>(c)FDmy6HeH|$T%NS(!Pr1}i)~)iAE6R4=d2-9Q3TPGMha(Va1O-di&jn)n`uqvwxpU>6_Sje15xafNS*QiJN-mVix&4 z_@!S9S5fXa>iY|oCtT?SMHtu=MA^J5G>n-0rySnMm@h%G>s?FJHCOL$N;JpLps*ua>MOEe2Mmk?lkwc^3A{T9ui~he{ z9YLxJs0`W8N3`FFW!0_@mdk`FPx>0#9x^{re&4>``*7xY+$aTVmRr;~t^tv5S0Oh4M~AKn8o*P^$nj-zl?)DU7S zJ$i})k^!uO=dZdTND$jc#zNzSU%}$9zo@0wMDMg$p5DhG_mT)Zhi(to1s%g=+UJIs z=F-*|%mszpxVn8TlJItCH?9@}sgvBl4=SYJbAj)vb3ErU*6U!Fa2@kWOt!{US#CFi zJ&JUW&$Ao}ZOQyL$d-t0h1&Vgf}Vg;1|hqx{gOl8E4H z?tZ9Y%TeD@g_6;Kz_BzxBU#@A)954E##J@)f^kvYxPdwX597fv%ocPdoSg_oa4&$CF8o3i zUn!YiY~5(g^`f=2_UhFGsv#Rt&f-&3}gKEXYL(7!wP+WKIYO4}clx_F? z#c2o0o^c<5)xNFfg_gb@UJ(p09`$?Uef%lQ^Pna$Jq0h;GR5IErQyA3Z6^c9y-1#T zYGCs_{_T8I$VEOkZC>0qUn`_SL_Sc@Utw_9W*}#t7!ch)FSIn7Ur?F9Lp4zXn!%Wx zpIDCzkF+Y8*_~VOR$waic4oU6R+;&^n~ujUeeXU>i@aa&#Cnff|G-B1@3y>L|Jtl5 zwM}srk!ROexoS=RrJ1cIAa=pKw{X%o=Rwcjq*;qzRp&}~f;c-(R;13mq*Ct_X@ zzpiKq&}jcrIja#lea!H$^WC+AvWf4b092VzsJ#a7GKuPfbKjeF6dM6K!djv4<9?eM zsVOGM0>OUjP$;Hk-%bQNqDf{GRmxesr*6sej6CQ&$f4bVF=|FS)D33EJ5*V`@v?14 z+dR9c?tRnC9!^x;-_DtyFfAa@-s6poX14d6V#a@nGXM+*1r_Qc8N|?EYjIW3gvuGEOXQS z6_t$rs*}S}d>;WTYOQBo>DWtkOjOlQI|Ho{+_UBBiJe40+L8`)rTPmtECiS~PHHW< z9oGxT^)kZLf9+XS5aP+449wSSmfr09X}x7bqp(uA3e0X%9Dq*RBIo@z&|7n&zHM75 zwqElZAandXe8CapUaqii$+O*_79rnDuWwyczd-p$2ZO3Eu0QrT95wfjRX3N@@@Ib> zhtYFVv63y;VqHkzZHHA3ABho=H@YAlQgS5*$O{;|=he5b#@5!)4RG3QN{SJ82$G%} zn$)^-Bw7T}O27vXepq1r9O$9aT!nocFqW)Hwld;?tW>)4N^1s9bnVunnIzLQ>|MBZD{`aTAhft)tI_rN=j}Eq+UoQDbJGaaONc%*}25!c_`e3WZ z5(_}5j*>b&nMm@CagOfGsPX=mN_Yh@y$4>pS-BWc6Rwd}5G>*8a_?!JEbB&^^$9uG zoV1xeJoyLy&+xkCyNrBN@|-sHa;|CUg}oPeCSbH1!wSIJtrdZ{*)Xt{i3^)YoxU;C z_`)4VtZ&j5?KE%Spl)&c@Z&^DwA61#*9lpGzb1LDq4d&=i*7dilltR<*j9zJ(*Fz` zI4{hekNC-om;k)55*WW3!WoGcWVH*k$h2I+OPKH_x#l=uY06z`$VN2?QY4G!j~Sp{ zCDylB*O1y`dLnnFV3Sg5!QYn~B)8>7HR8p(@^+9dTD(-UcWIlf$(a?N2u#h1!I@gL zEDg-N7Wd^?K3AvpeA^l)U2HEGGOR=UvYV%8W`@l}AT~ZvKJGa?YWc^8-PAi!xO{(? zT3=IR7$^S)cz8HI%s_B_BUv?&Yb~bchU24lUQl%a@q#L}tt8FyiO40!fq;mf3TN$) z?#DIZ4;cMd7g?{BDxuKDJuW30F^SQKk??iGGaD{~$?}NaN?<%S*ojRh_DZIzTaYW% zdx>_LT9)z{96*&85x{Yds{QOBGgEo zd^x(9)Me>q&uk#tgL^;!?fNb|(|$?qn{s}7o?@B)71H0wkNlgnHuUkmlUX<>_U_km zQ@EJZLLXbY-s%`2^dzdGG4AF%Ryr<$FPvrzuTgEvn!`=dS}b$B$jQM~(Aq!%StnMQ z<=DA8x)NU~1+vqYD$4@jt1DhNJ}TXS_qr4!pev;(SS&AvcJyQ6)eKcb{= zSn}y*2H?>l7a8k@)Nc;2h@l+hPIP$Wm?4B~2#fBw7u_;}E}nvh2t}KtAGOqlVnUG5 zM%<^m8p`e>wdQGMg%|-3(G#?n+vT*?{(-4dkFGmdn5c^(`anS{j}r&2a>Ap=9#keg z3c8k(`Xx>rM5pH~JZ_-{pGuhs&&_bpSxpUmd^o!x`zo>SpY|mkZ$NwsWOnr~m#c(C zd2p|ZwsaXZouy@*()1K8y0rqi%YPdGwEfEm`_$GUSgkBi(SR4IHb2hKxrR}R-qe|v zIixtid>Az2WA-C?BGW#3^J9|^et06KNZpKhRqL%F8K#p;4WYZhamB1uO0ixKIGlZrBbxgAR~1#bUbwWkl54_A74<*y5TFDJ`JS?W?3BZah{m-pm0oc5f5sI#{Gu*YaKC~vRH zr5M)us+OLX2IcFz%QwM_$h`&)wPt%f}4x{5BBcSEk$h=HKK&}9&Rwj@IImjKM+6P=*qzblYSmLdg zHjG2DvgI^~pj#g&g5gyjV1i(FqPZ+kM?w9Hs?cPj`9`;DW9s3e;_}L7hwQt(T-%() zxa>rp@NKxJknTF?)z-5SOZwLj-n(((uwZMl;qXh%O$&~SKi*@z<;F?fS(B#pFYOA9 zI=Av^xnv%@v&9Me$+qrJ?Y&LDZ7V;ce4O41S{s@6*(4{2utl{z0u+$so|N2EKN>5C=WKC@Rp+?ic(-{iLxE zWe*#4Ba0w=;4B-y1OGY0eVVN>H3#CKEsmSp4KE2{2ilZ$e;n?i?&z(gJ1@iom&w-) zO*f!W;hG}tyk5`ArM>0RnAId;w3q1`sB4Y-?ux6_w0mUFfhe)CUp)?(R>i3JR`%n% z950ewZZ?}c*yc-0(;7cyf{T8((?#NJ3jcoPP((x=URjI(IHwWkQ5UKl=9XdFtDtpK z@9`(aPHWQ()G$I7<2tPQrO$WurAtM%$@jC-`rawJ+hGwM!i0v-#BIjS>DsC;7R6|l z#%`q@9+05D=}H8A^9CnUJvu5CSyHQ`8;!fG!dm+4+m-CaF#|tVNpjRgU`okVVxNo4 z0H>sYFgxGt1yqKN@}Ei6Tcl1zZII&~oEv}6w%Qj|ZC`ETSTyu3J!P&cPq!y&=qhPY zTdBkH)~rXFs_*V}xrya_g%jv(jceSO`4cJhmx=E4e&p^zb(kAmzu$6X0ihZ8@q;#X z?EGJ$njySdm%~xt3p?2H(CE$tIe<^CVw|ut?ayxrZQYA4`#IDH3B6 zM0juecf9|=^W`~?=Ub0d?)%Dht>IkjTI*aG{Cp`YPln4lffe?W#%b<3ed_$se!?Gy z_mj+x8UM&Hlqgito~WR$B8^Uiit9tM-u<3vY@d4Fmm+Z*tbya$^hf}3 zF`eHx|Hd>-SC!bM0;JkwqZ0PlI08ecCSTO%epu z(-bvz>f{*mBxA`fE>6v$(a}25u&i<;m^-ueA;;envizWVw{j7wM-; zzUCYi?SICI%^@~8t>(@A@i_s=8_*@-I*a{tAzBh;iF#e}Ow$VzT439)^6BhM1;v=YHbyX9-^YO8!iE&WF2RREHB7@gLoar55Q=P}cKbsE*dwPBwcphf| zZLEk&PbJTfJv`Dh^Ul%NIk#4)=*`Ojbp#n>zLPFRY-MZIHfjaP_ zc9392nuu>94&H@E*Lzw^(&;x}aY`iMpB5p)|8elH?zSAQc$z6^%dUgA<96sPQxXp2 zjZp+qUbz*Nr*7*=^n^Yf_dkg9(s}4lCtHR#Sn=t7?I$D>phKw?JDeZWziyUf!%t6C zfuU?1&*QQgL2C2bO*5%w@+VSRgJg#gI@E%X9Q;Pz=x|dSF9)Hv8%-_+B4+buOzPtN zo(5mFX7xBW3f}x-kGR%Y(oGEO|6N&=NYA-o^!xA|+{*)xB@bTv@;7)P&t|-v87WV&FLDi{(R1W41@m0 zo?9Zd95d%~szvoF)#`IYly+*i5)K}7Q+Z=9&IP&!FR8i)&B8K-_zPw7Bv+L->7xJ2 zB|hc^*DeF3d-2-;RpW)69B_FwL3r-Tg#SX|7m0b5k!+D!;{5h^s*6g1lI{GGF{0>w zaj9Qx8OV2e;(H29mO8IciTI&6T)e_8fDV3GDU$SJ^}Sn}k1ARHroAgn?rBz17= zR96W;sm}-}ktw{e{YDfXvCg_+3!?hPMtS8CNuuh0^h>Wg)x8C*CY{R(6y0EMWscy& z;i(fPrz@n69RKu=<+L&TEX;b6V(HjP7Gq5g$%L3eYu2qmg_~aRs~?=X|0aoYGeHA4 zl~0DfRSJi*CI&5#h`2W`_`xw_P8uFcJZ%kUr7g;cY&KhDou)2_HnSNo&<)2R>_|a5 zrjtPf&#(TBi|mA%f!#Nl&RrWoExw@u0MHYyCSg-1j<`y)TtHU65&`#c9Q}rvh;<%f z81w#+3+N6AuXs0a{qBw^(C=e6w<67WwvGI7R4JiRp)R(2#|HXz>J&+_Zsn?a=mLbt z0Hqy-d;9o3Q_dVqBnvTOS%lVCa3gZ?P9KSa)+s(p?Y6(d7cu%Ol5s}6vCo^Z~A%0Bs>NPRyBarG@|S4nrsxT49rc{1$q5x{bA+^W{YmC~RowdOr1PXbG4v}aoX4<*lgt;N{pq}F3%3ne z-ndjg`nx3Iq>q-)^eaYTkk?AA3nuQcm_QzzzLGc+{Ity)LfH8}+1e6gxE8hD!w822 zWL=_4L-i8ncviflz(lN5C|*M2x)nEHf^n|iuwI(PAvLUwdIr=u56&_sTwbs{H)CDy zIfRj6uRp}k;OX;Yn3-{MLm2(=?S17QB#~vim7=V!F?F>Sj0moTx_Vh6T94LttsAs3 zDn_#Hf7WFyPuJ7t+%~Ux);Q8-%JukGJvOC)vYUb_Ie1qW4XH1s`0K{KJwR`;`scF! z8PciP5yhIJ+P9i}*MigJQbDaJcY>b*C>@92s#h*qxT}<;s^}+Z*U7DQ7D0vHJ^0If zJB9RzRzsF&E%dAIx#-hVrKO$?zhf|>{t&t(o!j8{Vza{9+!2@SUtIgERaN^&9N!uGtq z@{2ia1+B7f^1p0iu$Iaa**aUQCI@*=`?1n0Ee%~=(Nax5)x*;rCxV@YVO+FR^SQRw z$VOQcpE{0YKn?4x8aF{D8)7y%oTO+DzZ#|deQG})vr?YvwgtWuk0hakQ^EQ=EK{-h|+Rc<6im+N3LRGYD1_I zINP~0D&L#OSd;N;+7e$}>E$@VyueeQHl+O!t4f7#W0X#tkeru;=Sd=x^u|haZF=v? zA-RmuPyOgU%SKxyVHkf?s&6%RX0dkF7>bg2CY&n^10Pk%2?M$V_to6kTXFpwC{?w& zKXF^1T_x+4pC!L5brGYTPKJdtqmk0Ii&kt!Ii;hL&R^j| zdYO&2*^h?s#~QPFACC6LCOV9aZ_~p0%|Y~uZ@`sAn&b9N1FpbxFIQY)R=p?tQtAR& zd6lB)ym)$EgkyNrpH-<>PXqq9+zg;Ii#)4E@E2*+tif`#y7lfA=O%Fy6S`Z^x0e!TOrG3qn zjluPvZlGiIZ}$>YWm#7fYPnw@Pe|3*pu#WUKArWHJW!n`Q(^LCVps-c0}JV|rk*(J zvaEX?ec^fHh?uZ~iRcAM`US}%TKQho#;6w~wJETdhP2%26*=0L9>v4GVo2W7MYheD0PD1d4HqM~+=pr~7 z#zN{gBm9Y>pZJylV>oc#E-ALn9v9T7&FrE6L@@m2c#JBh;gE>|xq?G4pWPBX$*W!OSWGWoTkma0N}cXj2P$U921(CrUG5}m z`teIrQ%n!mL#7sOm{`L6w}IFEVDDVTB6m)3Dph@&xz0R^TS-$gnaNQql3sihCka0G zwsez4vO0pXjGvDa_adizK$>~;a(D0lal(m5dV8{B0e%(GmBw;FVxSdqQbpxj{7?On z_I!0m)0zaz{3o9V#Rb)dGXW!{A>T!~G5Nk)4_5q){l-Hj&@X#9gJul3mz(9tnMfw- z%;yj4^tW5H=!uXs#$*RpFu1#{j&eOsZgwSh?`C;ZN}0M9*(%Em)pa#NS$QUAT>Fc; zK6L%R{&PQXLd{d4xgn#lzGigB0reJy+H9j!jum|yfSh?{+Vy<2h{`zP>)Wo9rZm65 z^#N8#K6~{<*fM)uM=MIT3+?#AMZ#cIN9OYw?I$G4%(y?^S08&J$`txA5hHud!BN+Xm6zSZMGk?E|I?$m zqVoIeDOZ)I=j|(0>ud-r-Y)pNOT06wGocyM=mK#hz-Nz_a8j!qM1&FFvpGm(u^JjYXg_0rUT_b8;v zdQr&MizUFow-2f0TRgoahEReZ);7LNG#ety!on9KRUcsd%ai=4N~4fP5@$#c(ne&r ziT}(vhikQj>SOQud0QI2K)l*uIf!nN-2It%l z6uq_YABts9oxFS^QM5ANDwl4g$okYT{P|5*iFMXcKQOXOe@L-uv&$)*R-r!U87;z3yOoBI^zTWOuA|Qmk=D|0R4}k+JSTy0;~=N<%hhhj#zWe5 z^7Nvs_`ykVo8Gze=A|BS?f)J-VlpV$tUtHr(opMlliLLotd`p=&gg8AmJtfG4y7x@ z)TaJw-(_BdMN_Wh%-JcIag;E*jp`EE`1oDWf`c~-Y@o}qzgrS|Xy+<%-eu2Tz1$CP zJR&-?$4E6DU;g}5X;1$xNdBt$OROe9y-B1;CF={2uta^v9$2|k$alsayQ||#hPY1A`(FIoHJ5X1wHMv^hm5andGY1T zbVJv=-0MQQq05g!((6ceoVg-VN811v8v51OQmK`5K@#?tC@_C8@UI{^%gK`^wimm4 zz^|A~#~${5x{#DOwU~aBfBWCPVyB*|o;t64KUXR04p-u?mlz#R-(!zHiibV~xtSlb zmp(eUR;heHtnQ55XX8>w$Qyb^8dd$VWGM2X;ijHs|7&Bj^S<8(Hkj)yW_QtJP970^ z1T=)GpbzS(=pB`IgL3B>3z~zgUWhF?KGj%XqP#y6Qfi-8QtRPnFQ}FDx(n+sk0>69 zi4sML*R#DG>TxcCH^!On_(LB51*hqnLA7t@U;VVn-wdnt6MCY(NRN?f2FQ zz_j|Pu)&o|;Y@w`tnr1l6<`wF5nkJxei^*1;gC{htyf9O*+NBCbnJ!FLDm&HTQ7ai zNduf!2&X?J(rY=*LTQ;01$KmY7IGQMk_^SfymVyyzM-58`*reu?~$UmN>ob}?}7I$ zN6#5OA9;27l4LRD*VD^mW;fKYLO#&7Bop|IgwtVuE|X^#={B&L{R>R7*5KVn^J3*IG3;ST>(8NFuvpDX`=*5$7h&aroBwX}-P{b$il z`_qvkW%-DxuE#QKZwvw&ICeM)YDToC&?YRdt4%ugT@(4OPXSqEh;fzc(FP0{96C{N z6O9Uqi>#j~Ik4509cN`o#({9JE_Vb=b(!q>2Stv)m#EYGALZGFRjjAs;*6jnQ;F{q zzR>B09&j7gdt4SiL~#sU1j|p@jMADmt#{@r;Y$3n$zdO_}}MH7wZ2YTu|;+l&WzJJJGv`Dx{HZB)36qCDZtjnbn zYBy@$NL*GVc7kwdZ6D?L;#xkFmG*NBnINrX3M`9JxoERShmyqLL-wIwnUk+(gXA`N zgKj^5VWED)JQL_hco^P+@^eEf0t%93HASdCQ@e+ssjD8*14L`h%i`u#FW^ z2dq`f@Ik&KyJON?lDNysIt@@Y$*FC&yauW^_WtjRt{!Mz-wg-H{%g`?>;&#)0YnI6 zc1m(t*r?-%%RrZFCLT@0HzE@k8iO0!OhyGK$(*i0)k3~HiU%V$cF*3GhzYO5)`u)G z-T{@kWZUA#$yU-nMu4QXCu)$d=U5w;FaLRQRWo%w;yC+lbColK~r1RpEjaeJx^p$nx-r9z~%)XHK+D)^tbBFR@1 zfm~WV(;X$&6l-EFY;bG%PA!G+cT`cpQ*Hp zT6PkIn4p>L%Q)iJExM7beF}btr*oA(&^~@5!PlkY+4|;}#3V?JWDSU@Q`G5O-A%6H z#|34cQF(H6>E(eTCkA{e;A32OcJa_!#&E6ILy&~;vKNU)wr%3v^?IwCz2amZ3Q_Vb zQ5ZOuz`tvFH)lBlEm$#z_SePf(Kh4EMMtQ2B>;ZW?y2QqYr|}EwYL`;e@OY&_^{{0 zMKGcC7)rw0Dqk>w?Rvg5IMY^cXt_&11FP+KjNj!#)oz;5-g*_cz00tX7PMeEr!4qH zA+Kx~m+XWEECV$x=0x7kcSFmb>S0e*!vJKUK24R74!9w^0N(daR@Q00B6edG%gq+U zrwVuP8}+9fH$?cO^Bj8_Mxm^{$r88R%B#Q+I>We@D5r%TekMM370$|&`H-cu?c%G@ zvTOsEUiQg8OPmQo!WnThE+Q4RNuVX;y8eUQ#)tMqkJT3fKVjgGm8P56QbZ0OPNNz? zLSoQUDg69|TfJ|OLY-|YGR9s+=gVBS*X2^3sZGm48*hIegtDeDUy8S{|Q5n5bz9jj!EVf2BqMBckrG6c6e zvaY4H4o>iRr_dBiexyY8yQJKDk9D2v*U=qVk_ODd{*r^~Fkw&pN4i4%kL3rYyxj$q zi%BaO0N18VT{~X?n^Q+z1*jGSc~2)HW0MdcfL~#ZJqB_4y7}mv=8H*0ficgH_oAB5 zj$@njhAoz}Pk!EK3(~6{niNrAnA?IIL8_!DHz@EJkubiCQvWS{5uV&>W~ZPuEIGtf)obmZF-d(?2OluppOx58B3%cSqI0P&*{88H zsstv3oI?(q<5U_{bDqBl*!eAlN>av+EWqC;uJ|7SA3G^_=Cs7nv+g39R?*Bf^rYv- zo|0-eEFMTQWVbF}vrb%iq_{dX)NztRvxzq3-@`*sd~Jk2s+oW6#{4k6I#lT^S-Ua$ zlCEvu#oY_{4%-}_bIwy7!1!f0MDIPgmKO@#_KV+1!si@&K zL_!pE`Zz5+556u6fMTENU8=iI5Z|?FVI?PRy%g58%il*w?UFlE)W;UX6PUOCch-UG zOnXX8T;t5z!v0ZBSN!0Hf+)8?Oc~7IK zB)r_10}ezYW=XDhMWHxs^Cus zJ$4mbg2@lXl??a|T4T`aE0cx;X(Ej0p5>)?o$BJSi8{U7mwSc;-nUSsCvO3=&N##(=sInwZGC=YTjz+|?z{b?%mz*HU zuZ}zH7{8IY+LPfNr^U)OofD3HHmk8zRkz>X=OGLL+shI81|<7Xzj^m>O(SCE3m%^> z4-TmtH3qW#FN`&J=e$ma+^!P{ zbQM3uD`&N=Ib-*SI%lX;gQwmABT=Nj+P+(u8X~FTu3bM zKT}D)BF|yDpgDUe{UOg;@d1jhKk=Ym#aKuOV=upiVj`^s(;R*`n)7fRBIYMD?#z!_ zbcza^q%K(jgq_;EgPwxxQT->ju7po_kLdGhCJm)YE6Jl&(@Gk+V$g+oJZ!bJa)#op|ssw%R-{+k-uW>FRp)5T6mU@Y^Rfo7e zidjCy>|6F7>7d`30Vu?`=1h{sXx%NblIszYfRj1`5ssKd@0PkQhQ2TlD#pkXa1J>M z?dpM9^(IIt^4dbiJHPK<@CgSP7?l_wU!XJ@^mn;}3ny7&kXI);LL;MU2*PPPbz3LG z_#>-_rQb_%a$c@?c#`nto=h90k8`3*wDe4{9?p+4dz1wcL+lhbvWZv4{#!+uEP_pj21OQa!#)*KLS^NtmMQZ>;O@-{x_bre*BVA<%b z~WItcZZN=*0sDnh3xH9@{|je-;_+UFXK>M~)9m#A%J_8i65S^q@^ zxU<$y@DKY^zB)|a$Zka`P)BmCUN!cFB|@$gfnpk=(Y)K%j>R;c+uG9Rsd&$KD#qxZau86=O{<(bOtx>@SMd zj;AD-vzH_SLn96{sKzxq2{TT-h(wHZPJ!a+F--RPxkPvr8R2q8{I? z*~-PNA&**g2X~62K1=@P8_Ib)HMz4xt3Pt$LQzZNq~yJ%>k^Vi8nI;_e;<~Lmdy_~ z&U^DD=IU!%)uTt2Z{)?e{w2KQalia8(_i;v+*)Ps<=c#j%D(s&tgX5~T3- zlON zzP_~rTIN9)%cIW2xYyvx{ShbXg&#cbz42c}x({9kKZ$stCxHraw_%?0F?Y|#Dqv8k z+J@mJ(Q)+c7RMUeZ9cvBTofcox5RDp85N4?+m5DR#Da09VP8te0@+ckqh?@hLPzvu z#t%+oDA~cpiQ4C<`9-pW=XXc28rYl62H`C()Qkqwo(|Z$k!-|E%;&#~Nd6e`#E%BX zT{tN>!mPK!$rtdy^BtH)wZTogY`^W-x|bxduMJc(u&?o1&A_8tbCgZlam{nQR3?T4 zHQCB~e_vro>Au`FcMFZF<4H#T?6oRlsmA|bq*>13h3=$%By}*Gk6K6iP<`bW7Z^3Z|f{@p>61%Gab@1 z*iPr2@ZPc+x*u**gL7LjFe0$3Eo%(eIG|$sjv1`*&#DFvoZvZLBpHi(AFDTtJ~D&N z?uF>G>-c$PGz*&#i>`$6u2rbFduHrPpQ~PJnPFksIfgF#shz>??)z!}AEz&ViX2jo5O4p*2e?or-dc$US zYSSEQy`f#4k%Tlx*%Ms5_ffx0wMoBi2@BNQFvD6eLUENsl!JBhbnSd0gG50<Ul@ z0+lp_h^tWRIW=YXZ00Syu?F8>4ZO0WkK+?GB8MQZsIOD+<{+8!MPsac*l;+?4j<4I zx0;d`b{MII;Sa!5mffzo`&cC7ZklAOF+&S5PW}DRUcXz~QCVr*h88(}2A`Z5nDH38 z78GZ5JhIR4V+BH$I`>h(AZA63c5`m0HdXcFOwDyxy(-9LwaS!dK?x!Z2N3$82JtGz zuF>=LXgC_K02s)@5T8D{zrObs)pv!c`T|D7hh*jxH3=+{wgepkrTN#Z+&WL+>z-d5 z5%hdDnJr#W);IEb@}fohbfCOhpFsG|>gX%$R=sxbjLAsBO4NOKJ4wOM+}hQYLVZ)m zRQ}sS=`fu;taXv%%VtLkunqjufX?eq1sIk}C*{H5yV?))pSvAXKT!@vLaejTcYDW8 zJ7(8q>vS_&b)P@12yzDC`1h%+HE`P8%&m9S>;5xwD-TS(zcd8vMp9NsoPLflKXAu4 ze8^5z#BtM(pzw=VEiEk%_V<7ba^9)pM=p;bnI-DSDx~qVUpx_`Tq|6aZ^KSiQ&r-c z?vX`GGrCm6-=T9fwd4GyqSGE0-g=`fWM(1FDkS66(v|$pjH)~RaK4#8zi6iWL*J-5 z+<`}BMR!~T3{)HXCT)3TghZT@>47H>k3pDFOSAogl#lQct6 zb)kVN^>tV5bfpKN53XkUDQgB{EYGnbn1uW<&3^QXSd5<0+}l+DLDyf>zDA38A}t3f z_iLXxQ&au+KPpK;NOT1I*TdXU~}VAMZ?I zZ+?85fxGFd`6V$!I^Qp~hJ`%f3F)MLr;YW7tyiA$FbT;}abWS^WO-0oo;dw`gZu{? zLC34}Z%zcf&$#21Vhu^*K&zlpmgfM0Uah!DLQox~jWyTKQ`fTnn}Q1*>H~)wmb8O@ zk`N3t{`H2%z8&hc{vs+eWm-@gviV+WXZ$HYmS1_U!@oX5Z=CaCru#KTx44}`tr!J? z%#TtxKbw;K>eAkEE&XTsJF$IL@|{^A97s*Y{rEk7ZsUC7vwc8+BvEXhmC^nLFbBQLi;UH9#l@FR|>q|J^MTkJf`(=C8O8- zcOAcEtBO?x!r9mA<0t%LrPXM(j{`4UHG*Er!HL=*4K=!?H8TeML-3f;Aft_J48Z6# zRr3!#vj0`zCGT#`*H+4H=BJPYC|kA6SN)H{CoqhD^YDk3W+6B?pWCo(cFy!$?rncl z$5(x=v=)ll41LASqCO#$`-a|eyRXr_mGz$9;jchyl=xUj@ABWNzO=0M^Aqb6usBAD z{bwaQc)G@6VCQ(>WglZ#&5+}2`E|V{CKK`WIMshNpiV8P#)6IH%%_aLP3@)FCg@J5 zKVAE20t{d>T>t%Ih+!HzVcuwrByse#giRB z3-X+ap`>fyAQ-8(Q2YqP7TMMs4u}1ruD@`U*x{c-#R23#;&})7Ok6WCkn`y%fKK{c z3tjs-hj=&Ntu)-}1ds(IlTzET(kFap?f@2TA!zo_)xa0j@!ou5G#Id7(B17n`T?NS zw0l&Vcl+H+ueZf|8e-z|8PnUeHNc_YvLlwMrYr~gL+&Zyy}K-I`{7M}40lJHQYeHw z=;Aa`Y^!dSnb1>hK*$h$&QCZnNluicdE*^q3_^oz!tmo^C>`MMTFRVSn$_wT-g+mh z;A&3|sKGY9Hm>BN8Hwn66vuRmv1c@5(>G2JPB8q}$sok)Qn z>TS(w$LuFNPaOJT__Tq(DFpoZJ@ig^Ku$_&$nYyB0r*PM-|S7gKF_vyMzs%wq#KO=)o`IP>e zo*E12ug4Px_vPj)`m~tLsB;%mt!092ypfKzTU8CyKBu!#wy8Bt#DkmX&J~`rIGe5? zvQkl0ROFg(l9+ITk~$hRj;2WH{El#y^KDqJV89+yQo|1yUsl~Y_5B1RVS44MdQfWs=pEu&KUeo(Q z*Z&#Y%vW{9IS4&CXI3Y|`TWv?O~6AF1Em^68DB8j zcJO+~QV&83#bb}UTzcs@bx28u>OUAx*tGkjbf|yL!yZh~&X~t1wO^x*1Q4$Kx(T7iah);v>TN4x|v9$jC z;Ed8L>o0pg#|tZbeu!vXdC-j=NMV1`9t%GGC;P=_S2gl}uI zzoHb8=TT*MZW&6gI|lUKHeJ)JH={70G%hL3f9TqGQLZU(_k`l;mlTqe@1!3(XZAsc z4gBu(znW959p9(?%zVFg=sh4e>iV0r`Rdw_By;D+o5F#O-<}nkZ#xW55?P(;PRx^) ztrCN5uTM_IC})3H3(Eu}JOVcrjKZG2l=zi@KcPo67b8s=ecB>Ek!tVr1@V#ymd_JO zi9a21B5Ez_#>BnNG+5aC(Mn?QGX2s{vV~cUu4b%4zsP5K;A|xxSXz?qiaRjIYP@MO z*u1D8RX(F&)Ifbg0H|qeCEX(5f2K?QkKDg@?Eb=uo4n&l3QJG_0YKfARtbE+r!M+Jzs^#^7EsN*aXH<;GUnbkhy#6+JMAM zEiAQ20$bAG-#>)4u-MH1N4ixiv-cZxS2v7>E{gwmG&Ein0 zXe26hLR9Ae>c_eL6%1z?A)%wo{noHZL&2Q+#5Yl;gWdY8chR8JBBA%v+A-v8TSrR`%_o!d1;HFpcRjniBY#!8bxY8^DotOfU;v5I z<-a&hx(*i-`p1{J@pwpbdL`aB^t9YB3qbgU4pn>fUwn13r8!c}8u~Z+Fjz>4;tgbT z)F0^f8UPw8WY>UV*C{t_Z!*=bbjQN@tfzo6cXD?3J(hH?W;Wl0s?dNP8Z>!avnf_(U`yvL2mXKZ5 z@hgvCjy0mlHlrF)HQS0a9|DKnCw{+>l-QLXlDn4w^p8uXgrG&mF(7g&1J>fUTyQ(> z)%Cbzk%6q?NYBzntq;NU7+b;!pj<<_#p8?Pz#bN}dB6_-9k#VzHdmPWN?2syOCVp; z)=Qe(qq{3Vsu}wR`-fc994afrJFo`0vAD4{hnndj|ADnNVCjd`@*#LFYin4GW-!5g4&6|f!OfnCgXWzz7z z!4r|v&c(IBe}d(Lo7U-ZPuq^Wy)>54J)`Hx{`JKoUBmsp&el?kOZ%ZN-I)(hw>M_o zUMwt4v{Y8{#={NuRt7BRz_-ljuBu1AXc$HFMu(In?~hBZ^A{>5O=g86QoRu`Hg=Vp zetwezYB~sfDO^U|2bzAx%`A1G+LQXU=c;IABX3MEj%m9HeoMXAxX%+J;)v?KS?0ONhBFgJOG*GK}W!GIq(R`aA#rQGg1c8YY1J2(q)jDmi599w*Lg>kmRnTHx z_++YoH$*#g{^K*4$fn+6!x6>c-_MqQnkYs7%r}WTov44A&vF1EHm0?kjw87X$9Fxi z6lM;A3|pp?hOP(o6`E2KmMk`37jPR%y6^xu6{^5Y8QE>IKXm+I5sq0TChfA||x~)v|KQkJst|$ej^W0xV46L9%4a zE`Vx(KmRYPfDZvjRB18$alDOVU%PMGStC7e`%jrGkMC%uGrc`ze_S_N6aTg=9oId- zA10VRW1gyZ%Ry@aroF_ElJTYFO1J)%Ep|{-+yza1xqtx*k10^{7Gy=tzoIT_)eylcg4nS zP~%Yof_coRj*+@>s;!2F z$xbl_?0q|@;|qd5|K}{K04rV^Z0YVYN4kVG-!#=8O?Fr~*NEhFgh^8){g8bbv)g_^ z!Jf5s0}c9Oi6jmC5D5!iCVAoP$22rwDL%|c{Jt0jxE2gP8_la9k1yBM4qO;q2wU5A zfQ7}MM1Rb-KP89?rrw!%jTIH*Yv%$sToNJ20K@$@tBsmTl{NPD??bb9o#Z8IvVGgF z>pVgeXTaVzeb=`#Qq>$}?eIPdE-6>dQ&uk^!;D3R9_o&YINmgw?Ka#AEg7fjUIUPX zI^c5dIyfk}Fx)rS=A6h1L9feYcOQB*PlfCTz!VpTQ>J%Kd)KM;Fa8$*yd!kC6>tiF zu>p&@Ft&WZZ}m4vD8sh$v6RA($p5s|n3u@3s%n3y?B;9jDFNVT3+|t`kgH~rWwr|c z!!Z_YA)zTD5x@oO?i$4ZX>>f#iONp^ zUz`^DdJ90j@9hWy?oUPuh}Zo!T068b?vUF=izG1j5MwBt(JUASpg_w$Ij^KhVGT(j z0J;+|r>*t@2#D0JXd6JQ!>&V>vA-g+@8F*n7_<2DYTv2vZzhVcxL9=|rYwLvT<5cb z#$xg;8vzTbUiCNuh;|U*bP5376pn!$v$>^|GWvBC>UkPo#vqt-&fM`!-quBdlVH=E# zCiGb5^NR4TNu>z@d`Ew&z7XCG;T?pbIhH@grcy82{zVk9e4}=KntDM^q2t!TA;2;1 z2V5n9-I=l?;GQ2{I+L%#qQliJ034|yk&QTEho%xqIju{VY|2jPl`N`^qe*C4m~SJe z!tk61Rp}UDGTv+$@E4BQ@9mM7>3Q~e0DwS$hi`Am*z;lq(jcweoTttECo(*7F+hCq zN?75^WAEWA_W^8^+~r&zkP_Hp!gdb4J7;=ecPgy30TBQE3k$|DX3sZ16PTvBcZFE>H`7h1{;qK3O`W;PX0>f-%i(bIKM26cO%M~uB?`GnO^rj=K;s` z!YqhcQv=wQfRs%jAcJe|jPZ!l*~s}XHiR&OIQyNs!R`-#F(-vO0X5D&C{62Q&6&Q$<1XPD0gAVGXRzDm|}Ma$Pyfs!tiI}ej)@_ez4L`7;Ei|tNMV^(!ME_OQu&yA;s3s}A0k-m%k!qyU>k2iyRBiP z-X2KTBE=h55c9v%D=tFv(LX<42Qqt(g*D1S-84x`v zb7(hRAcJKMdzQxMS+c6Bj_MCaHkqYL#zGy3d3hpddjO~k_EowaFROVG3aE;@RsKod zXG)jeqP!!3p=Gf5#{I;a4mcv`3YGxC>S`j$STng+UK%H%0M=kK9||8OM@VQ?vPbKq zbp5AWZAO7eDQefQ05QmBRzqH?mV&O|Bkob1pZ%=y%>jJH(>#ZuQB6I8FOol;d-a8L zaTu^aA-|LLdS0&`t9W&A8ZbL5;Pg1P+`jP%@#DYxfa&6X?PmdHpJ<69-;GyyX#-M1 zx4r>y&ELd5(jGM!NV*D>{^ku}ao{vL4iAC=Ss(-P`n~bH;&l~@UkSS_%m=pF@Kvnd z%JgFw{c@>xqDoHtX22{@LUC%bC0Hv$m_gzb-k z^k`zFfJ+0lS@rIHLU;>ICRuC1rSAZ)*tw&qDK0L%ei!@zsR6N>0AYvumq*P0q&fsS zyzVzIh4(X|!pX;gkb4hUfc~~u`VrFv_rtXb$|{|e)np^B9rWhVo8|+_VeiooJ=|@B-CMVD>1v9v%!z} z-F}?{6k=pa?P7fy5usfDl>Ufqpd#bt3kmS~whl_MZDq3-&FhGZkWg}3McsLyg~4)H zz4Y6FS#1FnKs(U;LKGl~5pynX7!BVMI4%ADroKDN}SOCv4L_7%&OW2ZbVfFAhcEl)q|5nqAU( zY*+E7M-nxdO*~+9UINAkur_hWR1W|SG^RXdV9o2<{9MUX;OE-i{QuT>VRqFn+t4*@udR(f z9+2xUQ|Q8Tbs76n28Mu>THmJZHhsoX32bK?hTIm5&JlEGUj|2>2nI*%36Q7 zipR|Wj*c>y8!Ef?bSMIqgnON5um|`9@BHcx>N)-FD8*~v#yAp zVM=$*yrTS$$$nNVJ)13ozE+v;{-Rr4pAU3zz4PsDxtn+NEwTUq=kuG$cfi5igdM;l zY4ew5zyV)|0KFp11^a=Ws6v8uz-`FlnA?0!6$vFY2__rNk07@2D)@xJht%_vRT2W;*izp<+1^sTVy zTv4m?cQV1(_9;5IL6Z*SUv>MRo;+)#tJ;3=DgqVL;3PHa_XS_~^Piuc?FJ5qf4=oq zNGw(){JIvJQzpV&3345}CVVA2=kSV#~Xc!jx?A%;wvzsZC zLAh{?SJl+tz~0+oV3SfJd1^8+NANsjXxJ2VNx9^*a)`DoaE$ygyL^oR$ei{~JNRRl zuh$0N0Chfrk3qvd)o{V^UVE_V8 LS3j3^P6 + + + + + Wall Localization Preview + + + +
+
+ + +
+ +
+ + + + diff --git a/src/origincar_base/src/origincar_base.cpp b/src/origincar_base/src/origincar_base.cpp index 43b47f2..8613cc6 100644 --- a/src/origincar_base/src/origincar_base.cpp +++ b/src/origincar_base/src/origincar_base.cpp @@ -41,6 +41,24 @@ namespace return std::fabs(vx) < kStationaryLinearSpeedThreshold && std::fabs(vy) < kStationaryLinearSpeedThreshold; } + + std::vector<::origincar_wall::WallPoint> scanMsgToPoints(const sensor_msgs::msg::LaserScan &msg, int stride) + { + std::vector<::origincar_wall::WallPoint> points; + stride = std::max(1, stride); + points.reserve(msg.ranges.size() / static_cast(stride) + 1); + for (std::size_t index = 0; index < msg.ranges.size(); index += static_cast(stride)) + { + const auto distance = static_cast(msg.ranges[index]); + if (!std::isfinite(distance) || distance < msg.range_min || distance > msg.range_max) + { + continue; + } + const auto angle = static_cast(msg.angle_min) + static_cast(index) * msg.angle_increment; + points.push_back({distance * std::cos(angle), distance * std::sin(angle)}); + } + return points; + } } int main(int argc, char *argv[]) @@ -150,7 +168,7 @@ void origincar_base::Sign_Switch_Callback(const std_msgs::msg::Int32::SharedPtr if (sign_switch->data == -1) { memset(&Robot_Pos, 0, sizeof(Robot_Pos)); - memset(&Mpu6050, 0, sizeof(Mpu6050)); + Mpu6050 = sensor_msgs::msg::Imu(); Robot_Pos.X = 0.54; Robot_Pos.Y = 0.2; Robot_Pos.Z = 0; @@ -225,9 +243,10 @@ void origincar_base::Publish_Odom() odom.pose.pose.position.z = 0.0; odom.pose.pose.orientation = odom_quat; - odom.pose.covariance[0] = odom_pose_cov_x_; - odom.pose.covariance[7] = odom_pose_cov_y_; - odom.pose.covariance[35] = odom_pose_cov_yaw_; + const auto covariance = wall_filter_ ? wall_filter_->covariance() : ::origincar_wall::WallKalmanNoise{odom_pose_cov_x_, odom_pose_cov_y_, odom_pose_cov_yaw_}; + odom.pose.covariance[0] = covariance.x; + odom.pose.covariance[7] = covariance.y; + odom.pose.covariance[35] = covariance.theta; // odom_quat; // odom_quat; @@ -263,6 +282,49 @@ void origincar_base::Publish_Odom() robotvel_publisher->publish(robotvel); } +void origincar_base::Scan_Callback(const sensor_msgs::msg::LaserScan::SharedPtr scan) +{ + const auto scan_points = scanMsgToPoints(*scan, wall_scan_stride_); + const auto pose_frame_points = ::origincar_wall::transformScanPointsToPoseFrame(scan_points, laser_pose_); + std::lock_guard lock(wall_scan_mutex_); + latest_scan_points_ = pose_frame_points; + has_latest_scan_ = true; + latest_scan_consumed_ = false; +} + +void origincar_base::Apply_Wall_Update() +{ + if (!wall_filter_) + { + return; + } + std::vector<::origincar_wall::WallPoint> scan_points; + { + std::lock_guard lock(wall_scan_mutex_); + if (!has_latest_scan_ || latest_scan_consumed_) + { + return; + } + scan_points = latest_scan_points_; + latest_scan_consumed_ = true; + } + + const auto result = ::origincar_wall::localizeFromScan(scan_points, wall_fit_config_, wall_filter_->pose()); + if (!result.ok) + { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000, + "wall scan update skipped: %s", result.message.c_str()); + return; + } + + ::origincar_wall::WallKalmanNoise noise{0.015, 0.015, 0.025}; + if (result.quality.wall_count < 3 || (result.quality.has_mean_error && result.quality.mean_error > 0.18)) + { + noise = {0.08, 0.08, 0.12}; + } + wall_filter_->correct(result.pose, noise); +} + void origincar_base::Publish_Voltage() { std_msgs::msg::Float32 voltage_msgs; @@ -448,14 +510,20 @@ void origincar_base::Control() Sampling_Time = (current_time - last_time).seconds(); if (true == Get_Sensor_Data()) { - Robot_Pos.X += 1.03 * (Robot_Vel.X * cos(Robot_Pos.Z) - Robot_Vel.Y * sin(Robot_Pos.Z)) * Sampling_Time; - Robot_Pos.Y += 1.01 * (Robot_Vel.X * sin(Robot_Pos.Z) + Robot_Vel.Y * cos(Robot_Pos.Z)) * Sampling_Time; // 1.125 - Robot_Pos.Z += Robot_Vel.Z * Sampling_Time; + rclcpp::spin_some(this->get_node_base_interface()); + if (wall_filter_) + { + wall_filter_->predict(1.03 * Robot_Vel.X, 1.01 * Robot_Vel.Y, Robot_Vel.Z, Sampling_Time); + Apply_Wall_Update(); + const auto fused_pose = wall_filter_->pose(); + Robot_Pos.X = static_cast(fused_pose.x); + Robot_Pos.Y = static_cast(fused_pose.y); + Robot_Pos.Z = static_cast(fused_pose.theta); + } Publish_ImuSensor(); Publish_GyroDebug(); Publish_Voltage(); Publish_Odom(); - rclcpp::spin_some(this->get_node_base_interface()); } last_time = current_time; } @@ -480,18 +548,26 @@ origincar_base::origincar_base() gyro_z_median_count_ = 0; gyro_z_low_pass_ = 0.0f; gyro_z_low_pass_initialized_ = false; - Robot_Pos.X = 0.54; - Robot_Pos.Y = 0.2; + has_latest_scan_ = false; + latest_scan_consumed_ = true; int serial_baud_rate = 115200; this->declare_parameter("usart_port_name", "/dev/ttyCH343USB0"); + this->declare_parameter("serial_baud_rate", 115200); this->declare_parameter("cmd_vel", "cmd_vel"); this->declare_parameter("akm_cmd_vel", "ackermann_cmd"); this->declare_parameter("odom_frame_id", "odom"); - this->declare_parameter("robot_frame_id", "base_link"); + this->declare_parameter("robot_frame_id", "base_footprint"); this->declare_parameter("gyro_frame_id", "gyro_link"); - this->declare_parameter("publish_tf", false); + this->declare_parameter("combined_odom_topic", "odom_combined"); + this->declare_parameter("scan_topic", "/scan"); + this->declare_parameter("wall_config_path", "/home/sunrise/yiliao_ws/src/origincar_base/config/wall_fit.json"); + this->declare_parameter("publish_tf", true); + this->declare_parameter("wall_scan_stride", 2); + this->declare_parameter("laser_x", 0.0); + this->declare_parameter("laser_y", 0.0); + this->declare_parameter("laser_yaw", 0.0); this->declare_parameter("gyro_z_low_pass_alpha", kDefaultGyroZLowPassAlpha); // Odom covariance parameters (tunable via YAML) @@ -506,7 +582,16 @@ origincar_base::origincar_base() this->get_parameter("odom_frame_id", odom_frame_id); this->get_parameter("robot_frame_id", robot_frame_id); this->get_parameter("gyro_frame_id", gyro_frame_id); + this->get_parameter("combined_odom_topic", combined_odom_topic_); + this->get_parameter("scan_topic", scan_topic_); + this->get_parameter("wall_config_path", wall_config_path_); this->get_parameter("publish_tf", publish_tf_); + this->get_parameter("wall_scan_stride", wall_scan_stride_); + laser_pose_ = { + this->get_parameter("laser_x").as_double(), + this->get_parameter("laser_y").as_double(), + this->get_parameter("laser_yaw").as_double(), + }; gyro_z_low_pass_alpha_ = static_cast(this->get_parameter("gyro_z_low_pass_alpha").as_double()); if (gyro_z_low_pass_alpha_ < 0.0f) @@ -521,7 +606,18 @@ origincar_base::origincar_base() this->get_parameter("odom_pose_cov_y", odom_pose_cov_y_); this->get_parameter("odom_pose_cov_yaw", odom_pose_cov_yaw_); - odom_publisher = create_publisher("odom", 10); + wall_fit_config_ = ::origincar_wall::loadWallFitConfig(wall_config_path_); + const auto initial_pose = ::origincar_wall::Pose2D{ + wall_fit_config_.coordinate_frame.odom_zero_map.x, + wall_fit_config_.coordinate_frame.odom_zero_map.y, + 0.0, + }; + wall_filter_ = std::make_unique<::origincar_wall::WallKalmanFilter>(initial_pose); + Robot_Pos.X = static_cast(initial_pose.x); + Robot_Pos.Y = static_cast(initial_pose.y); + Robot_Pos.Z = static_cast(initial_pose.theta); + + odom_publisher = create_publisher(combined_odom_topic_, 10); imu_publisher = create_publisher("imu/data_raw", 10); @@ -543,12 +639,14 @@ origincar_base::origincar_base() cmd_vel, 1, std::bind(&origincar_base::Cmd_Vel_Callback, this, _1)); Akm_Cmd_Vel_Sub = create_subscription( akm_cmd_vel, 1, std::bind(&origincar_base::Akm_Cmd_Vel_Callback, this, _1)); + Scan_Sub = create_subscription( + scan_topic_, 10, std::bind(&origincar_base::Scan_Callback, this, _1)); Sign_Switch_Sub = create_subscription( "/sign4return", 1, std::bind(&origincar_base::Sign_Switch_Callback, this, _1)); try { - Stm32_Serial.setPort("/dev/ttyACM0"); + Stm32_Serial.setPort(usart_port_name); Stm32_Serial.setBaudrate(serial_baud_rate); serial::Timeout _time = serial::Timeout::simpleTimeout(2000); Stm32_Serial.setTimeout(_time); @@ -605,7 +703,7 @@ void sigintHandler(int sig) { } } - // 关闭ROS2接口，清除资源 + // Shutdown ROS2 and release resources. rclcpp::shutdown(); } @@ -613,3 +711,4 @@ origincar_base::~origincar_base() { RCLCPP_INFO(this->get_logger(), "Shutting down"); } + diff --git a/src/origincar_base/src/wall_fit_calibrator.cpp b/src/origincar_base/src/wall_fit_calibrator.cpp new file mode 100644 index 0000000..a823398 --- /dev/null +++ b/src/origincar_base/src/wall_fit_calibrator.cpp @@ -0,0 +1,246 @@ +#include "origincar_base/wall_fit_core.hpp" + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace +{ +std::vector scanMsgToPoints(const sensor_msgs::msg::LaserScan &msg) +{ + std::vector points; + points.reserve(msg.ranges.size()); + for (std::size_t index = 0; index < msg.ranges.size(); ++index) + { + const auto distance = static_cast(msg.ranges[index]); + if (!std::isfinite(distance) || distance < msg.range_min || distance > msg.range_max) + { + continue; + } + const auto angle = static_cast(msg.angle_min) + static_cast(index) * msg.angle_increment; + points.push_back({distance * std::cos(angle), distance * std::sin(angle)}); + } + return points; +} + +origincar_wall::Pose2D odomMsgToMapPose(const nav_msgs::msg::Odometry &msg, + const origincar_wall::WallFitConfig &config) +{ + const auto &q = msg.pose.pose.orientation; + const auto siny_cosp = 2.0 * (q.w * q.z + q.x * q.y); + const auto cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z); + return { + msg.pose.pose.position.x + config.coordinate_frame.odom_zero_map.x, + msg.pose.pose.position.y + config.coordinate_frame.odom_zero_map.y, + std::atan2(siny_cosp, cosy_cosp), + }; +} +} // namespace + +class WallFitCalibrator : public rclcpp::Node +{ +public: + WallFitCalibrator() + : Node("wall_fit_calibrator") + { + declare_parameter("scan_topic", "/scan"); + declare_parameter("odom_topic", "/odom_combined"); + declare_parameter("config_path", "/home/sunrise/yiliao_ws/src/origincar_base/config/wall_fit.json"); + declare_parameter("laser_x", 0.0); + declare_parameter("laser_y", 0.0); + declare_parameter("laser_yaw", 0.0); + + config_path_ = get_parameter("config_path").as_string(); + config_ = origincar_wall::loadWallFitConfig(config_path_); + laser_pose_ = { + get_parameter("laser_x").as_double(), + get_parameter("laser_y").as_double(), + get_parameter("laser_yaw").as_double(), + }; + + scan_sub_ = create_subscription( + get_parameter("scan_topic").as_string(), 10, + [this](sensor_msgs::msg::LaserScan::SharedPtr msg) { + std::lock_guard lock(mutex_); + latest_scan_ = origincar_wall::transformScanPointsToPoseFrame(scanMsgToPoints(*msg), laser_pose_); + has_scan_ = true; + }); + odom_sub_ = create_subscription( + get_parameter("odom_topic").as_string(), 10, + [this](nav_msgs::msg::Odometry::SharedPtr msg) { + std::lock_guard lock(mutex_); + latest_odom_ = odomMsgToMapPose(*msg, config_); + has_odom_ = true; + }); + + printHelp(); + keyboard_thread_ = std::thread([this]() { keyboardLoop(); }); + } + + ~WallFitCalibrator() override + { + running_ = false; + if (keyboard_thread_.joinable()) + { + keyboard_thread_.join(); + } + } + +private: + void printHelp() + { + RCLCPP_INFO(get_logger(), "Wall calibration order: P -> task_release -> passage -> clinic"); + RCLCPP_INFO(get_logger(), "Keys: Space=capture, A=previous, D=next, Q=clear, Enter=save"); + printSelected(); + } + + void printSelected() + { + RCLCPP_INFO(get_logger(), "Selected [%zu/%zu]: %s", + selected_index_ + 1, reference_order_.size(), reference_order_[selected_index_].c_str()); + } + + void captureCurrent() + { + std::lock_guard lock(mutex_); + if (!has_scan_ || !has_odom_) + { + RCLCPP_WARN(get_logger(), "Waiting for both /scan and /odom_combined before capture"); + return; + } + const auto &key = reference_order_[selected_index_]; + const auto reference = config_.reference_points.at(key); + origincar_wall::WallFitCapture capture; + capture.key = key; + capture.map = reference.map; + capture.odom_pose = {reference.map.x, reference.map.y, latest_odom_.theta}; + capture.scan_points = latest_scan_; + captures_[key] = capture; + RCLCPP_INFO(get_logger(), "Captured %s with %zu scan points", key.c_str(), latest_scan_.size()); + } + + void clearCaptures() + { + std::lock_guard lock(mutex_); + captures_.clear(); + RCLCPP_WARN(get_logger(), "Cleared all wall calibration captures"); + } + + void saveCaptures() + { + std::lock_guard lock(mutex_); + std::vector ordered_captures; + for (const auto &key : reference_order_) + { + const auto found = captures_.find(key); + if (found == captures_.end()) + { + RCLCPP_WARN(get_logger(), "Missing capture for %s; not saving", key.c_str()); + return; + } + ordered_captures.push_back(found->second); + } + try + { + const auto fitted = origincar_wall::fitWallConfigFromCaptures(ordered_captures, config_); + origincar_wall::saveWallFitConfig(config_path_, fitted); + config_ = fitted; + const auto bounds = origincar_wall::wallBounds(config_); + RCLCPP_INFO(get_logger(), "Saved %s; bounds x=[%.3f, %.3f] y=[%.3f, %.3f]", + config_path_.c_str(), bounds.x_min, bounds.x_max, bounds.y_min, bounds.y_max); + } + catch (const std::exception &error) + { + RCLCPP_ERROR(get_logger(), "Failed to save wall fit: %s", error.what()); + } + } + + void keyboardLoop() + { + termios old_terminal{}; + if (tcgetattr(STDIN_FILENO, &old_terminal) != 0) + { + RCLCPP_WARN(get_logger(), "Keyboard control unavailable: stdin is not a terminal"); + return; + } + termios raw_terminal = old_terminal; + raw_terminal.c_lflag &= static_cast(~(ICANON | ECHO)); + raw_terminal.c_cc[VMIN] = 0; + raw_terminal.c_cc[VTIME] = 0; + tcsetattr(STDIN_FILENO, TCSANOW, &raw_terminal); + + while (running_ && rclcpp::ok()) + { + pollfd descriptor{STDIN_FILENO, POLLIN, 0}; + const auto ready = poll(&descriptor, 1, 100); + if (ready <= 0 || !(descriptor.revents & POLLIN)) + { + continue; + } + char key = 0; + if (read(STDIN_FILENO, &key, 1) != 1) + { + continue; + } + if (key == ' ') + { + captureCurrent(); + } + else if (key == 'q' || key == 'Q') + { + clearCaptures(); + } + else if (key == 'a' || key == 'A') + { + selected_index_ = selected_index_ == 0 ? reference_order_.size() - 1 : selected_index_ - 1; + printSelected(); + } + else if (key == 'd' || key == 'D') + { + selected_index_ = (selected_index_ + 1) % reference_order_.size(); + printSelected(); + } + else if (key == '\n' || key == '\r') + { + saveCaptures(); + } + } + tcsetattr(STDIN_FILENO, TCSANOW, &old_terminal); + } + + std::string config_path_; + origincar_wall::WallFitConfig config_; + origincar_wall::Pose2D laser_pose_{0.0, 0.0, 0.0}; + std::vector reference_order_{"P", "task_release", "passage", "clinic"}; + std::size_t selected_index_{0}; + std::mutex mutex_; + bool has_scan_{false}; + bool has_odom_{false}; + std::vector latest_scan_; + origincar_wall::Pose2D latest_odom_{0.0, 0.0, 0.0}; + std::map captures_; + std::atomic running_{true}; + std::thread keyboard_thread_; + rclcpp::Subscription::SharedPtr scan_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; +}; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/origincar_base/src/wall_fit_core.cpp b/src/origincar_base/src/wall_fit_core.cpp new file mode 100644 index 0000000..12729b5 --- /dev/null +++ b/src/origincar_base/src/wall_fit_core.cpp @@ -0,0 +1,605 @@ +#include "origincar_base/wall_fit_core.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace origincar_wall +{ +namespace +{ +constexpr double kPi = 3.14159265358979323846; +constexpr double kYawSearchRange = 8.0 * kPi / 180.0; +constexpr double kYawSearchStep = 1.0 * kPi / 180.0; +constexpr double kWallGroupMaxDistance = 0.45; +constexpr int kMinScanPoints = 8; +constexpr int kMinWallPoints = 3; + +double mean(const std::vector &values) +{ + if (values.empty()) + { + throw std::runtime_error("mean requires values"); + } + double total = 0.0; + for (const auto value : values) + { + total += value; + } + return total / static_cast(values.size()); +} + +double median(std::vector values) +{ + if (values.empty()) + { + throw std::runtime_error("median requires values"); + } + std::sort(values.begin(), values.end()); + const auto middle = values.size() / 2; + if (values.size() % 2 == 1) + { + return values[middle]; + } + return (values[middle - 1] + values[middle]) / 2.0; +} + +double percentile(std::vector values, double fraction) +{ + if (values.empty()) + { + throw std::runtime_error("percentile requires values"); + } + std::sort(values.begin(), values.end()); + const auto raw_index = static_cast(std::round((values.size() - 1) * fraction)); + const auto index = std::max(0, std::min(raw_index, static_cast(values.size() - 1))); + return values[static_cast(index)]; +} + +WallLine wallByName(const WallFitConfig &config, const std::string &name) +{ + for (const auto &wall : config.walls) + { + if (wall.name == name) + { + return wall; + } + } + throw std::runtime_error("missing wall " + name); +} + +double verticalWallX(const WallLine &wall) +{ + if (std::fabs(wall.a) < 1e-9 || std::fabs(wall.b) > 1e-6) + { + throw std::runtime_error("wall " + wall.name + " must be vertical"); + } + return -wall.c / wall.a; +} + +double horizontalWallY(const WallLine &wall) +{ + if (std::fabs(wall.b) < 1e-9 || std::fabs(wall.a) > 1e-6) + { + throw std::runtime_error("wall " + wall.name + " must be horizontal"); + } + return -wall.c / wall.b; +} + +std::pair nearestWallResidual(const WallPoint &point, const WallBounds &bounds) +{ + std::array, 4> candidates{{ + {"left", point.x - bounds.x_min}, + {"right", point.x - bounds.x_max}, + {"bottom", point.y - bounds.y_min}, + {"top", point.y - bounds.y_max}, + }}; + return *std::min_element( + candidates.begin(), candidates.end(), + [](const auto &left, const auto &right) { return std::fabs(left.second) < std::fabs(right.second); }); +} + +std::map> groupScanPointsByWall( + const std::vector &scan_map, const WallBounds &bounds) +{ + std::map> groups{ + {"left", {}}, {"right", {}}, {"bottom", {}}, {"top", {}}}; + for (const auto &point : scan_map) + { + const auto nearest = nearestWallResidual(point, bounds); + if (std::fabs(nearest.second) <= kWallGroupMaxDistance) + { + groups[nearest.first].push_back(point); + } + } + return groups; +} + +std::vector yawCandidates(double center_theta) +{ + std::vector candidates; + const auto steps = static_cast(std::round(kYawSearchRange / kYawSearchStep)); + for (int index = -steps; index <= steps; ++index) + { + candidates.push_back(normalizeAngle(center_theta + index * kYawSearchStep)); + } + return candidates; +} + +LocalizationResult solveWallOffsetsForPose(const std::vector &scan_points, + const WallBounds &bounds, + const Pose2D &pose) +{ + const auto scan_map = transformScanPointsToMap(scan_points, pose); + const auto groups = groupScanPointsByWall(scan_map, bounds); + std::vector x_corrections; + std::vector y_corrections; + std::vector matched_walls; + + for (const auto &entry : groups) + { + const auto &name = entry.first; + const auto &group_points = entry.second; + if (static_cast(group_points.size()) < kMinWallPoints) + { + continue; + } + const bool x_axis = (name == "left" || name == "right"); + const double target = name == "left" ? bounds.x_min + : name == "right" ? bounds.x_max + : name == "bottom" ? bounds.y_min + : bounds.y_max; + std::vector observed_values; + observed_values.reserve(group_points.size()); + for (const auto &point : group_points) + { + observed_values.push_back(x_axis ? point.x : point.y); + } + const auto observed = median(observed_values); + const auto correction = target - observed; + std::vector errors; + errors.reserve(group_points.size()); + for (const auto &value : observed_values) + { + errors.push_back(std::fabs((value + correction) - target)); + } + matched_walls.push_back( + {name, static_cast(group_points.size()), observed, target, correction, mean(errors)}); + if (x_axis) + { + x_corrections.push_back(correction); + } + else + { + y_corrections.push_back(correction); + } + } + + LocalizationQuality quality; + quality.wall_count = static_cast(matched_walls.size()); + quality.has_mean_error = !matched_walls.empty(); + quality.mean_error = 0.0; + if (quality.has_mean_error) + { + std::vector wall_errors; + for (const auto &wall : matched_walls) + { + wall_errors.push_back(wall.mean_error); + } + quality.mean_error = mean(wall_errors); + } + + const bool valid = matched_walls.size() >= 2 && !x_corrections.empty() && !y_corrections.empty(); + if (!valid) + { + return {false, pose, scan_map, matched_walls, quality, "fewer than two usable perpendicular wall constraints"}; + } + + Pose2D corrected_pose{ + pose.x + mean(x_corrections), + pose.y + mean(y_corrections), + normalizeAngle(pose.theta), + }; + return { + true, + corrected_pose, + transformScanPointsToMap(scan_points, corrected_pose), + matched_walls, + quality, + "localized from odom prediction and calibrated walls"}; +} + +nlohmann::json pointToJson(const WallPoint &point) +{ + return nlohmann::json{{"x", point.x}, {"y", point.y}}; +} + +WallPoint pointFromJson(const nlohmann::json &json) +{ + return {json.at("x").get(), json.at("y").get()}; +} + +nlohmann::json wallToJson(const WallLine &wall) +{ + return nlohmann::json{{"name", wall.name}, {"a", wall.a}, {"b", wall.b}, {"c", wall.c}, {"color", wall.color}}; +} + +WallLine wallFromJson(const nlohmann::json &json) +{ + return { + json.at("name").get(), + json.at("a").get(), + json.at("b").get(), + json.at("c").get(), + json.value("color", std::string("#64748b"))}; +} + +nlohmann::json configToJsonObject(const WallFitConfig &config) +{ + nlohmann::json walls = nlohmann::json::array(); + for (const auto &wall : config.walls) + { + walls.push_back(wallToJson(wall)); + } + nlohmann::json refs = nlohmann::json::object(); + for (const auto &entry : config.reference_points) + { + refs[entry.first] = { + {"label", entry.second.label}, + {"image", pointToJson(entry.second.image)}, + {"map", pointToJson(entry.second.map)}}; + } + nlohmann::json frame = { + {"origin", config.coordinate_frame.origin}, + {"origin_note", config.coordinate_frame.origin_note}, + {"width_m", config.coordinate_frame.width_m}, + {"height_m", config.coordinate_frame.height_m}, + {"image_width_px", config.coordinate_frame.image_width_px}, + {"image_height_px", config.coordinate_frame.image_height_px}, + {"pixels_per_meter", config.coordinate_frame.pixels_per_meter}, + {"p_image_px", pointToJson(config.coordinate_frame.p_image_px)}, + {"odom_zero_reference", config.coordinate_frame.odom_zero_reference}, + {"odom_zero_map", pointToJson(config.coordinate_frame.odom_zero_map)}}; + + nlohmann::json result = {{"coordinate_frame", frame}, {"walls", walls}, {"reference_points", refs}}; + if (!config.generated_at.empty()) + { + result["generated_at"] = config.generated_at; + } + return result; +} + +WallFitConfig configFromJsonObject(const nlohmann::json &json) +{ + auto config = defaultWallFitConfig(); + const auto &frame = json.at("coordinate_frame"); + config.coordinate_frame.origin = frame.at("origin").get(); + config.coordinate_frame.origin_note = frame.value("origin_note", config.coordinate_frame.origin_note); + config.coordinate_frame.width_m = frame.value("width_m", config.coordinate_frame.width_m); + config.coordinate_frame.height_m = frame.value("height_m", config.coordinate_frame.height_m); + config.coordinate_frame.image_width_px = frame.value("image_width_px", config.coordinate_frame.image_width_px); + config.coordinate_frame.image_height_px = frame.value("image_height_px", config.coordinate_frame.image_height_px); + config.coordinate_frame.pixels_per_meter = frame.value("pixels_per_meter", config.coordinate_frame.pixels_per_meter); + if (frame.contains("p_image_px")) + { + config.coordinate_frame.p_image_px = pointFromJson(frame.at("p_image_px")); + } + config.coordinate_frame.odom_zero_reference = + frame.value("odom_zero_reference", config.coordinate_frame.odom_zero_reference); + if (frame.contains("odom_zero_map")) + { + config.coordinate_frame.odom_zero_map = pointFromJson(frame.at("odom_zero_map")); + } + + config.walls.clear(); + for (const auto &wall_json : json.at("walls")) + { + config.walls.push_back(wallFromJson(wall_json)); + } + + if (json.contains("reference_points")) + { + config.reference_points.clear(); + for (auto it = json.at("reference_points").begin(); it != json.at("reference_points").end(); ++it) + { + config.reference_points[it.key()] = { + it.value().value("label", it.key()), + pointFromJson(it.value().at("image")), + pointFromJson(it.value().at("map"))}; + } + } + config.generated_at = json.value("generated_at", std::string()); + if (config.coordinate_frame.origin != "map_lower_left") + { + throw std::runtime_error("wall_fit coordinate_frame.origin must be map_lower_left"); + } + wallBounds(config); + return config; +} + +std::string utcNow() +{ + const auto now = std::chrono::system_clock::now(); + const auto time = std::chrono::system_clock::to_time_t(now); + std::tm tm{}; + gmtime_r(&time, &tm); + std::ostringstream stream; + stream << std::put_time(&tm, "%Y-%m-%dT%H:%M:%SZ"); + return stream.str(); +} +} // namespace + +WallFitConfig defaultWallFitConfig() +{ + WallFitConfig config; + config.coordinate_frame = { + "map_lower_left", + "map (0,0) is the lower-left map corner; odom zero is placed at the P point", + 5.0, + 5.0, + 500, + 500, + 100, + {56.0, 482.0}, + "P", + {0.56, 0.18}, + }; + config.walls = { + {"top", 0.0, -1.0, 5.0, "#d62728"}, + {"bottom", 0.0, 1.0, 0.0, "#1f77b4"}, + {"left", 1.0, 0.0, 0.0, "#2ca02c"}, + {"right", -1.0, 0.0, 5.0, "#ff7f0e"}, + }; + config.reference_points = { + {"P", {"P", {56.0, 482.0}, {0.56, 0.18}}}, + {"task_release", {"task_release_left_midpoint", {450.0, 319.0}, {4.5, 1.81}}}, + {"passage", {"passage", {250.0, 240.0}, {2.5, 2.6}}}, + {"clinic", {"clinic_room", {280.0, 123.0}, {2.8, 3.77}}}, + }; + return config; +} + +WallBounds wallBounds(const WallFitConfig &config) +{ + const WallBounds bounds{ + verticalWallX(wallByName(config, "left")), + verticalWallX(wallByName(config, "right")), + horizontalWallY(wallByName(config, "bottom")), + horizontalWallY(wallByName(config, "top")), + }; + if (bounds.x_min >= bounds.x_max || bounds.y_min >= bounds.y_max) + { + throw std::runtime_error("wall bounds are invalid"); + } + return bounds; +} + +double normalizeAngle(double angle) +{ + return std::fmod(angle + kPi, 2.0 * kPi) < 0.0 + ? std::fmod(angle + kPi, 2.0 * kPi) + kPi + : std::fmod(angle + kPi, 2.0 * kPi) - kPi; +} + +Pose2D predictPoseFromOdomDelta(const Pose2D &previous_wall_pose, + const Pose2D &previous_odom_pose, + const Pose2D ¤t_odom_pose) +{ + return { + previous_wall_pose.x + current_odom_pose.x - previous_odom_pose.x, + previous_wall_pose.y + current_odom_pose.y - previous_odom_pose.y, + normalizeAngle(previous_wall_pose.theta + normalizeAngle(current_odom_pose.theta - previous_odom_pose.theta)), + }; +} + +std::vector transformScanPointsToMap(const std::vector &scan_points, + const Pose2D &pose) +{ + const auto cos_t = std::cos(pose.theta); + const auto sin_t = std::sin(pose.theta); + std::vector result; + result.reserve(scan_points.size()); + for (const auto &point : scan_points) + { + result.push_back({pose.x + point.x * cos_t - point.y * sin_t, + pose.y + point.x * sin_t + point.y * cos_t}); + } + return result; +} + +std::vector transformScanPointsToPoseFrame(const std::vector &scan_points, + const Pose2D &laser_pose) +{ + return transformScanPointsToMap(scan_points, laser_pose); +} + +std::vector mapPointsToScan(const std::vector &map_points, + const Pose2D &pose) +{ + const auto cos_t = std::cos(pose.theta); + const auto sin_t = std::sin(pose.theta); + std::vector result; + result.reserve(map_points.size()); + for (const auto &point : map_points) + { + const auto dx = point.x - pose.x; + const auto dy = point.y - pose.y; + result.push_back({dx * cos_t + dy * sin_t, -dx * sin_t + dy * cos_t}); + } + return result; +} + +LocalizationResult localizeFromScan(const std::vector &scan_points, + const WallFitConfig &config, + const Pose2D &predicted_pose) +{ + if (static_cast(scan_points.size()) < kMinScanPoints) + { + return { + false, + predicted_pose, + transformScanPointsToMap(scan_points, predicted_pose), + {}, + {0, 0.0, false}, + "not enough scan points for wall localization"}; + } + const auto bounds = wallBounds(config); + const auto center_theta = predicted_pose.theta; + std::vector valid_results; + for (const auto theta : yawCandidates(center_theta)) + { + auto solution = solveWallOffsetsForPose(scan_points, bounds, {predicted_pose.x, predicted_pose.y, theta}); + if (solution.ok) + { + valid_results.push_back(solution); + } + } + if (valid_results.empty()) + { + return solveWallOffsetsForPose(scan_points, bounds, predicted_pose); + } + return *std::min_element( + valid_results.begin(), valid_results.end(), + [center_theta](const auto &left, const auto &right) { + if (std::fabs(left.quality.mean_error - right.quality.mean_error) > 1e-9) + { + return left.quality.mean_error < right.quality.mean_error; + } + if (left.quality.wall_count != right.quality.wall_count) + { + return left.quality.wall_count > right.quality.wall_count; + } + return std::fabs(normalizeAngle(left.pose.theta - center_theta)) < + std::fabs(normalizeAngle(right.pose.theta - center_theta)); + }); +} + +WallFitConfig fitWallConfigFromCaptures(const std::vector &captures, + const WallFitConfig &base_config) +{ + if (captures.size() < 4) + { + throw std::runtime_error("wall calibration requires four captures"); + } + std::vector all_points; + for (const auto &capture : captures) + { + const auto scan_map = transformScanPointsToMap(capture.scan_points, capture.odom_pose); + all_points.insert(all_points.end(), scan_map.begin(), scan_map.end()); + } + if (all_points.size() < 16) + { + throw std::runtime_error("wall calibration requires enough scan points"); + } + std::vector xs; + std::vector ys; + xs.reserve(all_points.size()); + ys.reserve(all_points.size()); + for (const auto &point : all_points) + { + if (std::isfinite(point.x)) + { + xs.push_back(point.x); + } + if (std::isfinite(point.y)) + { + ys.push_back(point.y); + } + } + const auto left_cut = percentile(xs, 0.08); + const auto right_cut = percentile(xs, 0.92); + const auto bottom_cut = percentile(ys, 0.08); + const auto top_cut = percentile(ys, 0.92); + std::vector left_values; + std::vector right_values; + std::vector bottom_values; + std::vector top_values; + for (const auto value : xs) + { + if (value <= left_cut) + { + left_values.push_back(value); + } + if (value >= right_cut) + { + right_values.push_back(value); + } + } + for (const auto value : ys) + { + if (value <= bottom_cut) + { + bottom_values.push_back(value); + } + if (value >= top_cut) + { + top_values.push_back(value); + } + } + + const auto x_min = median(left_values); + const auto x_max = median(right_values); + const auto y_min = median(bottom_values); + const auto y_max = median(top_values); + + auto config = base_config; + config.coordinate_frame = defaultWallFitConfig().coordinate_frame; + config.reference_points = defaultWallFitConfig().reference_points; + config.walls = { + {"top", 0.0, -1.0, y_max, "#d62728"}, + {"bottom", 0.0, 1.0, -y_min, "#1f77b4"}, + {"left", 1.0, 0.0, -x_min, "#2ca02c"}, + {"right", -1.0, 0.0, x_max, "#ff7f0e"}, + }; + config.generated_at = utcNow(); + wallBounds(config); + return config; +} + +WallFitConfig loadWallFitConfig(const std::string &path) +{ + std::ifstream file(path); + if (!file.good()) + { + return defaultWallFitConfig(); + } + nlohmann::json json; + file >> json; + return configFromJsonObject(json); +} + +void saveWallFitConfig(const std::string &path, const WallFitConfig &config) +{ + const auto json = configToJsonObject(config); + const auto temp_path = path + ".tmp"; + { + std::ofstream file(temp_path, std::ios::trunc); + if (!file.good()) + { + throw std::runtime_error("failed to open temporary wall fit config"); + } + file << json.dump(2) << "\n"; + } + if (std::rename(temp_path.c_str(), path.c_str()) != 0) + { + std::remove(temp_path.c_str()); + throw std::runtime_error("failed to replace wall fit config"); + } +} + +std::string wallFitConfigToJson(const WallFitConfig &config) +{ + return configToJsonObject(config).dump(2); +} + +} // namespace origincar_wall diff --git a/src/origincar_base/src/wall_kalman_filter.cpp b/src/origincar_base/src/wall_kalman_filter.cpp new file mode 100644 index 0000000..c007ebf --- /dev/null +++ b/src/origincar_base/src/wall_kalman_filter.cpp @@ -0,0 +1,66 @@ +#include "origincar_base/wall_kalman_filter.hpp" + +#include +#include + +namespace origincar_wall +{ + +WallKalmanFilter::WallKalmanFilter(const Pose2D &initial_pose) + : pose_(initial_pose), + covariance_{0.10, 0.10, 0.10}, + process_noise_per_second_{0.04, 0.04, 0.06} +{ + pose_.theta = normalizeAngle(pose_.theta); +} + +void WallKalmanFilter::reset(const Pose2D &pose) +{ + pose_ = pose; + pose_.theta = normalizeAngle(pose_.theta); + covariance_ = {0.10, 0.10, 0.10}; +} + +void WallKalmanFilter::predict(double vx, double vy, double yaw_rate, double dt) +{ + dt = std::max(0.0, dt); + const auto cos_t = std::cos(pose_.theta); + const auto sin_t = std::sin(pose_.theta); + pose_.x += (vx * cos_t - vy * sin_t) * dt; + pose_.y += (vx * sin_t + vy * cos_t) * dt; + pose_.theta = normalizeAngle(pose_.theta + yaw_rate * dt); + covariance_.x += process_noise_per_second_.x * dt; + covariance_.y += process_noise_per_second_.y * dt; + covariance_.theta += process_noise_per_second_.theta * dt; +} + +void WallKalmanFilter::correct(const Pose2D &measurement, const WallKalmanNoise &measurement_noise) +{ + const auto noise_x = std::max(1e-6, measurement_noise.x); + const auto noise_y = std::max(1e-6, measurement_noise.y); + const auto noise_theta = std::max(1e-6, measurement_noise.theta); + + const auto kx = covariance_.x / (covariance_.x + noise_x); + const auto ky = covariance_.y / (covariance_.y + noise_y); + const auto kt = covariance_.theta / (covariance_.theta + noise_theta); + + pose_.x += kx * (measurement.x - pose_.x); + pose_.y += ky * (measurement.y - pose_.y); + pose_.theta = normalizeAngle(pose_.theta + kt * normalizeAngle(measurement.theta - pose_.theta)); + + covariance_.x = (1.0 - kx) * covariance_.x; + covariance_.y = (1.0 - ky) * covariance_.y; + covariance_.theta = (1.0 - kt) * covariance_.theta; +} + +Pose2D WallKalmanFilter::pose() const +{ + return pose_; +} + +WallKalmanNoise WallKalmanFilter::covariance() const +{ + return covariance_; +} + +} // namespace origincar_wall diff --git a/src/origincar_base/src/wall_localization_node.cpp b/src/origincar_base/src/wall_localization_node.cpp new file mode 100644 index 0000000..8b71028 --- /dev/null +++ b/src/origincar_base/src/wall_localization_node.cpp @@ -0,0 +1,256 @@ +#include "origincar_base/wall_fit_core.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace +{ +std::vector scanMsgToPoints(const sensor_msgs::msg::LaserScan &msg, int stride) +{ + std::vector points; + stride = std::max(1, stride); + points.reserve(msg.ranges.size() / static_cast(stride) + 1); + for (std::size_t index = 0; index < msg.ranges.size(); index += static_cast(stride)) + { + const auto distance = static_cast(msg.ranges[index]); + if (!std::isfinite(distance) || distance < msg.range_min || distance > msg.range_max) + { + continue; + } + const auto angle = static_cast(msg.angle_min) + static_cast(index) * msg.angle_increment; + points.push_back({distance * std::cos(angle), distance * std::sin(angle)}); + } + return points; +} + +double quaternionToYaw(const geometry_msgs::msg::Quaternion &q) +{ + const auto siny_cosp = 2.0 * (q.w * q.z + q.x * q.y); + const auto cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z); + return std::atan2(siny_cosp, cosy_cosp); +} + +geometry_msgs::msg::Quaternion yawToQuaternion(double yaw) +{ + geometry_msgs::msg::Quaternion q; + const auto half = yaw / 2.0; + q.x = 0.0; + q.y = 0.0; + q.z = std::sin(half); + q.w = std::cos(half); + return q; +} + +origincar_wall::Pose2D odomMsgToMapPose(const nav_msgs::msg::Odometry &msg, + const origincar_wall::WallFitConfig &config) +{ + return { + msg.pose.pose.position.x + config.coordinate_frame.odom_zero_map.x, + msg.pose.pose.position.y + config.coordinate_frame.odom_zero_map.y, + quaternionToYaw(msg.pose.pose.orientation), + }; +} +} // namespace + +class WallLocalizationNode : public rclcpp::Node +{ +public: + WallLocalizationNode() + : Node("wall_localization_node"), + tf_broadcaster_(std::make_unique(*this)) + { + declare_parameter("scan_topic", "/scan"); + declare_parameter("odom_topic", "/odom_combined"); + declare_parameter("wall_odom_topic", "/odom_wall"); + declare_parameter("config_path", "/home/sunrise/yiliao_ws/src/origincar_base/config/wall_fit.json"); + declare_parameter("odom_frame", "odom"); + declare_parameter("base_frame", "base_footprint"); + declare_parameter("publish_tf", true); + declare_parameter("localization_hz", 10.0); + declare_parameter("scan_stride", 2); + declare_parameter("laser_x", 0.0); + declare_parameter("laser_y", 0.0); + declare_parameter("laser_yaw", 0.0); + + config_ = origincar_wall::loadWallFitConfig(get_parameter("config_path").as_string()); + wall_odom_topic_ = get_parameter("wall_odom_topic").as_string(); + odom_frame_ = get_parameter("odom_frame").as_string(); + base_frame_ = get_parameter("base_frame").as_string(); + publish_tf_ = get_parameter("publish_tf").as_bool(); + scan_stride_ = get_parameter("scan_stride").as_int(); + const auto localization_hz = std::max(1.0, get_parameter("localization_hz").as_double()); + min_scan_period_ = rclcpp::Duration::from_seconds(1.0 / localization_hz); + laser_pose_ = { + get_parameter("laser_x").as_double(), + get_parameter("laser_y").as_double(), + get_parameter("laser_yaw").as_double(), + }; + + wall_odom_pub_ = create_publisher(wall_odom_topic_, 10); + scan_sub_ = create_subscription( + get_parameter("scan_topic").as_string(), 10, + [this](sensor_msgs::msg::LaserScan::SharedPtr msg) { onScan(*msg); }); + odom_sub_ = create_subscription( + get_parameter("odom_topic").as_string(), 10, + [this](nav_msgs::msg::Odometry::SharedPtr msg) { onOdom(*msg); }); + + const auto bounds = origincar_wall::wallBounds(config_); + RCLCPP_INFO(get_logger(), "Loaded wall fit; bounds x=[%.3f, %.3f] y=[%.3f, %.3f]", + bounds.x_min, bounds.x_max, bounds.y_min, bounds.y_max); + RCLCPP_INFO(get_logger(), "Publishing %s; publish_tf=%s", wall_odom_topic_.c_str(), publish_tf_ ? "true" : "false"); + } + +private: + void onOdom(const nav_msgs::msg::Odometry &msg) + { + std::lock_guard lock(mutex_); + latest_twist_ = msg.twist.twist; + has_twist_ = true; + const auto current_odom_pose = odomMsgToMapPose(msg, config_); + if (!has_wall_pose_) + { + predicted_pose_ = current_odom_pose; + last_wall_pose_ = current_odom_pose; + has_wall_pose_ = true; + } + else if (has_odom_pose_) + { + predicted_pose_ = origincar_wall::predictPoseFromOdomDelta(last_wall_pose_, last_odom_pose_, current_odom_pose); + } + else + { + predicted_pose_ = last_wall_pose_; + } + last_odom_pose_ = current_odom_pose; + has_odom_pose_ = true; + has_prediction_ = true; + } + + void onScan(const sensor_msgs::msg::LaserScan &msg) + { + const auto now = get_clock()->now(); + if (has_last_scan_time_ && (now - last_scan_time_) < min_scan_period_) + { + return; + } + last_scan_time_ = now; + has_last_scan_time_ = true; + + origincar_wall::Pose2D predicted_pose; + { + std::lock_guard lock(mutex_); + if (!has_prediction_) + { + predicted_pose = {config_.coordinate_frame.odom_zero_map.x, config_.coordinate_frame.odom_zero_map.y, 0.0}; + predicted_pose_ = predicted_pose; + has_prediction_ = true; + } + else + { + predicted_pose = predicted_pose_; + } + } + + const auto scan_points = + origincar_wall::transformScanPointsToPoseFrame(scanMsgToPoints(msg, scan_stride_), laser_pose_); + const auto result = origincar_wall::localizeFromScan(scan_points, config_, predicted_pose); + + origincar_wall::Pose2D pose_to_publish = result.ok ? result.pose : predicted_pose; + { + std::lock_guard lock(mutex_); + last_wall_pose_ = pose_to_publish; + predicted_pose_ = pose_to_publish; + has_wall_pose_ = true; + has_prediction_ = true; + } + publishWallOdom(pose_to_publish, msg.header.stamp, result.ok); + if (publish_tf_) + { + publishTf(pose_to_publish, msg.header.stamp); + } + if (!result.ok) + { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, "Wall localization degraded: %s", + result.message.c_str()); + } + } + + void publishWallOdom(const origincar_wall::Pose2D &pose, + const builtin_interfaces::msg::Time &stamp, + bool valid) + { + nav_msgs::msg::Odometry odom; + odom.header.stamp = stamp; + odom.header.frame_id = odom_frame_; + odom.child_frame_id = base_frame_; + odom.pose.pose.position.x = pose.x; + odom.pose.pose.position.y = pose.y; + odom.pose.pose.position.z = 0.0; + odom.pose.pose.orientation = yawToQuaternion(pose.theta); + odom.pose.covariance[0] = valid ? 0.02 : 0.25; + odom.pose.covariance[7] = valid ? 0.02 : 0.25; + odom.pose.covariance[35] = valid ? 0.03 : 0.3; + { + std::lock_guard lock(mutex_); + if (has_twist_) + { + odom.twist.twist = latest_twist_; + } + } + wall_odom_pub_->publish(odom); + } + + void publishTf(const origincar_wall::Pose2D &pose, + const builtin_interfaces::msg::Time &stamp) + { + geometry_msgs::msg::TransformStamped transform; + transform.header.stamp = stamp; + transform.header.frame_id = odom_frame_; + transform.child_frame_id = base_frame_; + transform.transform.translation.x = pose.x; + transform.transform.translation.y = pose.y; + transform.transform.translation.z = 0.0; + transform.transform.rotation = yawToQuaternion(pose.theta); + tf_broadcaster_->sendTransform(transform); + } + + std::mutex mutex_; + origincar_wall::WallFitConfig config_; + std::string wall_odom_topic_; + std::string odom_frame_; + std::string base_frame_; + bool publish_tf_{true}; + int scan_stride_{2}; + rclcpp::Duration min_scan_period_{0, 100000000}; + bool has_last_scan_time_{false}; + rclcpp::Time last_scan_time_{0, 0, RCL_ROS_TIME}; + origincar_wall::Pose2D laser_pose_{0.0, 0.0, 0.0}; + origincar_wall::Pose2D last_odom_pose_{0.0, 0.0, 0.0}; + origincar_wall::Pose2D last_wall_pose_{0.0, 0.0, 0.0}; + origincar_wall::Pose2D predicted_pose_{0.0, 0.0, 0.0}; + bool has_odom_pose_{false}; + bool has_wall_pose_{false}; + bool has_prediction_{false}; + geometry_msgs::msg::Twist latest_twist_; + bool has_twist_{false}; + rclcpp::Publisher::SharedPtr wall_odom_pub_; + rclcpp::Subscription::SharedPtr scan_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + std::unique_ptr tf_broadcaster_; +}; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/origincar_base/src/wall_preview_server.cpp b/src/origincar_base/src/wall_preview_server.cpp new file mode 100644 index 0000000..629ad2e --- /dev/null +++ b/src/origincar_base/src/wall_preview_server.cpp @@ -0,0 +1,332 @@ +#include "origincar_base/wall_fit_core.hpp" + +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace +{ +std::vector scanMsgToPoints(const sensor_msgs::msg::LaserScan &msg, int stride) +{ + std::vector points; + stride = std::max(1, stride); + points.reserve(msg.ranges.size() / static_cast(stride) + 1); + for (std::size_t index = 0; index < msg.ranges.size(); index += static_cast(stride)) + { + const auto distance = static_cast(msg.ranges[index]); + if (!std::isfinite(distance) || distance < msg.range_min || distance > msg.range_max) + { + continue; + } + const auto angle = static_cast(msg.angle_min) + static_cast(index) * msg.angle_increment; + points.push_back({distance * std::cos(angle), distance * std::sin(angle)}); + } + return points; +} + +double quaternionToYaw(const geometry_msgs::msg::Quaternion &q) +{ + const auto siny_cosp = 2.0 * (q.w * q.z + q.x * q.y); + const auto cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z); + return std::atan2(siny_cosp, cosy_cosp); +} + +nlohmann::json poseJson(double x, double y, double theta) +{ + return nlohmann::json{{"x", x}, {"y", y}, {"theta", theta}}; +} + +std::string readTextFile(const std::string &path) +{ + std::ifstream file(path); + if (!file.good()) + { + return ""; + } + std::ostringstream stream; + stream << file.rdbuf(); + return stream.str(); +} + +std::vector readBinaryFile(const std::string &path) +{ + std::ifstream file(path, std::ios::binary); + if (!file.good()) + { + return {}; + } + return std::vector((std::istreambuf_iterator(file)), std::istreambuf_iterator()); +} + +std::string requestPath(const std::string &request) +{ + const auto first_space = request.find(' '); + if (first_space == std::string::npos) + { + return "/"; + } + const auto second_space = request.find(' ', first_space + 1); + auto path = request.substr(first_space + 1, second_space - first_space - 1); + const auto query = path.find('?'); + if (query != std::string::npos) + { + path = path.substr(0, query); + } + return path; +} +} // namespace + +class WallPreviewServer : public rclcpp::Node +{ +public: + WallPreviewServer() + : Node("wall_preview_server") + { + declare_parameter("host", "0.0.0.0"); + declare_parameter("port", 8772); + declare_parameter("config_path", "/home/sunrise/yiliao_ws/src/origincar_base/config/wall_fit.json"); + declare_parameter("preview_dir", "/home/sunrise/yiliao_ws/src/origincar_base/preview"); + declare_parameter("scan_topic", "/scan"); + declare_parameter("combined_odom_topic", "/odom_combined"); + declare_parameter("scan_stride", 3); + declare_parameter("preview_hz", 8.0); + + host_ = get_parameter("host").as_string(); + port_ = get_parameter("port").as_int(); + config_path_ = get_parameter("config_path").as_string(); + preview_dir_ = get_parameter("preview_dir").as_string(); + scan_stride_ = get_parameter("scan_stride").as_int(); + preview_period_ms_ = static_cast(1000.0 / std::max(1.0, get_parameter("preview_hz").as_double())); + wall_fit_ = origincar_wall::loadWallFitConfig(config_path_); + + combined_odom_sub_ = create_subscription( + get_parameter("combined_odom_topic").as_string(), 10, + [this](nav_msgs::msg::Odometry::SharedPtr msg) { + std::lock_guard lock(mutex_); + combined_pose_ = poseJson(msg->pose.pose.position.x, msg->pose.pose.position.y, + quaternionToYaw(msg->pose.pose.orientation)); + has_combined_pose_ = true; + }); + scan_sub_ = create_subscription( + get_parameter("scan_topic").as_string(), 10, + [this](sensor_msgs::msg::LaserScan::SharedPtr msg) { + std::lock_guard lock(mutex_); + if (!has_combined_pose_) + { + return; + } + const origincar_wall::Pose2D pose{ + combined_pose_.at("x").get(), + combined_pose_.at("y").get(), + combined_pose_.at("theta").get(), + }; + latest_scan_map_ = origincar_wall::transformScanPointsToMap(scanMsgToPoints(*msg, scan_stride_), pose); + }); + + server_thread_ = std::thread([this]() { serve(); }); + RCLCPP_INFO(get_logger(), "Wall preview: http://%s:%d", host_.c_str(), port_); + } + + ~WallPreviewServer() override + { + running_ = false; + if (server_fd_ >= 0) + { + close(server_fd_); + } + if (server_thread_.joinable()) + { + server_thread_.join(); + } + } + +private: + nlohmann::json snapshot() + { + std::lock_guard lock(mutex_); + nlohmann::json scan_points = nlohmann::json::array(); + for (const auto &point : latest_scan_map_) + { + scan_points.push_back({{"x", point.x}, {"y", point.y}}); + } + return { + {"status", "ok"}, + {"wall_fit", nlohmann::json::parse(origincar_wall::wallFitConfigToJson(wall_fit_))}, + {"odom_combined", has_combined_pose_ ? combined_pose_ : nlohmann::json(nullptr)}, + {"scan_map", {{"points", scan_points}}}, + }; + } + + void sendAll(int fd, const std::string &data) + { + const char *buffer = data.data(); + std::size_t remaining = data.size(); + while (remaining > 0) + { + const auto sent = send(fd, buffer, remaining, MSG_NOSIGNAL); + if (sent <= 0) + { + return; + } + buffer += sent; + remaining -= static_cast(sent); + } + } + + void sendBytes(int fd, const std::string &content_type, const std::vector &bytes) + { + std::ostringstream header; + header << "HTTP/1.1 200 OK\r\nContent-Type: " << content_type + << "\r\nContent-Length: " << bytes.size() + << "\r\nConnection: close\r\n\r\n"; + sendAll(fd, header.str()); + if (!bytes.empty()) + { + send(fd, bytes.data(), bytes.size(), MSG_NOSIGNAL); + } + } + + void sendText(int fd, const std::string &content_type, const std::string &body) + { + std::ostringstream response; + response << "HTTP/1.1 200 OK\r\nContent-Type: " << content_type + << "\r\nContent-Length: " << body.size() + << "\r\nConnection: close\r\n\r\n" + << body; + sendAll(fd, response.str()); + } + + void sendNotFound(int fd) + { + sendText(fd, "text/plain; charset=utf-8", "not found\n"); + } + + void sendEvents(int fd) + { + sendAll(fd, "HTTP/1.1 200 OK\r\nContent-Type: text/event-stream\r\nCache-Control: no-cache\r\nConnection: keep-alive\r\n\r\n"); + while (running_ && rclcpp::ok()) + { + const auto payload = std::string("event: snapshot\ndata: ") + snapshot().dump() + "\n\n"; + if (send(fd, payload.data(), payload.size(), MSG_NOSIGNAL) <= 0) + { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(preview_period_ms_)); + } + } + + void handleClient(int fd) + { + std::array buffer{}; + const auto received = recv(fd, buffer.data(), buffer.size() - 1, 0); + if (received <= 0) + { + close(fd); + return; + } + const auto path = requestPath(std::string(buffer.data(), static_cast(received))); + if (path == "/" || path == "/wall_localization.html") + { + const auto html = readTextFile(preview_dir_ + "/wall_localization.html"); + html.empty() ? sendNotFound(fd) : sendText(fd, "text/html; charset=utf-8", html); + } + else if (path == "/wall_fit.json") + { + wall_fit_ = origincar_wall::loadWallFitConfig(config_path_); + sendText(fd, "application/json; charset=utf-8", origincar_wall::wallFitConfigToJson(wall_fit_)); + } + else if (path == "/map_reference.png") + { + const auto bytes = readBinaryFile(preview_dir_ + "/map_reference.png"); + bytes.empty() ? sendNotFound(fd) : sendBytes(fd, "image/png", bytes); + } + else if (path == "/events") + { + sendEvents(fd); + } + else + { + sendNotFound(fd); + } + close(fd); + } + + void serve() + { + server_fd_ = socket(AF_INET, SOCK_STREAM, 0); + if (server_fd_ < 0) + { + RCLCPP_ERROR(get_logger(), "Failed to create preview socket"); + return; + } + int opt = 1; + setsockopt(server_fd_, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)); + sockaddr_in address{}; + address.sin_family = AF_INET; + address.sin_port = htons(static_cast(port_)); + address.sin_addr.s_addr = host_ == "0.0.0.0" ? INADDR_ANY : inet_addr(host_.c_str()); + if (bind(server_fd_, reinterpret_cast(&address), sizeof(address)) < 0) + { + RCLCPP_ERROR(get_logger(), "Failed to bind preview port %d", port_); + return; + } + if (listen(server_fd_, 8) < 0) + { + RCLCPP_ERROR(get_logger(), "Failed to listen on preview port %d", port_); + return; + } + while (running_ && rclcpp::ok()) + { + const auto client_fd = accept(server_fd_, nullptr, nullptr); + if (client_fd < 0) + { + continue; + } + std::thread([this, client_fd]() { handleClient(client_fd); }).detach(); + } + } + + std::string host_; + int port_{8772}; + std::string config_path_; + std::string preview_dir_; + int scan_stride_{3}; + int preview_period_ms_{125}; + int server_fd_{-1}; + std::atomic running_{true}; + std::thread server_thread_; + std::mutex mutex_; + origincar_wall::WallFitConfig wall_fit_; + nlohmann::json combined_pose_; + bool has_combined_pose_{false}; + std::vector latest_scan_map_; + rclcpp::Subscription::SharedPtr combined_odom_sub_; + rclcpp::Subscription::SharedPtr scan_sub_; +}; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/origincar_base/test/__pycache__/test_wall_fit_core.cpython-310.pyc b/src/origincar_base/test/__pycache__/test_wall_fit_core.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..0e7e0df8f43ac93eea032390fa21790baed4cf74 GIT binary patch literal 4644 zcmbtYO>7&-72erhE|(NZOBC(cj*}X7>V`#XB~Drgv18J;<n%CT9v0jjeFU?-Q@aENd=lbmB+3T}& z7iXPo-@7t5e?v}=N^V}bdeymnWnMnh4ie{{8-|V-_1ZzlX^W`qcy2!#2tStPFmicX zj*7AxMxGl6tA1K7_S~MVcHO?ykAhwjJ4xgy-Lby#dEh0`$64KoqhIcai7RWdd)FU# z>bObN4LmuaX0Z?rdOY@MNjo1MnI_)&9E7jY24iy>*Wb}v1~<5gFE)%@_~v+?+xS|% zz>E0i`2;WFYxD9AtzMCnH?iEyL2@w?{=6S2vB%Kq=%KWQ_afdngcxcYTEaLxf}_NX zv3Tyj{y=A=5-TggsC!BZSxFHy*xja-m6Um9RKhjP?!cU6*Q9|h-TKq_fBo#ck3MW{ z760(_{_DlxHa2z1mL*$}rNoVIJHiirH}*T2BA#lq^Xc=q-uunp9yesZ@5Zs)85Nn$ z&hLLmkJ*%#7Y4n++iI)FYIf?>=J`squFDCu@olTYA`OB^TsV}r1w zbO)fD+@qU1qN%>B=xbbi!$c3$s9htZDQp~^+rsqleA37cU2CR^rr+E&L={B>Or^mHeB;B_Uu8Xg;J>e$XF&OD<6|-boWIo%UC)nI zx1AI;|89scjr;sj`+Yf;8G8`*oH!Z?&zHq*Yp>^|*#$4jyl{Eo_F_>$YdMvI?Lyd% z;tX*7Ld8||c{hj#aS9)zksUhKyV6a00|68z&bF$MO~>a7wW)ZTI(rnA|ZeAwQ-~tTOtQ^qO7+3~Ow4c7GUt!TxuFk*tokM}ct(i3HTM&HzDxGNm4F z-B$#M9OzBuCXm(@)Tpe2oXrmaX2^BSA>&i$G=eAk(LrX$Q0G;&okpG7eRBuAq)C=1 zp24^o(tw}i$gSp2oc7!oPkVHjJ=mmVu4D_6c{1lkVI*V@*1wo!u>9m<2CrHZE!*ug z?H7G%Ekp^3zVox6(zDq>$xH1sXBJ+OMiTY6Qif6u_%~0VIsK~4kw1t>Kz+ECHGH^f z%G|Qk4SMS1F3a3Xn#0FkuA9;%&m+h^iwP16S4N5}EA?_T8}c)I5%P6~5E|G?E5H-> z9Dh0Sd)$czNgQxrF;mb5zn(~xrs;{2k9~w{E@h(gd^^G0UV`MrS z;o*|@b`|HzHsF?Uq6SYU_BEi~RJ>Pj*Js&l)#9ta2Z#^DqjAjN!d*u5{%?x6bO>v4) zF{O4ALepx^43+;J5$iujiQ`T2ELBY533WR3-y2{?74k?ux-CuOqLhD+qxL3~ITh(B z+DzFpMem;*bz53)zYjc#ub`?R3<)}7ktac@mBo}B=f%JmUxlJ>r{FDaM_Osm#Q3t1 zpq>h}WMxc!Y2H-9n1w)Y5GMO${j&ib0zpu!hiGnj#c_dqcNw@45q_Zrl!avq<1pK=i7#^gv48#fWRn)t07OzYV%fj{m6{Pg<)Ugb96 zkN{}|uL?vmT@HD=Rl$@g(&okVzNH{MF}GS)eiUmwwG>A^Iq5}_;6cw#=&DK4iIrYP zWQBHh`%WPz$48m%z9?fkDGtZMvchydFKqy+-wr|qp7TgW!WAo*0%FEUtO&Z03L>Wz zU>2oRpkZbJqt*@&1e^m!zZPAjKO2&MO<`ug`>-P@LU9ri^kawCc?(0tWHLo?ZV@mz z_PAz!nXR7P9|BL*=N$emupNg&I>(Vk$LU6V5K_A2ICKw8OT;l+-U}oyk+@FcZ4w<4 z6e8~s6l;qT1R0W=v8~z6w(OE!wx>&nR2dE1b%UPc)TdKVxPaQy9`u5Q_Jl66$zm8R zsGXJvV-FjV#HU6_PhQGbCZ8UFvUPO|c^Y1MREnh2Ga^qS?RoTj5tman0kKFrTup-B jj=8I)sLO(VF5Qds^oKx9ETvX~y?;TkmhJfxGyn5ng~ac& literal 0 HcmV?d00001 diff --git a/src/origincar_base/test/test_wall_fit_core.py b/src/origincar_base/test/test_wall_fit_core.py new file mode 100644 index 0000000..ffa25f7 --- /dev/null +++ b/src/origincar_base/test/test_wall_fit_core.py @@ -0,0 +1,104 @@ +import math +import tempfile +import unittest +from pathlib import Path + +from scripts.wall_fit_core import ( + DEFAULT_REFERENCE_POINTS, + DEFAULT_WALL_FIT, + fit_wall_config_from_captures, + load_wall_fit, + localize_from_scan, + map_points_to_scan, + predict_pose_from_odom_delta, + save_wall_fit_atomic, + wall_bounds, +) + + +class WallFitCoreTests(unittest.TestCase): + def test_reference_points_use_lower_left_map_frame(self): + self.assertEqual(DEFAULT_REFERENCE_POINTS["P"]["map"], {"x": 0.56, "y": 0.18}) + self.assertEqual(DEFAULT_REFERENCE_POINTS["task_release"]["map"], {"x": 4.5, "y": 1.81}) + self.assertEqual(DEFAULT_REFERENCE_POINTS["passage"]["map"], {"x": 2.5, "y": 2.6}) + self.assertEqual(DEFAULT_REFERENCE_POINTS["clinic"]["map"], {"x": 2.8, "y": 3.77}) + + def test_predict_pose_from_odom_delta_advances_wall_pose(self): + previous_wall_pose = {"x": 1.0, "y": 2.0, "theta": math.radians(170.0)} + previous_odom_pose = {"x": 0.56, "y": 0.18, "theta": math.radians(175.0)} + current_odom_pose = {"x": 0.86, "y": 0.08, "theta": math.radians(-175.0)} + + predicted = predict_pose_from_odom_delta(previous_wall_pose, previous_odom_pose, current_odom_pose) + + self.assertAlmostEqual(predicted["x"], 1.3, places=6) + self.assertAlmostEqual(predicted["y"], 1.9, places=6) + self.assertAlmostEqual(predicted["theta"], math.radians(-180.0), places=6) + self.assertEqual(predicted["prediction_source"], "odom_delta") + + def test_wall_bounds_can_extend_outside_map_image(self): + config = dict(DEFAULT_WALL_FIT) + config["walls"] = [ + {"name": "left", "a": 1.0, "b": 0.0, "c": 0.08, "color": "#2ca02c"}, + {"name": "right", "a": -1.0, "b": 0.0, "c": 5.12, "color": "#ff7f0e"}, + {"name": "bottom", "a": 0.0, "b": 1.0, "c": 0.05, "color": "#1f77b4"}, + {"name": "top", "a": 0.0, "b": -1.0, "c": 5.2, "color": "#d62728"}, + ] + + bounds = wall_bounds(config) + + self.assertEqual(bounds, {"x_min": -0.08, "x_max": 5.12, "y_min": -0.05, "y_max": 5.2}) + + def test_localize_from_scan_corrects_xy_and_theta(self): + map_points = [] + for y in [0.6, 1.2, 1.8, 2.4, 3.0, 3.6, 4.2]: + map_points.append({"x": 0.0, "y": y}) + map_points.append({"x": 5.0, "y": y}) + for x in [0.6, 1.2, 1.8, 2.4, 3.0, 3.6, 4.2]: + map_points.append({"x": x, "y": 0.0}) + map_points.append({"x": x, "y": 5.0}) + true_pose = {"x": 1.6, "y": 1.5, "theta": math.radians(8.0)} + predicted_pose = {"x": 1.55, "y": 1.45, "theta": 0.0} + scan = {"points": map_points_to_scan(map_points, true_pose)} + + result = localize_from_scan(scan, DEFAULT_WALL_FIT, predicted_pose) + + self.assertTrue(result["ok"]) + self.assertAlmostEqual(result["pose"]["x"], true_pose["x"], delta=0.08) + self.assertAlmostEqual(result["pose"]["y"], true_pose["y"], delta=0.08) + self.assertAlmostEqual(result["pose"]["theta"], true_pose["theta"], delta=math.radians(1.1)) + + def test_fit_wall_config_from_captures_uses_outer_points_beyond_map_edges(self): + captures = [] + true_bounds = {"x_min": -0.08, "x_max": 5.12, "y_min": -0.05, "y_max": 5.2} + map_points = [] + for y in [0.7, 1.4, 2.1, 2.8, 3.5, 4.2]: + map_points.append({"x": true_bounds["x_min"], "y": y}) + map_points.append({"x": true_bounds["x_max"], "y": y}) + for x in [0.7, 1.4, 2.1, 2.8, 3.5, 4.2, 4.9]: + map_points.append({"x": x, "y": true_bounds["y_min"]}) + map_points.append({"x": x, "y": true_bounds["y_max"]}) + for key, reference in DEFAULT_REFERENCE_POINTS.items(): + pose = {"x": reference["map"]["x"], "y": reference["map"]["y"], "theta": 0.0} + captures.append({"key": key, "map": reference["map"], "odom_pose": pose, "scan": {"points": map_points_to_scan(map_points, pose)}}) + + config = fit_wall_config_from_captures(captures, DEFAULT_WALL_FIT) + bounds = wall_bounds(config) + + self.assertAlmostEqual(bounds["x_min"], true_bounds["x_min"], delta=0.04) + self.assertAlmostEqual(bounds["x_max"], true_bounds["x_max"], delta=0.04) + self.assertAlmostEqual(bounds["y_min"], true_bounds["y_min"], delta=0.04) + self.assertAlmostEqual(bounds["y_max"], true_bounds["y_max"], delta=0.04) + + def test_save_wall_fit_atomic_round_trips_json(self): + with tempfile.TemporaryDirectory() as temp_dir: + path = Path(temp_dir) / "wall_fit.json" + save_wall_fit_atomic(path, DEFAULT_WALL_FIT) + + loaded = load_wall_fit(path) + + self.assertEqual(loaded["coordinate_frame"]["origin"], "map_lower_left") + self.assertEqual(loaded["reference_points"]["P"]["map"], {"x": 0.56, "y": 0.18}) + + +if __name__ == "__main__": + unittest.main() diff --git a/src/origincar_base/test/wall_fit_core_test.cpp b/src/origincar_base/test/wall_fit_core_test.cpp new file mode 100644 index 0000000..dcdf0d8 --- /dev/null +++ b/src/origincar_base/test/wall_fit_core_test.cpp @@ -0,0 +1,164 @@ +#include "origincar_base/wall_fit_core.hpp" + +#include +#include +#include +#include +#include +#include + +namespace +{ +constexpr double kPi = 3.14159265358979323846; + +void require(bool condition, const std::string &message) +{ + if (!condition) + { + throw std::runtime_error(message); + } +} + +void requireNear(double actual, double expected, double tolerance, const std::string &message) +{ + if (std::fabs(actual - expected) > tolerance) + { + throw std::runtime_error(message + " actual=" + std::to_string(actual) + + " expected=" + std::to_string(expected)); + } +} + +std::vector makeWallPoints(const origincar_wall::WallBounds &bounds) +{ + std::vector points; + for (double y : {0.6, 1.2, 1.8, 2.4, 3.0, 3.6, 4.2}) + { + points.push_back({bounds.x_min, y}); + points.push_back({bounds.x_max, y}); + } + for (double x : {0.6, 1.2, 1.8, 2.4, 3.0, 3.6, 4.2}) + { + points.push_back({x, bounds.y_min}); + points.push_back({x, bounds.y_max}); + } + return points; +} + +void testReferencePointsUseLowerLeftMapFrame() +{ + const auto config = origincar_wall::defaultWallFitConfig(); + requireNear(config.reference_points.at("P").map.x, 0.56, 1e-9, "P x"); + requireNear(config.reference_points.at("P").map.y, 0.18, 1e-9, "P y"); + requireNear(config.reference_points.at("task_release").map.x, 4.5, 1e-9, "task x"); + requireNear(config.reference_points.at("task_release").map.y, 1.81, 1e-9, "task y"); + requireNear(config.reference_points.at("passage").map.x, 2.5, 1e-9, "passage x"); + requireNear(config.reference_points.at("passage").map.y, 2.6, 1e-9, "passage y"); + requireNear(config.reference_points.at("clinic").map.x, 2.8, 1e-9, "clinic x"); + requireNear(config.reference_points.at("clinic").map.y, 3.77, 1e-9, "clinic y"); +} + +void testPredictPoseFromOdomDeltaAdvancesWallPose() +{ + const origincar_wall::Pose2D previous_wall_pose{1.0, 2.0, 170.0 * kPi / 180.0}; + const origincar_wall::Pose2D previous_odom_pose{0.56, 0.18, 175.0 * kPi / 180.0}; + const origincar_wall::Pose2D current_odom_pose{0.86, 0.08, -175.0 * kPi / 180.0}; + + const auto predicted = + origincar_wall::predictPoseFromOdomDelta(previous_wall_pose, previous_odom_pose, current_odom_pose); + + requireNear(predicted.x, 1.3, 1e-9, "predicted x"); + requireNear(predicted.y, 1.9, 1e-9, "predicted y"); + requireNear(predicted.theta, -kPi, 1e-9, "predicted theta"); +} + +void testWallBoundsCanExtendOutsideMapImage() +{ + auto config = origincar_wall::defaultWallFitConfig(); + config.walls = { + {"left", 1.0, 0.0, 0.08, "#2ca02c"}, + {"right", -1.0, 0.0, 5.12, "#ff7f0e"}, + {"bottom", 0.0, 1.0, 0.05, "#1f77b4"}, + {"top", 0.0, -1.0, 5.2, "#d62728"}, + }; + + const auto bounds = origincar_wall::wallBounds(config); + + requireNear(bounds.x_min, -0.08, 1e-9, "x_min"); + requireNear(bounds.x_max, 5.12, 1e-9, "x_max"); + requireNear(bounds.y_min, -0.05, 1e-9, "y_min"); + requireNear(bounds.y_max, 5.2, 1e-9, "y_max"); +} + +void testLocalizeFromScanCorrectsXYAndTheta() +{ + const auto config = origincar_wall::defaultWallFitConfig(); + const auto points = makeWallPoints({0.0, 5.0, 0.0, 5.0}); + const origincar_wall::Pose2D true_pose{1.6, 1.5, 8.0 * kPi / 180.0}; + const origincar_wall::Pose2D predicted_pose{1.55, 1.45, 0.0}; + const auto scan = origincar_wall::mapPointsToScan(points, true_pose); + + const auto result = origincar_wall::localizeFromScan(scan, config, predicted_pose); + + require(result.ok, "localization should be ok"); + requireNear(result.pose.x, true_pose.x, 0.08, "localized x"); + requireNear(result.pose.y, true_pose.y, 0.08, "localized y"); + requireNear(result.pose.theta, true_pose.theta, 1.1 * kPi / 180.0, "localized theta"); +} + +void testFitWallConfigFromCapturesUsesOuterPointsBeyondMapEdges() +{ + const auto base_config = origincar_wall::defaultWallFitConfig(); + const origincar_wall::WallBounds true_bounds{-0.08, 5.12, -0.05, 5.2}; + const auto points = makeWallPoints(true_bounds); + std::vector captures; + for (const auto &entry : base_config.reference_points) + { + origincar_wall::WallFitCapture capture; + capture.key = entry.first; + capture.map = entry.second.map; + capture.odom_pose = {entry.second.map.x, entry.second.map.y, 0.0}; + capture.scan_points = origincar_wall::mapPointsToScan(points, capture.odom_pose); + captures.push_back(capture); + } + + const auto config = origincar_wall::fitWallConfigFromCaptures(captures, base_config); + const auto bounds = origincar_wall::wallBounds(config); + + requireNear(bounds.x_min, true_bounds.x_min, 0.04, "fit x_min"); + requireNear(bounds.x_max, true_bounds.x_max, 0.04, "fit x_max"); + requireNear(bounds.y_min, true_bounds.y_min, 0.04, "fit y_min"); + requireNear(bounds.y_max, true_bounds.y_max, 0.04, "fit y_max"); +} + +void testSaveWallFitAtomicRoundTripsJson() +{ + const std::string path = "/tmp/origincar_wall_fit_core_test.json"; + const auto config = origincar_wall::defaultWallFitConfig(); + origincar_wall::saveWallFitConfig(path, config); + const auto loaded = origincar_wall::loadWallFitConfig(path); + std::remove(path.c_str()); + + require(loaded.coordinate_frame.origin == "map_lower_left", "loaded origin"); + requireNear(loaded.reference_points.at("P").map.x, 0.56, 1e-9, "loaded P x"); + requireNear(loaded.reference_points.at("P").map.y, 0.18, 1e-9, "loaded P y"); +} +} // namespace + +int main() +{ + try + { + testReferencePointsUseLowerLeftMapFrame(); + testPredictPoseFromOdomDeltaAdvancesWallPose(); + testWallBoundsCanExtendOutsideMapImage(); + testLocalizeFromScanCorrectsXYAndTheta(); + testFitWallConfigFromCapturesUsesOuterPointsBeyondMapEdges(); + testSaveWallFitAtomicRoundTripsJson(); + } + catch (const std::exception &error) + { + std::cerr << "wall_fit_core_test failed: " << error.what() << std::endl; + return 1; + } + return 0; +} diff --git a/src/origincar_base/test/wall_kalman_filter_test.cpp b/src/origincar_base/test/wall_kalman_filter_test.cpp new file mode 100644 index 0000000..8dc0a57 --- /dev/null +++ b/src/origincar_base/test/wall_kalman_filter_test.cpp @@ -0,0 +1,79 @@ +#include "origincar_base/wall_kalman_filter.hpp" + +#include +#include +#include +#include + +namespace +{ +constexpr double kPi = 3.14159265358979323846; + +void requireNear(double actual, double expected, double tolerance, const std::string &message) +{ + if (std::fabs(actual - expected) > tolerance) + { + throw std::runtime_error(message + " actual=" + std::to_string(actual) + + " expected=" + std::to_string(expected)); + } +} + +void require(bool condition, const std::string &message) +{ + if (!condition) + { + throw std::runtime_error(message); + } +} + +void testPredictUsesWheelVelocityAndImuYawRate() +{ + origincar_wall::WallKalmanFilter filter({0.56, 0.18, 0.0}); + + filter.predict(1.0, 0.0, 0.2, 0.5); + const auto pose = filter.pose(); + + requireNear(pose.x, 1.06, 1e-9, "predicted x"); + requireNear(pose.y, 0.18, 1e-9, "predicted y"); + requireNear(pose.theta, 0.1, 1e-9, "predicted theta"); +} + +void testScanUpdateTrustsLowNoiseMoreThanHighNoise() +{ + origincar_wall::WallKalmanFilter weak_filter({0.0, 0.0, 0.0}); + origincar_wall::WallKalmanFilter strong_filter({0.0, 0.0, 0.0}); + weak_filter.predict(1.0, 0.0, 0.0, 1.0); + strong_filter.predict(1.0, 0.0, 0.0, 1.0); + + weak_filter.correct({2.0, 0.0, 0.0}, {1.0, 1.0, 1.0}); + strong_filter.correct({2.0, 0.0, 0.0}, {0.01, 0.01, 0.01}); + + require(strong_filter.pose().x > weak_filter.pose().x, "low scan noise should pull x closer to scan"); + requireNear(strong_filter.pose().x, 2.0, 0.08, "strong scan update x"); +} + +void testYawCorrectionUsesWrappedInnovation() +{ + origincar_wall::WallKalmanFilter filter({0.0, 0.0, 179.0 * kPi / 180.0}); + + filter.correct({0.0, 0.0, -179.0 * kPi / 180.0}, {0.01, 0.01, 0.01}); + + require(std::fabs(filter.pose().theta) > 175.0 * kPi / 180.0, "yaw should stay near wrap boundary"); +} +} // namespace + +int main() +{ + try + { + testPredictUsesWheelVelocityAndImuYawRate(); + testScanUpdateTrustsLowNoiseMoreThanHighNoise(); + testYawCorrectionUsesWrappedInnovation(); + } + catch (const std::exception &error) + { + std::cerr << "wall_kalman_filter_test failed: " << error.what() << std::endl; + return 1; + } + return 0; +}