SubsumptionArchitecture.cpp

#include <memory>

struct SensorData { // contains all sensor data, e.g. IR distance etc
	int data;
};

struct ActuatorData { // contains parameters for actuators, e.g. speed, direction etc
	int actuator;
};

class Level { // derive from this class to create your own subsumption levels
public:
	Level() : higherLevel(nullptr) {}
	virtual ~Level(){ delete higherLevel; } // rule of five: delete, ctors and = operators
	Level(const Level&) = delete;
	Level(const Level&&) = delete;
	Level& operator=(const Level&) = delete;
	Level& operator=(const Level&&) = delete;

	virtual SensorData suppressInput(const SensorData& data , int suppressControl) {
		return data; // sensor data is copied here to local (sensor) data. every level has its own copy.
	}
	virtual ActuatorData inhibitOutput(const ActuatorData& actuator , const ActuatorData& actuatorDataFromHigherLayer){
		return actuator; // every level also has its own copy of actuatorData.
	}
	virtual ActuatorData executeLevel(const SensorData& data){
		//set suppressControl for the lower level
		return actuatorOutput; // this just returns the local actuatorData.
	}

	ActuatorData startLevel(const SensorData& sensorData){
		if ( higherLevel == nullptr ){ // this is the top level
			actuatorOutput = executeLevel(sensorData);
		}
		else { // this is a non-top level.
			higherLevel->startLevel(sensorData); // start the higherLevel first.
			sensorInput = suppressInput(sensorData , higherLevel->suppressControl );
			actuatorOutput = executeLevel(sensorInput);
			actuatorOutput = inhibitOutput(actuatorOutput , higherLevel->actuatorOutput);
		}
		return actuatorOutput;
	}

	Level* higherLevel;  // dependency: higher level provides suppress and inhibit values
	int suppressControl; // this value is the input to the suppress-function for lower level
	SensorData sensorInput; // result of suppress-function and input to level-function
	ActuatorData actuatorOutput;  // output of level-function and input to inhibit-function
};

class Level0 : public Level { }; // overwrite suppressInput, executeLevel and inhibitOutput
class Level1 : public Level { };
class Level2 : public Level { };

class Controller { // the subsumption controller allows to  register the levels and start the whole thing
public:
	Controller() : rootLevel(nullptr)  {}
	~Controller() { delete rootLevel; 	} // this will iteratively delete all levels
	// don´t forget the rule of five (see class Level) which is omitted for clarity
	Level* rootLevel;
	void registerLevel(Level* level){ // create a linked list of levels
		 level->higherLevel = rootLevel;
		 rootLevel = level;
	}

	void arbitrate(const SensorData& data){
		rootLevel->startLevel(data);
	}
	const ActuatorData& getOutput(){// this returns the final result of calculation of the whole thing
		return rootLevel->actuatorOutput;
	}
};

int main() {
	Controller controller;
	SensorData data;
	controller.registerLevel(new Level2);// register levels from highest to lowest
	controller.registerLevel(new Level1);
	controller.registerLevel(new Level0);

	while(1) { // this shall be a controlled loop, e.g. synchronized with read from sensors etc
		controller.arbitrate(data); // uses sensor data to run through the levels
		controller.getOutput();     // do something with the output, e.g. write it to actuators
	}

	return 0;
}