Initial commit

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Models/Mapping/PoseExtrapolatorOptions.cs

@@ -0,0 +1,111 @@
+/*
+ * Copyright 2018 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+using System.Text.Json.Serialization;
+
+namespace CartographerSharp.Models.Mapping;
+
+/// <summary>
+/// Protocol buffer representation of pose extrapolator options.
+/// </summary>
+public struct PoseExtrapolatorOptions
+{
+    [JsonPropertyName("use_imu_based")]
+    public bool UseImuBased { get; set; }
+
+    [JsonPropertyName("constant_velocity")]
+    public ConstantVelocityPoseExtrapolatorOptions ConstantVelocity { get; set; }
+
+    [JsonPropertyName("imu_based")]
+    [JsonIgnore(Condition = JsonIgnoreCondition.WhenWritingNull)]
+    public ImuBasedPoseExtrapolatorOptions? ImuBased { get; set; }
+
+    /// <summary>
+    /// Velocity threshold for detecting robot movement (m/s).
+    /// Robot is considered "moving" if horizontal velocity exceeds this threshold.
+    /// Default: 0.003 m/s (3 mm/s)
+    /// </summary>
+    [JsonPropertyName("velocity_threshold")]
+    public double? VelocityThreshold { get; set; }
+
+    /// <summary>
+    /// Acceleration threshold for detecting IMU acceleration (m/s²).
+    /// IMU is considered "accelerating" if horizontal acceleration exceeds this threshold.
+    /// Default: 0.4 m/s²
+    /// </summary>
+    [JsonPropertyName("acceleration_threshold")]
+    public double? AccelerationThreshold { get; set; }
+
+    /// <summary>
+    /// Gravity deviation threshold for detecting vertical acceleration changes (m/s²).
+    /// Used to detect changes in vertical acceleration (gravity deviation).
+    /// Default: 0.1 m/s²
+    /// </summary>
+    [JsonPropertyName("gravity_deviation_threshold")]
+    public double? GravityDeviationThreshold { get; set; }
+
+    /// <summary>
+    /// If true, use odometry pose directly at the time of range data instead of calculating velocity.
+    /// This is useful when odometry data is accurate and reliable.
+    /// When enabled, ExtrapolatePose will use the latest odometry data at or before the requested time,
+    /// instead of extrapolating from the last pose using calculated velocity.
+    /// Default: false (use velocity-based extrapolation)
+    /// </summary>
+    [JsonPropertyName("use_odometry_directly")]
+    public bool UseOdometryDirectly { get; set; }
+
+    public PoseExtrapolatorOptions(
+        bool useImuBased = false,
+        ConstantVelocityPoseExtrapolatorOptions? constantVelocity = null,
+        ImuBasedPoseExtrapolatorOptions? imuBased = null,
+        double? velocityThreshold = null,
+        double? accelerationThreshold = null,
+        double? gravityDeviationThreshold = null,
+        bool useOdometryDirectly = false)
+    {
+        UseImuBased = useImuBased;
+        ConstantVelocity = constantVelocity ?? new ConstantVelocityPoseExtrapolatorOptions();
+        ImuBased = imuBased;
+        VelocityThreshold = velocityThreshold;
+        AccelerationThreshold = accelerationThreshold;
+        GravityDeviationThreshold = gravityDeviationThreshold;
+        UseOdometryDirectly = useOdometryDirectly;
+    }
+}
+
+/// <summary>
+/// Constant velocity pose extrapolator options.
+/// </summary>
+public struct ConstantVelocityPoseExtrapolatorOptions
+{
+    /// <summary>
+    /// Time constant in seconds for the orientation moving average based on
+    /// observed gravity via the IMU.
+    /// </summary>
+    [JsonPropertyName("imu_gravity_time_constant")]
+    public double ImuGravityTimeConstant { get; set; }
+
+    [JsonPropertyName("pose_queue_duration")]
+    public double PoseQueueDuration { get; set; }
+
+    public ConstantVelocityPoseExtrapolatorOptions(
+        double imuGravityTimeConstant = 10.0,
+        double poseQueueDuration = 0.001)
+    {
+        ImuGravityTimeConstant = imuGravityTimeConstant;
+        PoseQueueDuration = poseQueueDuration;
+    }
+}