benz-obstacle-detection/BenzObstacleDetect/Detection.cs

using System;
using System.Linq;
using System.Net;
using Autolabor.Benz.ObstacleDetection.DataType;
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;

namespace Autolabor.Benz.ObstacleDetection
{
    [Serializable]
    public class Detection : GroupComponent
    {
        // 驱动层组件
        private Ld19DriverNew _lidarDriver;
        private SerialRelayDriver _serialRelay;
        
        // 算法层组件
        private LidarRoiFilter _lidarFrontRoiFilter;
        private LidarRoiFilter _lidarRearRoiFilter;
        private ObstacleDetector _frontObstacleDetector;
        private ObstacleDetector _rearObstacleDetector;
        private BenzObstacleControl _benzObstacleController;
        
        // 模拟器配置
        private int _FrontLidarRpcPort = 50120;
        private int _FrontLidarListenPort = 40120;
        private IPAddress _serverIp = IPAddress.Loopback;
        
        // 配置过滤区域
        private LidarRoi[] _lidarFrontRoi;
        private LidarRoi[] _lidarRearRoi;

        public Detection(RectRoiSettings roiSettings)
        {
            // 1. 获取感兴趣区域
            var rois = roiSettings.RectRois.Select(roi => LidarRoiFactory.Make(roi.LeftUpX, roi.LeftUpY, roi.RightDownX, roi.RightDownY)).ToList();
            _lidarFrontRoi = rois.Take(1).ToArray();
            _lidarRearRoi = rois.Skip(1).ToArray();

            // 2. 创建各个功能组件
            // 驱动组件
            _lidarDriver = new Ld19DriverNew();
            _serialRelay = new SerialRelayDriver();
        
            // 算法组件
            _frontObstacleDetector = new ObstacleDetector();
            _rearObstacleDetector = new ObstacleDetector();
            _benzObstacleController = new BenzObstacleControl();
            _lidarRearRoiFilter = new LidarRoiFilter();
            _lidarFrontRoiFilter = new LidarRoiFilter();
            
            // 连接到绘图
            // var lidarRender = DataRenderFactory.Make<Pose2D[]>("Benz","lidar",false,true);
            // var obstacleRender = DataRenderFactory.Make<Pose2D[]>("Benz","obstacle",false,true);
            // Add(lidarRender);
            // Add(obstacleRender);
            // _lidarDriver.OutputLocalLidarPose2D.Connect(lidarRender.Input);
            // _frontObstacleDetector.OutputObstaclePoses.Connect(obstacleRender.Input);
            
            // 3. 添加组件到容器
            Add(_lidarDriver);
            Add(_serialRelay);
            Add(_lidarRearRoiFilter);
            Add(_lidarFrontRoiFilter);
            Add(_frontObstacleDetector);
            Add(_rearObstacleDetector);
            Add(_benzObstacleController);
            
            // 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");
            
            // 配置过滤器
            _lidarFrontRoiFilter.SetProperty("roiArray", _lidarFrontRoi);
            _lidarRearRoiFilter.SetProperty("roiArray", _lidarRearRoi);
            // 障碍物检测器配置
            _frontObstacleDetector.SetProperty("MinClusterDistance", 0.05);
            _frontObstacleDetector.SetProperty("MinClusterSize", 3);
            _frontObstacleDetector.SetProperty("MaxPrjDistance", 0.05);
            _frontObstacleDetector.SetProperty("GuessDistance", 1.0);
            _rearObstacleDetector.SetProperty("MinClusterDistance", 0.05);
            _rearObstacleDetector.SetProperty("MinClusterSize", 3);
            _rearObstacleDetector.SetProperty("MaxPrjDistance", 0.05);
            _rearObstacleDetector.SetProperty("GuessDistance", 1.0);
            // 配置发现和丢失次数
            _benzObstacleController.SetProperty("FoundObstacleThreshold", 2);
            _benzObstacleController.SetProperty("LostObstacleThreshold", 3);
        
            // 5. 连接组件
            // 雷达数据流向避障检测
            _lidarDriver.OutputLocalLidarPose2D.Connect(_lidarFrontRoiFilter.InputLidarPoints);
            _lidarFrontRoiFilter.OutputFilteredPoints.Connect(_frontObstacleDetector.InputLidarPoints);
            _lidarDriver.OutputLocalLidarPose2D.Connect(_lidarRearRoiFilter.InputLidarPoints);
            _lidarRearRoiFilter.OutputFilteredPoints.Connect(_rearObstacleDetector.InputLidarPoints);
            // 雷达诊断信号连接到奔驰障碍物控制器
            _lidarDriver.OutputLidarDiagnostic.Connect(_benzObstacleController.InputLidarDiagnostics);
            // 避障检测结果流向奔驰避障控制器
            _frontObstacleDetector.OutputObstacles.Connect(_benzObstacleController.InputFrontObstacles);
            _rearObstacleDetector.OutputObstacles.Connect(_benzObstacleController.InputRearObstacles);
            // 奔驰避障控制器输出到继电器
            _benzObstacleController.OutputRelayControl.Connect(_serialRelay.InputRelayControl);
            _benzObstacleController.OutputRelayQuery.Connect(_serialRelay.InputRelayQuery);
            _serialRelay.OutputRelayFeedback.Connect(_benzObstacleController.InputRelayStatus);
            _serialRelay.OutputSerialConnect.Connect(_benzObstacleController.InputSerialConnect);
            
        }
    }
}