public MainWindow()
        {
            InitializeComponent();

            routePlanner = new RoutePlanner(mapperVicinity);

            this.Closing += new System.ComponentModel.CancelEventHandler(MainWindow_Closing);

            //create picpxmod device.
            _picpxmod = new ProximityModule(0x0925, 0x7001);    // see PIC Firmware - usb_descriptors.c lines 178,179

            _picpxmod.HasReadFrameEvent += pmFrameCompleteHandler;

            _picpxmod.DeviceAttachedEvent += new EventHandler<ProximityModuleEventArgs>(_picpxmod_DeviceAttachedEvent);
            _picpxmod.DeviceDetachedEvent += new EventHandler<ProximityModuleEventArgs>(_picpxmod_DeviceDetachedEvent);

            bool deviceAttached = _picpxmod.FindTheHid();

            pmValuesLabel.Content = deviceAttached ?
                "Proximity Board Found" : string.Format("Proximity Board NOT Found\r\nYour USB Device\r\nmust have:\r\n vendorId=0x{0:X}\r\nproductId=0x{1:X}", _picpxmod.vendorId, _picpxmod.productId);

            mapperVicinity.robotPosition = (GeoPosition)robotPositionDefault.Clone();

            mapperVicinity.robotDirection = (Direction)robotDirectionDefault.Clone();

            // we will need this later:
            robotCornerDistanceMeters = Math.Sqrt(mapperVicinity.robotState.robotLengthMeters * mapperVicinity.robotState.robotLengthMeters + mapperVicinity.robotState.robotWidthMeters * mapperVicinity.robotState.robotWidthMeters) / 2.0d;

            // --------- debug ------------
            GeoPosition pos1 = (GeoPosition)mapperVicinity.robotPosition.Clone();

            //pos1.translate(new Distance(1.0d), new Distance(1.0d));     // geo coordinates - East North

            pos1.translate(new Direction() { heading = mapperVicinity.robotDirection.heading, bearingRelative = 45.0d }, new Distance(1.0d));     // robot coordinates - forward right

            DetectedObstacle dobst1 = new DetectedObstacle(pos1) { color = Colors.Red };

            mapperVicinity.Add(dobst1);

            GeoPosition pos2 = (GeoPosition)mapperVicinity.robotPosition.Clone();

            //pos2.translate(new Distance(-1.0d), new Distance(1.0d));     // geo coordinates - West North

            pos2.translate(new Direction() { heading = mapperVicinity.robotDirection.heading, bearingRelative = -45.0d }, new Distance(1.0d));     // robot coordinates - forward left

            DetectedObstacle dobst2 = new DetectedObstacle(pos2) { color = Colors.Yellow };

            //mapperVicinity.Add(dobst2);

            mapperVicinity.computeMapPositions();
            // --------- end debug ------------
        }
        /// <summary>
        /// Service constructor
        /// </summary>
        public TrackroamerLocationAndMappingService(DsspServiceCreationPort creationPort)
            : base(creationPort)
        {
            _routePlanner = new RoutePlanner(_mapperVicinity);

            _mapperVicinity.robotDirection = new Direction() { heading = 45.0d, bearing = 110.0d };
        }
        public void setMapper(MapperVicinity mapper, RoutePlanner routePlanner)
        {
            if (mapWindow != null)
            {
                _mapperVicinity = mapper;
                _routePlanner = routePlanner;

                mapWindow.mapperViewControl1.CurrentMapper = mapper;
                mapWindow.mapperViewControl1.CurrentRoutePlanner = routePlanner;
            }
        }
        public TrackRoamerBehaviorsService(DsspServiceCreationPort creationPort)
            : base(creationPort)
        {
            LogInfo("DriveBehaviorServiceBase:DriveBehaviorServiceBase() -- port: " + creationPort.ToString() + "  threads per CPU: " + Dispatcher.ThreadsPerCpu);

            _routePlanner = new RoutePlanner(_mapperVicinity);

            _mapperVicinity.robotDirection = new Direction() { heading = 45.0d, bearing = -110.0d };

            _panTiltAlignment = PanTiltAlignment.RestoreOrDefault();

            // Kinect UI - by default, lets show all the good stuff:
            this.IncludeDepth = true;
            this.IncludeVideo = true;
            this.IncludeSkeletons = true;
        }