SIM_aircraft Waypoint Update (#1453)

* Set up a waypoint system that reads waypoints from an input file.

On branch aircraftUpdate
Changes to be committed:
    new file:   trick_sims/SIM_aircraft/Modified_data/default.waypoints
	modified:   trick_sims/SIM_aircraft/RUN_test/input.py
	modified:   trick_sims/SIM_aircraft/models/Aircraft/include/Aircraft.hh
	modified:   trick_sims/SIM_aircraft/models/Aircraft/src/Aircraft.cpp
	modified:   trick_sims/SIM_aircraft/models/graphics/src/trick/AircraftDisplay.java

* Fixed the Segmentation Fault and completed the waypoint configuration.

On branch aircraftUpdate
Changes to be committed:
	modified:   trick_sims/SIM_aircraft/RUN_test/input.py
	modified:   trick_sims/SIM_aircraft/models/Aircraft/include/Aircraft.hh
	modified:   trick_sims/SIM_aircraft/models/Aircraft/src/Aircraft.cpp

* Created README.md to document recent changes

* Adding in ability to set icons for waypoints.
  On branch aircraftUpdate_WaypointIcons
  Changes to be committed:
	modified:   trick_sims/SIM_aircraft/Modified_data/default.waypoints
	new file:   trick_sims/SIM_aircraft/images/CompassRose.png
	new file:   trick_sims/SIM_aircraft/images/Figure1.png
	new file:   trick_sims/SIM_aircraft/images/Figure2.png
	new file:   trick_sims/SIM_aircraft/images/cheese_64x64.png
	new file:   trick_sims/SIM_aircraft/images/mouse_128x128.png
	new file:   trick_sims/SIM_aircraft/images/strawberry_64x64.png
	new file:   trick_sims/SIM_aircraft/images/twoWheelRover.png
	new file:   trick_sims/SIM_aircraft/images/wp0.png
	new file:   trick_sims/SIM_aircraft/images/wp1.png
	new file:   trick_sims/SIM_aircraft/images/wp10.png
	new file:   trick_sims/SIM_aircraft/images/wp11.png
	new file:   trick_sims/SIM_aircraft/images/wp2.png
	new file:   trick_sims/SIM_aircraft/images/wp3.png
	new file:   trick_sims/SIM_aircraft/images/wp4.png
	new file:   trick_sims/SIM_aircraft/images/wp5.png
	new file:   trick_sims/SIM_aircraft/images/wp6.png
	new file:   trick_sims/SIM_aircraft/images/wp7.png
	new file:   trick_sims/SIM_aircraft/images/wp8.png
	new file:   trick_sims/SIM_aircraft/images/wp9.png
	modified:   trick_sims/SIM_aircraft/models/graphics/src/trick/AircraftDisplay.java

* Reformatted the README file

* Updating the build process to use Maven

* Updating the README file

* Finalized the implementation for adding icons for each waypoint.

* Removed commented code that will not be used.

* Updated the SIM_aircraft documentation to include the waypoint icon implementation.

* Minor README fix and file cleanup

trick_sims/SIM_aircraft/Modified_data/default.waypoints

@@ -0,0 +1,6 @@
+     0.0,  25000.0,images/wp0.png
+ 21650.0,  12500.0,images/wp1.png
+ 21650.0, -12500.0,images/wp2.png
+     0.0, -25000.0,images/wp3.png
+-21650.0, -12500.0,images/wp4.png
+-21650.0,  12500.0,images/wp5.png

trick_sims/SIM_aircraft/README.md

@@ -16,7 +16,7 @@ In this simulation, we can manually control the aircraft by specifying:
 or automatically.
 
 ## Building the Simulation
-In the ```SIM_aircrafte``` directory, type **```trick-CP```** to build the simulation executable. When it's complete, you should see:
+In the ```SIM_aircraft``` directory, type **```trick-CP```** to build the simulation executable. When it's complete, you should see:
 
 ```
 === Simulation make complete ===
@@ -32,9 +32,36 @@ In the SIM_aircraft directory:
 The Sim Control Panel, and a graphics client called "Aircraft Display" should appear.
 
 Click Start on the Trick Sim Control Panel.
-q
+
 ![](images/GraphicsClient.png)
 
+### Manual Control of the Aircraft
+The two sliders on the bottom left and right are for setting the aircraft's desired speed and heading, respectively. The desired speed can range from 0 to 250 meters per second. The desired heading is measured in radians, from - $\pi$ to $\pi$.
+
+### Aircraft Autopilot
+The autopilot feature is toggled on and off by the ```Autopilot OFF/ON``` button at the bottom of the graphics client. 
+
+![](images/GraphicsClientAutopilot.png)
+
+When active, the aircraft's heading is automatically calculated to approach a series of waypoints. These waypoints are marked on the map by the image provided in the input file.
+
+#### Adding Waypoints with an Input File
+Waypoints can be added to the simulation using an input file. 
+
+![](images/WaypointInputFile.png)
+
+When the simulation starts they are read into the simulation from the specified file. Currently that file is `default.waypoints`  within the `Modified_data` folder. 
+
+#### Sample Input in [`default.waypoints`](Modified\_data/default.waypoints):
+
+         0.0,  25000.0,images/wp0.png
+     21650.0,  12500.0,images/wp1.png
+     21650.0, -12500.0,images/wp2.png
+         0.0, -25000.0,images/wp3.png
+    -21650.0, -12500.0,images/wp4.png
+    -21650.0,  12500.0,images/wp5.png
+Each line should have three pieces of data, separated by commas. The first two are the coordinates. They are formatted like GPS coordinates, with the first number being the vertical distance, or 'North', and the second being the horizontal, or 'West', distance. The third part of the line is the path to the image that will be used as the waypoint marker.
+
 ## Dynamics Model
 
 ### General Strategy for Motion
@@ -93,25 +120,25 @@ So, we must limit $thrust_{actual}$ to that which our engine can actually produc
 ![](images/Equation_6.png)
 ![](images/Equation_7.png)
 
-So, the magnitude of the thrust force vector is $thrust_{actual}$ , and the direction is [1,0] in Body coordinates. 
+So, the magnitude of the thrust force vector is $thrust_{actual}$ , and the direction is [1,0] in Body coordinates. 
 
 ### Aircraft Heading
 
-If the aircraft is moving directly north, $\psi = 0$ . Directly west is $\pi/2$ . Directly east is $-\pi/2$ .
+If the aircraft is moving directly north, $\psi = 0$ . Directly west is $\pi/2$ . Directly east is $-\pi/2$ .
 
 ![](images/AircraftHeading.png)
 
 ### Heading Control
 
-To control our heading, we’d like to calculate a heading rate that is proportional to the difference between our current heading and our desired heading. 
+To control our heading, we’d like to calculate a heading rate that is proportional to the difference between our current heading and our desired heading. 
 
 ![](images/HeadingControl.png)
 
 ![](images/Equation_8.png)
 
-Notice (in the figure) that $\psi_{diff}$ is in the direction that we want to turn. But, what if ( $\left| \psi_{diff} \right| > \pi$ )? That is, it’s greater than 180 degrees?
+Notice (in the figure) that $\psi_{diff}$ is in the direction that we want to turn. But, what if ( $\left| \psi_{diff} \right| > \pi$ )? That is, it’s greater than 180 degrees?
 
-Do you ever have to turn more than 𝜋 radians (180 degrees) to face any direction? No, you don’t.
+Do you ever have to turn more than 𝜋 radians (180 degrees) to face any direction? No, you don’t.
 
 Let’s now define $\pis_{error}$ as the actual direction of turning.
 
@@ -121,19 +148,19 @@ This is how we will calculate our desired heading rate:
 
 ![](images/Equation_10.png)
 
-That is, our desired heading rate is proportional to the amount and direction that we need to turn.
+That is, our desired heading rate is proportional to the amount and direction that we need to turn.
 
 $G_{\psi}$ is a proportionality constant.
 
 ### Turning Force
 
-The turning force we needed to turn the desired rate is :
+The turning force we needed to turn the desired rate is :
 
 ![](images/Equation_11.png)
 
-Like our desired speed, just we want something doesn’t mean that the aircraft is capable of it. In our case we need to limit the magnitude of our force to 1 G ( force of gravity), that is 9.8 m/s2 * m. So if our mass is 5000 kg, we need to limit our turning force to between -49000 .. 49000 Newtons.
+Like our desired speed, just we want something doesn’t mean that the aircraft is capable of it. In our case we need to limit the magnitude of our force to 1 G ( force of gravity), that is 9.8 m/s2 * m. So if our mass is 5000 kg, we need to limit our turning force to between -49000 .. 49000 Newtons.
 
-But we need that total force in world coordinates so we can move our aircraft around in the world.
-So, if our heading of our aircraft is 𝜓, what is our direction vector expressed in world coordinates? 
+But we need that total force in world coordinates so we can move our aircraft around in the world.
+So, if our heading of our aircraft is 𝜓, what is our direction vector expressed in world coordinates? 
 
 ![](images/Equation_12.png)

trick_sims/SIM_aircraft/RUN_test/input.py

@@ -7,6 +7,17 @@ dyn.aircraft.set_desired_compass_heading(45.0);
 dyn.aircraft.autoPilot = True;
 dyn.aircraft.desired_speed = 200 # meters per second
 
+#==========================================
+# Read the waypoints from the input file
+#==========================================
+
+waypoints_path = "Modified_data/default.waypoints"
+fp = open(waypoints_path, "r")
+for line in fp:
+    fields = line.split(",")
+    dyn.aircraft.add_waypoint( float(fields[0]), float(fields[1]))
+dyn.aircraft.reset_trip()
+
 #==========================================
 # Start the Satellite Graphics Client
 #==========================================
@@ -16,6 +27,7 @@ AircraftDisplay_path = "models/graphics/build/AircraftDisplay.jar"
 if (os.path.isfile(AircraftDisplay_path)) :
     AircraftDisplay_cmd = "java -jar " \
                    + AircraftDisplay_path \
+                   + " -w " + waypoints_path \
                    + " " + str(varServerPort) + " &" ;
     print(AircraftDisplay_cmd)
     os.system( AircraftDisplay_cmd);

trick_sims/SIM_aircraft/models/Aircraft/include/Aircraft.hh

@@ -6,6 +6,13 @@ LIBRARY DEPENDENCIES:
 #ifndef Aircraft_HH
 #define Aircraft_HH
 
+#include <vector>
+
+class WayPoint {
+    public:
+    double pos[2];
+};
+
 class Aircraft {
     public:
     // Calculated by numeric integration of state derivatives.
@@ -13,7 +20,8 @@ class Aircraft {
   	double vel[2];
 
     // Updated by control logic;
-    int current_waypoint;
+    std::vector<WayPoint> waypointQueue;
+    std::vector<WayPoint>::iterator current_waypoint;
 
     // Static Parameters (Don't change during the simulation.)
   	double mass;
@@ -40,6 +48,8 @@ class Aircraft {
     int control();
 
     void set_desired_compass_heading( double compass_heading);
+    void add_waypoint(double n, double w);
+    void reset_trip();
 
     void calc_total_force( double (&F_total_body)[2]);
     void calc_drag_force( double (&F_drag_body)[2]);

trick_sims/SIM_aircraft/models/Aircraft/src/Aircraft.cpp

@@ -10,28 +10,12 @@ LIBRARY DEPENDENCY:
 #include <math.h>
 #include <iostream>
 
-class WayPoint {
-    public:
-    double pos[2];
-};
-
-// Waypoints are in World Coordinates (West, North).
-#define NUMBER_OF_WAYPOINTS 6
-WayPoint waypoint[ NUMBER_OF_WAYPOINTS ] = {
-    {    0.0,  25000.0},
-    { 21650.0,  12500.0},
-    { 21650.0, -12500.0},
-    {    0.0,  -25000.0},
-    {-21650.0, -12500.0},
-    {-21650.0,  12500.0}
-};
-
 int Aircraft::default_data() {
     pos[0] = 0.0;         // m
     pos[1] = 0.0;
     vel[0] = 100.0;       // m/s
     vel[1] = 0.0;
-    current_waypoint = 0;
+    
     mass = 5000;              // kg
     thrust_mag_max = 45000;   // N
     K_drag = 0.72;
@@ -39,7 +23,7 @@ int Aircraft::default_data() {
     set_desired_compass_heading(45.0);
     desired_speed = 200; // m/s
     autoPilot = false;
-    current_waypoint = 0;
+    current_waypoint = waypointQueue.begin();
     return (0);
 }
 
@@ -140,21 +124,34 @@ void Aircraft::rotateBodyToWorld( double (&F_total_world)[2], double (&F_total_b
     F_total_world[1] =  sin(heading) * F_total_body[0] + cos(heading) * F_total_body[1];
 }
 
+void Aircraft::reset_trip() {
+    current_waypoint = waypointQueue.begin();
+}
+
+void Aircraft::add_waypoint(double n, double w) {
+    WayPoint wp = { {n, w} };
+    waypointQueue.push_back(wp);
+}
+
 int Aircraft::control() {
     if (autoPilot) {
-        if (NUMBER_OF_WAYPOINTS > 0) {
+        if (waypointQueue.size() > 0) {
             // Calculate the difference between where we want to be, and where we are.
             double posDiff[2];
-            vector_difference(posDiff, waypoint[current_waypoint].pos, pos);
+            vector_difference(posDiff, current_waypoint->pos, pos);
             // Calculate bearing to waypoint.
             desired_heading = northWestToPsi(posDiff);
             // Calculate distance to waypoint.
             double distanceToWaypoint = vector_magnitude(posDiff);
             // If we've arrived, that is we're close enough, go to the next waypoint.
             if (distanceToWaypoint < 100.0) {
-              current_waypoint ++;
-              current_waypoint = current_waypoint % NUMBER_OF_WAYPOINTS;
+                ++current_waypoint;
+                if (current_waypoint == waypointQueue.end()) {
+                    current_waypoint = waypointQueue.begin();
+                }
             }
+        } else {
+            std::cout << "!!!  Waypoint List Empty  !!!" << std::endl;
         }
     }
     return 0;

trick_sims/SIM_aircraft/models/graphics/Makefile

@@ -3,4 +3,4 @@ all:
 	mvn package
 
 clean:
-	rm -rf build
+	rm -rf build

trick_sims/SIM_aircraft/models/graphics/pom.xml

@@ -118,4 +118,4 @@
       </plugins>
     </pluginManagement>
   </build>
-</project>
+</project>

trick_sims/SIM_aircraft/models/graphics/src/main/java/trick/aircraftDisplay/AircraftDisplay.java

@@ -13,6 +13,7 @@ import java.awt.image.AffineTransformOp;
 import java.awt.image.BufferedImage;
 import java.awt.event.ActionListener; 
 import java.awt.event.ActionEvent;
+import java.awt.Toolkit;
 import javax.swing.event.ChangeEvent;
 import javax.swing.event.ChangeListener;
 import java.awt.event.ItemEvent;
@@ -24,6 +25,7 @@ import java.io.DataOutputStream;
 import java.io.IOException;
 import java.io.InputStreamReader;
 import java.io.FileReader;
+import java.io.FileNotFoundException;
 import java.lang.Math;
 import java.net.Socket;
 import java.util.*;
@@ -40,11 +42,25 @@ import javax.swing.border.EtchedBorder;
 import javax.swing.BoxLayout;
 import java.awt.Color;
 
+import javax.imageio.ImageIO;
+import java.io.File;
+
 /**
  *
  * @author penn
  */
 
+class Waypoint {
+    public double north, west;
+    public BufferedImage icon;
+    
+    public Waypoint(double n, double w, BufferedImage i) {
+        north = n;
+        west = w;
+        icon = i;
+    }
+}
+
 class ScenePoly {
     public Color color;
     public int n;
@@ -70,6 +86,8 @@ class SkyView extends JPanel {
     private double desired_heading;
     private Boolean autopilot;
 
+    private ArrayList<Waypoint> waypoints;
+
     // Origin of world coordinates in jpanel coordinates.
     private int worldOriginX;
     private int worldOriginY;
@@ -97,6 +115,19 @@ class SkyView extends JPanel {
 
         workPolyX = new int[30];
         workPolyY = new int[30];
+
+        waypoints = new ArrayList<Waypoint>();
+    }
+
+    public void addWaypoint( double n, double w, String fp) {
+        BufferedImage img;
+        try {
+            img = ImageIO.read(new File(fp));
+            waypoints.add(new Waypoint(n,w,img));
+        } catch(Exception e) {
+            System.out.printf("Waypoint (%.1f,%.1f) not added to map.");
+        }
+
     }
 
     public void setAircraftPos( double n, double w) {
@@ -166,6 +197,13 @@ class SkyView extends JPanel {
         g.fillOval(x,y,r,r);
     }
 
+    public void drawWaypoint(Graphics2D g, Waypoint wp) {
+        int x = 0, y = 0;
+        x = (int)((worldOriginX - scale * wp.west) - (wp.icon.getWidth()/2));
+        y = (int)((worldOriginY - scale * wp.north) - (wp.icon.getHeight()/2));
+        g.drawImage(wp.icon, x, y, null);
+    }
+
     public void drawScenePoly(Graphics2D g, ScenePoly p, double angle_r , double north, double west) {
 
         double mag = 800.0;
@@ -196,12 +234,10 @@ class SkyView extends JPanel {
         g2d.drawLine( worldOriginX, 0, worldOriginX, height);
 
         //  Draw Waypoints
-        drawScenePoly(g2d, wpmarker, 0.0,      0.0,  25000.0 );
-        drawScenePoly(g2d, wpmarker, 0.0,  21650.0,  12500.0 );
-        drawScenePoly(g2d, wpmarker, 0.0,  21650.0, -12500.0 );
-        drawScenePoly(g2d, wpmarker, 0.0,      0.0, -25000.0 );
-        drawScenePoly(g2d, wpmarker, 0.0, -21650.0, -12500.0 );
-        drawScenePoly(g2d, wpmarker, 0.0, -21650.0,  12500.0 );
+        for(int i = 0; i < waypoints.size(); i++) {
+            Waypoint wp = waypoints.get(i);
+            drawWaypoint(g2d, wp);
+        }
 
         //  Draw Aircraft
         drawScenePoly(g2d, aircraft, heading, aircraftPos[0], aircraftPos[1] );
@@ -441,7 +477,7 @@ public class AircraftDisplay extends JFrame {
 
         String host = "localHost";
         int port = 0;
-        String vehicleImageFile = null;
+        String waypointInputFile = null;
 
         int ii = 0;
         while (ii < args.length) {
@@ -451,6 +487,10 @@ public class AircraftDisplay extends JFrame {
                     printHelpText();
                     System.exit(0);
                 } break;
+                case "-w" :
+                case "--waypoints" : {
+                    waypointInputFile = args[++ii];
+                } break;
                 default : {
                     port = (Integer.parseInt(args[ii]));
                 } break;
@@ -464,6 +504,12 @@ public class AircraftDisplay extends JFrame {
             System.exit(0);
         }
 
+        if(waypointInputFile == null) {
+            System.out.println("No waypoint file specified");
+            printHelpText();
+            System.exit(0);
+        }
+
         double mapScale = 0.01; // 100 meters per pixel
         SkyView skyview = new SkyView( mapScale);
         AircraftDisplay sd = new AircraftDisplay(skyview);
@@ -478,6 +524,18 @@ public class AircraftDisplay extends JFrame {
         Boolean autopilot = false;
         double desired_heading = 0.0;
 
+        try {
+            BufferedReader br = new BufferedReader(new FileReader(waypointInputFile));
+            String line;
+            while((line = br.readLine()) != null) {
+                String[] parsedLine = line.split(",");
+                sd.skyView.addWaypoint(Double.parseDouble(parsedLine[0]), Double.parseDouble(parsedLine[1]), parsedLine[2]);
+            }
+        } catch(FileNotFoundException e) {
+            System.out.printf("'%s' not found", args[ii+1]);
+            System.exit(0);
+        }
+
         System.out.println("Connecting to: " + host + ":" + port);
         sd.connectToServer(host, port);