diff --git a/BenzObstacleDetect.sln b/BenzObstacleDetect.sln new file mode 100644 index 0000000..f5ff430 --- /dev/null +++ b/BenzObstacleDetect.sln @@ -0,0 +1,16 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "BenzObstacleDetect", "BenzObstacleDetect\BenzObstacleDetect.csproj", "{D158026B-CF72-4CB7-85D1-D6DF91F9D71E}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Release|Any CPU = Release|Any CPU + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {D158026B-CF72-4CB7-85D1-D6DF91F9D71E}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {D158026B-CF72-4CB7-85D1-D6DF91F9D71E}.Debug|Any CPU.Build.0 = Debug|Any CPU + {D158026B-CF72-4CB7-85D1-D6DF91F9D71E}.Release|Any CPU.ActiveCfg = Release|Any CPU + {D158026B-CF72-4CB7-85D1-D6DF91F9D71E}.Release|Any CPU.Build.0 = Release|Any CPU + EndGlobalSection +EndGlobal diff --git a/BenzObstacleDetect/.gitignore b/BenzObstacleDetect/.gitignore index f65be8c..9359306 100644 --- a/BenzObstacleDetect/.gitignore +++ b/BenzObstacleDetect/.gitignore @@ -1,5 +1,8 @@ ## 忽略 Visual Studio 临时文件、用户设置以及编译输出目录 +# idea +.idea/ + # Visual Studio 相关文件 .vs/ *.suo diff --git a/BenzObstacleDetect/BenzObstacleDetect.csproj b/BenzObstacleDetect/BenzObstacleDetect.csproj new file mode 100644 index 0000000..264556f --- /dev/null +++ b/BenzObstacleDetect/BenzObstacleDetect.csproj @@ -0,0 +1,30 @@ + + + + net472 + 9 + enable + Autolabor + disable + Autolabor.Benz.ObstacleDetection + dotnet-BenzObstacleDetect-E0768EF0-F5A4-4FC1-983F-62D3BEA23B87 + true + + + + + + + all + runtime; build; native; contentfiles; analyzers; buildtransitive + + + + + + + + + + + diff --git a/BenzObstacleDetect/Detection.cs b/BenzObstacleDetect/Detection.cs new file mode 100644 index 0000000..6688104 --- /dev/null +++ b/BenzObstacleDetect/Detection.cs @@ -0,0 +1,116 @@ +using System; +using System.Net; +using Autolabor.Robobase; +using Autolabor.Robobase.Component.Algorithm.LidarDetection; +using Autolabor.Robobase.Component.Algorithm.LidarDetection.DataType; +using Autolabor.Robobase.Component.Business.Benz; +using Autolabor.Robobase.Component.Driver; +using Autolabor.Robobase.Component.Driver.SerialRelay; +using Autolabor.Robobase.Component.Tools; +using Autolabor.Robobase.Geometry; +using Autolabor.Robobase.Tools.DataRender; + +namespace Autolabor.Benz.ObstacleDetection +{ + [Serializable] + public class Detection : GroupComponent + { + // 驱动层组件 + private Ld19DriverNew _lidarDriver; + private SerialRelayDriver _serialRelay; + + // 算法层组件 + private LidarRoiFilter _lidarRoiFilter; + private ObstacleDetector _obstacleDetector; + private BenzObstacleControl _benzObstacleController; + + // 显示组件 + private DataRender _lidarRender; + private DataRender _obstacleRender; + + // 模拟器配置 + private int _FrontLidarRpcPort = 50120; + private int _FrontLidarListenPort = 40120; + private IPAddress _serverIp = IPAddress.Loopback; + + // 配置过滤区域 + private LidarRoi[] _lidarRois; + + public Detection() + { + // 2. 创建各个功能组件 + // 驱动组件 + _lidarDriver = new Ld19DriverNew(); + _serialRelay = new SerialRelayDriver(); + + // 算法组件 + _obstacleDetector = new ObstacleDetector(); + _benzObstacleController = new BenzObstacleControl(); + _lidarRoiFilter = new LidarRoiFilter(); + + // 显示组件 + _lidarRender = DataRenderFactory.Make("Benz","lidar",false,true); + _obstacleRender = DataRenderFactory.Make("Benz","obstacle",false,true); + + // 3. 添加组件到容器 + Add(_lidarDriver); + Add(_serialRelay); + Add(_lidarRoiFilter); + Add(_obstacleDetector); + Add(_benzObstacleController); + Add(_lidarRender); + Add(_obstacleRender); + + // 4. 配置组件 + // 激光雷达配置 + _lidarDriver.SetProperty("lidar.port", "/dev/tty.usbserial-2120"); + _lidarDriver.SetProperty("lidar.x", 0.0); + _lidarDriver.SetProperty("lidar.y", 0.0); + _lidarDriver.SetProperty("lidar.originName", "frontLidarOrigin"); + _lidarDriver.SetProperty("lidar.rotation", Math.PI / 2.0); + _lidarDriver.SetProperty("lidar.startRad", -Math.PI); // 要提取的数据范围 + _lidarDriver.SetProperty("lidar.rangeRad", 2.0 * Math.PI); + _lidarDriver.SetProperty("lidar.display.interval", 1000); + _lidarDriver.SetProperty("lidar.intensityLevel", 200.0); + _lidarDriver.SetProperty("sim.lidar.server.ip", _serverIp); + _lidarDriver.SetProperty("sim.lidar.rpc.port", _FrontLidarRpcPort); + _lidarDriver.SetProperty("sim.lidar.listen.port", _FrontLidarListenPort); + + // 串口继电器配置 + _serialRelay.SetProperty("serial.relay.port", "/dev/tty.usbserial-AC012VIB"); + + // 创建感兴趣区 + _lidarRois = new[] + { + LidarRoiFactory.Make(0.6, 0.5, 0.1, -0.5), + }; + // 配置过滤器 + _lidarRoiFilter.SetProperty("roiArray", _lidarRois); + // 障碍物检测器配置 + _obstacleDetector.SetProperty("MinClusterDistance", 0.05); + _obstacleDetector.SetProperty("MinClusterSize", 3); + _obstacleDetector.SetProperty("MaxPrjDistance", 0.05); + _obstacleDetector.SetProperty("GuessDistance", 1.0); + // 配置发现和丢失次数 + _benzObstacleController.SetProperty("FoundObstacleThreshold", 3); + _benzObstacleController.SetProperty("LostObstacleThreshold", 5); + + // 5. 连接组件 + // 雷达数据流向避障检测 + _lidarDriver.OutputLocalLidarPose2D.Connect(_lidarRoiFilter.InputLidarPoints); + _lidarRoiFilter.OutputFilteredPoints.Connect(_obstacleDetector.InputLidarPoints); + // 雷达诊断信号连接到奔驰障碍物控制器 + _lidarDriver.OutputLidarDiagnostic.Connect(_benzObstacleController.InputDiagnostics); + // 避障检测结果流向奔驰避障控制器 + _obstacleDetector.OutputObstacles.Connect(_benzObstacleController.InputObstacles); + // 奔驰避障控制器输出到继电器 + _benzObstacleController.OutputRelayControl.Connect(_serialRelay.InputRelayControl); + _benzObstacleController.OutputRelayQuery.Connect(_serialRelay.InputRelayQuery); + _serialRelay.OutputRelayFeedback.Connect(_benzObstacleController.InputRelayStatus); + + // 连接到绘图 + _lidarDriver.OutputLocalLidarPose2D.Connect(_lidarRender.Input); + _obstacleDetector.OutputObstaclePoses.Connect(_obstacleRender.Input); + } + } +} \ No newline at end of file diff --git a/BenzObstacleDetect/Program.cs b/BenzObstacleDetect/Program.cs new file mode 100644 index 0000000..466e3f2 --- /dev/null +++ b/BenzObstacleDetect/Program.cs @@ -0,0 +1,9 @@ +using Autolabor.Benz.ObstacleDetection; +using Microsoft.Extensions.DependencyInjection; +using Microsoft.Extensions.Hosting; + +var builder = Host.CreateApplicationBuilder(args); +builder.Services.AddHostedService(); + +var host = builder.Build(); +host.Run(); \ No newline at end of file diff --git a/BenzObstacleDetect/Properties/launchSettings.json b/BenzObstacleDetect/Properties/launchSettings.json new file mode 100644 index 0000000..999973e --- /dev/null +++ b/BenzObstacleDetect/Properties/launchSettings.json @@ -0,0 +1,12 @@ +{ + "$schema": "http://json.schemastore.org/launchsettings.json", + "profiles": { + "BenzObstacleDetect": { + "commandName": "Project", + "dotnetRunMessages": true, + "environmentVariables": { + "DOTNET_ENVIRONMENT": "Development" + } + } + } +} diff --git a/BenzObstacleDetect/Worker.cs b/BenzObstacleDetect/Worker.cs new file mode 100644 index 0000000..d7a7f68 --- /dev/null +++ b/BenzObstacleDetect/Worker.cs @@ -0,0 +1,43 @@ +using System; +using System.Threading; +using System.Threading.Tasks; +using Autolabor.Robobase; +using Microsoft.Extensions.Hosting; +using Microsoft.Extensions.Logging; + +namespace Autolabor.Benz.ObstacleDetection +{ + public class Worker : BackgroundService + { + private readonly ILogger _logger; + + public Worker(ILogger logger) + { + _logger = logger; + } + + protected override async Task ExecuteAsync(CancellationToken stoppingToken) + { + + // 创建用户配置文件 + var app = Kits.GetAutolaborDataPath(); + var path = System.IO.Path.Combine(app, "BenzObstacleDetection.bin"); + UserDataManager.Initialize(path); + + // 输出日志 + TestKit.Logger.Trace("BenzObstacleDetection start"); + // 启动顶层组件 + var detection = new Detection(); + detection.Enable = true; + // while (!stoppingToken.IsCancellationRequested) + // { + // if (_logger.IsEnabled(LogLevel.Information)) + // { + // _logger.LogInformation("Worker running at: {time}", DateTimeOffset.Now); + // } + // + // await Task.Delay(1000, stoppingToken); + // } + } + } +} \ No newline at end of file