QUYVN/I150
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
QUYVN
2026-07-03 16:37:12 +07:00
commit 63b8c1ea8b
1931 changed files with 640587 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

220
srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Common/Threading/Task.cs Normal file
View File

@@ -0,0 +1,220 @@
/*
* 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.
*/
namespace CartographerSharp.Common.Threading;
/// <summary>
/// Task state enumeration.
/// </summary>
public enum TaskState
{
New,
Dispatched,
DependenciesCompleted,
Running,
Completed
}
/// <summary>
/// Represents a task that can be executed by a thread pool.
/// Tasks can have dependencies on other tasks.
/// </summary>
public class Task : IDisposable
{
private readonly Lock _lock = new();
private Action? _workItem;
private ThreadPoolInterface? _threadPoolToNotify;
private TaskState _state = TaskState.New;
private int _uncompletedDependencies;
private readonly HashSet<Task> _dependentTasks = [];
private bool _disposed = false;
/// <summary>
/// Gets the current state of the task.
/// </summary>
public TaskState GetState()
{
lock (_lock)
{
return _state;
}
}
/// <summary>
/// Sets the work item to execute. State must be 'New'.
/// </summary>
public void SetWorkItem(Action workItem)
{
ArgumentNullException.ThrowIfNull(workItem);
lock (_lock)
{
if (_state != TaskState.New)
{
throw new InvalidOperationException($"Cannot set work item when state is {_state}");
}
_workItem = workItem;
}
}
/// <summary>
/// Adds a dependency on another task. State must be 'New'.
/// 'dependency' may be null, in which case it is assumed completed.
/// </summary>
public void AddDependency(WeakReference<Task>? dependency)
{
Task? depTask = null;
lock (_lock)
{
if (_state != TaskState.New)
{
throw new InvalidOperationException($"Cannot add dependency when state is {_state}");
}
if (dependency != null && dependency.TryGetTarget(out depTask))
{
_uncompletedDependencies++;
}
}
// Call AddDependentTask outside the lock to match C++ implementation
depTask?.AddDependentTask(this);
}
/// <summary>
/// Adds a dependent task that depends on this task.
/// </summary>
internal void AddDependentTask(Task dependentTask)
{
bool shouldNotifyImmediately = false;
lock (_lock)
{
// If this task is already completed, notify the dependent task immediately
if (_state == TaskState.Completed)
{
shouldNotifyImmediately = true;
}
else
{
if (!_dependentTasks.Add(dependentTask))
{
throw new InvalidOperationException("Given dependency is already a dependency.");
}
}
}
// Call OnDependencyCompleted outside the lock to avoid potential deadlock
if (shouldNotifyImmediately)
{
dependentTask.OnDependencyCompleted();
}
}
/// <summary>
/// Sets the thread pool that will execute this task. State must be 'New'.
/// </summary>
internal void SetThreadPool(ThreadPoolInterface threadPool)
{
lock (_lock)
{
if (_state != TaskState.New)
{
throw new InvalidOperationException($"Cannot set thread pool when state is {_state}");
}
_threadPoolToNotify = threadPool;
_state = TaskState.Dispatched;
if (_uncompletedDependencies == 0)
{
_state = TaskState.DependenciesCompleted;
_threadPoolToNotify.NotifyDependenciesCompleted(this);
}
}
}
/// <summary>
/// Called when a dependency completes. State must be 'New' or 'Dispatched'.
/// If 'Dispatched', may become 'DependenciesCompleted'.
/// </summary>
internal void OnDependencyCompleted()
{
ThreadPoolInterface? threadPoolToNotify = null;
lock (_lock)
{
if (_state != TaskState.New && _state != TaskState.Dispatched)
{
throw new InvalidOperationException($"Cannot complete dependency when state is {_state}");
}
_uncompletedDependencies--;
if (_uncompletedDependencies == 0 && _state == TaskState.Dispatched)
{
_state = TaskState.DependenciesCompleted;
threadPoolToNotify = _threadPoolToNotify;
}
}
threadPoolToNotify?.NotifyDependenciesCompleted(this);
}
/// <summary>
/// Executes the task. State must be 'DependenciesCompleted' and becomes 'Completed'.
/// </summary>
internal void Execute()
{
Action? workItem;
lock (_lock)
{
if (_state != TaskState.DependenciesCompleted)
{
throw new InvalidOperationException($"Cannot execute task when state is {_state}");
}
_state = TaskState.Running;
workItem = _workItem;
}
// Execute the work item outside the lock
try
{
workItem?.Invoke();
}
finally
{
// Set state to Completed and notify all dependent tasks in a single lock
lock (_lock)
{
_state = TaskState.Completed;
// Notify all dependent tasks
foreach (var dependentTask in _dependentTasks)
{
dependentTask.OnDependencyCompleted();
}
}
}
}
public void Dispose()
{
if (!_disposed)
{
_disposed = true;
}
GC.SuppressFinalize(this);
}
}

222
srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Common/Threading/ThreadPool.cs Normal file
View File

@@ -0,0 +1,222 @@
/*
* 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 Microsoft.Extensions.Logging;
using System.Runtime.InteropServices;
namespace CartographerSharp.Common.Threading;
/// <summary>
/// Interface for thread pool implementations.
/// </summary>
public abstract class ThreadPoolInterface
{
/// <summary>
/// Schedules a task for execution.
/// </summary>
/// <param name="task">The task to schedule</param>
/// <returns>A weak reference to the task</returns>
public abstract WeakReference<Task> Schedule(Task task);
/// <summary>
/// Executes a task.
/// </summary>
protected static void Execute(Task task)
{
task.Execute();
}
/// <summary>
/// Sets the thread pool for a task.
/// </summary>
protected void SetThreadPool(Task task)
{
task.SetThreadPool(this);
}
/// <summary>
/// Notifies that dependencies of a task are completed.
/// </summary>
internal abstract void NotifyDependenciesCompleted(Task task);
}
/// <summary>
/// A fixed number of threads working on tasks. Adding a task does not block.
/// Tasks may be added whether or not their dependencies are completed.
/// When all dependencies of a task are completed, it is queued up for execution
/// in a background thread. The queue must be empty before calling the destructor.
/// The thread pool will then wait for the currently executing work items to finish
/// and then destroy the threads.
/// </summary>
public partial class ThreadPool : ThreadPoolInterface, IDisposable
{
private readonly object _lock = new();
private readonly List<Thread> _pool = [];
private readonly Queue<Task> _taskQueue = new();
private readonly Dictionary<Task, Task> _tasksNotReady = [];
private bool _running = true;
private bool _disposed = false;
/// <summary>
/// Creates a new thread pool with the specified number of threads.
/// </summary>
public ThreadPool(int numThreads)
{
if (numThreads <= 0)
{
throw new ArgumentException("ThreadPool requires a positive num_threads!", nameof(numThreads));
}
lock (_lock)
{
for (int i = 0; i < numThreads; i++)
{
var thread = new Thread(DoWork)
{
IsBackground = false,
Name = $"CartographerThreadPool-{i}"
};
_pool.Add(thread);
thread.Start();
}
}
}
/// <summary>
/// Schedules a task for execution.
/// </summary>
public override WeakReference<Task> Schedule(Task task)
{
ArgumentNullException.ThrowIfNull(task);
lock (_lock)
{
if (!_running)
{
throw new InvalidOperationException("ThreadPool is not running");
}
if (_tasksNotReady.ContainsKey(task))
{
throw new InvalidOperationException("Schedule called twice for the same task");
}
_tasksNotReady[task] = task;
}
SetThreadPool(task);
return new WeakReference<Task>(task);
}
/// <summary>
/// Notifies that dependencies of a task are completed.
/// </summary>
internal override void NotifyDependenciesCompleted(Task task)
{
lock (_lock)
{
if (!_tasksNotReady.TryGetValue(task, out var storedTask))
{
throw new InvalidOperationException("Task not found in tasks_not_ready");
}
_taskQueue.Enqueue(storedTask);
_tasksNotReady.Remove(task);
Monitor.PulseAll(_lock);
}
}
/// <summary>
/// Worker thread method.
/// </summary>
private void DoWork()
{
Thread.BeginThreadAffinity();
try
{
while (true)
{
Task? task = null;
lock (_lock)
{
while (_taskQueue.Count == 0 && _running)
{
Monitor.Wait(_lock);
}
if (!_running && _taskQueue.Count == 0)
{
return;
}
if (_taskQueue.Count > 0)
{
task = _taskQueue.Dequeue();
}
}
if (task != null)
{
if (task.GetState() != TaskState.DependenciesCompleted)
{
continue;
}
Execute(task);
}
}
}
finally
{
Thread.EndThreadAffinity();
}
}
/// <summary>
/// Disposes the thread pool, waiting for all tasks to complete.
/// </summary>
public void Dispose()
{
if (_disposed)
{
return;
}
lock (_lock)
{
if (!_running)
{
return;
}
_running = false;
Monitor.PulseAll(_lock);
}
foreach (var thread in _pool)
{
thread.Join();
}
_disposed = true;
GC.SuppressFinalize(this);
}
[LibraryImport("libc", SetLastError = true)]
private static partial int nice(int inc);
}