SIM_aircraft Waypoint Update (#1466)

* 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.

* Created a menu bar for the GUI

* Minor README fix and file cleanup

* Implemented a view menu to hide and show the info on the map

* Cleared out commented code.

* Last minute code cleanup and update of the documentation.

* Updated the View menu with more options and updated documentation.

* Testing basic Variable Server Client usage.

* Waypoints shared from sim to graphics client.
Need to use list and fix some formatting.

* Refactored and simplified Waypoint implementation

* Documentation and comment updates

* Simplified waypoint structure
Implemented file support in graphics client

* Updated Documentation

* Created a default waypoint icon when unspecified

* Fixed privacy issues in the WaypointList class

* Cleaned up unnecessary file and added comments

trick_sims/SIM_aircraft/README.md

@@ -46,13 +46,13 @@ The autopilot feature is toggled on and off by the ```Autopilot OFF/ON``` button
 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. 
+Waypoints can be added to the simulation using an input file specified in `input.py`. 
 
 ![](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):
+#### Sample Input in [`default.waypoints`](Modified\_data/default.wps):
 
          0.0,  25000.0,images/wp0.png
      21650.0,  12500.0,images/wp1.png
@@ -64,6 +64,11 @@ Each line should have three pieces of data, separated by commas. The first two a
 
 ### Graphics Client Menu
 
+#### File
+
+The `File` menu allows the user to open a waypoint input file, save the current waypoint list to a new file, or clear the existing waypoint list.
+![](images/GraphicsClient_FileOptions.png)
+
 #### View
 
 The `View` menu on the menu bar allows the user to choose the information displayed by the client.

trick_sims/SIM_aircraft/RUN_test/input.py

@@ -11,12 +11,8 @@ dyn.aircraft.desired_speed = 200 # meters per second
 # Read the waypoints from the input file
 #==========================================
 
-waypoints_path = "Modified_data/default.waypoints"
+waypoints_path = "Modified_data/default.wps"
-fp = open(waypoints_path, "r")
+dyn.aircraft.flightPath.load(waypoints_path)
-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

trick_sims/SIM_aircraft/S_define

@@ -2,21 +2,26 @@
 PURPOSE:
     ( Simulate a Aircraft. )
 LIBRARY DEPENDENCIES:
-    ((Aircraft/src/Aircraft.o)
+    (
+        (Aircraft/src/Aircraft.cpp)
+        (Aircraft/src/Waypoint.cpp)
+    )
 **************************************************************************/
 #include "sim_objects/default_trick_sys.sm"
 ##include "Aircraft/include/Aircraft.hh"
+##include "Aircraft/include/Waypoint.hh"
 
 class AircraftSimObject : public Trick::SimObject {
     public:
     Aircraft aircraft;
 
     AircraftSimObject() {
-        ("default_data")            aircraft.default_data() ;
+        ("default_data")            aircraft.default_data();
         ("initialization")          aircraft.state_init();
-        (0.1,"scheduled")               aircraft.control() ;
+        (0.1,"scheduled")           aircraft.control();
-        ("derivative")              aircraft.state_deriv() ;
+        (0.1,"scheduled")           aircraft.cycleWaypoints();
-        ("integration") trick_ret = aircraft.state_integ() ;
+        ("derivative")              aircraft.state_deriv();
+        ("integration") trick_ret = aircraft.state_integ();
     }
 };
 

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

@@ -6,12 +6,10 @@ LIBRARY DEPENDENCIES:
 #ifndef Aircraft_HH
 #define Aircraft_HH
 
-#include <vector>
+#include "Aircraft/include/Waypoint.hh"
 
-class WayPoint {
+#include <vector>
-    public:
+#include <string>
-    double pos[2];
-};
 
 class Aircraft {
     public:
@@ -20,8 +18,13 @@ class Aircraft {
   	double vel[2];
 
     // Updated by control logic;
-    std::vector<WayPoint> waypointQueue;
+    WaypointList flightPath;
-    std::vector<WayPoint>::iterator current_waypoint;
+    int cWP;
+
+    // Waypoint List Information Variables
+    double wpPos[2];
+    std::string wpImg;
+    int wpIdx;
 
     // Static Parameters (Don't change during the simulation.)
   	double mass;
@@ -47,16 +50,15 @@ class Aircraft {
     int state_integ();
     int control();
 
+    int cycleWaypoints();
+
     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]);
     void calc_thrust_force(double (&F_thrust_body)[2]);
     void calc_turning_force(double (&F_turning_body)[2]);
     void rotateBodyToWorld(double (&F_total_world)[2], double (&F_total_body)[2] );
-
 };
 
 double psiToCompass( double psi );

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

@@ -0,0 +1,41 @@
+/************************************************************************
+PURPOSE: (Handles the waypoints.)
+**************************************************************************/
+#ifndef Waypoint_HH
+#define Waypoint_HH
+
+#ifndef ARRAY_SIZE
+#define ARRAY_SIZE  10
+#endif
+
+#include <string>
+#include <vector>
+#include <fstream>
+#include <iostream>
+
+class WaypointList {
+    public:
+    WaypointList();
+    WaypointList(std::string path);
+
+    int add(double n, double w, std::string i);
+    void remove(int index);
+    void clear();
+    
+    void load(std::string path);
+    void append(std::string path);
+    void save(std::string path);
+
+    void getPosition(int index, double (&pos)[2]);
+    std::string getImage(int index);
+    int size();
+
+    private:
+    double north[ARRAY_SIZE];
+    double west[ARRAY_SIZE];
+    std::string img[ARRAY_SIZE];
+    
+    int length;
+};
+
+#endif

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

@@ -9,6 +9,7 @@ LIBRARY DEPENDENCY:
 #include <stdlib.h>
 #include <math.h>
 #include <iostream>
+#include "Aircraft.hh"
 
 int Aircraft::default_data() {
     pos[0] = 0.0;         // m
@@ -23,12 +24,15 @@ int Aircraft::default_data() {
     set_desired_compass_heading(45.0);
     desired_speed = 200; // m/s
     autoPilot = false;
-    current_waypoint = waypointQueue.begin();
+
+    flightPath = WaypointList();
+    cWP = 0;
+    wpIdx = -1;
+
     return (0);
 }
 
 int Aircraft::state_init() {
-
     heading = northWestToPsi(vel);
     return (0);
 }
@@ -124,39 +128,38 @@ 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 (waypointQueue.size() > 0) {
+        if (flightPath.size() > 0) {
             // Calculate the difference between where we want to be, and where we are.
             double posDiff[2];
-            vector_difference(posDiff, current_waypoint->pos, pos);
+            double _wp[2];
+            flightPath.getPosition(cWP, _wp);
+            vector_difference(posDiff, _wp, 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;
+                cWP = (cWP + 1) % flightPath.size();
-                if (current_waypoint == waypointQueue.end()) {
-                    current_waypoint = waypointQueue.begin();
-                }
             }
-        } else {
-            std::cout << "!!!  Waypoint List Empty  !!!" << std::endl;
         } 
     }
     return 0;
 }
 
+int Aircraft::cycleWaypoints() {
+    if(flightPath.size() > 0) {
+        wpIdx = (wpIdx + 1) % flightPath.size();
+        flightPath.getPosition(wpIdx, wpPos);
+        wpImg = flightPath.getImage(wpIdx);
+    } else {
+        wpIdx = -1;
+    }
+    return 0;
+}
+
 void Aircraft::set_desired_compass_heading( double compass_heading) {
     desired_heading = compassToPsi( compass_heading );
 }

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

@@ -0,0 +1,111 @@
+/********************************* TRICK HEADER *******************************
+PURPOSE: ( Handles the waypoints. )
+*******************************************************************************/
+#include "Aircraft/include/Waypoint.hh"
+
+#include <stdlib.h>
+#include <iostream>
+#include <fstream>
+#include "Waypoint.hh"
+
+WaypointList::WaypointList() {
+    length = 0;
+}
+
+WaypointList::WaypointList(std::string path) {
+    length = 0;
+
+    load(path);
+}
+
+int WaypointList::add(double n, double w, std::string i="images/defaultIcon.png")
+{
+    if (length < ARRAY_SIZE)
+    {
+        north[length] = n;
+        west[length] = w;
+        img[length] = i;
+        length++;
+        return length-1;
+    }
+    
+    return -1;
+}
+
+void WaypointList::remove(int index)
+{
+    if (index < 0 || index >= length) {
+        std::cerr << "Invalid Index" << std::endl;
+        return;
+    }
+
+    length--;
+    for(int i = index; i < length; i++) {
+        north[i] = north[i+1];
+        west[i] = west[i+1];
+        img[i] = img[i+1];
+    }
+}
+
+void WaypointList::clear() {
+    length = 0;
+}
+
+void WaypointList::load(std::string path) {
+    clear();
+    append(path);
+}
+
+void WaypointList::append(std::string path) {
+    std::ifstream in(path);
+
+    if(in.is_open()) {
+        std::string str;
+        // Expected input formatting: <North>,<West> OR <North>,<West>,<Image Path>
+        while(getline(in,str)) {
+            int k = str.find(','), j = str.find(',',k+1);
+            double n,w;
+            if (j>=0) {
+                n = std::stod(str.substr(0,k));
+                w = std::stod(str.substr(k+1,(j-k-1)));
+                std::string i = str.substr(j+1);
+                add(n,w,i);
+            } else {
+                n = std::stod(str.substr(0,k));
+                w = std::stod(str.substr(k+1));
+                add(n,w);
+            }
+            
+        }
+        in.close();
+    } else {
+        std::cerr << "FILE DID NOT OPEN" << std::endl;
+    }
+}
+
+void WaypointList::save(std::string path) {
+    std::ofstream out(path);
+
+    if(out.is_open()) {
+        for(int i = 0; i < length; i++) {
+            out << std::to_string(north[i]) + "," + std::to_string(west[i]) + "," + img[i] << std::endl;
+        }
+    }
+
+    out.close();
+}
+
+void WaypointList::getPosition(int index, double (&pos)[2]) {
+    pos[0] = north[index];
+    pos[1] = west[index];
+}
+
+std::string WaypointList::getImage(int index)
+{
+    return img[index];
+}
+
+int WaypointList::size()
+{
+    return length;
+}

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

@@ -30,6 +30,9 @@ import java.lang.Math;
 import java.net.Socket;
 import java.util.*;
 import java.util.Hashtable;
+import java.util.regex.Matcher;
+import java.util.regex.Pattern;
+
 import javax.swing.ImageIcon;
 import javax.swing.JFrame;
 import javax.swing.JPanel;
@@ -40,6 +43,7 @@ import javax.swing.JMenuBar;
 import javax.swing.JMenuItem;
 import javax.swing.JButton;
 import javax.swing.JCheckBoxMenuItem;
+import javax.swing.JFileChooser;
 import javax.swing.JToggleButton;
 import javax.swing.BorderFactory; 
 import javax.swing.border.EtchedBorder;
@@ -60,10 +64,15 @@ class Waypoint {
     public double north, west;
     public BufferedImage icon;
     
-    public Waypoint(double n, double w, BufferedImage i) {
+    public Waypoint(double n, double w, String i) {
         north = n;
         west = w;
-        icon = i;
+        try {
+            icon = ImageIO.read(new File(i));
+        } catch(Exception e) {
+            icon = null;
+            System.out.printf("Waypoint (%.1f,%.1f) not added to map.");
+        }
     }
 }
 
@@ -75,7 +84,7 @@ class ScenePoly {
 }
 
 class SimulationMenuBar extends JMenuBar {
-    private JMenu _file;
+    private FileMenu _file;
     private JMenu _edit;
     private JMenu _tools;
     private JMenu _help;
@@ -85,11 +94,19 @@ class SimulationMenuBar extends JMenuBar {
 
     public SimulationMenuBar(SkyView sv) {
         skyView = sv;
+        _file = new FileMenu("File");
         _view = new ViewMenu("View");
 
+        add(_file);
         add(_view);
     }
 
+    public void setupFileMenu(ActionListener newFileListener, ActionListener saveFileListener, ActionListener openFileListener) {
+        _file.newFile.addActionListener(newFileListener);
+        _file.saveFile.addActionListener(saveFileListener);
+        _file.openFile.addActionListener(openFileListener);
+    }
+
     public void setEnabled_DisabledViewCB(boolean s) {
         _view.disabledView.setEnabled(s);
     }
@@ -119,23 +136,6 @@ class SimulationMenuBar extends JMenuBar {
         add(_edit);
     }
 
-    private void initFileMenu() {
-        _file = new JMenu("File");      // New, Open, Save, Save As, Exit
-        JMenuItem mi;
-        mi = new JMenuItem("New");
-        _file.add(mi);
-        mi = new JMenuItem("Open");
-        _file.add(mi);
-        mi = new JMenuItem("Save");
-        _file.add(mi);
-        mi = new JMenuItem("Save As");
-        _file.add(mi);
-        mi = new JMenuItem("Exit");
-        _file.add(mi);
-        
-        add(_file);
-    }
-
     private class ViewMenu extends JMenu {
         public JCheckBoxMenuItem posView, velView, scaleView, controlView, disabledView;
         ViewMenu(String name) {
@@ -179,6 +179,21 @@ class SimulationMenuBar extends JMenuBar {
             disabledView.addItemListener(e -> skyView.setDisabledView(((JCheckBoxMenuItem) e.getItem()).isSelected()));
         }
     }
+
+    private class FileMenu extends JMenu {
+        public JMenuItem newFile, saveFile, openFile;
+        FileMenu(String name) {
+            super(name);
+
+            newFile = new JMenuItem("New File");
+            saveFile = new JMenuItem("Save File");
+            openFile = new JMenuItem("Open File");
+
+            add(newFile);
+            add(saveFile);
+            add(openFile);
+        }
+    }
 }
 
 class SkyView extends JPanel {
@@ -199,7 +214,7 @@ class SkyView extends JPanel {
     private double desired_heading;
     private Boolean autopilot;
 
-    private ArrayList<Waypoint> waypoints;
+    private Waypoint[] waypoints;
 
     // Origin of world coordinates in jpanel coordinates.
     private int worldOriginX;
@@ -232,22 +247,11 @@ class SkyView extends JPanel {
         workPolyX = new int[30];
         workPolyY = new int[30];
 
-        waypoints = new ArrayList<Waypoint>();
+        waypoints = new Waypoint[10];
 
         setAllView(true);
     }
 
-    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) {
         aircraftPos[0] = n;
         aircraftPos[1] = w;
@@ -303,6 +307,17 @@ class SkyView extends JPanel {
         desired_heading = n;
     }
 
+    public void resetWaypoints() {
+        for(int i = 0; i < waypoints.length; i++)   waypoints[i] = null;
+    }
+
+    public boolean setWaypoint(int i, Waypoint wp) {
+        if(i < 0 || i >= waypoints.length) return false;
+
+        waypoints[i] = wp;
+        return true;
+    }
+
     // Getters and setters for all the 'View' variables
     public boolean getPosView()             {  return posView;  }
     public void setPosView(boolean v)       {  posView = v;  }
@@ -376,9 +391,9 @@ class SkyView extends JPanel {
         g2d.drawLine( worldOriginX, 0, worldOriginX, height);
 
         //  Draw Waypoints
-        for(int i = 0; i < waypoints.size(); i++) {
+        for(int i = 0; i < waypoints.length; i++) {
-            Waypoint wp = waypoints.get(i);
+            Waypoint wp = waypoints[i];
-            drawWaypoint(g2d, wp);
+            if(wp != null)  drawWaypoint(g2d, wp);
         }
 
         //  Draw Aircraft
@@ -442,8 +457,8 @@ class ControlPanel extends JPanel implements ActionListener {
   
 
 
-    public ControlPanel(SkyView skyView){
+    public ControlPanel(SkyView view){
-        skyView = skyView;
+        skyView = view;
         setLayout(new BoxLayout(this, BoxLayout.X_AXIS));
 
 
@@ -574,6 +589,7 @@ class AutoPilotCtrlPanel extends JPanel implements ItemListener {
         autoPilotButton.addItemListener(this);
         add(autoPilotButton);
     }
+
     public void itemStateChanged(ItemEvent e){
         if (e.getStateChange() == ItemEvent.SELECTED){
             skyView.setAutoPilot(true);
@@ -594,8 +610,8 @@ public class AircraftDisplay extends JFrame {
     private JPanel panelGroup0, panelGroup1;
     private ControlPanel controlPanel;
 
-    public AircraftDisplay(SkyView sky) {
+    public AircraftDisplay(SkyView view) {
-        skyView = sky;
+        skyView = view;
         simMenu = new SimulationMenuBar(skyView);
         setJMenuBar(simMenu);
         add( skyView);
@@ -687,21 +703,71 @@ public class AircraftDisplay extends JFrame {
         Boolean autopilot = false;
         double desired_heading = 0.0;
 
-        try {
+        int wpIndex = -1;
-            BufferedReader br = new BufferedReader(new FileReader(waypointInputFile));
+        double wpNorth = 0.0;
-            String line;
+        double wpWest = 0.0;
-            while((line = br.readLine()) != null) {
+        String wpImage = "";
-                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);
         
+        JFileChooser browse = new JFileChooser("Modified_data");
+
+        ActionListener new_file = new ActionListener() {
+
+            @Override
+            public void actionPerformed(ActionEvent e) {
+                try {
+                    sd.out.writeBytes("dyn.aircraft.flightPath.clear() ;");
+                    sd.out.flush();
+                } catch (Exception ex) {
+
+                }
+            }
+        
+        };
+
+        ActionListener save_file = new ActionListener() {
+
+            @Override
+            public void actionPerformed(ActionEvent e) {
+                int option = browse.showSaveDialog(sd);
+                if(option != JFileChooser.APPROVE_OPTION) return;
+
+                String path = browse.getSelectedFile().getPath();
+
+                try {
+                    sd.out.writeBytes(String.format("dyn.aircraft.flightPath.save(\"%s\") ;",path));
+                    sd.out.flush();
+                } catch (Exception ex) {
+
+                }
+            }
+        
+        };
+
+        ActionListener open_file = new ActionListener() {
+
+            @Override
+            public void actionPerformed(ActionEvent e) {
+                int option = browse.showOpenDialog(sd);
+                if(option != JFileChooser.APPROVE_OPTION) return;
+
+                String path = browse.getSelectedFile().getPath();
+                
+                try {
+                    sd.out.writeBytes(String.format("dyn.aircraft.flightPath.load(\"%s\") ;",path));
+                    sd.out.writeBytes("dyn.aircraft.wpIdx = -1 ;");
+                    sd.out.flush();
+                } catch (Exception ex) {
+
+                }
+            }
+        
+        };
+
+        sd.simMenu.setupFileMenu( new_file, save_file, open_file);
+
         sd.out.writeBytes("trick.var_set_client_tag(\"AircraftDisplay\") \n" +
                           "trick.var_pause() \n" +
                           "trick.var_add(\"dyn.aircraft.pos[0]\") \n" +
@@ -710,6 +776,10 @@ public class AircraftDisplay extends JFrame {
                           "trick.var_add(\"dyn.aircraft.vel[1]\") \n" +
                           "trick.var_add(\"dyn.aircraft.desired_speed\") \n" +
                           "trick.var_add(\"dyn.aircraft.desired_heading\") \n" +
+                          "trick.var_add(\"dyn.aircraft.wpIdx\") \n" +
+                          "trick.var_add(\"dyn.aircraft.wpPos[0]\") \n" +
+                          "trick.var_add(\"dyn.aircraft.wpPos[1]\") \n" +
+                          "trick.var_add(\"dyn.aircraft.wpImg\") \n" +
                           "trick.var_ascii() \n" +
                           "trick.var_cycle(0.1) \n" +
                           "trick.var_unpause()\n" );
@@ -730,6 +800,17 @@ public class AircraftDisplay extends JFrame {
                 desired_speed = Double.parseDouble( field[5]);
                 desired_heading = Double.parseDouble(field[6]);
 
+                wpIndex = Integer.parseInt(field[7]);
+
+                if(wpIndex < 0) {
+                    sd.skyView.resetWaypoints();
+                } else {
+                    wpNorth = Double.parseDouble(field[8]);
+                    wpWest = Double.parseDouble(field[9]);
+                    wpImage = field[10];
+                    sd.skyView.setWaypoint(wpIndex, new Waypoint(wpNorth, wpWest, wpImage));
+                }
+
                 // Set the Aircraft position
                 skyview.setAircraftPos(posNorth, posWest);
                 skyview.setAircraftVel(velNorth, velWest);