Implement support for logging

language/nl.tue.robotsupervisorycontrollerdsl/src/nl/tue/robotsupervisorycontrollerdsl/generator/common/ros/AbstractCommunicationTypeGenerator.xtend

@@ -17,6 +17,7 @@ import nl.tue.robotsupervisorycontrollerdsl.generator.cpp.data.EliminationHelper
 import nl.tue.robotsupervisorycontrollerdsl.generator.cpp.expressions.ExpressionGenerator
 import nl.tue.robotsupervisorycontrollerdsl.generator.common.naming.IdentifierNamerInterface
 import nl.tue.robotsupervisorycontrollerdsl.generator.common.naming.DefaultIdentifierNamer
+import nl.tue.robotsupervisorycontrollerdsl.generator.config.model.Config
 
 abstract class AbstractCommunicationTypeGenerator<T extends CommunicationType> {
 	@Inject extension MethodNames
@@ -28,9 +29,9 @@ abstract class AbstractCommunicationTypeGenerator<T extends CommunicationType> {
 	@Inject DefaultIdentifierNamer defaultIdentifierNamer
 	@Inject extension ExpressionGenerator
 
-	abstract def CharSequence initializeField(T entity, Robot robot)
-	abstract def CharSequence declareField(T entity, Robot robot)
-	abstract def CharSequence functions(T entity, Robot robot)
+	abstract def CharSequence initializeField(T entity, Robot robot, Config config)
+	abstract def CharSequence declareField(T entity, Robot robot, Config config)
+	abstract def CharSequence functions(T entity, Robot robot, Config config)
 
 	protected def prepareResult(CommunicationType entity, DataType type, Robot robot, String access)'''
 	«IF type.simple»

language/nl.tue.robotsupervisorycontrollerdsl/src/nl/tue/robotsupervisorycontrollerdsl/generator/config/model/Config.java

@@ -4,6 +4,7 @@ public class Config {
 	private Output output = new Output();
 	private boolean publishStateInformation = true;
 	private Supervisor supervisor = null;
+	private boolean writeEventsToLog = false;
 
 	public Output getOutput() {
 		return output;
@@ -28,4 +29,12 @@ public Supervisor getSupervisor() {
 	public void setSupervisor(Supervisor supervisor) {
 		this.supervisor = supervisor;
 	}
+
+	public boolean isWriteEventsToLog() {
+		return writeEventsToLog;
+	}
+
+	public void setWriteEventsToLog(boolean writeEventsToLog) {
+		this.writeEventsToLog = writeEventsToLog;
+	}
 }

language/nl.tue.robotsupervisorycontrollerdsl/src/nl/tue/robotsupervisorycontrollerdsl/generator/cpp/logging/LogGenerator.xtend

@@ -0,0 +1,66 @@
+package nl.tue.robotsupervisorycontrollerdsl.generator.cpp.logging
+
+import javax.inject.Singleton
+
+@Singleton
+class LogGenerator {
+	def compileImports()'''
+	#include <iostream>
+	#include <fstream>
+	'''
+
+	def compileFields()'''
+	std::ofstream logging_out_file;
+	std::ofstream logging_in_file;
+	'''
+	
+	def compileFunctions()'''
+	std::string logging_time_human() {
+		std::time_t rawtime;
+		char buffer[80];
+
+		std::time (&rawtime);
+		auto local = std::localtime(&rawtime);
+
+		std::strftime(buffer, sizeof(buffer), "%Y-%m-%d %H:%M:%S", local);
+		std::string output(buffer);
+
+		return output;
+	}
+
+	std::string logging_time_machine() {
+		std::time_t rawtime;
+		char buffer[80];
+
+		std::time (&rawtime);
+		auto local = std::localtime(&rawtime);
+
+		std::strftime(buffer, sizeof(buffer), "%Y_%m_%d_%H_%M_%S", local);
+		std::string output(buffer);
+
+		return output;
+	}
+
+	void start_logging() {
+		logging_out_file.open("node_out_" + logging_time_machine() + ".log");
+		logging_in_file.open("node_in_" + logging_time_machine() + ".log");
+	}
+
+	void write_to_incoming_log(std::string value) {
+	   logging_out_file << "[" << logging_time_human() << "] "<< value << std::endl;
+	}
+
+	void write_to_outgoing_log(std::string value) {
+	   logging_out_file << "[" << logging_time_human() << "] "<< value << std::endl;
+	}
+	'''
+	
+	def compileInitialization()'''
+	this->start_logging();
+	'''
+	
+	def compileDestruction()'''
+	logging_in_file.close();
+	logging_out_file.close();
+	'''
+}

language/nl.tue.robotsupervisorycontrollerdsl/src/nl/tue/robotsupervisorycontrollerdsl/generator/cpp/logging/LogOutputConverter.xtend

@@ -0,0 +1,24 @@
+package nl.tue.robotsupervisorycontrollerdsl.generator.cpp.logging
+
+import nl.tue.robotsupervisorycontrollerdsl.robotSupervisoryControllerDSL.Message
+import javax.inject.Singleton
+import nl.tue.robotsupervisorycontrollerdsl.generator.common.naming.DefaultIdentifierNamer
+import javax.inject.Inject
+import nl.tue.robotsupervisorycontrollerdsl.robotSupervisoryControllerDSL.Service
+import nl.tue.robotsupervisorycontrollerdsl.robotSupervisoryControllerDSL.Action
+
+@Singleton
+class LogOutputConverter {
+	@Inject DefaultIdentifierNamer namer
+
+	dispatch def responseOutput(Message entity)'''Response from message «entity.name» (identifier «namer.name(entity)») '''
+	dispatch def responseOutput(Service entity)'''Response from service «entity.name» (identifier «namer.name(entity)») '''
+	dispatch def responseOutput(Action entity)'''Response from action «entity.name» (identifier «namer.name(entity)») '''
+
+	dispatch def requestOutput(Message entity)'''Request to message «entity.name» (identifier «namer.name(entity)») '''
+	dispatch def requestOutput(Service entity)'''Request to service «entity.name» (identifier «namer.name(entity)») '''
+	dispatch def requestOutput(Action entity)'''Request to action «entity.name» (identifier «namer.name(entity)») '''
+
+	def feedbackOutput(Action entity)'''Feedback from action «entity.name» (identifier «namer.name(entity)») '''
+	def cancelOutput(Action entity)'''Cancelling action «entity.name» (identifier «namer.name(entity)») '''
+}

language/nl.tue.robotsupervisorycontrollerdsl/src/nl/tue/robotsupervisorycontrollerdsl/generator/ros1/Ros1Generator.xtend

@@ -23,6 +23,7 @@ import nl.tue.robotsupervisorycontrollerdsl.generator.cpp.info.InfoUtilitiesGene
 import nl.tue.robotsupervisorycontrollerdsl.generator.cpp.engine.SerializationHelperGenerator
 import nl.tue.robotsupervisorycontrollerdsl.generator.config.model.Config
 import nl.tue.robotsupervisorycontrollerdsl.generator.cpp.output.OutputCopyUtil
+import nl.tue.robotsupervisorycontrollerdsl.generator.cpp.logging.LogGenerator
 
 @Singleton
 class Ros1Generator implements GeneratorInterface {
@@ -41,6 +42,7 @@ class Ros1Generator implements GeneratorInterface {
 	@Inject CifSynthesisTool cifSynthesisTool
 	@Inject PlatformTypeGenerator platformTypeGenerator
 	@Inject OutputCopyUtil outputCopyUtil
+	@Inject LogGenerator logGenerator
 
 	override generate(Robot robot, IFileSystemAccess2 fileSystemAccess, Config config) {
 		val base = '''«robot.name»/ros1/controller'''
@@ -62,6 +64,7 @@ class Ros1Generator implements GeneratorInterface {
 
 	def controller(Robot robot, Config config) '''
 	«robot.determineRequiredImports»
+	«logGenerator.compileImports»
 
 	// Utility functions
 	«shuffleHelperGenerator.generateShuffleFunction»
@@ -78,27 +81,41 @@ class Ros1Generator implements GeneratorInterface {
 		// Enum conversions
 		«FOR component : robot.definitions.filter(EnumDataType)»«component.compile(platformTypeGenerator)»«ENDFOR»
 
-		«robot.compileCommunicationFieldDefinitions»
+		«robot.compileCommunicationFieldDefinitions(config)»
 		«IF config.publishStateInformation»
 		ros::Publisher state_information;
 		«ENDIF»
 		«robot.compileActivationFields»
 
 		void start(ros::NodeHandle& node) {
-			«robot.compileCommunicationFieldInitializations»
+			«robot.compileCommunicationFieldInitializations(config)»
 
 			«IF config.publishStateInformation»
 			state_information = node.advertise<std_msgs::String>("/state", 10);
 			«ENDIF»
 			timer = node.createTimer(ros::Duration(0.1), &Controller::tick, this);
 			«CifSynthesisTool.codePrefix»_EngineFirstStep();
+
+			«IF config.writeEventsToLog»
+			«logGenerator.compileInitialization»
+			«ENDIF»
 		}
 
-		«robot.compileCommunicationFunctions»
+		«robot.compileCommunicationFunctions(config)»
 
 		«IF config.publishStateInformation»
 		«robot.compileInfoFunction(platformTypeGenerator)»
 		«ENDIF»
+
+		«IF config.writeEventsToLog»
+		«logGenerator.compileFunctions»
+		«ENDIF»
+
+		~Controller() {
+			«IF config.writeEventsToLog»
+			«logGenerator.compileDestruction»
+			«ENDIF»
+		}
 	private:
 		// Heart of the controller
 		void tick(const ros::TimerEvent &) {
@@ -110,6 +127,9 @@ class Ros1Generator implements GeneratorInterface {
 		}
 
 		ros::Timer timer;
+		«IF config.writeEventsToLog»
+		«logGenerator.compileFields»
+		«ENDIF»
 	};
 
 	std::shared_ptr<Controller> node = nullptr;

language/nl.tue.robotsupervisorycontrollerdsl/src/nl/tue/robotsupervisorycontrollerdsl/generator/ros1/communication/ActionGenerator.xtend

@@ -11,6 +11,8 @@ import nl.tue.robotsupervisorycontrollerdsl.generator.cif.synthesis.CifSynthesis
 import nl.tue.robotsupervisorycontrollerdsl.generator.cpp.naming.TransitionNames
 import nl.tue.robotsupervisorycontrollerdsl.generator.cpp.data.DataPlantHelper
 import nl.tue.robotsupervisorycontrollerdsl.generator.common.ros.AbstractCommunicationTypeGenerator
+import nl.tue.robotsupervisorycontrollerdsl.generator.config.model.Config
+import nl.tue.robotsupervisorycontrollerdsl.generator.cpp.logging.LogOutputConverter
 
 @Singleton
 class ActionGenerator extends AbstractCommunicationTypeGenerator<Action> {
@@ -19,34 +21,37 @@ class ActionGenerator extends AbstractCommunicationTypeGenerator<Action> {
 	@Inject extension MethodNames
 	@Inject extension TransitionNames
 	@Inject extension DataPlantHelper
+	@Inject extension LogOutputConverter
 
-	override initializeField(Action entity, Robot robot) ''''''
-	override declareField(Action entity, Robot robot) '''actionlib::SimpleActionClient<«entity.links.actionType»Action>«entity.fieldName»;'''
+	override initializeField(Action entity, Robot robot, Config config) ''''''
+	override declareField(Action entity, Robot robot, Config config) '''actionlib::SimpleActionClient<«entity.links.actionType»Action>«entity.fieldName»;'''
 	def constructorInvocation(Action entity, Robot robot) '''«entity.fieldName»("«entity.topicName»", false)'''
 
-	override functions(Action entity, Robot robot)'''
+	override functions(Action entity, Robot robot, Config config)'''
 	void «entity.responseMethod»(const actionlib::SimpleClientGoalState& state, const «entity.links.actionType»ResultConstPtr& result) {
 		«entity.prepareResult(entity.responseType, robot, 'result.result')»
 
-		fprintf(stderr, "[debug] Received action response\n");
-
 		// Call engine function		        
 		if (state == actionlib::SimpleClientGoalState::SUCCEEDED) {
-			fprintf(stderr, "[debug] Received response from action.\n");
 			«CifSynthesisTool.codePrefix»_EnginePerformEvent(«entity.responseTransitionName»);
 		} else {
-			fprintf(stderr, "[debug] Received error from action.\n");
 			«CifSynthesisTool.codePrefix»_EnginePerformEvent(«entity.errorTransitionName»);
 		}
+
+		«IF config.writeEventsToLog»
+		this->write_to_incoming_log("«entity.responseOutput»");
+		«ENDIF»
 	}
 
 	void «entity.feedbackMethod»(const «entity.links.actionType»FeedbackConstPtr& feedback) {
 		«entity.prepareResult(entity.responseType, robot, 'feedback')»
-
-		fprintf(stderr, "[debug] Received action feedback\n");
 
 		// Call engine function
 		«CifSynthesisTool.codePrefix»_EnginePerformEvent(«entity.feedbackTransitionName»);
+
+		«IF config.writeEventsToLog»
+		this->write_to_incoming_log("«entity.feedbackOutput»");
+		«ENDIF»
 	}
 
 	void «entity.callMethod»() {
@@ -56,10 +61,18 @@ class ActionGenerator extends AbstractCommunicationTypeGenerator<Action> {
 		«entity.compileDataStates(entity.requestType, 'goal_msg', robot, false)»
 
 		«entity.fieldName».sendGoal(goal_msg, boost::bind(&Controller::«entity.responseMethod», this, _1, _2), actionlib::SimpleActionClient<«entity.links.actionType»Action>::SimpleActiveCallback(), boost::bind(&Controller::«entity.feedbackMethod», this, _1));
+
+		«IF config.writeEventsToLog»
+		this->write_to_outgoing_log("«entity.requestOutput»");
+		«ENDIF»
 	}
 
 	void «entity.cancelMethod»() {
 		this->«entity.fieldName».cancelAllGoals();
+
+		«IF config.writeEventsToLog»
+		this->write_to_outgoing_log("«entity.cancelOutput»");
+		«ENDIF»
 	}
 	'''
 	

language/nl.tue.robotsupervisorycontrollerdsl/src/nl/tue/robotsupervisorycontrollerdsl/generator/ros1/communication/CommunicationTypeGenerator.xtend

@@ -8,28 +8,29 @@ import nl.tue.robotsupervisorycontrollerdsl.robotSupervisoryControllerDSL.Servic
 import nl.tue.robotsupervisorycontrollerdsl.robotSupervisoryControllerDSL.Action
 import nl.tue.robotsupervisorycontrollerdsl.robotSupervisoryControllerDSL.Message
 import javax.inject.Inject
+import nl.tue.robotsupervisorycontrollerdsl.generator.config.model.Config
 
 @Singleton
 class CommunicationTypeGenerator {
 	@Inject MessageGenerator messageGenerator
 	@Inject ServiceGenerator serviceGenerator
 	@Inject ActionGenerator actionGenerator
 
-	def compileCommunicationFieldDefinitions(Robot robot)'''
+	def compileCommunicationFieldDefinitions(Robot robot, Config config)'''
 	«FOR communication : ModelHelper.findWithinRobot(robot, CommunicationType)»
-	«communication.declareField(robot)»
+	«communication.declareField(robot, config)»
 	«ENDFOR»
 	'''
 
-	def compileCommunicationFieldInitializations(Robot robot)'''
+	def compileCommunicationFieldInitializations(Robot robot, Config config)'''
 	«FOR communication : ModelHelper.findWithinRobot(robot, CommunicationType)»
-	«communication.initializeField(robot)»
+	«communication.initializeField(robot, config)»
 	«ENDFOR»
 	'''
 
-	def compileCommunicationFunctions(Robot robot)'''
+	def compileCommunicationFunctions(Robot robot, Config config)'''
 	«FOR communication : ModelHelper.findWithinRobot(robot, CommunicationType)»
-	«communication.functions(robot)»
+	«communication.functions(robot, config)»
 	«ENDFOR»
 	'''
 
@@ -39,17 +40,17 @@ class CommunicationTypeGenerator {
 			.map[it.constructorInvocation(robot)]
 	}
 
-	private def dispatch initializeField(Message entity, Robot robot) '''«messageGenerator.initializeField(entity, robot)»'''
-	private def dispatch initializeField(Service entity, Robot robot) '''«serviceGenerator.initializeField(entity, robot)»'''
-	private def dispatch initializeField(Action entity, Robot robot) '''«actionGenerator.initializeField(entity, robot)»'''
+	private def dispatch initializeField(Message entity, Robot robot, Config config) '''«messageGenerator.initializeField(entity, robot, config)»'''
+	private def dispatch initializeField(Service entity, Robot robot, Config config) '''«serviceGenerator.initializeField(entity, robot, config)»'''
+	private def dispatch initializeField(Action entity, Robot robot, Config config) '''«actionGenerator.initializeField(entity, robot, config)»'''
 
-	private def dispatch declareField(Message entity, Robot robot) '''«messageGenerator.declareField(entity, robot)»'''
-	private def dispatch declareField(Service entity, Robot robot) '''«serviceGenerator.declareField(entity, robot)»'''
-	private def dispatch declareField(Action entity, Robot robot) '''«actionGenerator.declareField(entity, robot)»'''
+	private def dispatch declareField(Message entity, Robot robot, Config config) '''«messageGenerator.declareField(entity, robot, config)»'''
+	private def dispatch declareField(Service entity, Robot robot, Config config) '''«serviceGenerator.declareField(entity, robot, config)»'''
+	private def dispatch declareField(Action entity, Robot robot, Config config) '''«actionGenerator.declareField(entity, robot, config)»'''
 
-	private def dispatch functions(Message entity, Robot robot) '''«messageGenerator.functions(entity, robot)»'''
-	private def dispatch functions(Service entity, Robot robot) '''«serviceGenerator.functions(entity, robot)»'''
-	private def dispatch functions(Action entity, Robot robot) '''«actionGenerator.functions(entity, robot)»'''
+	private def dispatch functions(Message entity, Robot robot, Config config) '''«messageGenerator.functions(entity, robot, config)»'''
+	private def dispatch functions(Service entity, Robot robot, Config config) '''«serviceGenerator.functions(entity, robot, config)»'''
+	private def dispatch functions(Action entity, Robot robot, Config config) '''«actionGenerator.functions(entity, robot, config)»'''
 
 	private def dispatch CharSequence constructorInvocation(Message entity, Robot robot) {
 		return null