public LocalMapRequestSender(int robotID)
{
this.robotID = robotID;
addressProvider = new HardcodedAddressProvider();
localMapRequestServer = new GenericMulticastServer <LocalMapRequestMessage>(addressProvider.GetAddressByName("LocalMapRequest"), new CSharpMulticastSerializer <LocalMapRequestMessage>(true));
localMapRequestServer.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wireless));
}
void InitializeNetwork()
{
addressProvider = new HardcodedAddressProvider();
targetListServer = new GenericMulticastServer <TargetListMessage>(addressProvider.GetAddressByName("TargetList"), new CSharpMulticastSerializer <TargetListMessage>(true));
targetNoImageClient = new GenericMulticastClient <TargetListNoImageMessage>(addressProvider.GetAddressByName("TargetListNoImg"), new CSharpMulticastSerializer <TargetListNoImageMessage>(true));
queryClient = new GenericMulticastClient <TargetQueryMessage>(addressProvider.GetAddressByName("TargetListQuery"), new CSharpMulticastSerializer <TargetQueryMessage>(true));
indivTargetClient = new GenericMulticastClient <TargetMessage>(addressProvider.GetAddressByName("TrackedTarget"), new CSharpMulticastSerializer <TargetMessage>(true));
targetToCentralServer = new GenericMulticastServer <TargetList2CentralNodeMessage>(addressProvider.GetAddressByName("TargetListToCentral"), new CSharpMulticastSerializer <TargetList2CentralNodeMessage>(true));
confirmOOIClient = new GenericMulticastClient <ConfirmOOIMessage>(addressProvider.GetAddressByName("ConfirmOOIMessage"), new CSharpMulticastSerializer <ConfirmOOIMessage>(true));
clearTargetListClient = new GenericMulticastClient <ClearTargetListMessage>(addressProvider.GetAddressByName("ClearTarget"), new CSharpMulticastSerializer <ClearTargetListMessage>(true));
unconfirmedTargetNoServer = new GenericMulticastServer <UnconfirmedTargetNumberMessage>(addressProvider.GetAddressByName("UnconfirmedTargetNumber"), new CSharpMulticastSerializer <UnconfirmedTargetNumberMessage>(true));
detectionStateScanServer = new GenericMulticastServer <LidarPoseTargetMessage>(addressProvider.GetAddressByName("TargetDetectionLaserScan"), new CSharpMulticastSerializer <LidarPoseTargetMessage>(true));
targetListServer.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));
targetToCentralServer.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));
targetNoImageClient.Start();
queryClient.Start();
indivTargetClient.Start();
confirmOOIClient.Start();
clearTargetListClient.Start();
unconfirmedTargetNoServer.Start();
detectionStateScanServer.Start();
targetNoImageClient.MsgReceived += new EventHandler <MsgReceivedEventArgs <TargetListNoImageMessage> >(targetNoImageClient_MsgReceived);
queryClient.MsgReceived += new EventHandler <MsgReceivedEventArgs <TargetQueryMessage> >(queryClient_MsgReceived);
indivTargetClient.MsgReceived += new EventHandler <MsgReceivedEventArgs <TargetMessage> >(indivTargetClient_MsgReceived);
confirmOOIClient.MsgReceived += new EventHandler <MsgReceivedEventArgs <ConfirmOOIMessage> >(confirmOOIClient_MsgReceived);
clearTargetListClient.MsgReceived += new EventHandler <MsgReceivedEventArgs <ClearTargetListMessage> >(clearTargetListClient_MsgReceived);
}
public LocalMapRequestSender(int robotID)
{
this.robotID = robotID;
addressProvider = new HardcodedAddressProvider();
localMapRequestServer = new GenericMulticastServer<LocalMapRequestMessage>(addressProvider.GetAddressByName("LocalMapRequest"), new CSharpMulticastSerializer<LocalMapRequestMessage>(true));
localMapRequestServer.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wireless));
}
static void StartNetwork()
{
addressProvider = new HardcodedAddressProvider();
poseClient = new GenericMulticastClient<RobotPoseMessage>(addressProvider.GetAddressByName("RobotPoseTRUTH"), new CSharpMulticastSerializer<RobotPoseMessage>(true));
poseClient.Start();
poseClient.MsgReceived += new EventHandler<MsgReceivedEventArgs<RobotPoseMessage>>(poseClient_MsgReceived);
protoPoseServer = new GenericMulticastServer<Magic.Proto.Pose>(new NetworkAddress("RobotPosePROTO", IPAddress.Parse("239.132.1.198"), 30198, NetworkAddressProtocol.UDP_MULTI),
new ProtoBuffSerializer<Magic.Proto.Pose>());
protoPoseServer.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));
}
static void StartNetwork()
{
addressProvider = new HardcodedAddressProvider();
poseClient = new GenericMulticastClient <RobotPoseMessage>(addressProvider.GetAddressByName("RobotPoseTRUTH"), new CSharpMulticastSerializer <RobotPoseMessage>(true));
poseClient.Start();
poseClient.MsgReceived += new EventHandler <MsgReceivedEventArgs <RobotPoseMessage> >(poseClient_MsgReceived);
protoPoseServer = new GenericMulticastServer <Magic.Proto.Pose>(new NetworkAddress("RobotPosePROTO", IPAddress.Parse("239.132.1.198"), 30198, NetworkAddressProtocol.UDP_MULTI),
new ProtoBuffSerializer <Magic.Proto.Pose>());
protoPoseServer.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));
}
public GlobalObstaclesMap(GenericMulticastClient <LidarFilterPackageMessage> filterLidarClient, double extent, double resolution)
{
this.ocGrid = new OccupancyGrid2D(resolution, resolution, extent, extent);
ocGridLogOdd = new OccupancyGrid2DLogOdds(this.ocGrid);
ocGrid4Poly = new OccupancyGrid2D(this.ocGrid);
ocGrid2Poly = new Sensors.OcGrid2Poly.OcGrid2Poly(this.ocGrid4Poly, 0.1);
// network stuff
addressProvider = new HardcodedAddressProvider();
globalObstacleServer = new GenericMulticastServer <GlobalObstaclesMessage>(addressProvider.GetAddressByName("GlobalObstacles"), new CSharpMulticastSerializer <GlobalObstaclesMessage>(true));
globalObstacleServer.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));
filterLidarClient.MsgReceived += new EventHandler <MsgReceivedEventArgs <LidarFilterPackageMessage> >(filterLidarClient_MsgReceived);
ocGridLogOdd.NewGridAvailable += new EventHandler <Magic.Common.Sensors.NewOccupancyGrid2DAvailableEventArgs>(ocGridLogOdd_NewGridAvailable);
}
public SegwayRMP50(NetworkAddress feedback, NetworkAddress control, bool simmode, bool isBackwards, int id)
: this(feedback, control, isBackwards, id)
{
if (simmode)
{
INetworkAddressProvider addrProvider = new HardcodedAddressProvider();
simRobotCommandServer = new GenericMulticastServer <SimMessage <RobotTwoWheelCommand> >(addrProvider.GetAddressByName("RobotSimCommands"), new CSharpMulticastSerializer <SimMessage <RobotTwoWheelCommand> >(true));
simRobotCommandServer.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));
//Fake Segway feedback (for Odom)
simSegwayFeedback = new GenericMulticastClient <SimMessage <IRobotTwoWheelStatus> >(addrProvider.GetAddressByName("SimSegwayFeedback"), new CSharpMulticastSerializer <SimMessage <IRobotTwoWheelStatus> >(true));
simSegwayFeedback.MsgReceived += new EventHandler <MsgReceivedEventArgs <SimMessage <IRobotTwoWheelStatus> > >(simSegwayFeedback_MsgReceived);
simSegwayFeedback.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));
}
this.simMode = simmode;
}
public Pioneer3(NetworkAddress feedback, NetworkAddress control, bool simmode, bool isBackwards, int id)
: this(feedback, control, isBackwards, id)
{
if (simmode)
{
INetworkAddressProvider addrProvider = new HardcodedAddressProvider();
simRobotCommandServer = new GenericMulticastServer<SimMessage<RobotTwoWheelCommand>>(addrProvider.GetAddressByName("RobotSimCommands"), new CSharpMulticastSerializer<SimMessage<RobotTwoWheelCommand>>(true));
simRobotCommandServer.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));
//Fake Segway feedback (for Odom)
simSegwayFeedback = new GenericMulticastClient<SimMessage<IRobotTwoWheelStatus>>(addrProvider.GetAddressByName("SimSegwayFeedback"), new CSharpMulticastSerializer<SimMessage<IRobotTwoWheelStatus>>(true));
simSegwayFeedback.MsgReceived += new EventHandler<MsgReceivedEventArgs<SimMessage<IRobotTwoWheelStatus>>>(simSegwayFeedback_MsgReceived);
simSegwayFeedback.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));
}
this.simMode = simmode;
}
public GlobalGaussianMixMap(OccupancyGrid2D globalOcGrid)
{
addressProvider = new HardcodedAddressProvider();
//robotPoseClient = new GenericMulticastClient<RobotPoseMessage>(addressProvider.GetAddressByName("RobotPose"), new CSharpMulticastSerializer<RobotPoseMessage>(true));
//lidarScanClient = new GenericMulticastClient<LidarScanMessage>(addressProvider.GetAddressByName("LidarScan"), new CSharpMulticastSerializer<LidarScanMessage>(true));
//sensorPoseClient = new GenericMulticastClient<SensorPoseMessage>(addressProvider.GetAddressByName("SensorPose"), new CSharpMulticastSerializer<SensorPoseMessage>(true));
lidarPosePackageClient = new GenericMulticastClient <LidarPosePackageMessage>(addressProvider.GetAddressByName("LidarPosePackage"), new CSharpMulticastSerializer <LidarPosePackageMessage>(true));
simLidarPosePackageClient = new GenericMulticastClient <SimMessage <LidarPosePackageMessage> >(addressProvider.GetAddressByName("SimLidarPosePackage"), new CSharpMulticastSerializer <SimMessage <LidarPosePackageMessage> >(true));
globalGaussianMixMapServer = new GenericMulticastServer <GlobalMapCell>(addressProvider.GetAddressByName("GlobalGaussianMixMap"), new CSharpMulticastSerializer <GlobalMapCell>(true));
globalGaussianMixMapServer.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));
robotIDToPose = new Dictionary <int, RobotPose>();
robotIDToScan = new Dictionary <int, ILidarScan <ILidar2DPoint> >();
robotIDToSensorPose = new Dictionary <int, SensorPose>();
robotIDToTimestamp = new Dictionary <int, double>();
robotIDToPastTimestamp = new Dictionary <int, double>();
globalOcGridByEachRobot = new Dictionary <int, OccupancyGrid2D>();
robotIDToDynamicObstacles = new Dictionary <int, List <Polygon> >();
this.globalOcGrid = new OccupancyGrid2D(globalOcGrid);
laserToRobot = new SensorPose(0, 0, 0.5, 0, 0 / 180.0, 0, 0);
gaussianMixMapAlgorithm = new GaussianMixMapping(globalOcGrid, laserToRobot);
globalOcGridByEachRobotAlgorithm = new Dictionary <int, GaussianMixMapping>();
lidarPosePackageClient.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));
simLidarPosePackageClient.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));
lidarPosePackageClient.MsgReceived += new EventHandler <MsgReceivedEventArgs <LidarPosePackageMessage> >(lidarPosePackageClient_MsgReceived);
simLidarPosePackageClient.MsgReceived += new EventHandler <MsgReceivedEventArgs <SimMessage <LidarPosePackageMessage> > >(simLidarPosePackageClient_MsgReceived);
// local map request from robots
localMapRequestClient = new GenericMulticastClient <LocalMapRequestMessage>(addressProvider.GetAddressByName("LocalMapRequest"), new CSharpMulticastSerializer <LocalMapRequestMessage>(true));
localMapRequestClient.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));
localMapRequestClient.MsgReceived += new EventHandler <MsgReceivedEventArgs <LocalMapRequestMessage> >(localMapRequestClient_MsgReceived);
// local map update sender
localMapReponseServer = new GenericMulticastServer <UpdateMapDataMessage>(addressProvider.GetAddressByName("LocalMapResponse"), new CSharpMulticastSerializer <UpdateMapDataMessage>(true));
localMapReponseServer.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));
Thread t = new Thread(new ParameterizedThreadStart(UpdateGlobalMap));
t.Start();
//Thread t2 = new Thread(new ParameterizedThreadStart(SendGlobalUpdate));
//t2.Start();
}
public GlobalGaussianMixMap(OccupancyGrid2D globalOcGrid)
{
addressProvider = new HardcodedAddressProvider();
//robotPoseClient = new GenericMulticastClient<RobotPoseMessage>(addressProvider.GetAddressByName("RobotPose"), new CSharpMulticastSerializer<RobotPoseMessage>(true));
//lidarScanClient = new GenericMulticastClient<LidarScanMessage>(addressProvider.GetAddressByName("LidarScan"), new CSharpMulticastSerializer<LidarScanMessage>(true));
//sensorPoseClient = new GenericMulticastClient<SensorPoseMessage>(addressProvider.GetAddressByName("SensorPose"), new CSharpMulticastSerializer<SensorPoseMessage>(true));
lidarPosePackageClient = new GenericMulticastClient<LidarPosePackageMessage>(addressProvider.GetAddressByName("LidarPosePackage"), new CSharpMulticastSerializer<LidarPosePackageMessage>(true));
simLidarPosePackageClient = new GenericMulticastClient<SimMessage<LidarPosePackageMessage>>(addressProvider.GetAddressByName("SimLidarPosePackage"), new CSharpMulticastSerializer<SimMessage<LidarPosePackageMessage>>(true));
globalGaussianMixMapServer = new GenericMulticastServer<GlobalMapCell>(addressProvider.GetAddressByName("GlobalGaussianMixMap"), new CSharpMulticastSerializer<GlobalMapCell>(true));
globalGaussianMixMapServer.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));
robotIDToPose = new Dictionary<int, RobotPose>();
robotIDToScan = new Dictionary<int, ILidarScan<ILidar2DPoint>>();
robotIDToSensorPose = new Dictionary<int, SensorPose>();
robotIDToTimestamp = new Dictionary<int, double>();
robotIDToPastTimestamp = new Dictionary<int, double>();
globalOcGridByEachRobot = new Dictionary<int, OccupancyGrid2D>();
robotIDToDynamicObstacles = new Dictionary<int, List<Polygon>>();
this.globalOcGrid = new OccupancyGrid2D(globalOcGrid);
laserToRobot = new SensorPose(0, 0, 0.5, 0, 0 / 180.0, 0, 0);
gaussianMixMapAlgorithm = new GaussianMixMapping(globalOcGrid, laserToRobot);
globalOcGridByEachRobotAlgorithm = new Dictionary<int, GaussianMixMapping>();
lidarPosePackageClient.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));
simLidarPosePackageClient.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));
lidarPosePackageClient.MsgReceived += new EventHandler<MsgReceivedEventArgs<LidarPosePackageMessage>>(lidarPosePackageClient_MsgReceived);
simLidarPosePackageClient.MsgReceived += new EventHandler<MsgReceivedEventArgs<SimMessage<LidarPosePackageMessage>>>(simLidarPosePackageClient_MsgReceived);
// local map request from robots
localMapRequestClient = new GenericMulticastClient<LocalMapRequestMessage>(addressProvider.GetAddressByName("LocalMapRequest"), new CSharpMulticastSerializer<LocalMapRequestMessage>(true));
localMapRequestClient.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));
localMapRequestClient.MsgReceived += new EventHandler<MsgReceivedEventArgs<LocalMapRequestMessage>>(localMapRequestClient_MsgReceived);
// local map update sender
localMapReponseServer = new GenericMulticastServer<UpdateMapDataMessage>(addressProvider.GetAddressByName("LocalMapResponse"), new CSharpMulticastSerializer<UpdateMapDataMessage>(true));
localMapReponseServer.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));
Thread t = new Thread(new ParameterizedThreadStart(UpdateGlobalMap));
t.Start();
//Thread t2 = new Thread(new ParameterizedThreadStart(SendGlobalUpdate));
//t2.Start();
}
public static void Start()
{
lidarScanServer.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wireless));
}
void InitializeNetwork()
{
addressProvider = new HardcodedAddressProvider();
targetListServer = new GenericMulticastServer<TargetListMessage>(addressProvider.GetAddressByName("TargetList"), new CSharpMulticastSerializer<TargetListMessage>(true));
targetNoImageClient = new GenericMulticastClient<TargetListNoImageMessage>(addressProvider.GetAddressByName("TargetListNoImg"), new CSharpMulticastSerializer<TargetListNoImageMessage>(true));
queryClient = new GenericMulticastClient<TargetQueryMessage>(addressProvider.GetAddressByName("TargetListQuery"), new CSharpMulticastSerializer<TargetQueryMessage>(true));
indivTargetClient = new GenericMulticastClient<TargetMessage>(addressProvider.GetAddressByName("TrackedTarget"), new CSharpMulticastSerializer<TargetMessage>(true));
targetToCentralServer = new GenericMulticastServer<TargetList2CentralNodeMessage>(addressProvider.GetAddressByName("TargetListToCentral"), new CSharpMulticastSerializer<TargetList2CentralNodeMessage>(true));
confirmOOIClient = new GenericMulticastClient<ConfirmOOIMessage>(addressProvider.GetAddressByName("ConfirmOOIMessage"), new CSharpMulticastSerializer<ConfirmOOIMessage>(true));
clearTargetListClient = new GenericMulticastClient<ClearTargetListMessage>(addressProvider.GetAddressByName("ClearTarget"), new CSharpMulticastSerializer<ClearTargetListMessage>(true));
unconfirmedTargetNoServer = new GenericMulticastServer<UnconfirmedTargetNumberMessage>(addressProvider.GetAddressByName("UnconfirmedTargetNumber"), new CSharpMulticastSerializer<UnconfirmedTargetNumberMessage>(true));
detectionStateScanServer = new GenericMulticastServer<LidarPoseTargetMessage>(addressProvider.GetAddressByName("TargetDetectionLaserScan"), new CSharpMulticastSerializer<LidarPoseTargetMessage>(true));
targetListServer.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));
targetToCentralServer.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));
targetNoImageClient.Start();
queryClient.Start();
indivTargetClient.Start();
confirmOOIClient.Start();
clearTargetListClient.Start();
unconfirmedTargetNoServer.Start();
detectionStateScanServer.Start();
targetNoImageClient.MsgReceived += new EventHandler<MsgReceivedEventArgs<TargetListNoImageMessage>>(targetNoImageClient_MsgReceived);
queryClient.MsgReceived += new EventHandler<MsgReceivedEventArgs<TargetQueryMessage>>(queryClient_MsgReceived);
indivTargetClient.MsgReceived += new EventHandler<MsgReceivedEventArgs<TargetMessage>>(indivTargetClient_MsgReceived);
confirmOOIClient.MsgReceived += new EventHandler<MsgReceivedEventArgs<ConfirmOOIMessage>>(confirmOOIClient_MsgReceived);
clearTargetListClient.MsgReceived += new EventHandler<MsgReceivedEventArgs<ClearTargetListMessage>>(clearTargetListClient_MsgReceived);
}