IEnumerator<ITask> OnSonarReplaceHandler(pxsonar.Replace replace)
        {
            if (_driveControl != null)
            {
                WinFormsServicePort.FormInvoke(
                    delegate()
                    {
                        // LogInfo("Llamando a ReplaceSonarData");
                        _driveControl.ReplaceSonarData(replace.Body);
                    }
                );
            }

            // LogObject(replace.Body);
            yield break;
        }
        public IEnumerator<ITask> ReplaceHandler(pxsonar.Replace replace)
        {
            _state = replace.Body;
            if (replace.ResponsePort != null)
                replace.ResponsePort.Post(dssp.DefaultReplaceResponseType.Instance);

            // issue notification
            _subMgrPort.Post(new submgr.Submit(_state, dssp.DsspActions.ReplaceRequest));
            yield break;
        }
 // Raul - Move for Sonar data
 /// <summary>
 /// Implements the "Moving" meta state.
 /// </summary>
 /// <param name="sonarData">most recently sensed sonar range data</param>
 /// <param name="distance">closest obstacle in corridor ahead</param>
 private void Move(pxsonar.SonarState sonarData, double distance)
 {
     switch (_state.LogicalState)
     {
         case LogicalState.AdjustHeading:
             AdjustHeading();
             break;
         case LogicalState.FreeForwards:
             AdjustVelocity(sonarData, distance);
             break;
         default:
             LogInfo("ExplorerSimSonar.Move() called in illegal state");
             break;
     }
 }
        // Raul - Map with Sonar data
        /// <summary>
        /// Implements the "Mapping" meta state.
        /// </summary>
        /// <param name="sonarData">most recently sensed sonar range data</param>
        /// <param name="distance">closest obstacle in corridor ahead</param>
        private void Map(pxsonar.SonarState sonarData, double distance)
        {
            switch (_state.LogicalState)
            {
                case LogicalState.RandomTurn:
                    LogInfo("Random Turn");         // TT
                    RandomTurn();
                    break;
                case LogicalState.MapSurroundings:
                    _state.Mapped = true;
                    LogInfo("Turning 180 deg to map");
                    Turn(180);

                    _state.LogicalState = LogicalState.MapSouth;
                    _state.Countdown = 15;
                    break;
                case LogicalState.MapSouth:
                    LogInfo("Mapping the View South");
                    _state.SouthSonar = sonarData;
                    Turn(180);

                    _state.LogicalState = LogicalState.MapNorth;
                    _state.Countdown = 15;
                    break;
                case LogicalState.MapNorth:
                    LogInfo("Mapping the View North");
                    _state.NewHeading = FindBestComposite(_state.SouthSonar, sonarData);
                    LogInfo("Map suggest turn: " + _state.NewHeading);
                    _state.South = null;
                    _state.LogicalState = LogicalState.AdjustHeading;
                    break;
                default:
                    LogInfo("ExplorerSimSonar.Map() called in illegal state");
                    break;
            }
        }
        // Raul - August 2007. Draw SONAR ring readings
        private Bitmap DrawSonarRing(pxsonar.SonarState stateType)
        {
            // Graphic scale factor (milimeters to pixels)
            // This is appropiate for 8 sonar transducers (just one sonar array)!
            const int ScaleFactor = 40;
            const int bottomMargin = 10;
            float half_cone_rads = (float)((options.SonarTransducerAngularRange / 2) * Math.PI) / 180;
            Color col;

            Bitmap bmp = new Bitmap( 
                (options.SonarRange * 2)/ScaleFactor,                  // Width
                (options.SonarRange/ScaleFactor) + bottomMargin );     // Height           

            Graphics g = Graphics.FromImage(bmp);
            g.Clear(Color.White);

            float x1 = bmp.Width / 2;
            float y1 = bmp.Height - bottomMargin;


            if (stateType.DistanceMeasurements.Length != options.SonarRadians.Length)
            {
                // This code needs to be improved. Graphical representation depends on Sonar Array arrangement.
            }
            else
            {
                lblSonarS0.Text = String.Format("S0: {0}", (int)stateType.DistanceMeasurements[0]);
                lblSonarS1.Text = String.Format("S1: {0}", (int)stateType.DistanceMeasurements[1]);
                lblSonarS2.Text = String.Format("S2: {0}", (int)stateType.DistanceMeasurements[2]);
                lblSonarS3.Text = String.Format("S3: {0}", (int)stateType.DistanceMeasurements[3]);
                lblSonarS4.Text = String.Format("S4: {0}", (int)stateType.DistanceMeasurements[4]);
                lblSonarS5.Text = String.Format("S5: {0}", (int)stateType.DistanceMeasurements[5]);
                lblSonarS6.Text = String.Format("S6: {0}", (int)stateType.DistanceMeasurements[6]);
                lblSonarS7.Text = String.Format("S7: {0}", (int)stateType.DistanceMeasurements[7]);

                for (int i = 0; i < stateType.DistanceMeasurements.Length; i++)
                {
                    int range = (int)stateType.DistanceMeasurements[i];

                    if (range > 0 && range <= options.SonarRange)
                    {
                        float scaled_range = (float)range / ScaleFactor;
                        col = LinearColor(Color.DarkBlue, Color.LightGray, 0, (int)options.SonarRange, range);
                        Pen myPen = new Pen(col);
                        
                        // X = scaled_range * Sin( transducer_orientation )
                        float x2 = bmp.Width - x1 - scaled_range * (float)Math.Sin(options.SonarRadians[i]);

                        // Y = scaled_range * Cos (transduCer_orientation)                                                        
                        float y2 = y1 - (scaled_range * (float)Math.Cos(options.SonarRadians[i]));

                        float m3 = scaled_range * (float)Math.Cos(half_cone_rads); // Side of the Cone = range * Cos(8 degrees)
         
                        float y3 = y1 - (m3 * (float)Math.Cos(options.SonarRadians[i] - half_cone_rads));
                        float x3 = x1 - (m3 * (float)Math.Sin(options.SonarRadians[i] - half_cone_rads));

                        float y4 = y1 - (m3 * (float)Math.Cos(options.SonarRadians[i] + half_cone_rads));
                        float x4 = x1 - (m3 * (float)Math.Sin(options.SonarRadians[i] + half_cone_rads));

                        // Raul - Here I draw all the lines
                        // You can comment out the lines you don't want to have in the Control Panel representation

                        // Draw the 2D cone bisector
                        g.DrawLine(new Pen(Color.OrangeRed), 
                            x1,      // I consider all rays coming out from a single point.
                            y1,      // and from the bottom of the image.
                            x2, 
                            y2);

                        // Draw right frontal range
                        g.DrawLine(myPen, x2, y2, x3, y3);

                        // Draw left frontal range
                        g.DrawLine(myPen, x2, y2, x4, y4);

                        // Draw right side of the cone
                        g.DrawLine(myPen, x1, y1, x3, y3);

                        // Draw left side of the cone
                        g.DrawLine(myPen, x1, y1, x4, y4);
                    }
                }
            }
            // Return the image
            return bmp;
        }
 // Raul - UpdateLogicalState for Sonar
 /// <summary>
 /// Transitions to the most appropriate state.
 /// </summary>
 /// <param name="sonarData">most recently sensed sonar range data</param>
 /// <param name="distance">closest obstacle in corridor ahead</param>
 private void UpdateLogicalState(pxsonar.SonarState sonarData, double distance)
 {
     if (_state.Countdown > 0)
     {
         _state.Countdown--;
     }
     else if (_state.IsUnknown)
     {
         StartMapping(sonarData, distance);
     }
     else if (_state.IsMoving)
     {
         Move(sonarData, distance);
     }
     else if (_state.IsMapping)
     {
         Map(sonarData, distance);
     }
 }
        // Raul - FindBestFrom for Sonar data
        /// <summary>
        /// Finds the best heading in a 180 degree sonar scan
        /// </summary>
        /// <param name="sonarData">sonar scan</param>
        /// <param name="dx">horizontal offset</param>
        /// <param name="dy">vertical offset</param>
        /// <param name="width">width of corridor that must be free</param>
        /// <returns>best heading in degrees</returns>
        private int FindBestFrom(pxsonar.SonarState sonarData, double dx, double dy, double width)
        {
            int count = sonarData.DistanceMeasurements.Length;
            // double span = Math.PI * sonarData.AngularRange / 180.0;
            int result;

            List<RangeData> ranges = new List<RangeData>();

            for (int i = 0; i < count; i++)
            {
                double range = sonarData.DistanceMeasurements[i];
                double angle; // Raul - angle = span * i / count - span / 2.0;

                if (i < _state.SonarTransducers)
                {
                    angle = _state.SonarRadians[i];
                }
                else
                {
                    angle = -_state.SonarRadians[i-_state.SonarTransducers];
                }

                double x = range * Math.Sin(angle) - dx;
                double y = range * Math.Cos(angle) - dy;

                angle = Math.Atan2(-x, y);
                range = (int)Math.Sqrt(x * x + y * y);

                ranges.Add(new RangeData(range, angle));
            }

            ranges.Sort(RangeData.ByDistance);

            for (int i = 0; i < ranges.Count; i++)
            {
                RangeData curr = ranges[i];

                double delta = Math.Atan2(width, curr.Distance);
                double low = curr.Heading - delta;
                double high = curr.Heading + delta;

                for (int j = i + 1; j < ranges.Count; j++)
                {
                    if (ranges[j].Heading > low &&
                        ranges[j].Heading < high)
                    {
                        ranges.RemoveAt(j);
                        j--;
                    }

                }
            }

            ranges.Reverse();

            double bestDistance = ranges[0].Distance;
            double bestHeading = ranges[0].Heading;
            Random rand = new Random();

            for (int i = 0; i < ranges.Count; i++)
            {
                if (ranges[i].Distance < bestDistance)
                {
                    break;
                }
                if (rand.Next(i + 1) == 0)
                {
                    bestHeading = ranges[i].Heading;
                }
            }

            // TT - Log the value
            result = -(int)Math.Round(180 * bestHeading / Math.PI);
            LogInfo("Find Best Heading: " + result);

            return result;
        }
Exemple #8
0
        public virtual IEnumerator<ITask> SubscribeHandler(pxsonar.Subscribe subscribe)
        {
            SubscribeRequestType request = subscribe.Body;
            LogInfo("Subscribe request from: " + request.Subscriber);

            // Use the Subscription Manager to insert the new subscriber
            yield return Arbiter.Choice(
                SubscribeHelper(_submgrPort, request, subscribe.ResponsePort),
                    delegate(SuccessResult success)
                    {
                        base.SendNotification<Replace>(_submgrPort, request.Subscriber, _state);
                    },
                    delegate(Exception e)
                    {
                        LogError(null, "Subscribe failed", e);
                    }
            );          
            yield break;
        }
        /// <summary>
        /// Gets the most recent laser notification. Older notifications are dropped.
        /// </summary>
        /// <param name="laserData">last known laser data</param>
        /// <returns>most recent laser data</returns>
        private pxsonar.SonarState GetMostRecentSonarNotification(pxsonar.SonarState sonarData)
        {
            pxsonar.Replace testReplace;

            // _laserNotify is a PortSet<>, P3 represents IPort<sicklrf.Replace> that
            // the portset contains
            int count = _sonarNotify.P3.ItemCount - 1;

            for (int i = 0; i < count; i++)
            {
                testReplace = _sonarNotify.Test<pxsonar.Replace>();
                if (testReplace.Body.TimeStamp > sonarData.TimeStamp)
                {
                    sonarData = testReplace.Body;
                }
            }
           
            if (count > 0)
            {
                LogInfo(string.Format("Dropped {0} sonar readings (sonar start)", count));
            }
            return sonarData;
        }
        // Raul - Find best composite for Sonar Data.
        /// <summary>
        /// Finds the best free corridor (maximum free space ahead) in a 360 degree scan.
        /// </summary>
        /// <param name="south">the backward half of the scan</param>
        /// <param name="north">the forward half of the scan</param>
        /// <returns>best heading in degrees</returns>
        private int FindBestComposite(pxsonar.SonarState south, pxsonar.SonarState north)
        {
            pxsonar.SonarState composite = new pxsonar.SonarState();

            // Raul - I am assuming just one 8 transducer frontal sonar ring.
            // Raul - That means just 8 measuments per 180 degrees. 
            // Raul - Therefore a 360 scan has SonarTranducers * 2 measurements.
            composite.DistanceMeasurements = new double[_state.SonarTransducers*2];

            for (int i = 0; i < _state.SonarTransducers*2; i++)
            {
                if (i < _state.SonarTransducers)
                {
                    composite.DistanceMeasurements[i] = south.DistanceMeasurements[i];
                }
                else
                {
                    composite.DistanceMeasurements[i] = north.DistanceMeasurements[i-_state.SonarTransducers];
                }
            }

            composite.AngularResolution = 22.5;
            composite.AngularRange = 360;
            // composite.Units = north.Units;

            return FindBestFrom(composite, 0, 0, CorridorWidthMoving);
        }
        // Raul - Start mapping for Sonar data
        /// <summary>
        /// Transitions to "Mapping" meta state or "AdjustHeading" state depending on
        /// environment.
        /// </summary>
        /// <param name="sonarData">most recently sensed sonar range data</param>
        /// <param name="distance">closest obstacle in corridor ahead</param>
        private void StartMapping(pxsonar.SonarState sonarData, double distance)
        {
            StopMoving();

            if (distance < ObstacleDistance)
            {
                if (_state.Mapped)
                {
                    // We have been mapping before but do not seem to
                    // have found anything.
                    _state.LogicalState = LogicalState.RandomTurn;
                }
                else
                {
                    _state.LogicalState = LogicalState.MapSurroundings;
                }
            }
            else
            {
                double step = Math.Min(ObstacleDistance, distance - CorridorWidthMapping);
                // find the best angle from step mm in front of
                // our current position
                _state.NewHeading = FindBestFrom(sonarData, 0, step, CorridorWidthMapping);

                LogInfo("Step: " + step + " Turn: " + _state.NewHeading);
                Translate((int)step);

                _state.LogicalState = LogicalState.AdjustHeading;
                _state.Countdown = (int)step / 50 + Math.Abs(_state.NewHeading / 10);
            }
        }
        // Raul - Adjust velocity for sonarData
        /// <summary>
        /// Adjusts the velocity based on environment.
        /// </summary>
        /// <param name="sonarData">most recently sensed sonar range data</param>
        /// <param name="distance">closest obstacle in corridor ahead</param>
        private void AdjustVelocity(pxsonar.SonarState sonarData, double distance)
        {
            _state.Mapped = false;
            // Why does AdjustVelocity call Turn() ???
            // This does not seem to make any sense, and the robot gets
            // stuck in oscillations to the left and right
            double test = FindBestFrom(sonarData, 0, _state.Velocity / 10, CorridorWidthMoving);

            if (distance > FreeDistance)
            {
                MoveForward(MaximumForwardVelocity);

                //if (Math.Abs(test) < 10)
                if (Math.Abs(test) < 10)
                {
                    Turn((int)test / 2);
                    // TT Jun-2007 - Changed to slow down
                    _state.Countdown = (int)Math.Abs((test / 2) / 5);
                }
            }
            else if (distance > AwareOfObstacleDistance)
            {
                MoveForward(MaximumForwardVelocity / 2);

                //if (Math.Abs(test) < 45)
                if (Math.Abs(test) < 45)
                {
                    Turn((int)test / 2);
                    // TT Jun-2007 - Changed
                    _state.Countdown = (int)Math.Abs((test / 2) / 5);
                }
            }
            else
            {
                MoveForward(MaximumForwardVelocity / 4);

                if (Math.Abs(test) < 60)
                {
                    Turn((int)test);
                    // TT Jun-2007 - Changed
                    //_state.Countdown = Math.Abs(test / 10);
                    _state.Countdown = 5;
                }
            }
        }
        public void ReplaceSonarData(pxsonar.SonarState stateType)
        {            
            if (stateType.TimeStamp < _lastSonar)
            {
                return;
            }
            _lastSonar = stateType.TimeStamp;
            TimeSpan delay = DateTime.Now - stateType.TimeStamp;
            lblSonarDelay.Text = delay.ToString();

            // Raul - Sept 2007
            if (options.DisplaySonarMap)
            {
                picSonar.Image = DrawSonarMap(stateType);
            }
            else
            {
                picSonar.Image = DrawSonarRing(stateType);
            }

            if (btnConnectSonar.Enabled)
            {
                btnConnectSonar.Enabled = false;
            }
            if (!btnDisconnectSonar.Enabled)
            {
                btnDisconnectSonar.Enabled = true;
            }
        }
Exemple #14
0
 public virtual IEnumerator<ITask> GetHandler(pxsonar.Get get)
 {
     get.ResponsePort.Post(_state);
     yield break;
 }
        // Raul - Finds closest obstacle in a corridor for sonar data
        /// <summary>
        /// Finds closest obstacle in a corridor.
        /// </summary>
        /// <param name="sonarData">sonar scan</param>
        /// <param name="width">corridor width</param>
        /// <param name="fov">field of view in degrees</param>
        /// <returns>distance to the closest obstacle</returns>
        private double FindNearestObstacleInCorridor(pxsonar.SonarState sonarData, int width, int fov)
        {
            int index;
            double best = 8192;
            // TT - There is a timing issue during startup whereby the
            // LRF actually does not supply any data! Just ignore it.
            if (sonarData == null || sonarData.DistanceMeasurements == null)
                return 0;
            int count = sonarData.DistanceMeasurements.Length;
            double rangeLow = -sonarData.AngularRange / 2.0;
            double rangeHigh = sonarData.AngularRange / 2.0;
            double span = sonarData.AngularRange;

            for (index = 0; index < count; index++)
            {
                double angle = rangeLow + (span * index) / count;
                if (Math.Abs(angle) < fov)
                {
                    angle = angle * Math.PI / 180;

                    double range = sonarData.DistanceMeasurements[index];

                    int x = (int)(range * Math.Sin(angle));
                    int y = (int)(range * Math.Cos(angle));

                    if (Math.Abs(x) < width)
                    {
                        if (range < best)
                        {
                            best = range;
                        }
                    }
                }
            }

            // TT - Log the value
            //LogInfo("Nearest Obstacle: " + best);
            return best;
        }
Exemple #16
0
 public virtual IEnumerator<ITask> ReplaceHandler(pxsonar.Replace replace)
 {
     _state = replace.Body;
     replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);
     yield break;
 }
        // Raul - Sonar replace notification
        /// <summary>
        /// Handles Replace notifications from the Sonar partner
        /// </summary>
        /// <remarks>Posts a <typeparamref name="LaserRangeFinderUpdate"/> to itself.</remarks>
        /// <param name="replace">notification</param>
        /// <returns>task enumerator</returns>
        IEnumerator<ITask> SonarReplaceNotification(pxsonar.Replace replace)
        {
            // When this handler is called a couple of notifications may
            // have piled up. We only want the most recent one.
            pxsonar.SonarState sonarData = replace.Body;

            SonarUpdate request = new SonarUpdate(sonarData);

            _mainPort.Post(request);

            yield return Arbiter.Choice(
                request.ResponsePort,
                delegate(DefaultUpdateResponseType response) { },
                delegate(Fault fault) { }
            );

            // Skip messages that have been queued up in the meantime.
            // The notification that are lingering are out of data by now.
            GetMostRecentSonarNotification(sonarData);


            // Reactivate the handler.
            Activate(
                Arbiter.ReceiveWithIterator<pxsonar.Replace>(false, _sonarNotify, SonarReplaceNotification)
            );
        }
Exemple #18
0
 public virtual IEnumerator<ITask> ReliableSubscribeHandler(pxsonar.ReliableSubscribe subscribe)
 {
     yield return Arbiter.Choice(
         SubscribeHelper(_submgrPort, subscribe.Body, subscribe.ResponsePort),
         delegate(SuccessResult success)
         {
             _submgrPort.Post(new submgr.Submit(
                 subscribe.Body.Subscriber, dssp.DsspActions.ReplaceRequest, _state, null));
         },
         delegate(Exception ex) { LogError(ex); }
     ); ;
 }
        /*
         
        DateTime lastFace = new DateTime(1999, 1, 1);
        DateTime lastLeftHand = new DateTime(1999, 1, 1);
        DateTime lastRightHand = new DateTime(1999, 1, 1);
        DateTime lastObject = new DateTime(1999, 1, 1);


        public void DrawMotionImage(Bitmap image)
        {
            this.MotionImage = image;
            
            TimeSpan span = DateTime.Now - lastFace;
            if (span.Seconds > 1)
            {
                picVisionFace.Visible = false;
            }
            
            span = DateTime.Now - lastLeftHand;
            if (span.Seconds > 1)
            {
                picLeftHand.Visible = false;
            }

            span = DateTime.Now - lastRightHand;
            if (span.Seconds > 1)
            {
                picRightHand.Visible = false;
            }

            span = DateTime.Now - lastObject;
            if (span.Seconds > 1)
            {
                picVisionObject.Visible = false;
            }

        }

        public void DrawFace(vision.FaceResult face)
        {
            if (face.HeadFound)
            {
                picVisionFace.Visible = true;
                lastFace = DateTime.Now;
            }        
        }


        public void DrawHand(vision.HandGestureResult hand)
        {
            if (hand.LeftHandGestureFound)
            {
                if (hand.LeftHandGestureFound)
                {
                    picLeftHand.Visible = true;
                    lastLeftHand = DateTime.Now;
                }
                if ( hand.RightHandGestureFound)
                {
                    picRightHand.Visible = true;
                    lastRightHand = DateTime.Now;
                }
            }
        }

        public void DrawObject(vision.ObjectResult vobject)
        {
            if (vobject.ObjectFound)
            {
                picVisionObject.Visible = true;
                lastObject = DateTime.Now;
            }
        }
        */
        #endregion

        // Raul - Aug 2007 - Draw an occupancy grid style map using the SONAR data
        private Bitmap DrawSonarMap(pxsonar.SonarState stateType)
        {
            // Graphic scale factor (milimeters to pixels)
            const int ScaleFactor = 40;
            const int bottomMargin = 10;

            float scaled_maxRange = options.SonarRange / ScaleFactor;
            float half_cone = options.SonarTransducerAngularRange / 2;

            // Set the dimension of the local map image
            Bitmap bmp = new Bitmap(
                (options.SonarRange * 2) / ScaleFactor,   // Width
                (options.SonarRange / ScaleFactor) );     // Height          

            Graphics g = Graphics.FromImage(bmp);
            g.Clear(Color.LightGray);

            // This is only valid for a single 8 transducer array so far
            // !!!!
            if (stateType.DistanceMeasurements.Length > 8)
            {              
                return bmp;
            }

            // Draw the free space. Same as noted by Trevor in DrawLRFMap.
            // "slivers" that are not filled in between the cones.
            double angle;
            double cone_angles;
            double scaled_range;
            double rx, ry;
            int ix, iy;            
            float x3=0, y3=0, x4=0, y4=0;
            Pen fatPen = new Pen(Color.White, 2.0f);            

            // For each Sonar transducer
            for (int i = 0; i < stateType.DistanceMeasurements.Length; i++)
            {                
                scaled_range = stateType.DistanceMeasurements[i] / ScaleFactor;

                angle = options.SonarRadians[i];

                // Draw current cone
                for (float j = -half_cone; j <= half_cone; j+=1.0f)
                {
                    cone_angles = options.SonarRadians[i] + j * Math.PI / 180;
                    rx = scaled_range * Math.Cos(cone_angles);
                    ry = scaled_range * Math.Sin(cone_angles);

                    ix = bmp.Width - bmp.Width / 2 - (int)(ry * scaled_maxRange / scaled_maxRange);
                    iy = bmp.Height - bottomMargin - (int)(rx * scaled_maxRange / scaled_maxRange);

                    // get the borders of this cone
                    if (j == -half_cone) // center of the cone
                    {
                        x3 = ix;
                        y3 = iy;
                    }
                    if (j == half_cone)
                    {
                        x4 = ix;
                        y4 = iy;
                    }
                    
                    g.DrawLine(fatPen,
                        bmp.Width / 2,
                        bmp.Height - bottomMargin,
                        ix,
                        iy);

                }

                // Draw obstacles at the points of Sonar hits
                if (scaled_range < scaled_maxRange)
                {
                    g.DrawLine(Pens.Black, x3, y3, x4, y4);
                }


            }    
                   
            // Return the image
            return bmp;
        }