347 lines
15 KiB
C#
347 lines
15 KiB
C#
/*
|
|
* Copyright 2016 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.Threading.Channels;
|
|
using CartographerSharp.Models.Mapping;
|
|
using CartographerSharp.Sensor;
|
|
using CartographerSharp.Transform;
|
|
using RobotNet10.Shared.Numbers;
|
|
|
|
namespace CartographerSharp.Mapping.D2D;
|
|
|
|
/// <summary>
|
|
/// The first active submap will be created on the insertion of the first range
|
|
/// data. Except during this initialization when no or only one single submap
|
|
/// exists, there are always two submaps into which range data is inserted: an
|
|
/// old submap that is used for matching, and a new one, which will be used for
|
|
/// matching next, that is being initialized.
|
|
///
|
|
/// Once a certain number of range data have been inserted, the new submap is
|
|
/// considered initialized: the old submap is no longer changed, the "new" submap
|
|
/// is now the "old" submap and is used for scan-to-map matching. Moreover, a
|
|
/// "new" submap gets created. The "old" submap is forgotten by this object.
|
|
///
|
|
/// The front (old) submap is always inserted synchronously since it is used for
|
|
/// scan matching immediately. The back (new) submap is inserted asynchronously
|
|
/// via a background worker to reduce per-frame blocking latency.
|
|
/// </summary>
|
|
public class ActiveSubmaps2D(SubmapsOptions2D options) : IDisposable
|
|
{
|
|
private const int kInitialSubmapSize = 100;
|
|
|
|
private readonly SubmapsOptions2D _options = options;
|
|
private readonly List<Submap2D> _submaps = [];
|
|
private readonly ValueConversionTables _conversionTables = new();
|
|
private IRangeDataInserter? _rangeDataInserter;
|
|
private readonly Lock _insertLock = new(); // Lock to prevent concurrent inserts
|
|
|
|
// Async back submap insertion infrastructure.
|
|
// The back submap is queued to a Channel and processed by a single long-running
|
|
// worker task, preserving insertion order (SingleReader). The front submap is
|
|
// always inserted synchronously for immediate scan matching availability.
|
|
private readonly Channel<(RangeData rangeData, Submap2D submap, IRangeDataInserter inserter)>
|
|
_backSubmapChannel = Channel.CreateUnbounded<(RangeData, Submap2D, IRangeDataInserter)>(
|
|
new UnboundedChannelOptions { SingleReader = true });
|
|
private Task? _backSubmapWorker;
|
|
private int _backSubmapExpectedCount; // Tracks intended NumRangeData of back submap (including queued)
|
|
private int _backSubmapPendingCount; // Items queued but not yet processed (Interlocked)
|
|
|
|
/// <summary>
|
|
/// Inserts 'range_data' into the Submap collection.
|
|
/// Front submap: synchronous (used for scan matching immediately).
|
|
/// Back submap: queued to background worker (fire-and-forget).
|
|
/// </summary>
|
|
public List<Submap2D> InsertRangeData(RangeData rangeData)
|
|
{
|
|
lock (_insertLock)
|
|
{
|
|
var submapOrigin = new Vector2(rangeData.Origin.X, rangeData.Origin.Y);
|
|
var submapPose = new Rigid3d(
|
|
new Vector3(submapOrigin.X, submapOrigin.Y, 0.0),
|
|
Quaternion.Identity);
|
|
|
|
// Use _backSubmapExpectedCount instead of back submap's NumRangeData
|
|
// because the back submap's actual count may lag (async insertions).
|
|
if (_submaps.Count == 0 ||
|
|
(_submaps.Count > 0 && _backSubmapExpectedCount == _options.NumRangeData))
|
|
{
|
|
// Drain all pending back submap insertions before submap rotation
|
|
// so the back submap is fully up-to-date when it becomes the front.
|
|
DrainBackSubmapQueue();
|
|
AddSubmap(submapPose);
|
|
_backSubmapExpectedCount = 0;
|
|
}
|
|
|
|
_rangeDataInserter ??= CreateRangeDataInserter();
|
|
|
|
if (_submaps.Count >= 2)
|
|
{
|
|
// Front submap: SYNCHRONOUS (used for scan matching immediately)
|
|
_submaps[0].InsertRangeData(rangeData, _rangeDataInserter);
|
|
|
|
// Back submap: ASYNC (queue to background worker)
|
|
// Increment pending BEFORE writing to channel to ensure drain correctness.
|
|
Interlocked.Increment(ref _backSubmapPendingCount);
|
|
_backSubmapChannel.Writer.TryWrite((rangeData, _submaps[1], _rangeDataInserter));
|
|
_backSubmapWorker ??= Task.Factory.StartNew(
|
|
ProcessBackSubmapQueue, TaskCreationOptions.LongRunning);
|
|
}
|
|
else
|
|
{
|
|
// Only 1 submap - insert synchronously
|
|
for (int si = 0; si < _submaps.Count; si++)
|
|
{
|
|
_submaps[si].InsertRangeData(rangeData, _rangeDataInserter);
|
|
}
|
|
}
|
|
_backSubmapExpectedCount++;
|
|
|
|
// Finish front submap when it reaches 2x threshold.
|
|
// Front is always up-to-date (synchronous insert).
|
|
if (_submaps.Count > 0 && _submaps[0].NumRangeData == _options.NumRangeData * 2)
|
|
{
|
|
_submaps[0].Finish();
|
|
}
|
|
|
|
return [.. _submaps];
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Forces the current front submap to finish, creates a new submap at the range data origin,
|
|
/// and inserts range data into all active submaps. Used to break the deadlock when the robot
|
|
/// enters genuinely new territory and consecutive hard-limit Ceres failures accumulate.
|
|
/// </summary>
|
|
public List<Submap2D> ForceNewSubmapAndInsert(RangeData rangeData)
|
|
{
|
|
lock (_insertLock)
|
|
{
|
|
// Drain any pending back submap insertions before manipulating submaps.
|
|
DrainBackSubmapQueue();
|
|
|
|
var submapOrigin = new Vector2(rangeData.Origin.X, rangeData.Origin.Y);
|
|
var submapPose = new Rigid3d(
|
|
new Vector3(submapOrigin.X, submapOrigin.Y, 0.0),
|
|
Quaternion.Identity);
|
|
|
|
if (_submaps.Count == 0)
|
|
{
|
|
// No submaps yet - just create the first one via normal flow
|
|
AddSubmap(submapPose);
|
|
}
|
|
else if (_submaps.Count == 1)
|
|
{
|
|
// FIX: When only 1 submap exists, finish it and REMOVE it before creating the new one.
|
|
// Previously, finishing + AddSubmap would leave [finished, new] and the subsequent
|
|
// InsertRangeData loop would crash on the finished front submap.
|
|
if (!_submaps[0].InsertionFinished)
|
|
{
|
|
_submaps[0].Finish();
|
|
}
|
|
_submaps.RemoveAt(0);
|
|
AddSubmap(submapPose);
|
|
}
|
|
else
|
|
{
|
|
// 2 submaps: finish front if needed, then AddSubmap removes it and creates new
|
|
if (!_submaps[0].InsertionFinished)
|
|
{
|
|
_submaps[0].Finish();
|
|
}
|
|
AddSubmap(submapPose);
|
|
}
|
|
|
|
// Reset expected count after submap manipulation.
|
|
_backSubmapExpectedCount = 0;
|
|
|
|
// Insert range data into all active submaps sequentially.
|
|
// ForceNewSubmap is a rare recovery path (consecutive Ceres failures),
|
|
// so sequential insert is simpler and avoids thread-safety risks.
|
|
_rangeDataInserter ??= CreateRangeDataInserter();
|
|
|
|
for (int si = 0; si < _submaps.Count; si++)
|
|
{
|
|
_submaps[si].InsertRangeData(rangeData, _rangeDataInserter);
|
|
}
|
|
_backSubmapExpectedCount++;
|
|
|
|
return [.. _submaps];
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets the current active submaps.
|
|
/// </summary>
|
|
public List<Submap2D> Submaps()
|
|
{
|
|
return [.. _submaps];
|
|
}
|
|
|
|
/// <summary>
|
|
/// Background worker that sequentially processes queued back submap insertions.
|
|
/// Uses per-item error handling for resilience - a single failed insertion
|
|
/// should not crash the entire SLAM pipeline.
|
|
/// </summary>
|
|
private async Task ProcessBackSubmapQueue()
|
|
{
|
|
await foreach (var (rangeData, submap, inserter) in _backSubmapChannel.Reader.ReadAllAsync())
|
|
{
|
|
try
|
|
{
|
|
submap.InsertRangeData(rangeData, inserter);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Console.WriteLine($"[SUBMAP_ASYNC] Back submap insert error: {ex.Message}");
|
|
}
|
|
Interlocked.Decrement(ref _backSubmapPendingCount);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Waits for all pending back submap insertions to complete.
|
|
/// Called before submap rotation (AddSubmap) to ensure the back submap is
|
|
/// fully up-to-date before it becomes the front submap used for scan matching.
|
|
/// Called infrequently (every NumRangeData frames, typically ~90).
|
|
/// </summary>
|
|
private void DrainBackSubmapQueue()
|
|
{
|
|
if (Volatile.Read(ref _backSubmapPendingCount) == 0) return;
|
|
var sw = new SpinWait();
|
|
while (Volatile.Read(ref _backSubmapPendingCount) > 0)
|
|
{
|
|
sw.SpinOnce();
|
|
}
|
|
}
|
|
|
|
public void Dispose()
|
|
{
|
|
_backSubmapChannel.Writer.Complete();
|
|
_backSubmapWorker?.GetAwaiter().GetResult();
|
|
}
|
|
|
|
private IRangeDataInserter CreateRangeDataInserter()
|
|
{
|
|
// Match C++ logic: switch case with LOG(FATAL) for unknown types
|
|
var options = _options.RangeDataInserterOptions;
|
|
|
|
switch (options.RangeDataInserterTypeValue)
|
|
{
|
|
case RangeDataInserterOptions.RangeDataInserterType.ProbabilityGridInserter2D:
|
|
if (options.ProbabilityGridRangeDataInserterOptions2D.HasValue)
|
|
{
|
|
return new ProbabilityGridRangeDataInserter2D(options.ProbabilityGridRangeDataInserterOptions2D.Value);
|
|
}
|
|
throw new ArgumentException("ProbabilityGridRangeDataInserterOptions2D is required for ProbabilityGrid inserter");
|
|
|
|
case RangeDataInserterOptions.RangeDataInserterType.TsdfInserter2D:
|
|
if (options.TsdfRangeDataInserterOptions2D.HasValue)
|
|
{
|
|
return new TSDFRangeDataInserter2D(options.TsdfRangeDataInserterOptions2D.Value);
|
|
}
|
|
throw new ArgumentException("TSDFRangeDataInserterOptions2D is required for TSDF inserter");
|
|
|
|
default:
|
|
throw new ArgumentException($"Unknown RangeDataInserterType: {options.RangeDataInserterTypeValue}");
|
|
}
|
|
}
|
|
|
|
private Grid2D CreateGrid(Vector2 origin)
|
|
{
|
|
return CreateGridWithOptions(origin, _options.GridOptions2D);
|
|
}
|
|
|
|
private Grid2D? CreateHighResGrid(Vector2 origin)
|
|
{
|
|
if (_options.HighResGridOptions2D == null)
|
|
return null;
|
|
return CreateGridWithOptions(origin, _options.HighResGridOptions2D.Value);
|
|
}
|
|
|
|
private Grid2D CreateGridWithOptions(Vector2 origin, GridOptions2D gridOptions)
|
|
{
|
|
var resolution = gridOptions.Resolution;
|
|
// Calculate initial size to cover ±18m (matching MaxRange) at this resolution
|
|
var initialSize = Math.Max(100, (int)(kInitialSubmapSize * _options.GridOptions2D.Resolution / resolution));
|
|
var mapLimits = new MapLimits(
|
|
resolution,
|
|
new Vector2(
|
|
origin.X + 0.5 * initialSize * resolution,
|
|
origin.Y + 0.5 * initialSize * resolution
|
|
),
|
|
new CellLimits(initialSize, initialSize)
|
|
);
|
|
|
|
// Match C++ logic: switch case with LOG(FATAL) for unknown types
|
|
switch (gridOptions.GridTypeValue)
|
|
{
|
|
case GridOptions2D.GridType.ProbabilityGrid:
|
|
return new ProbabilityGrid(mapLimits, _conversionTables);
|
|
|
|
case GridOptions2D.GridType.Tsdf:
|
|
// Match C++: Get truncation_distance and maximum_weight from range_data_inserter_options
|
|
// C++: options_.range_data_inserter_options().tsdf_range_data_inserter_options_2d()
|
|
if (_options.RangeDataInserterOptions.TsdfRangeDataInserterOptions2D.HasValue)
|
|
{
|
|
var tsdfOptions = _options.RangeDataInserterOptions.TsdfRangeDataInserterOptions2D.Value;
|
|
return new TSDF2D(
|
|
mapLimits,
|
|
tsdfOptions.TruncationDistance,
|
|
tsdfOptions.MaximumWeight,
|
|
_conversionTables
|
|
);
|
|
}
|
|
throw new ArgumentException("TSDFRangeDataInserterOptions2D is required for TSDF grid type");
|
|
|
|
case GridOptions2D.GridType.InvalidGrid:
|
|
throw new ArgumentException("Invalid grid type specified");
|
|
|
|
default:
|
|
throw new ArgumentException($"Unknown grid type: {gridOptions.GridTypeValue}");
|
|
}
|
|
}
|
|
|
|
private void AddSubmap(Rigid3d localSubmapPose)
|
|
{
|
|
// Match C++ logic: if (submaps_.size() >= 2) { CHECK(submaps_.front()->insertion_finished()); submaps_.erase(submaps_.begin()); }
|
|
if (_submaps.Count >= 2)
|
|
{
|
|
// This will crop the finished Submap before inserting a new Submap to
|
|
// reduce peak memory usage a bit.
|
|
if (!_submaps[0].InsertionFinished)
|
|
{
|
|
throw new InvalidOperationException("First submap must be finished before adding a new one");
|
|
}
|
|
_submaps.RemoveAt(0);
|
|
}
|
|
|
|
// Match C++: Extract origin from pose (C++ passes Vector2f directly, but we have Rigid3d)
|
|
var origin = new Vector2(localSubmapPose.Translation.X, localSubmapPose.Translation.Y);
|
|
var grid = CreateGrid(origin);
|
|
var highResGrid = CreateHighResGrid(origin);
|
|
|
|
// Match C++: Submap2D(origin, grid, conversion_tables)
|
|
// C++ constructor takes Vector2f origin, but C# Submap2D takes Rigid3d (which includes origin)
|
|
var submap = new Submap2D(
|
|
localSubmapPose, // Pass the full Rigid3d pose including rotation
|
|
grid,
|
|
_conversionTables,
|
|
highResGrid
|
|
);
|
|
_submaps.Add(submap);
|
|
}
|
|
}
|