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 0000000..ebaa822 Binary files /dev/null and b/src/origincar_base/preview/map_reference.png differ diff --git a/src/origincar_base/preview/wall_localization.html b/src/origincar_base/preview/wall_localization.html new file mode 100644 index 0000000..ba8cb11 --- /dev/null +++ b/src/origincar_base/preview/wall_localization.html @@ -0,0 +1,275 @@ + + + + + + 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 0000000..0e7e0df Binary files /dev/null and b/src/origincar_base/test/__pycache__/test_wall_fit_core.cpython-310.pyc differ 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; +}