Example #1
0
 internal int ts_distance_scan_to_map(ts_scan_t scan, ts_position_t pos)
 {
     double c, s;
     int i, x, y, nb_points = 0;
     Int64 sum;
     c = exMath.Cos(pos.theta * exMath.PI / 180);
     s = exMath.Sin(pos.theta * exMath.PI / 180);
     // Translate and rotate scan to robot position
     // and compute the distance
     for (i = 0, sum = 0; i != scan.nb_points; i++)
     {
         if (scan.value[i] != TS_NO_OBSTACLE)
         {
             x = (int)exMath.Floor((pos.x + c * scan.x[i] - s * scan.y[i]) * TS_MAP_SCALE + 0.5);
             y = (int)exMath.Floor((pos.y + s * scan.x[i] + c * scan.y[i]) * TS_MAP_SCALE + 0.5);
             //Check boundaries
             if (x >= 0 && x < TS_MAP_SIZE && y >= 0 && y < TS_MAP_SIZE)
             {
                 sum += map.map[y * TS_MAP_SIZE + x];
                 nb_points++;
             }
         }
     }
     if (nb_points > 0) sum = sum * 1024 / nb_points;
     else sum = 2000000000;
     return (int)sum;
 }
Example #2
0
        internal int ts_distance_scan_to_map(ts_scan_t scan, ts_position_t pos)
        {
            double c, s;
            int    i, x, y, nb_points = 0;
            Int64  sum;

            c = exMath.Cos(pos.theta * exMath.PI / 180);
            s = exMath.Sin(pos.theta * exMath.PI / 180);
            // Translate and rotate scan to robot position
            // and compute the distance
            for (i = 0, sum = 0; i != scan.nb_points; i++)
            {
                if (scan.value[i] != TS_NO_OBSTACLE)
                {
                    x = (int)exMath.Floor((pos.x + c * scan.x[i] - s * scan.y[i]) * TS_MAP_SCALE + 0.5);
                    y = (int)exMath.Floor((pos.y + s * scan.x[i] + c * scan.y[i]) * TS_MAP_SCALE + 0.5);
                    //Check boundaries
                    if (x >= 0 && x < TS_MAP_SIZE && y >= 0 && y < TS_MAP_SIZE)
                    {
                        sum += map.map[y * TS_MAP_SIZE + x];
                        nb_points++;
                    }
                }
            }
            if (nb_points > 0)
            {
                sum = sum * 1024 / nb_points;
            }
            else
            {
                sum = 2000000000;
            }
            return((int)sum);
        }
Example #3
0
 public static void NewScan(ts_scan_t scan, ts_position_t position)
 {
     //a.ts_distance_scan_to_map(scan, position);
     if (a != null)
     {
         a.ts_map_update(scan, position, 50);
     }
 }
Example #4
0
        internal void ts_map_update(ts_scan_t scan, ts_position_t pos, int quality)
        {
            //cos sin quality
            double c, s, q;
            //Pos scan rotated
            double x2p, y2p;
            //x1y1: Pos scaled ||xpyp: Absolute pos scan
            int    i, x1, y1, x2, y2, xp, yp, value;
            double add, dist;

            c  = exMath.Cos(pos.theta * exMath.PI / 180);
            s  = exMath.Sin(pos.theta * exMath.PI / 180);
            x1 = (int)exMath.Floor(pos.x * TS_MAP_SCALE + 0.5);
            y1 = (int)exMath.Floor(pos.y * TS_MAP_SCALE + 0.5);
            // Translate and rotate scan to robot position
            for (i = 0; i != scan.nb_points; i++)
            {
                //Before TS_HOLE_WIDTH
                x2p = c * scan.x[i] - s * scan.y[i];
                y2p = s * scan.x[i] + c * scan.y[i];
                xp  = (int)exMath.Floor((pos.x + x2p) * TS_MAP_SCALE + 0.5);
                yp  = (int)exMath.Floor((pos.y + y2p) * TS_MAP_SCALE + 0.5);
                /*************************************************************************/
                //CAUTION: If x2p or y2p are too high, the Sqrt operation will be very slow
                double x2p_meter = x2p / 1000;
                double y2p_meter = y2p / 1000;
                dist  = exMath.Sqrt(x2p_meter * x2p_meter + y2p_meter * y2p_meter);
                dist *= 1000;
                /************************************************************************/

                add = TS_HOLE_WIDTH / 2 / dist;
                //After TS_HOLE_WIDTH
                x2p *= TS_MAP_SCALE * (1 + add);
                y2p *= TS_MAP_SCALE * (1 + add);
                x2   = (int)exMath.Floor(pos.x * TS_MAP_SCALE + x2p + 0.5);
                y2   = (int)exMath.Floor(pos.y * TS_MAP_SCALE + y2p + 0.5);
                if (scan.value[i] == TS_NO_OBSTACLE)
                {
                    q     = quality / 2;
                    value = TS_NO_OBSTACLE;
                }
                else
                {
                    q     = quality;
                    value = TS_OBSTACLE;
                }
                ts_map_laser_ray(x1, y1, x2, y2, xp, yp, value, (int)q);
            }
        }
Example #5
0
        internal void ts_map_update(ts_scan_t scan, ts_position_t pos, int quality)
        {
            //cos sin quality
            double c, s, q;
            //Pos scan rotated
            double x2p, y2p;
            //x1y1: Pos scaled ||xpyp: Absolute pos scan
            int i, x1, y1, x2, y2, xp, yp, value;
            double add, dist;
            c = exMath.Cos(pos.theta * exMath.PI / 180);
            s = exMath.Sin(pos.theta * exMath.PI / 180);
            x1 = (int)exMath.Floor(pos.x * TS_MAP_SCALE + 0.5);
            y1 = (int)exMath.Floor(pos.y * TS_MAP_SCALE + 0.5);
            // Translate and rotate scan to robot position
            for (i = 0; i != scan.nb_points; i++)
            {
                //Before TS_HOLE_WIDTH
                x2p = c * scan.x[i] - s * scan.y[i];
                y2p = s * scan.x[i] + c * scan.y[i];
                xp = (int)exMath.Floor((pos.x + x2p) * TS_MAP_SCALE + 0.5);
                yp = (int)exMath.Floor((pos.y + y2p) * TS_MAP_SCALE + 0.5);
                /*************************************************************************/
                //CAUTION: If x2p or y2p are too high, the Sqrt operation will be very slow
                double x2p_meter = x2p / 1000;
                double y2p_meter = y2p / 1000;
                dist = exMath.Sqrt(x2p_meter * x2p_meter + y2p_meter * y2p_meter);
                dist *= 1000;
                /************************************************************************/

                add = TS_HOLE_WIDTH / 2 / dist;
                //After TS_HOLE_WIDTH
                x2p *= TS_MAP_SCALE * (1 + add);
                y2p *= TS_MAP_SCALE * (1 + add);
                x2 = (int)exMath.Floor(pos.x * TS_MAP_SCALE + x2p + 0.5);
                y2 = (int)exMath.Floor(pos.y * TS_MAP_SCALE + y2p + 0.5);
                if (scan.value[i] == TS_NO_OBSTACLE)
                {
                    q = quality / 2;
                    value = TS_NO_OBSTACLE;
                }
                else
                {
                    q = quality;
                    value = TS_OBSTACLE;
                }
                ts_map_laser_ray(x1, y1, x2, y2, xp, yp, value, (int)q);
            }
        }
Example #6
0
 public static void NewScan(ts_scan_t scan, ts_position_t position)
 {
     //a.ts_distance_scan_to_map(scan, position);
     if (a != null)
         a.ts_map_update(scan, position, 50);
 }