heading following example

examples/track_heading/Makefile

@@ -0,0 +1,10 @@
+TARGET := bin/enviro
+
+all: 
+	$(MAKE) -C src all
+
+clean:
+	$(MAKE) -C src clean
+
+
+

examples/track_heading/config.json

@@ -0,0 +1,21 @@
+{
+
+    "name": "My Enviro Project",
+    "ip": "0.0.0.0",
+    "port": 8765,
+
+    "buttons": [],
+
+    "agents": [ 
+        {
+            "definition": "defs/robot.json",
+            "style": { "fill": "lightgreen", "stroke": "black" },
+            "position": { "x": -200, "y": -200, "theta": 0 }
+        }       
+    ],
+
+    "statics": [
+
+    ]
+
+}

examples/track_heading/defs/README.md

@@ -0,0 +1 @@
+Put agent definitions in this folder. 

examples/track_heading/defs/robot.json

@@ -0,0 +1,32 @@
+{
+    "name": "Robot",
+    "type": "dynamic",
+    "description": "Replace this string with a description",
+    "shape": [
+      { "x": -10, "y": 10 },
+      { "x": 10, "y": 10 },
+      { "x": 10, "y": 2 },
+      { "x": 12, "y": 2 },
+      { "x": 12, "y": 6 },
+      { "x": 16, "y": 6 },
+      { "x": 16, "y": 4 },
+      { "x": 14, "y": 4 },
+      { "x": 14, "y": -4},
+      { "x": 16, "y": -4 },
+      { "x": 16, "y": -6 },
+      { "x": 12, "y": -6 },
+      { "x": 12, "y": -2 },
+      { "x": 10, "y": -2 },
+      { "x": 10, "y": -10 },
+      { "x": -10, "y": -10 },
+      { "x": -9, "y": 0 }
+    ],
+    "friction": {
+      "collision": 5,
+      "linear": 40,
+      "rotational": 600
+    },
+    "sensors": [],
+    "mass": 1,    
+    "controller": "lib/robot.so"
+}

examples/track_heading/lib/README.md

@@ -0,0 +1 @@
+Shared object libraries will be place here by make.

examples/track_heading/src/Makefile

@@ -0,0 +1,38 @@
+#Architecture
+ARCH := $(shell uname -m)
+
+#Compilers
+CC          := g++ -std=c++17 -Wno-psabi
+
+#The Target Library
+
+#The Directories, Source, Includes, Objects, Binary and Resources
+SRCEXT      := cc
+
+# Directories
+CHIPDIR     := /usr/local/src/Chipmunk2D
+ENVIRODIR   := /usr/local/src/enviro/server/include
+
+#Flags, Libraries and Includes
+CFLAGS      := -ggdb  -shared -fPIC
+INCLUDE		:= -I $(ENVIRODIR) -I $(CHIPDIR)/include/chipmunk 
+
+#Files
+
+TARGETDIR	 := ../lib
+SOURCES      := $(wildcard *.cc)
+HEADERS      := $(wildcard *.h)
+TARGETS		 := $(patsubst %.cc,%.so,$(wildcard *.cc))
+FULL_TARGETS := $(addprefix $(TARGETDIR)/, $(TARGETS))
+
+#Default Make
+all: $(FULL_TARGETS)
+
+#Clean only Objects
+clean:
+	@$(RM) -rf $(TARGETDIR)/*.so
+
+# Compile
+$(TARGETDIR)/%.so: %.cc %.h
+	$(CC) $(CFLAGS) $(INCLUDE) $< -o $@
+

examples/track_heading/src/robot.cc

@@ -0,0 +1,6 @@
+#include <iostream>
+#include "robot.h"
+
+using namespace enviro;
+
+// Put your implementations here

examples/track_heading/src/robot.h

@@ -0,0 +1,45 @@
+#ifndef __ROBOT_AGENT__H
+#define __ROBOT_AGENT__H 
+
+#include "enviro.h"
+#include "math.h"
+
+using namespace enviro;
+
+class RobotController : public Process, public AgentInterface {
+
+    public:
+    RobotController() : Process(), AgentInterface() {}
+
+    void init() {
+        desired_heading = 0;
+        counter = 0;
+    }
+    void start() {}
+    void update() {
+        if ( counter++ > 50 ) {
+            desired_heading += M_PI / 2;
+            counter = 0;
+        }
+        track_velocity(20,30*sin(desired_heading-angle()) - 100*angular_velocity());
+        label(std::to_string((int) (2*desired_heading/M_PI)) + " pi",15,-15);
+    }
+    void stop() {}
+
+    int counter;
+    double desired_heading;
+
+};
+
+class Robot : public Agent {
+    public:
+    Robot(json spec, World& world) : Agent(spec, world) {
+        add_process(c);
+    }
+    private:
+    RobotController c;
+};
+
+DECLARE_INTERFACE(Robot)
+
+#endif

server/src/world.cc

@@ -191,8 +191,9 @@ namespace enviro {
     }
 
     void World::remove(int id) {
-        std::cout << "marking " << id << " for removal" << "\n";
-        find_agent(id).mark_for_removal();
+        if ( exists(id) ) {
+            find_agent(id).mark_for_removal();
+        }
     }
 
     void World::remove_constraints_involving(int id) {