116 lines
5.1 KiB
C#
116 lines
5.1 KiB
C#
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<Pose2D[]> _lidarRender;
|
|
private DataRender<Pose2D[]> _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<Pose2D[]>("Benz","lidar",false,true);
|
|
_obstacleRender = DataRenderFactory.Make<Pose2D[]>("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/autolabor_ld19");
|
|
_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/autolabor_relay");
|
|
|
|
// 创建感兴趣区
|
|
_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);
|
|
}
|
|
}
|
|
} |