Ejemplo n.º 1
0
 /// <summary>
 /// creates some new poses
 /// </summary>
 private void createNewPoses(float x, float y, float pan)
 {
     for (int sample = Poses.Count; sample < survey_trial_poses; sample++)
     {
         particlePath path = new particlePath(x, y, pan, max_path_length, time_step, path_ID, rob.LocalGridDimension);
         createNewPose(path);
     }
 }
Ejemplo n.º 2
0
 /// <summary>
 /// constructor
 /// </summary>
 /// <param name="x">x position in millimetres</param>
 /// <param name="y">y position in millimetres</param>
 /// <param name="pan">pan angle in radians</param>
 /// <param name="path">the path which this pose belongs to</param>
 public particlePose(float x, float y, 
                     float pan, 
                     particlePath path)
 {
     this.x = x;
     this.y = y;
     this.pan = pan;
     this.path = path;
     observed_grid_cells = new List<particleGridCell>();
 }
Ejemplo n.º 3
0
 /// <summary>
 /// constructor
 /// </summary>
 /// <param name="x">x position in millimetres</param>
 /// <param name="y">y position in millimetres</param>
 /// <param name="pan">pan angle in radians</param>
 /// <param name="path">the path which this pose belongs to</param>
 public particlePose(float x, float y,
                     float pan,
                     particlePath path)
 {
     this.x              = x;
     this.y              = y;
     this.pan            = pan;
     this.path           = path;
     observed_grid_cells = new List <particleGridCell>();
 }
Ejemplo n.º 4
0
 /// <summary>
 /// increment or decrement the total number of child branches supported by this path
 /// </summary>
 /// <param name="path"></param>
 /// <param name="increment">increment of 1 or -1</param>
 private void incrementPathChildren(particlePath path, 
                                    int increment)
 {
     path.total_children += increment;
     particlePath p = path;
     while (p.branch_pose != null)
     {
         p = p.branch_pose.path;
         p.total_children += increment;
     }
 }
Ejemplo n.º 5
0
 /// <summary>
 /// create poses distributed over an area, suitable for
 /// monte carlo localisation
 /// </summary>
 /// <param name="tx">top left of the area in millimetres</param>
 /// <param name="ty">top left of the area in millimetres</param>
 /// <param name="bx">bottom right of the area in millimetres</param>
 /// <param name="by">bottom right of the area in millimetres</param>
 private void createNewPosesMonteCarlo(float tx, float ty, float bx, float by)
 {
     for (int sample = Poses.Count; sample < survey_trial_poses; sample++)
     {
         float        x    = tx + rnd.Next((int)(bx - tx));
         float        y    = ty + rnd.Next((int)(by - ty));
         float        pan  = 2 * (float)Math.PI * rnd.Next(100000) / 100000.0f;
         particlePath path = new particlePath(x, y, pan, max_path_length, time_step, path_ID, rob.LocalGridDimension);
         createNewPose(path);
     }
 }
Ejemplo n.º 6
0
        /// <summary>
        /// add a new path to the list
        /// </summary>
        /// <param name="path"></param>
        private void createNewPose(particlePath path)
        {
            Poses.Add(path);
            ActivePoses.Add(path);

            // increment the path ID
            path_ID++;
            if (path_ID > UInt32.MaxValue - 3)
            {
                path_ID = 0;                                // rollover
            }
        }
Ejemplo n.º 7
0
        /// <summary>
        /// increment or decrement the total number of child branches supported by this path
        /// </summary>
        /// <param name="path"></param>
        /// <param name="increment">increment of 1 or -1</param>
        private void incrementPathChildren(particlePath path,
                                           int increment)
        {
            path.total_children += increment;
            particlePath p = path;

            while (p.branch_pose != null)
            {
                p = p.branch_pose.path;
                p.total_children += increment;
            }
        }
Ejemplo n.º 8
0
 /// <summary>
 /// show the tree of possible paths
 /// </summary>
 /// <param name="img"></param>
 /// <param name="width"></param>
 /// <param name="height"></param>
 /// <param name="r"></param>
 /// <param name="g"></param>
 /// <param name="b"></param>
 /// <param name="line_thickness"></param>
 public void ShowTree(Byte[] img, int width, int height,
                      int r, int g, int b, int line_thickness)
 {
     for (int i = 0; i < ActivePoses.Count; i++)
     {
         bool clearBackground = false;
         if (i == 0)
         {
             clearBackground = true;
         }
         particlePath path = ActivePoses[i];
         path.Show(img, width, height, r, g, b, line_thickness,
                   min_tree_x, min_tree_y, max_tree_x, max_tree_y,
                   clearBackground, root_time_step);
     }
 }
Ejemplo n.º 9
0
        /// <summary>
        /// update the given pose using the motion model
        /// </summary>
        /// <param name="path">path to add the new estimated pose to</param>
        /// <param name="time_elapsed_sec">time elapsed since the last update in seconds</param>
        private void sample_motion_model_velocity(particlePath path, float time_elapsed_sec)
        {
            // calculate noisy velocities
            float fwd_velocity = forward_velocity + sample_normal_distribution(
                (motion_noise[0] * Math.Abs(forward_velocity)) +
                (motion_noise[1] * Math.Abs(angular_velocity)));

            float ang_velocity = angular_velocity + sample_normal_distribution(
                (motion_noise[2] * Math.Abs(forward_velocity)) +
                (motion_noise[3] * Math.Abs(angular_velocity)));

            float v = sample_normal_distribution(
                (motion_noise[4] * Math.Abs(forward_velocity)) +
                (motion_noise[5] * Math.Abs(angular_velocity)));

            float fraction = 0;

            if (Math.Abs(ang_velocity) > 0.000001f)
            {
                fraction = fwd_velocity / ang_velocity;
            }
            float current_pan = path.current_pose.pan;

            // if scan matching is active use the current estimated pan angle
            if (rob.ScanMatchingPanAngleEstimate != scanMatching.NOT_MATCHED)
            {
                current_pan = rob.ScanMatchingPanAngleEstimate;
            }

            float pan2 = current_pan - (ang_velocity * time_elapsed_sec);

            float new_y = path.current_pose.y + (fraction * (float)Math.Sin(current_pan)) -
                          (fraction * (float)Math.Sin(pan2));
            float new_x = path.current_pose.x - (fraction * (float)Math.Cos(current_pan)) +
                          (fraction * (float)Math.Cos(pan2));
            float new_pan = pan2 + (v * time_elapsed_sec);

            particlePose new_pose = new particlePose(new_x, new_y, new_pan, path);

            new_pose.time_step = time_step;
            path.Add(new_pose);
        }
Ejemplo n.º 10
0
        /// <summary>
        /// update all current poses with the current observation
        /// </summary>
        /// <param name="stereo_rays">list of evidence rays to be inserted into the grid</param>
        /// <param name="localiseOnly">if true does not add any mapping particles (pure localisation)</param>
        public void AddObservation(List <evidenceRay>[] stereo_rays, bool localiseOnly)
        {
            for (int p = 0; p < Poses.Count; p++)
            {
                particlePath path = Poses[p];
                float        logodds_localisation_score =
                    path.current_pose.AddObservation(stereo_rays,
                                                     rob, LocalGrid,
                                                     localiseOnly);

                if (logodds_localisation_score != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE)
                {
                    updatePoseScore(path, logodds_localisation_score);
                }
            }

            // adding an observation is sufficient to update the
            // pose localisation scores.  This is, after all, SLAM.
            PosesEvaluated = true;
        }
Ejemplo n.º 11
0
        /// <summary>
        /// constructor
        /// </summary>
        /// <param name="parent">parent path from which this originates</param>
        /// <param name="path_ID">unique ID for this path</param>
        /// <param name="grid_dimension_cells">dimension of the grid</param>
        public particlePath(particlePath parent,
                            UInt32 path_ID,
                            int grid_dimension_cells)
        {
            ID = path_ID;
            parent.current_pose.no_of_children++;
            current_pose = parent.current_pose;
            branch_pose  = parent.current_pose;

            incrementPathChildren(parent, 1);

            this.max_length = parent.max_length;
            path            = new List <particlePose>();
            total_score     = parent.total_score;
            total_poses     = parent.total_poses;
            Enabled         = true;

            // map cache for this path
            //map_cache = new ArrayList[grid_dimension_cells][][];
        }
Ejemplo n.º 12
0
        /// <summary>
        /// constructor
        /// </summary>
        /// <param name="parent">parent path from which this originates</param>
        /// <param name="path_ID">unique ID for this path</param>
        /// <param name="grid_dimension_cells">dimension of the grid</param>
        public particlePath(particlePath parent, 
                            UInt32 path_ID, 
                            int grid_dimension_cells)
        {
            ID = path_ID;
            parent.current_pose.no_of_children++;
            current_pose = parent.current_pose;
            branch_pose = parent.current_pose;

            incrementPathChildren(parent, 1);

            this.max_length = parent.max_length;
            path = new List<particlePose>();
            total_score = parent.total_score;
            total_poses = parent.total_poses;
            Enabled = true;

            // map cache for this path
            //map_cache = new ArrayList[grid_dimension_cells][][];
        }
Ejemplo n.º 13
0
        /// <summary>
        /// removes low probability poses
        /// Note that a maturity threshold is used, so that poses which may initially 
        /// be unfairly judged as improbable have time to prove their worth
        /// </summary>
        private void Prune()
        {
            float max_score = float.MinValue;
            best_path = null;

            // sort poses by score
            for (int i = 0; i < Poses.Count-1; i++)
            {
                particlePath p1 = Poses[i];

                // keep track of the bounding region within which the path tree occurs
                if (p1.current_pose.x < min_tree_x) min_tree_x = p1.current_pose.x;
                if (p1.current_pose.x > max_tree_x) max_tree_x = p1.current_pose.x;
                if (p1.current_pose.y < min_tree_y) min_tree_y = p1.current_pose.y;
                if (p1.current_pose.y > max_tree_y) max_tree_y = p1.current_pose.y;

                max_score = p1.total_score;
                particlePath winner = null;
                int winner_index = 0;
                for (int j = i + 1; j < Poses.Count; j++)
                {
                    particlePath p2 = Poses[i];
                    float sc = p2.total_score;
                    if ((sc > max_score) ||
                        ((max_score == 0) && (sc != 0)))
                    {
                        max_score = sc;
                        winner = p2;
                        winner_index = j;
                    }
                }
                if (winner != null)
                {
                    Poses[i] = winner;
                    Poses[winner_index] = p1;
                }
            }            

            // the best path is at the top
            best_path = Poses[0];

            // It's culling season
            int cull_index = (100 - cull_threshold) * Poses.Count / 100;
            if (cull_index > Poses.Count - 2) cull_index = Poses.Count - 2;
            for (int i = Poses.Count - 1; i > cull_index; i--)
            {
                particlePath path = Poses[i];
                if (path.path.Count >= pose_maturation)
                {                    
                    // remove mapping hypotheses for this path
                    path.Remove(LocalGrid);

                    // now remove the path itself
                    Poses.RemoveAt(i);                    
                }
            }

            // garbage collect any dead paths (R.I.P.)
            List<particlePath> possible_roots = new List<particlePath>(); // stores paths where all branches have coinverged to a single possibility
            for (int i = ActivePoses.Count - 1; i >= 0; i--)
            {
                particlePath path = ActivePoses[i];

                if ((!path.Enabled) ||
                    ((path.total_children == 0) && (path.branch_pose == null) && (path.current_pose.time_step < time_step - pose_maturation - 5)))
                {
                    ActivePoses.RemoveAt(i);
                }
                else
                {
                    // record any fully collapsed paths
                    if (!path.Collapsed)
                        if (path.branch_pose == null)
                            possible_roots.Add(path);
                        else
                        {
                            if (path.branch_pose.path.Collapsed)
                                possible_roots.Add(path);
                        }
                }
            }

            if (possible_roots.Count == 1)
            {
                // collapse tha psth
                particlePath path = possible_roots[0];

                if (path.branch_pose != null)
                {
                    particlePath previous_path = path.branch_pose.path;
                    previous_path.Distill(LocalGrid);
                    path.branch_pose.parent = null;
                    path.branch_pose = null;                    
                }
                
                path.Collapsed = true;

                // take note of the time step.  This is for use with the display functions only
                root_time_step = path.current_pose.time_step;
            }

            if (best_path != null)
            {
                // update the current robot position with the best available pose
                if (current_robot_pose == null) current_robot_pose = 
                    new pos3D(best_path.current_pose.x,
                              best_path.current_pose.y,
                              0);
                current_robot_pose.pan = best_path.current_pose.pan;
                current_robot_path_score = best_path.total_score;

                // generate new poses from the ones which have survived culling
                int new_poses_required = survey_trial_poses; // -Poses.Count;
                int max = Poses.Count;
                int n = 0, added = 0;
                while ((max > 0) &&
                       (n < new_poses_required*4) && 
                       (added < new_poses_required))
                {
                    // identify a potential parent at random, 
                    // from one of the surviving paths
                    int random_parent_index = rnd.Next(max-1);
                    particlePath parent_path = Poses[random_parent_index];
                    
                    // only mature parents can have children
                    if (parent_path.path.Count >= pose_maturation)
                    {
                        // generate a child branching from the parent path
                        particlePath child_path = new particlePath(parent_path, path_ID, rob.LocalGridDimension);
                        createNewPose(child_path);
                        added++;
                    }
                    n++;
                }
                
                // like salmon, after parents spawn they die
                if (added > 0)
                    for (int i = max - 1; i >= 0; i--)
                        Poses.RemoveAt(i);

                // if the robot has rotated through a large angle since
                // the previous time step then reset the scan matching estimate
                //if (Math.Abs(best_path.current_pose.pan - rob.pan) > rob.ScanMatchingMaxPanAngleChange * Math.PI / 180)
                    rob.ScanMatchingPanAngleEstimate = scanMatching.NOT_MATCHED;
            }


            PosesEvaluated = false;
        }
Ejemplo n.º 14
0
 /// <summary>
 /// updates the given path with the score obtained from the current pose
 /// Note that scores are always expressed as log odds matching probability
 /// from grid localisation
 /// </summary>
 /// <param name="path">path whose score is to be updated</param>
 /// <param name="new_score">the new score to be added to the path</param>
 public void updatePoseScore(particlePath path, float new_score)
 {
     path.total_score += new_score;
     path.local_score += new_score;
 }
Ejemplo n.º 15
0
 /// <summary>
 /// create poses distributed over an area, suitable for
 /// monte carlo localisation
 /// </summary>
 /// <param name="tx">top left of the area in millimetres</param>
 /// <param name="ty">top left of the area in millimetres</param>
 /// <param name="bx">bottom right of the area in millimetres</param>
 /// <param name="by">bottom right of the area in millimetres</param>
 private void createNewPosesMonteCarlo(float tx, float ty, float bx, float by)
 {
     for (int sample = Poses.Count; sample < survey_trial_poses; sample++)
     {
         float x = tx + rnd.Next((int)(bx - tx));
         float y = ty + rnd.Next((int)(by - ty));
         float pan = 2 * (float)Math.PI * rnd.Next(100000) / 100000.0f;
         particlePath path = new particlePath(x, y, pan, max_path_length, time_step, path_ID, rob.LocalGridDimension);
         createNewPose(path);
     }
 }
Ejemplo n.º 16
0
        /// <summary>
        /// update the given pose using the motion model
        /// </summary>
        /// <param name="path">path to add the new estimated pose to</param>
        /// <param name="time_elapsed_sec">time elapsed since the last update in seconds</param>
        private void sample_motion_model_velocity(particlePath path, float time_elapsed_sec)
        {
            // calculate noisy velocities
            float fwd_velocity = forward_velocity + sample_normal_distribution(
                (motion_noise[0] * Math.Abs(forward_velocity)) +
                (motion_noise[1] * Math.Abs(angular_velocity)));

            float ang_velocity = angular_velocity + sample_normal_distribution(
                (motion_noise[2] * Math.Abs(forward_velocity)) +
                (motion_noise[3] * Math.Abs(angular_velocity)));

            float v = sample_normal_distribution(
                (motion_noise[4] * Math.Abs(forward_velocity)) +
                (motion_noise[5] * Math.Abs(angular_velocity)));

            float fraction = 0;
            if (Math.Abs(ang_velocity) > 0.000001f) fraction = fwd_velocity / ang_velocity;
            float current_pan = path.current_pose.pan;

            // if scan matching is active use the current estimated pan angle
            if (rob.ScanMatchingPanAngleEstimate != scanMatching.NOT_MATCHED)
                current_pan = rob.ScanMatchingPanAngleEstimate;

            float pan2 = current_pan - (ang_velocity * time_elapsed_sec);

            float new_y = path.current_pose.y + (fraction * (float)Math.Sin(current_pan)) -
                  (fraction * (float)Math.Sin(pan2));
            float new_x = path.current_pose.x - (fraction * (float)Math.Cos(current_pan)) +
                              (fraction * (float)Math.Cos(pan2));
            float new_pan = pan2 + (v * time_elapsed_sec);

            particlePose new_pose = new particlePose(new_x, new_y, new_pan, path);
            new_pose.time_step = time_step;
            path.Add(new_pose);
        }
Ejemplo n.º 17
0
 /// <summary>
 /// creates some new poses
 /// </summary>
 private void createNewPoses(float x, float y, float pan)
 {
     for (int sample = Poses.Count; sample < survey_trial_poses; sample++)
     {
         particlePath path = new particlePath(x, y, pan, max_path_length, time_step, path_ID, rob.LocalGridDimension);
         createNewPose(path);
     }
 }
Ejemplo n.º 18
0
        /// <summary>
        /// add a new path to the list
        /// </summary>
        /// <param name="path"></param>
        private void createNewPose(particlePath path)
        {
            Poses.Add(path);            
            ActivePoses.Add(path);

            // increment the path ID
            path_ID++;
            if (path_ID > UInt32.MaxValue - 3) path_ID = 0; // rollover
        }
Ejemplo n.º 19
0
        /// <summary>
        /// removes low probability poses
        /// Note that a maturity threshold is used, so that poses which may initially
        /// be unfairly judged as improbable have time to prove their worth
        /// </summary>
        private void Prune()
        {
            float max_score = float.MinValue;

            best_path = null;

            // sort poses by score
            for (int i = 0; i < Poses.Count - 1; i++)
            {
                particlePath p1 = Poses[i];

                // keep track of the bounding region within which the path tree occurs
                if (p1.current_pose.x < min_tree_x)
                {
                    min_tree_x = p1.current_pose.x;
                }
                if (p1.current_pose.x > max_tree_x)
                {
                    max_tree_x = p1.current_pose.x;
                }
                if (p1.current_pose.y < min_tree_y)
                {
                    min_tree_y = p1.current_pose.y;
                }
                if (p1.current_pose.y > max_tree_y)
                {
                    max_tree_y = p1.current_pose.y;
                }

                max_score = p1.total_score;
                particlePath winner       = null;
                int          winner_index = 0;
                for (int j = i + 1; j < Poses.Count; j++)
                {
                    particlePath p2 = Poses[i];
                    float        sc = p2.total_score;
                    if ((sc > max_score) ||
                        ((max_score == 0) && (sc != 0)))
                    {
                        max_score    = sc;
                        winner       = p2;
                        winner_index = j;
                    }
                }
                if (winner != null)
                {
                    Poses[i]            = winner;
                    Poses[winner_index] = p1;
                }
            }

            // the best path is at the top
            best_path = Poses[0];

            // It's culling season
            int cull_index = (100 - cull_threshold) * Poses.Count / 100;

            if (cull_index > Poses.Count - 2)
            {
                cull_index = Poses.Count - 2;
            }
            for (int i = Poses.Count - 1; i > cull_index; i--)
            {
                particlePath path = Poses[i];
                if (path.path.Count >= pose_maturation)
                {
                    // remove mapping hypotheses for this path
                    path.Remove(LocalGrid);

                    // now remove the path itself
                    Poses.RemoveAt(i);
                }
            }

            // garbage collect any dead paths (R.I.P.)
            List <particlePath> possible_roots = new List <particlePath>(); // stores paths where all branches have coinverged to a single possibility

            for (int i = ActivePoses.Count - 1; i >= 0; i--)
            {
                particlePath path = ActivePoses[i];

                if ((!path.Enabled) ||
                    ((path.total_children == 0) && (path.branch_pose == null) && (path.current_pose.time_step < time_step - pose_maturation - 5)))
                {
                    ActivePoses.RemoveAt(i);
                }
                else
                {
                    // record any fully collapsed paths
                    if (!path.Collapsed)
                    {
                        if (path.branch_pose == null)
                        {
                            possible_roots.Add(path);
                        }
                        else
                        {
                            if (path.branch_pose.path.Collapsed)
                            {
                                possible_roots.Add(path);
                            }
                        }
                    }
                }
            }

            if (possible_roots.Count == 1)
            {
                // collapse tha psth
                particlePath path = possible_roots[0];

                if (path.branch_pose != null)
                {
                    particlePath previous_path = path.branch_pose.path;
                    previous_path.Distill(LocalGrid);
                    path.branch_pose.parent = null;
                    path.branch_pose        = null;
                }

                path.Collapsed = true;

                // take note of the time step.  This is for use with the display functions only
                root_time_step = path.current_pose.time_step;
            }

            if (best_path != null)
            {
                // update the current robot position with the best available pose
                if (current_robot_pose == null)
                {
                    current_robot_pose =
                        new pos3D(best_path.current_pose.x,
                                  best_path.current_pose.y,
                                  0);
                }
                current_robot_pose.pan   = best_path.current_pose.pan;
                current_robot_path_score = best_path.total_score;

                // generate new poses from the ones which have survived culling
                int new_poses_required = survey_trial_poses; // -Poses.Count;
                int max = Poses.Count;
                int n = 0, added = 0;
                while ((max > 0) &&
                       (n < new_poses_required * 4) &&
                       (added < new_poses_required))
                {
                    // identify a potential parent at random,
                    // from one of the surviving paths
                    int          random_parent_index = rnd.Next(max - 1);
                    particlePath parent_path         = Poses[random_parent_index];

                    // only mature parents can have children
                    if (parent_path.path.Count >= pose_maturation)
                    {
                        // generate a child branching from the parent path
                        particlePath child_path = new particlePath(parent_path, path_ID, rob.LocalGridDimension);
                        createNewPose(child_path);
                        added++;
                    }
                    n++;
                }

                // like salmon, after parents spawn they die
                if (added > 0)
                {
                    for (int i = max - 1; i >= 0; i--)
                    {
                        Poses.RemoveAt(i);
                    }
                }

                // if the robot has rotated through a large angle since
                // the previous time step then reset the scan matching estimate
                //if (Math.Abs(best_path.current_pose.pan - rob.pan) > rob.ScanMatchingMaxPanAngleChange * Math.PI / 180)
                rob.ScanMatchingPanAngleEstimate = scanMatching.NOT_MATCHED;
            }


            PosesEvaluated = false;
        }
Ejemplo n.º 20
0
 /// <summary>
 /// updates the given path with the score obtained from the current pose
 /// Note that scores are always expressed as log odds matching probability
 /// from grid localisation
 /// </summary>
 /// <param name="path">path whose score is to be updated</param>
 /// <param name="new_score">the new score to be added to the path</param>
 public void updatePoseScore(particlePath path, float new_score)
 {
     path.total_score += new_score;
     path.local_score += new_score;
 }
Ejemplo n.º 21
0
        /// <summary>
        /// turns a list of path segments into a list of individual poses
        /// </summary>
        private void updatePath()
        {
            particlePose prev_pose=null;
            path = new particlePath(999999999);

            min_x = 9999;
            min_y = 9999;
            max_x = -9999;
            max_y = -9999;
            for (int s = 0; s < pathSegments.Count; s++)
            {
                simulationPathSegment segment = (simulationPathSegment)pathSegments[s];

                // get the last pose
                if (s > 0)
                    prev_pose = (particlePose)path.path[path.path.Count - 1];

                // update the list of poses
                List<particlePose> poses = segment.getPoses();

                if (prev_pose != null)
                {
                    // is the last pose position the same as the first in this segment?
                    // if so, remove the last pose added to the path
                    particlePose firstPose = (particlePose)poses[0];
                    if (((int)firstPose.x == (int)prev_pose.x) &&
                        ((int)firstPose.y == (int)prev_pose.y) &&
                        (Math.Abs(firstPose.pan - prev_pose.pan) < 0.01f))
                        path.path.RemoveAt(path.path.Count - 1);
                }

                for (int i = 0; i < poses.Count; i++)
                {
                    particlePose pose = (particlePose)poses[i];
                    if (pose.x < min_x) min_x = pose.x;
                    if (pose.y < min_y) min_y = pose.y;
                    if (pose.x > max_x) max_x = pose.x;
                    if (pose.y > max_y) max_y = pose.y;
                    path.Add(pose);
                }
            }

            // update the path velocities
            velocities = path.getVelocities(0, 0, time_per_index_sec);
        }
Ejemplo n.º 22
0
        /// <summary>
        /// turns a list of path segments into a list of individual poses
        /// </summary>
        private void updatePath()
        {
            particlePose prev_pose = null;

            path = new particlePath(999999999);

            min_x = 9999;
            min_y = 9999;
            max_x = -9999;
            max_y = -9999;
            for (int s = 0; s < pathSegments.Count; s++)
            {
                simulationPathSegment segment = (simulationPathSegment)pathSegments[s];

                // get the last pose
                if (s > 0)
                {
                    prev_pose = (particlePose)path.path[path.path.Count - 1];
                }

                // update the list of poses
                List <particlePose> poses = segment.getPoses();

                if (prev_pose != null)
                {
                    // is the last pose position the same as the first in this segment?
                    // if so, remove the last pose added to the path
                    particlePose firstPose = (particlePose)poses[0];
                    if (((int)firstPose.x == (int)prev_pose.x) &&
                        ((int)firstPose.y == (int)prev_pose.y) &&
                        (Math.Abs(firstPose.pan - prev_pose.pan) < 0.01f))
                    {
                        path.path.RemoveAt(path.path.Count - 1);
                    }
                }

                for (int i = 0; i < poses.Count; i++)
                {
                    particlePose pose = (particlePose)poses[i];
                    if (pose.x < min_x)
                    {
                        min_x = pose.x;
                    }
                    if (pose.y < min_y)
                    {
                        min_y = pose.y;
                    }
                    if (pose.x > max_x)
                    {
                        max_x = pose.x;
                    }
                    if (pose.y > max_y)
                    {
                        max_y = pose.y;
                    }
                    path.Add(pose);
                }
            }

            // update the path velocities
            velocities = path.getVelocities(0, 0, time_per_index_sec);
        }
Ejemplo n.º 23
0
        /// <summary>
        /// plan a route to a given location
        /// </summary>
        /// <param name="destination_waypoint"></param>
        public void PlanRoute(String destination_waypoint)
        {
            // clear the planned path
            planned_path = null;

            // retrieve the waypoint from the list of work sites
            kmlPlacemarkPoint waypoint = worksites.GetPoint(destination_waypoint);
            if (waypoint != null)
            {
                // get the destination waypoint position in millimetres
                // (the KML format stores positions in degrees)
                float destination_x = 0, destination_y = 0;
                waypoint.GetPositionMillimetres(ref destination_x, ref destination_y);

                // the best performing local grid
                occupancygridMultiHypothesis grid = LocalGrid[best_grid_index];

                // create a planner
                createPlanner(grid);

                // set variables, in the unlikely case that the centre position
                // of the grid has been changed since the planner was initialised
                planner.init(grid.navigable_space, grid.x, grid.y);

                // update the planner, in order to assign safety scores to the
                // navigable space.  The efficiency of this could be improved
                // by updating only those areas of the map which have changed
                planner.Update(0, 0, LocalGridDimension - 1, LocalGridDimension - 1);

                // create the plan
                List<float> new_plan = planner.CreatePlan(x, y, destination_x, destination_y);
                if (new_plan.Count > 0)
                {
                    // convert the plan into a set of poses
                    planned_path = new particlePath(new_plan.Count / 2);
                    float prev_xx = 0, prev_yy = 0;
                    for (int i = 0; i < new_plan.Count; i += 2)
                    {
                        float xx = (float)new_plan[i];
                        float yy = (float)new_plan[i + 1];
                        if (i > 0)
                        {
                            float dx = xx - prev_xx;
                            float dy = yy - prev_yy;
                            float dist = (float)Math.Sqrt((dx * dx) + (dy * dy));
                            if (dist > 0)
                            {
                                // TODO: check this pan angle
                                float pan = (float)Math.Sin(dx / dist);
                                if (dy < 0) pan = (2 * (float)Math.PI) - pan;

                                // create a pose and add it to the planned path
                                particlePose new_pose = new particlePose(prev_xx, prev_yy, pan, planned_path);
                                planned_path.Add(new_pose);
                            }
                        }
                        prev_xx = xx;
                        prev_yy = yy;
                    }
                }
            }
        }