TrimPath.emam 3.46 KB
Newer Older
Alexander Ryndin's avatar
Alexander Ryndin committed
1
2
3
4
5
6
7
package de.rwth.armin.modeling.autopilot.behavior;

import de.rwth.armin.modeling.autopilot.common.*;

// trims the planned path
// such that the first segment
// is the closest to the current vehicle's position
Alexander Ryndin's avatar
Alexander Ryndin committed
8
// and the first point is the projection
Alexander Ryndin's avatar
Alexander Ryndin committed
9
// of the current position onto the planned path
Alexander Ryndin's avatar
Alexander Ryndin committed
10
11
component TrimPath {
  port
Alexander Ryndin's avatar
Alexander Ryndin committed
12
13
14
15
16
    in  Q (-oo m : 0.01 m : oo m)                currentPositionX,
    in  Q (-oo m : 0.01 m : oo m)                currentPositionY,
    in  Z (0 : 100)                              plannedTrajectoryLength,
    in  Q (-oo m : 0.01 m : oo m) ^ {1,100}      plannedTrajectoryX,
    in  Q (-oo m : 0.01 m : oo m) ^ {1,100}      plannedTrajectoryY,
Alexander Ryndin's avatar
Alexander Ryndin committed
17

Alexander Ryndin's avatar
Alexander Ryndin committed
18
19
20
    out Z (0 : 100)                              trimmedTrajectoryLength,
    out Q (-oo m : 0.01 m : oo m) ^ {1,100}      trimmedTrajectoryX,
    out Q (-oo m : 0.01 m : oo m) ^ {1,100}      trimmedTrajectoryY;
Alexander Ryndin's avatar
Alexander Ryndin committed
21
22

  implementation Math {
Alexander Ryndin's avatar
Alexander Ryndin committed
23
24
25
26
27
28
    trimmedTrajectoryLength = 0;
    if plannedTrajectoryLength == 1
      trimmedTrajectoryLength = 1;
      trimmedTrajectoryX(1,1) = plannedTrajectoryX(1,1);
      trimmedTrajectoryY(1,1) = plannedTrajectoryY(1,1);
    elseif plannedTrajectoryLength > 1
Alexander Ryndin's avatar
Alexander Ryndin committed
29
      Q closestSegmentIndex = -1;
Alexander Ryndin's avatar
Alexander Ryndin committed
30
      Q closestSegmentDistance = -1.0;
Alexander Ryndin's avatar
Alexander Ryndin committed
31
32
33
34
35
36
      Q lastSegmentIndex = plannedTrajectoryLength - 1;
      for i = 1:lastSegmentIndex
        Q p1x = plannedTrajectoryX(1,i);
        Q p1y = plannedTrajectoryY(1,i);
        Q p2x = plannedTrajectoryX(1,i+1);
        Q p2y = plannedTrajectoryY(1,i+1);
Alexander Ryndin's avatar
Alexander Ryndin committed
37
        // projection
Alexander Ryndin's avatar
Alexander Ryndin committed
38
39
        Q vx = currentPositionX - p1x;
        Q vy = currentPositionY - p1y;
Alexander Ryndin's avatar
Alexander Ryndin committed
40
41
42
43
44
        Q v12x = p2x - p1x;
        Q v12y = p2y - p1y;
        Q k = ( vx*v12x + vy*v12y ) / ( v12x*v12x + v12y*v12y );
        Q projection_x = p1x + k * v12x;
        Q projection_y = p1y + k * v12y;
Alexander Ryndin's avatar
Alexander Ryndin committed
45
46
47
48
        Q is_projection_on_segment = ((p1x-projection_x) * (p2x-projection_x) <= 0) && ((p1y-projection_y) * (p2y-projection_y) <= 0);
        if (is_projection_on_segment)
          Q d_proj_sqr = (currentPositionX-projection_x)*(currentPositionX-projection_x) + (currentPositionY-projection_y)*(currentPositionY-projection_y);
          Q d_proj = sqrt( d_proj_sqr );
Alexander Ryndin's avatar
Alexander Ryndin committed
49
          if (closestSegmentDistance < 0) || (d_proj < closestSegmentDistance)
Alexander Ryndin's avatar
Alexander Ryndin committed
50
51
            closestSegmentIndex = i;
            closestSegmentDistance = d_proj;
Alexander Ryndin's avatar
Alexander Ryndin committed
52
53
            trimmedTrajectoryX(1,1) = projection_x;
            trimmedTrajectoryY(1,1) = projection_y;
Alex Ryndin's avatar
Alex Ryndin committed
54
55
          end
        else
Alexander Ryndin's avatar
Alexander Ryndin committed
56
57
58
59
          Q d1_sqr = (currentPositionX-p1x)*(currentPositionX-p1x) + (currentPositionY-p1y)*(currentPositionY-p1y);
          Q d1 = sqrt( d1_sqr );
          Q d2_sqr = (currentPositionX-p2x)*(currentPositionX-p2x) + (currentPositionY-p2y)*(currentPositionY-p2y);
          Q d2 = sqrt( d2_sqr );
Alexander Ryndin's avatar
Alexander Ryndin committed
60
          Q d_min = min(d1,d2);
Alexander Ryndin's avatar
Alexander Ryndin committed
61
          if (closestSegmentDistance < 0) || (d_min < closestSegmentDistance)
Alexander Ryndin's avatar
Alexander Ryndin committed
62
63
            closestSegmentIndex = i;
            closestSegmentDistance = d_min;
Alexander Ryndin's avatar
Alexander Ryndin committed
64
65
            trimmedTrajectoryX(1,1) = projection_x;
            trimmedTrajectoryY(1,1) = projection_y;
Alex Ryndin's avatar
Alex Ryndin committed
66
67
          end
        end
Alexander Ryndin's avatar
Alexander Ryndin committed
68
      end
Alexander Ryndin's avatar
Alexander Ryndin committed
69
70
71
72
73
74
75
76
77
78
      if closestSegmentIndex > -1
        Q currentFree = 2; // 1st point is always the projection
        Q start = closestSegmentIndex + 1;
        for i = start:plannedTrajectoryLength
          trimmedTrajectoryX(1,currentFree) = plannedTrajectoryX(1,i);
          trimmedTrajectoryY(1,currentFree) = plannedTrajectoryY(1,i);
          currentFree += 1;
        end
        trimmedTrajectoryLength = currentFree - 1;
      end
Alex Ryndin's avatar
Alex Ryndin committed
79
    end
Alexander Ryndin's avatar
Alexander Ryndin committed
80
81
  }
}