From 4be0024d276e3b8d5cf09496b5e632794f0b2434 Mon Sep 17 00:00:00 2001 From: Svetlana Date: Mon, 21 May 2018 01:33:18 +0200 Subject: [PATCH] generation for emadl component --- RosWorkspace/build.sh | 2 +- ...ded_montiarc_behaviour_0_0_10_SNAPSHOT.xml | 13 -- TorcsEMAMGenerator/experiments.iml | 2 +- TorcsEMAMGenerator/pom.xml | 5 + .../monticar/torcs_dl/EMADLGeneratorImpl.java | 53 +++++ .../lang/monticar/torcs_dl/Experiments.java | 95 ++++++--- .../src/main/models/dp/Alexnet.emadl | 47 ----- .../src/main/models/dp/Featureextraction.cnnt | 7 + .../main/models/dp/Featureextraction.emadl | 34 +++ .../src/main/models/dp/Featureextraction.emam | 8 - .../src/main/resources/scripts/build_all.sh | 16 +- .../src/main/resources/scripts/run_all.sh | 10 + .../src/main/resources/torcs/build.sh | 21 ++ .../src/main/resources/torcs/run_all.sh | 7 + .../resources/torcs/src/camera/CMakeLists.txt | 12 ++ .../resources/torcs/src/camera/src/main.cpp | 32 +++ .../torcs/src/camera/src/torcsclient.cpp | 130 ++++++++++++ .../torcs/src/camera/src/torcsclient.h | 68 ++++++ .../resources/torcs/src/driver/CMakeLists.txt | 12 ++ .../resources/torcs/src/driver/src/main.cpp | 27 +++ .../torcs/src/driver/src/torcsclient.cpp | 130 ++++++++++++ .../torcs/src/driver/src/torcsclient.h | 68 ++++++ .../resources/torcs/src/torcs/CMakeLists.txt | 198 ++++++++++++++++++ .../resources/torcs/src/torcs/package.xml | 54 +++++ .../resources/torcs/src/torcs/src/HelperA.h | 91 ++++++++ .../torcs/src/torcs/src/tests_a_compA.h | 23 ++ .../src/torcs/src/tests_a_compA_RosWrapper.h | 33 +++ .../resources/torcs/src/torcs/src/torcs.cpp | 74 +++++++ .../torcs/src/torcs/src/torcsclient.cpp | 130 ++++++++++++ .../torcs/src/torcs/src/torcsclient.h | 68 ++++++ run_all.sh | 10 - 31 files changed, 1365 insertions(+), 115 deletions(-) delete mode 100644 TorcsEMAMGenerator/.idea/libraries/Maven__de_monticore_lang_monticar_embedded_montiarc_behaviour_0_0_10_SNAPSHOT.xml create mode 100644 TorcsEMAMGenerator/src/main/java/de/monticore/lang/monticar/torcs_dl/EMADLGeneratorImpl.java delete mode 100644 TorcsEMAMGenerator/src/main/models/dp/Alexnet.emadl create mode 100644 TorcsEMAMGenerator/src/main/models/dp/Featureextraction.cnnt create mode 100644 TorcsEMAMGenerator/src/main/models/dp/Featureextraction.emadl delete mode 100644 TorcsEMAMGenerator/src/main/models/dp/Featureextraction.emam rename build_all.sh => TorcsEMAMGenerator/src/main/resources/scripts/build_all.sh (50%) create mode 100755 TorcsEMAMGenerator/src/main/resources/scripts/run_all.sh create mode 100755 TorcsEMAMGenerator/src/main/resources/torcs/build.sh create mode 100755 TorcsEMAMGenerator/src/main/resources/torcs/run_all.sh create mode 100644 TorcsEMAMGenerator/src/main/resources/torcs/src/camera/CMakeLists.txt create mode 100644 TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/main.cpp create mode 100644 TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/torcsclient.cpp create mode 100644 TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/torcsclient.h create mode 100644 TorcsEMAMGenerator/src/main/resources/torcs/src/driver/CMakeLists.txt create mode 100644 TorcsEMAMGenerator/src/main/resources/torcs/src/driver/src/main.cpp create mode 100644 TorcsEMAMGenerator/src/main/resources/torcs/src/driver/src/torcsclient.cpp create mode 100644 TorcsEMAMGenerator/src/main/resources/torcs/src/driver/src/torcsclient.h create mode 100644 TorcsEMAMGenerator/src/main/resources/torcs/src/torcs/CMakeLists.txt create mode 100644 TorcsEMAMGenerator/src/main/resources/torcs/src/torcs/package.xml create mode 100644 TorcsEMAMGenerator/src/main/resources/torcs/src/torcs/src/HelperA.h create mode 100644 TorcsEMAMGenerator/src/main/resources/torcs/src/torcs/src/tests_a_compA.h create mode 100644 TorcsEMAMGenerator/src/main/resources/torcs/src/torcs/src/tests_a_compA_RosWrapper.h create mode 100644 TorcsEMAMGenerator/src/main/resources/torcs/src/torcs/src/torcs.cpp create mode 100644 TorcsEMAMGenerator/src/main/resources/torcs/src/torcs/src/torcsclient.cpp create mode 100644 TorcsEMAMGenerator/src/main/resources/torcs/src/torcs/src/torcsclient.h delete mode 100755 run_all.sh diff --git a/RosWorkspace/build.sh b/RosWorkspace/build.sh index 1311643..3d63b92 100755 --- a/RosWorkspace/build.sh +++ b/RosWorkspace/build.sh @@ -1,5 +1,5 @@ %!/bin/bash -source devel/setup.bash +#source devel/setup.bash catkin_make diff --git a/TorcsEMAMGenerator/.idea/libraries/Maven__de_monticore_lang_monticar_embedded_montiarc_behaviour_0_0_10_SNAPSHOT.xml b/TorcsEMAMGenerator/.idea/libraries/Maven__de_monticore_lang_monticar_embedded_montiarc_behaviour_0_0_10_SNAPSHOT.xml deleted file mode 100644 index 0c30ccb..0000000 --- a/TorcsEMAMGenerator/.idea/libraries/Maven__de_monticore_lang_monticar_embedded_montiarc_behaviour_0_0_10_SNAPSHOT.xml +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - \ No newline at end of file diff --git a/TorcsEMAMGenerator/experiments.iml b/TorcsEMAMGenerator/experiments.iml index a5f4d0f..f5d5025 100644 --- a/TorcsEMAMGenerator/experiments.iml +++ b/TorcsEMAMGenerator/experiments.iml @@ -56,7 +56,6 @@ - @@ -85,5 +84,6 @@ + \ No newline at end of file diff --git a/TorcsEMAMGenerator/pom.xml b/TorcsEMAMGenerator/pom.xml index 4a2f8d6..9d3f31d 100644 --- a/TorcsEMAMGenerator/pom.xml +++ b/TorcsEMAMGenerator/pom.xml @@ -46,6 +46,11 @@ embedded-montiarc-deeplearning 0.2.0-SNAPSHOT + + de.monticore.lang.monticar + embedded-montiarc-behaviour + 0.0.11-SNAPSHOT + \ No newline at end of file diff --git a/TorcsEMAMGenerator/src/main/java/de/monticore/lang/monticar/torcs_dl/EMADLGeneratorImpl.java b/TorcsEMAMGenerator/src/main/java/de/monticore/lang/monticar/torcs_dl/EMADLGeneratorImpl.java new file mode 100644 index 0000000..0542bc3 --- /dev/null +++ b/TorcsEMAMGenerator/src/main/java/de/monticore/lang/monticar/torcs_dl/EMADLGeneratorImpl.java @@ -0,0 +1,53 @@ +package de.monticore.lang.monticar.torcs_dl; + +import de.monticore.lang.embeddedmontiarc.embeddedmontiarc._symboltable.ExpandedComponentInstanceSymbol; +import de.monticore.lang.monticar.generator.FileContent; +import de.monticore.lang.monticar.emadl.generator.EMADLGenerator; +import de.monticore.lang.monticar.generator.middleware.helpers.NameHelper; +import de.monticore.lang.monticar.generator.middleware.helpers.TemplateHelper; +import de.monticore.lang.monticar.generator.middleware.impls.GeneratorImpl; +import de.monticore.lang.tagging._symboltable.TaggingResolver; + +import java.io.File; +import java.io.IOException; +import java.util.ArrayList; +import java.util.List; + +public class EMADLGeneratorImpl implements GeneratorImpl { + private String generationTargetPath; + private EMADLGenerator emadlGenerator; + + public EMADLGeneratorImpl(String modelPath){ + emadlGenerator = new EMADLGenerator(); + emadlGenerator.setModelsPath(modelPath); + } + + + @Override + public List generate(ExpandedComponentInstanceSymbol componentInstanceSymbol, TaggingResolver taggingResolver) throws IOException { + List files = new ArrayList<>(); + + emadlGenerator.setGenerationTargetPath(generationTargetPath); + List fileContents = emadlGenerator.generateStrings(taggingResolver, componentInstanceSymbol, taggingResolver); + + for (FileContent fileContent : fileContents) { + files.add(emadlGenerator.getEmamGen().generateFile(fileContent)); + } + + files.add(emadlGenerator.getEmamGen().generateFile((generateCMake(componentInstanceSymbol)))); + return files; + } + + @Override + public void setGenerationTargetPath(String path) { + this.generationTargetPath = path; + } + + private FileContent generateCMake(ExpandedComponentInstanceSymbol componentInstanceSymbol) { + FileContent cmake = new FileContent(); + cmake.setFileName("CMakeLists.txt"); + String name = NameHelper.getNameTargetLanguage(componentInstanceSymbol.getFullName()); + cmake.setFileContent(TemplateHelper.getCmakeCppTemplate().replace("${compName}", name)); + return cmake; + } +} diff --git a/TorcsEMAMGenerator/src/main/java/de/monticore/lang/monticar/torcs_dl/Experiments.java b/TorcsEMAMGenerator/src/main/java/de/monticore/lang/monticar/torcs_dl/Experiments.java index 82d3023..d8a00ec 100644 --- a/TorcsEMAMGenerator/src/main/java/de/monticore/lang/monticar/torcs_dl/Experiments.java +++ b/TorcsEMAMGenerator/src/main/java/de/monticore/lang/monticar/torcs_dl/Experiments.java @@ -1,11 +1,21 @@ package de.monticore.lang.monticar.torcs_dl; +import de.monticore.ModelingLanguageFamily; +import de.monticore.io.paths.ModelPath; +import de.monticore.lang.embeddedmontiarc.LogConfig; +import de.monticore.lang.embeddedmontiarc.embeddedmontiarc._symboltable.ComponentSymbol; +import de.monticore.lang.embeddedmontiarc.embeddedmontiarc._symboltable.ConstantPortSymbol; import de.monticore.lang.embeddedmontiarc.embeddedmontiarc._symboltable.ExpandedComponentInstanceSymbol; +import de.monticore.lang.embeddedmontiarc.embeddedmontiarcmath._symboltable.EmbeddedMontiArcMathLanguage; import de.monticore.lang.embeddedmontiarc.tagging.RosToEmamTagSchema; -import de.monticore.lang.monticar.emadl.generator.Generator; +import de.monticore.lang.monticar.Utils; +import de.monticore.lang.monticar.emadl._symboltable.EMADLLanguage; +import de.monticore.lang.monticar.enumlang._symboltable.EnumLangLanguage; +import de.monticore.lang.monticar.generator.cpp.converter.MathConverter; import de.monticore.lang.monticar.generator.middleware.MiddlewareGenerator; import de.monticore.lang.monticar.generator.middleware.impls.CPPGenImpl; import de.monticore.lang.monticar.generator.middleware.impls.RosCppGenImpl; +import de.monticore.lang.monticar.generator.optimization.ThreadingOptimizer; import de.monticore.lang.monticar.generator.order.nfp.TagBreakpointsTagSchema.TagBreakpointsTagSchema; import de.monticore.lang.monticar.generator.order.nfp.TagDelayTagSchema.TagDelayTagSchema; import de.monticore.lang.monticar.generator.order.nfp.TagExecutionOrderTagSchema.TagExecutionOrderTagSchema; @@ -14,58 +24,95 @@ import de.monticore.lang.monticar.generator.order.nfp.TagMinMaxTagSchema.TagMinM import de.monticore.lang.monticar.generator.order.nfp.TagTableTagSchema.TagTableTagSchema; import de.monticore.lang.monticar.generator.order.nfp.TagThresholdTagSchema.TagThresholdTagSchema; import de.monticore.lang.monticar.generator.roscpp.helper.TagHelper; +import de.monticore.lang.monticar.streamunits._symboltable.StreamUnitsLanguage; +import de.monticore.lang.monticar.struct._symboltable.StructLanguage; import de.monticore.lang.tagging._symboltable.TaggingResolver; +import de.monticore.symboltable.GlobalScope; import de.monticore.symboltable.Scope; +import de.se_rwth.commons.Splitters; +import de.se_rwth.commons.logging.Log; import freemarker.template.TemplateException; +import java.io.File; import java.io.IOException; +import java.nio.file.Files; import java.nio.file.Path; import java.nio.file.Paths; +import java.nio.file.StandardCopyOption; import java.util.Arrays; import java.util.LinkedList; +import java.util.List; -import static de.monticore.lang.monticar.generator.order.simulator.AbstractSymtab.createSymTab; +import de.monticore.lang.monticar.generator.order.simulator.AbstractSymtab; public class Experiments { public static final String TARGET_PATH_GENERATED = "./target/generated-sources-torcs-dl/"; public static final String MODELS_PATH = "src/main/models/"; + public static final String RESOURCES_PATH = "src/main/resources/"; public static void main(String[] args) { System.out.println("Starting code generation..."); - TaggingResolver taggingResolver = createSymTabAndTaggingResolver(MODELS_PATH); - RosToEmamTagSchema.registerTagTypes(taggingResolver); + generateForEmamComponent("Imagepreprocessing"); + generateForEmamComponent("Safetycontroller"); + generateForEmamComponent("Drivercontroller"); + generateForEmadlComponent("Featureextraction"); - LinkedList inputs = new LinkedList(); - inputs.add("Imagepreprocessing"); - inputs.add("Featureextraction"); - inputs.add("Safetycontroller"); - inputs.add("Drivercontroller"); + try { + Files.copy(Paths.get(RESOURCES_PATH+"scripts/build_all.sh"), Paths.get(TARGET_PATH_GENERATED+"build_all.sh"), StandardCopyOption.REPLACE_EXISTING); + Files.copy(Paths.get(RESOURCES_PATH+"scripts/run_all.sh"), Paths.get(TARGET_PATH_GENERATED+"run_all.sh"), StandardCopyOption.REPLACE_EXISTING); + } catch (IOException e) { + System.err.println("Failed to copy build and start scripts."); + e.printStackTrace(); + } + } + + private static void generateForEmamComponent(String componentName) { + TaggingResolver taggingResolver = createSymTabAndTaggingResolver( MODELS_PATH); + RosToEmamTagSchema.registerTagTypes(taggingResolver); try { - for (String inpu : inputs) { - System.out.println("Generating for " + inpu); + System.out.println("Generating for " + componentName); + ExpandedComponentInstanceSymbol componentInstanceSymbol = taggingResolver.resolve("dp."+componentName.toLowerCase(), ExpandedComponentInstanceSymbol.KIND).orElse(null); + + TagHelper.resolveTags(taggingResolver, componentInstanceSymbol); - ExpandedComponentInstanceSymbol componentInstanceSymbol = taggingResolver.resolve("dp."+inpu.toLowerCase(), ExpandedComponentInstanceSymbol.KIND).orElse(null); + MiddlewareGenerator middlewareGenerator = new MiddlewareGenerator(); + middlewareGenerator.add(new CPPGenImpl(), "cpp"); + middlewareGenerator.add(new RosCppGenImpl(), "roscpp"); + middlewareGenerator.setGenerationTargetPath(TARGET_PATH_GENERATED +componentName); + middlewareGenerator.generate(componentInstanceSymbol, taggingResolver); + } catch (IOException e) { + System.err.println("Failed to generate sources. Sorry."); + e.printStackTrace(); + } + } - TagHelper.resolveTags(taggingResolver, componentInstanceSymbol); + private static void generateForEmadlComponent(String componentName) { + TaggingResolver taggingResolver = de.monticore.lang.monticar.emadl.generator.AbstractSymtab.createSymTabAndTaggingResolver(MODELS_PATH); + RosToEmamTagSchema.registerTagTypes(taggingResolver); - MiddlewareGenerator middlewareGenerator = new MiddlewareGenerator(); + try { + System.out.println("Generating for " + componentName); + ExpandedComponentInstanceSymbol componentInstanceSymbol = taggingResolver.resolve("dp."+componentName.toLowerCase(), ExpandedComponentInstanceSymbol.KIND).orElse(null); - middlewareGenerator.add(new CPPGenImpl(), "cpp"); - middlewareGenerator.add(new RosCppGenImpl(), "roscpp"); - middlewareGenerator.setGenerationTargetPath(TARGET_PATH_GENERATED +inpu); - middlewareGenerator.generate(componentInstanceSymbol, taggingResolver); + TagHelper.resolveTags(taggingResolver, componentInstanceSymbol); - } + MiddlewareGenerator middlewareGenerator = new MiddlewareGenerator(); + middlewareGenerator.add(new EMADLGeneratorImpl(MODELS_PATH), "cpp"); + middlewareGenerator.add(new RosCppGenImpl(), "roscpp"); + middlewareGenerator.setGenerationTargetPath(TARGET_PATH_GENERATED +componentName); + middlewareGenerator.generate(componentInstanceSymbol, taggingResolver); } catch (IOException e) { + System.err.println("Failed to generate sources. Sorry."); e.printStackTrace(); } } - public static TaggingResolver createSymTabAndTaggingResolver(String... modelPath) { - Scope scope = createSymTab(modelPath); + + public static TaggingResolver createSymTabAndTaggingResolver( String... modelPath) { + Scope scope = AbstractSymtab.createSymTab(modelPath); TaggingResolver tagging = new TaggingResolver(scope, Arrays.asList(modelPath)); TagMinMaxTagSchema.registerTagTypes(tagging); TagTableTagSchema.registerTagTypes(tagging); @@ -77,10 +124,4 @@ public class Experiments { return tagging; } - - private void generateEMADL(String qualifiedName) throws IOException, TemplateException { - Path modelPath = Paths.get(MODELS_PATH); - Generator gen = new Generator(); - gen.generate(modelPath, qualifiedName); - } } diff --git a/TorcsEMAMGenerator/src/main/models/dp/Alexnet.emadl b/TorcsEMAMGenerator/src/main/models/dp/Alexnet.emadl deleted file mode 100644 index e3b1ee3..0000000 --- a/TorcsEMAMGenerator/src/main/models/dp/Alexnet.emadl +++ /dev/null @@ -1,47 +0,0 @@ -component Alexnet{ - ports in Z(0:255)^{3, 224, 224} image, - out Q(0:1)^{10,1,1} predictions; - - implementation CNN { - - def split1(i){ - [i] -> - Convolution(kernel=(5,5), channels=128) -> - Lrn(nsize=5, alpha=0.0001, beta=0.75) -> - Pooling(pool_type="max", kernel=(3,3), stride=(2,2), padding="no_loss") -> - Relu() - } - def split2(i){ - [i] -> - Convolution(kernel=(3,3), channels=192) -> - Relu() -> - Convolution(kernel=(3,3), channels=128) -> - Pooling(pool_type="max", kernel=(3,3), stride=(2,2), padding="no_loss") -> - Relu() - } - def fc(){ - FullyConnected(units=4096) -> - Relu() -> - Dropout() - } - - image -> - Convolution(kernel=(11,11), channels=96, stride=(4,4), padding="no_loss") -> - Lrn(nsize=5, alpha=0.0001, beta=0.75) -> - Pooling(pool_type="max", kernel=(3,3), stride=(2,2), padding="no_loss") -> - Relu() -> - Split(n=2) -> - split1(i=[0|1]) -> - Concatenate() -> - Convolution(kernel=(3,3), channels=384) -> - Relu() -> - Split(n=2) -> - split2(i=[0|1]) -> - Concatenate() -> - fc(->=2) -> - FullyConnected(units=10) -> - Softmax() -> - predictions - - } -} \ No newline at end of file diff --git a/TorcsEMAMGenerator/src/main/models/dp/Featureextraction.cnnt b/TorcsEMAMGenerator/src/main/models/dp/Featureextraction.cnnt new file mode 100644 index 0000000..00ffe86 --- /dev/null +++ b/TorcsEMAMGenerator/src/main/models/dp/Featureextraction.cnnt @@ -0,0 +1,7 @@ +configuration Featureextraction{ + num_epoch : 100 + batch_size : 64 + optimizer : sgd{ + learning_rate : 0.01 + } +} diff --git a/TorcsEMAMGenerator/src/main/models/dp/Featureextraction.emadl b/TorcsEMAMGenerator/src/main/models/dp/Featureextraction.emadl new file mode 100644 index 0000000..12fce64 --- /dev/null +++ b/TorcsEMAMGenerator/src/main/models/dp/Featureextraction.emadl @@ -0,0 +1,34 @@ +package dp; + +component Featureextraction { + ports + in Z(0:255)^{3, 210, 280} imageIn, + out Q(0:1)^{13,1,1} affordanceOut; + + implementation CNN { + + def conv(kernel, channels, hasPool=true, convStride=(1,1)){ + Convolution(kernel=kernel, channels=channels, stride=convStride) -> + Relu() -> + Pooling(pool_type="max", kernel=(3,3), stride=(2,2), ?=hasPool) + } + def fc(){ + FullyConnected(units=4096) -> + Relu() -> + Dropout() + } + + imageIn -> + conv(kernel=(11,11), channels=96, convStride=(4,4)) -> + conv(kernel=(5,5), channels=256, convStride=(4,4)) -> + conv(kernel=(3,3), channels=384, hasPool=false) -> + conv(kernel=(3,3), channels=384, hasPool=false) -> + conv(kernel=(3,3), channels=256) -> + fc() -> + fc() -> + FullyConnected(units=13) -> + Sigmoid() -> + affordanceOut + + } +} diff --git a/TorcsEMAMGenerator/src/main/models/dp/Featureextraction.emam b/TorcsEMAMGenerator/src/main/models/dp/Featureextraction.emam deleted file mode 100644 index f22d370..0000000 --- a/TorcsEMAMGenerator/src/main/models/dp/Featureextraction.emam +++ /dev/null @@ -1,8 +0,0 @@ -package dp; - -component Featureextraction { -ports - in Z(0:255)^{640,480} imageIn, - out Q(0:1)^{13,1} affordanceOut; -} - diff --git a/build_all.sh b/TorcsEMAMGenerator/src/main/resources/scripts/build_all.sh similarity index 50% rename from build_all.sh rename to TorcsEMAMGenerator/src/main/resources/scripts/build_all.sh index 2395c85..09505a9 100755 --- a/build_all.sh +++ b/TorcsEMAMGenerator/src/main/resources/scripts/build_all.sh @@ -1,36 +1,36 @@ %!/bin/bash echo "Building DriverController" -cd TorcsEMAMGenerator/target/generated-sources-torcs-dl/Drivercontroller +cd Drivercontroller mkdir -p build cd build cmake .. make -j -cd ../../../../.. +cd ../.. echo "Building SafetyController" -cd TorcsEMAMGenerator/target/generated-sources-torcs-dl/Safetycontroller +cd Safetycontroller mkdir -p build cd build cmake .. make -j -cd ../../../../.. +cd ../.. echo "Building FeatureExtraction" -cd TorcsEMAMGenerator/target/generated-sources-torcs-dl/Featureextraction +cd Featureextraction mkdir -p build cd build cmake .. make -j -cd ../../../../.. +cd ../.. echo "Building ImagePreprocessing" -cd TorcsEMAMGenerator/target/generated-sources-torcs-dl/Imagepreprocessing +cd Imagepreprocessing mkdir -p build cd build cmake .. make -j -cd ../../../../.. +cd ../.. echo echo "Well done!" diff --git a/TorcsEMAMGenerator/src/main/resources/scripts/run_all.sh b/TorcsEMAMGenerator/src/main/resources/scripts/run_all.sh new file mode 100755 index 0000000..4ad2cfc --- /dev/null +++ b/TorcsEMAMGenerator/src/main/resources/scripts/run_all.sh @@ -0,0 +1,10 @@ +%!/bin/bash + +xterm -T DriverController -e Drivercontroller/build/coordinator/Coordinator_dp_drivercontroller & + +xterm -T SafetyController -e Safetycontroller/build/coordinator/Coordinator_dp_safetycontroller & + +xterm -T FeatureExtraction -e Featureextraction/build/coordinator/Coordinator_dp_featureextraction & + +xterm -T ImagePreprocessing -e Imagepreprocessing/build/coordinator/Coordinator_dp_imagepreprocessing & + diff --git a/TorcsEMAMGenerator/src/main/resources/torcs/build.sh b/TorcsEMAMGenerator/src/main/resources/torcs/build.sh new file mode 100755 index 0000000..f2f0f8c --- /dev/null +++ b/TorcsEMAMGenerator/src/main/resources/torcs/build.sh @@ -0,0 +1,21 @@ +%!/bin/bash + +#source devel/setup.bash# +#catkin_make + +cd src + +cd camera +mkdir -p build +cd build +cmake .. +make -j +cd ../.. + +cd driver +mkdir -p build +cd build +cmake .. +make -j +cd ../.. + diff --git a/TorcsEMAMGenerator/src/main/resources/torcs/run_all.sh b/TorcsEMAMGenerator/src/main/resources/torcs/run_all.sh new file mode 100755 index 0000000..efb48f9 --- /dev/null +++ b/TorcsEMAMGenerator/src/main/resources/torcs/run_all.sh @@ -0,0 +1,7 @@ +%!/bin/bash + +source devel/setup.bash + +xterm -T driver -e src/driver/build/driver & +xterm -T camera -e src/camera/build/camera & + diff --git a/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/CMakeLists.txt b/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/CMakeLists.txt new file mode 100644 index 0000000..50ac0d9 --- /dev/null +++ b/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.5) +project (ROSCamera) +set (CXX_STANDARD_REQUIRED 14) + +find_package(roscpp REQUIRED) +find_package(std_msgs REQUIRED) + +add_executable(camera src/main.cpp src/torcsclient.cpp) + +set_property(TARGET camera PROPERTY CXX_STANDARD 14) +target_link_libraries(camera ${roscpp_LIBRARIES} ${std_msgs_LIBRARIES}) +target_include_directories(camera PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} ${roscpp_INCLUDE_DIRS} ${std_msgs_INCLUDE_DIRS}) diff --git a/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/main.cpp b/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/main.cpp new file mode 100644 index 0000000..932a418 --- /dev/null +++ b/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/main.cpp @@ -0,0 +1,32 @@ +#include "ros/ros.h" +#include "std_msgs/Float64MultiArray.h" +#include "torcsclient.h" + +#include + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "camera"); + ros::NodeHandle n; + ros::Publisher chatter_pub = n.advertise("/camera", 1000); + ros::Rate loop_rate(10); + int count = 0; + + torcs::TorcsClient client; + + while (ros::ok()) + { + std_msgs::Float64MultiArray::Ptr msg(new std_msgs::Float64MultiArray); + msg->data.resize(torcs::IMAGE_SIZE_BYTES); + + const auto imageData = client.getScreenshot(); + memcpy(msg->data.data(), imageData->data(), imageData->size()); + + ROS_INFO("%f", msg->data[0]); + chatter_pub.publish(msg); + ros::spinOnce(); + loop_rate.sleep(); + ++count; + } + return 0; +} diff --git a/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/torcsclient.cpp b/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/torcsclient.cpp new file mode 100644 index 0000000..a0d25d3 --- /dev/null +++ b/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/torcsclient.cpp @@ -0,0 +1,130 @@ +#include "torcsclient.h" + +#include +#include +#include +#include +#include +#include + +using namespace torcs; + +TorcsClient::TorcsClient() +{ + std::cout << "TORCS Client!" << std::endl; + + void *shm = NULL; + int shmid; + + shmid = shmget((key_t)4567, sizeof(struct shared_use_st), 0666); + if(shmid < 0) + { + std::cerr << "Failed to get shared memory!" << std::endl; + return; + } + + shm = shmat(shmid, 0, 0); + if(shm == (void*)-1) + { + std::cerr << "Failed to shmat()!" << std::endl; + return; + } + + connected = true; + + std::cout << "Started shared memory at " << std::hex << shm << std::endl; + + shared = (struct shared_use_st*)shm; + shared->written = 0; + shared->control = 0; + shared->pause = 0; + shared->fast = 0.0; + + shared->dist_L = 0.0; + shared->dist_R = 0.0; + + shared->toMarking_L = 0.0; + shared->toMarking_M = 0.0; + shared->toMarking_R = 0.0; + + shared->dist_LL = 0.0; + shared->dist_MM = 0.0; + shared->dist_RR = 0.0; + + shared->toMarking_LL = 0.0; + shared->toMarking_ML = 0.0; + shared->toMarking_MR = 0.0; + shared->toMarking_RR = 0.0; + + shared->toMiddle = 0.0; + shared->angle = 0.0; + shared->speed = 0.0; + + shared->steerCmd = 0.0; + shared->accelCmd = 0.0; + shared->brakeCmd = 0.0; +} + +std::unique_ptr TorcsClient::getScreenshot() +{ + assert(connected); + + while(shared->written == 0) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + auto result = std::make_unique(); + memcpy(result->data(), shared->data, result->size()); + shared->written = 0; + + return result; +} + +void TorcsClient::sendCommand(Command cmd) +{ + assert(connected); + + while (shared->control == 0) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + shared->control = 0; + //shared->accelCmd = 10.0; + //shared->steerCmd = 1.0; +} + +/*void writeScreenshot(shared_use_st* shared, unsigned int& num) +{ + std::cout << "Read image." << std::endl; + + FIBITMAP* newShot = FreeImage_Allocate(IMAGE_WIDTH, IMAGE_HEIGHT, 24); + if (!newShot) + { + std::cerr << "Failed to allocate freeimage file." << std::endl; + FreeImage_Unload(newShot); + return; + } + + BYTE *bits = (BYTE*)FreeImage_GetBits(newShot); + + for (int h = 0; h < IMAGE_HEIGHT; h++) { + BYTE *pixel = (BYTE*)bits; + + for (int w = 0; w < IMAGE_WIDTH; w++) { + pixel[FI_RGBA_RED] = shared->data[((h)*IMAGE_WIDTH+w)*3+0]; + pixel[FI_RGBA_GREEN] = shared->data[((h)*IMAGE_WIDTH+w)*3+1]; + pixel[FI_RGBA_BLUE] = shared->data[((h)*IMAGE_WIDTH+w)*3+2]; + pixel += 3; + } + bits += 3*IMAGE_WIDTH; + } + + FreeImage_Save(FIF_BMP, newShot, (std::string("shot-")+std::to_string(num)).c_str()); + FreeImage_Unload(newShot); + + shared->control = 0; + shared->written = 0; + std::this_thread::sleep_for(std::chrono::seconds(2));std::cout << "Read image." << std::endl; +}*/ + diff --git a/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/torcsclient.h b/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/torcsclient.h new file mode 100644 index 0000000..0edb26a --- /dev/null +++ b/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/torcsclient.h @@ -0,0 +1,68 @@ +#pragma once + +#include +#include +#include + +namespace torcs +{ + +constexpr auto IMAGE_WIDTH = 280; +constexpr auto IMAGE_HEIGHT = 210; +constexpr auto IMAGE_CHANNELS = 3; +constexpr auto IMAGE_SIZE_BYTES = IMAGE_WIDTH*IMAGE_HEIGHT*IMAGE_CHANNELS; + +struct shared_use_st +{ + int written = 0; //a label, if 1: available to read, if 0: available to write + uint8_t data[IMAGE_SIZE_BYTES]; // image data field + int control = 0; + int pause = 0; + double fast = 0.0; + + double dist_L = 0.0; + double dist_R = 0.0; + + double toMarking_L = 0.0; + double toMarking_M = 0.0; + double toMarking_R = 0.0; + + double dist_LL = 0.0; + double dist_MM = 0.0; + double dist_RR = 0.0; + + double toMarking_LL = 0.0; + double toMarking_ML = 0.0; + double toMarking_MR = 0.0; + double toMarking_RR = 0.0; + + double toMiddle = 0.0; + double angle = 0.0; + double speed = 0.0; + + double steerCmd = 0.0; + double accelCmd = 0.0; + double brakeCmd = 0.0; +}; + +struct Command +{ + +}; + +using DataContainer = std::array; + +class TorcsClient +{ +public: + TorcsClient(); + std::unique_ptr getScreenshot(); + void sendCommand(Command cmd); + +private: + struct shared_use_st *shared; + bool connected = false; +}; + +} + diff --git a/TorcsEMAMGenerator/src/main/resources/torcs/src/driver/CMakeLists.txt b/TorcsEMAMGenerator/src/main/resources/torcs/src/driver/CMakeLists.txt new file mode 100644 index 0000000..5b47524 --- /dev/null +++ b/TorcsEMAMGenerator/src/main/resources/torcs/src/driver/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.5) +project (ROSDriver) +set (CXX_STANDARD_REQUIRED 11) + +find_package(roscpp REQUIRED) +find_package(std_msgs REQUIRED) + +add_executable(driver src/main.cpp src/torcsclient.cpp) + +set_property(TARGET driver PROPERTY CXX_STANDARD 14) +target_link_libraries(driver ${roscpp_LIBRARIES} ${std_msgs_LIBRARIES}) +target_include_directories(driver PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} ${roscpp_INCLUDE_DIRS} ${std_msgs_INCLUDE_DIRS}) diff --git a/TorcsEMAMGenerator/src/main/resources/torcs/src/driver/src/main.cpp b/TorcsEMAMGenerator/src/main/resources/torcs/src/driver/src/main.cpp new file mode 100644 index 0000000..691573e --- /dev/null +++ b/TorcsEMAMGenerator/src/main/resources/torcs/src/driver/src/main.cpp @@ -0,0 +1,27 @@ +#include "ros/ros.h" +#include "std_msgs/Float64MultiArray.h" +#include "torcsclient.h" + +#include + +std::unique_ptr client; + +void chatterCallback(const std_msgs::Float64MultiArray::ConstPtr& msg) +{ + ROS_INFO("Received command: %f", msg->data[0]); + client->sendCommand(torcs::Command()); +} + +int main(int argc, char **argv) +{ + client = std::make_unique(); + + ros::init(argc, argv, "driver"); + + ros::NodeHandle n; + + ros::Subscriber sub = n.subscribe("/commands", 1000, chatterCallback); + ros::spin(); + + return 0; +} diff --git a/TorcsEMAMGenerator/src/main/resources/torcs/src/driver/src/torcsclient.cpp b/TorcsEMAMGenerator/src/main/resources/torcs/src/driver/src/torcsclient.cpp new file mode 100644 index 0000000..a0d25d3 --- /dev/null +++ b/TorcsEMAMGenerator/src/main/resources/torcs/src/driver/src/torcsclient.cpp @@ -0,0 +1,130 @@ +#include "torcsclient.h" + +#include +#include +#include +#include +#include +#include + +using namespace torcs; + +TorcsClient::TorcsClient() +{ + std::cout << "TORCS Client!" << std::endl; + + void *shm = NULL; + int shmid; + + shmid = shmget((key_t)4567, sizeof(struct shared_use_st), 0666); + if(shmid < 0) + { + std::cerr << "Failed to get shared memory!" << std::endl; + return; + } + + shm = shmat(shmid, 0, 0); + if(shm == (void*)-1) + { + std::cerr << "Failed to shmat()!" << std::endl; + return; + } + + connected = true; + + std::cout << "Started shared memory at " << std::hex << shm << std::endl; + + shared = (struct shared_use_st*)shm; + shared->written = 0; + shared->control = 0; + shared->pause = 0; + shared->fast = 0.0; + + shared->dist_L = 0.0; + shared->dist_R = 0.0; + + shared->toMarking_L = 0.0; + shared->toMarking_M = 0.0; + shared->toMarking_R = 0.0; + + shared->dist_LL = 0.0; + shared->dist_MM = 0.0; + shared->dist_RR = 0.0; + + shared->toMarking_LL = 0.0; + shared->toMarking_ML = 0.0; + shared->toMarking_MR = 0.0; + shared->toMarking_RR = 0.0; + + shared->toMiddle = 0.0; + shared->angle = 0.0; + shared->speed = 0.0; + + shared->steerCmd = 0.0; + shared->accelCmd = 0.0; + shared->brakeCmd = 0.0; +} + +std::unique_ptr TorcsClient::getScreenshot() +{ + assert(connected); + + while(shared->written == 0) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + auto result = std::make_unique(); + memcpy(result->data(), shared->data, result->size()); + shared->written = 0; + + return result; +} + +void TorcsClient::sendCommand(Command cmd) +{ + assert(connected); + + while (shared->control == 0) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + shared->control = 0; + //shared->accelCmd = 10.0; + //shared->steerCmd = 1.0; +} + +/*void writeScreenshot(shared_use_st* shared, unsigned int& num) +{ + std::cout << "Read image." << std::endl; + + FIBITMAP* newShot = FreeImage_Allocate(IMAGE_WIDTH, IMAGE_HEIGHT, 24); + if (!newShot) + { + std::cerr << "Failed to allocate freeimage file." << std::endl; + FreeImage_Unload(newShot); + return; + } + + BYTE *bits = (BYTE*)FreeImage_GetBits(newShot); + + for (int h = 0; h < IMAGE_HEIGHT; h++) { + BYTE *pixel = (BYTE*)bits; + + for (int w = 0; w < IMAGE_WIDTH; w++) { + pixel[FI_RGBA_RED] = shared->data[((h)*IMAGE_WIDTH+w)*3+0]; + pixel[FI_RGBA_GREEN] = shared->data[((h)*IMAGE_WIDTH+w)*3+1]; + pixel[FI_RGBA_BLUE] = shared->data[((h)*IMAGE_WIDTH+w)*3+2]; + pixel += 3; + } + bits += 3*IMAGE_WIDTH; + } + + FreeImage_Save(FIF_BMP, newShot, (std::string("shot-")+std::to_string(num)).c_str()); + FreeImage_Unload(newShot); + + shared->control = 0; + shared->written = 0; + std::this_thread::sleep_for(std::chrono::seconds(2));std::cout << "Read image." << std::endl; +}*/ + diff --git a/TorcsEMAMGenerator/src/main/resources/torcs/src/driver/src/torcsclient.h b/TorcsEMAMGenerator/src/main/resources/torcs/src/driver/src/torcsclient.h new file mode 100644 index 0000000..0edb26a --- /dev/null +++ b/TorcsEMAMGenerator/src/main/resources/torcs/src/driver/src/torcsclient.h @@ -0,0 +1,68 @@ +#pragma once + +#include +#include +#include + +namespace torcs +{ + +constexpr auto IMAGE_WIDTH = 280; +constexpr auto IMAGE_HEIGHT = 210; +constexpr auto IMAGE_CHANNELS = 3; +constexpr auto IMAGE_SIZE_BYTES = IMAGE_WIDTH*IMAGE_HEIGHT*IMAGE_CHANNELS; + +struct shared_use_st +{ + int written = 0; //a label, if 1: available to read, if 0: available to write + uint8_t data[IMAGE_SIZE_BYTES]; // image data field + int control = 0; + int pause = 0; + double fast = 0.0; + + double dist_L = 0.0; + double dist_R = 0.0; + + double toMarking_L = 0.0; + double toMarking_M = 0.0; + double toMarking_R = 0.0; + + double dist_LL = 0.0; + double dist_MM = 0.0; + double dist_RR = 0.0; + + double toMarking_LL = 0.0; + double toMarking_ML = 0.0; + double toMarking_MR = 0.0; + double toMarking_RR = 0.0; + + double toMiddle = 0.0; + double angle = 0.0; + double speed = 0.0; + + double steerCmd = 0.0; + double accelCmd = 0.0; + double brakeCmd = 0.0; +}; + +struct Command +{ + +}; + +using DataContainer = std::array; + +class TorcsClient +{ +public: + TorcsClient(); + std::unique_ptr getScreenshot(); + void sendCommand(Command cmd); + +private: + struct shared_use_st *shared; + bool connected = false; +}; + +} + diff --git a/TorcsEMAMGenerator/src/main/resources/torcs/src/torcs/CMakeLists.txt b/TorcsEMAMGenerator/src/main/resources/torcs/src/torcs/CMakeLists.txt new file mode 100644 index 0000000..9f44c91 --- /dev/null +++ b/TorcsEMAMGenerator/src/main/resources/torcs/src/torcs/CMakeLists.txt @@ -0,0 +1,198 @@ +cmake_minimum_required(VERSION 2.8.3) +project(torcs) + +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + std_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a run_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES torcs +# CATKIN_DEPENDS roscpp std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/torcs.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +add_executable(${PROJECT_NAME} src/torcs.cpp src/torcsclient.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_torcs.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/TorcsEMAMGenerator/src/main/resources/torcs/src/torcs/package.xml b/TorcsEMAMGenerator/src/main/resources/torcs/src/torcs/package.xml new file mode 100644 index 0000000..3f72d79 --- /dev/null +++ b/TorcsEMAMGenerator/src/main/resources/torcs/src/torcs/package.xml @@ -0,0 +1,54 @@ + + + torcs + 0.0.0 + The torcs package + + + + + Sveta + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + std_msgs + roscpp + std_msgs + + + + + + + + diff --git a/TorcsEMAMGenerator/src/main/resources/torcs/src/torcs/src/HelperA.h b/TorcsEMAMGenerator/src/main/resources/torcs/src/torcs/src/HelperA.h new file mode 100644 index 0000000..ccd99d0 --- /dev/null +++ b/TorcsEMAMGenerator/src/main/resources/torcs/src/torcs/src/HelperA.h @@ -0,0 +1,91 @@ +#ifndef HELPERA_H +#define HELPERA_H +#define _GLIBCXX_USE_CXX11_ABI 0 +#include +#include "armadillo.h" +#include +#include +using namespace arma; +class HelperA{ +public: +static mat getEigenVectors(mat A){ +vec eigenValues; +mat eigenVectors; +eig_sym(eigenValues,eigenVectors,A); +return eigenVectors; +} +static vec getEigenValues(mat A){ +vec eigenValues; +mat eigenVectors; +eig_sym(eigenValues,eigenVectors,A); +return eigenValues; +} + +static mat getKMeansClusters(mat A, int k){ +mat clusters; +kmeans(clusters,A.t(),k,random_subset,20,true); +printf("cluster centroid calculation done\n"); +std::ofstream myfile; + myfile.open("data after cluster.txt"); + myfile << A; + myfile.close(); + + std::ofstream myfile2; + myfile2.open("cluster centroids.txt"); + myfile2 << clusters; + myfile2.close(); +mat indexedData=getKMeansClustersIndexData(A.t(), clusters); + +std::ofstream myfile3; + myfile3.open("data after index.txt"); + myfile3 << indexedData; + myfile3.close(); + +return indexedData; +} + +static mat getKMeansClustersIndexData(mat A, mat centroids){ + mat result=mat(A.n_cols, 1); + for(int i=0;i +#include +#include +class tests_a_compA_RosWrapper{ + tests_a_compA component; + ros::Subscriber _clockSubscriber; + ros::Publisher _echoPublisher; + +public: + void _clockCallback(const rosgraph_msgs::Clock::ConstPtr& msg){ + component.rosIn = msg->clock.toSec(); + ROS_INFO("%f", msg->clock.toSec()); + } + + tests_a_compA_RosWrapper(ros::NodeHandle node_handle, ros::NodeHandle private_node_handle){ + _clockSubscriber = node_handle.subscribe("/clock" ,5,&tests_a_compA_RosWrapper::_clockCallback, this, ros::TransportHints().tcpNoDelay()); + _echoPublisher = node_handle.advertise("/echo",5); + } + + void publish0(){ + std_msgs::Float32 tmpMsg; + tmpMsg.data = component.rosOut; + _echoPublisher.publish(tmpMsg); + } + + void tick(){ + component.execute(); + publish0(); + } + +}; diff --git a/TorcsEMAMGenerator/src/main/resources/torcs/src/torcs/src/torcs.cpp b/TorcsEMAMGenerator/src/main/resources/torcs/src/torcs/src/torcs.cpp new file mode 100644 index 0000000..2980b53 --- /dev/null +++ b/TorcsEMAMGenerator/src/main/resources/torcs/src/torcs/src/torcs.cpp @@ -0,0 +1,74 @@ +// %Tag(FULLTEXT)% +// %Tag(ROS_HEADER)% +#include +// %EndTag(ROS_HEADER)% +// %Tag(MSG_HEADER)% +#include +// %EndTag(MSG_HEADER)% + +#include "torcsclient.h" + +#include + +int main(int argc, char **argv) +{ + +// %Tag(INIT)% + ros::init(argc, argv, "torcs"); +// %EndTag(INIT)% + +// %Tag(NODEHANDLE)% + ros::NodeHandle n; +// %EndTag(NODEHANDLE)% + +// %Tag(PUBLISHER)% + ros::Publisher pub = n.advertise("torcsClient", 1000); +// %EndTag(PUBLISHER)% + +// %Tag(LOOP_RATE)% + ros::Rate loop_rate(100); +// %EndTag(LOOP_RATE)% + + torcs::TorcsClient client; + +// %Tag(ROS_OK)% + + while (ros::ok()) + { + +// %EndTag(ROS_OK)% + +// %Tag(FILL_MESSAGE)% + +// %EndTag(FILL_MESSAGE)% + +// %Tag(ROSCONSOLE)% + +// %EndTag(ROSCONSOLE)% + + +// %Tag(PUBLISH)% + std_msgs::ByteMultiArray msg; + + msg.data.clear(); + msg.data.resize(torcs::IMAGE_SIZE_BYTES); + const auto imageData = client.getScreenshot(); + memcpy(msg.data.data(), imageData->data(), imageData->size()); + pub.publish(msg); + +// %EndTag(PUBLISH)% + +// %Tag(SPINONCE)% + +// %EndTag(SPINONCE)% + +// %Tag(RATE_SLEEP)% + loop_rate.sleep(); +// %EndTag(RATE_SLEEP)% + } + + + return 0; +} +// %EndTag(FULLTEXT)% + diff --git a/TorcsEMAMGenerator/src/main/resources/torcs/src/torcs/src/torcsclient.cpp b/TorcsEMAMGenerator/src/main/resources/torcs/src/torcs/src/torcsclient.cpp new file mode 100644 index 0000000..a0d25d3 --- /dev/null +++ b/TorcsEMAMGenerator/src/main/resources/torcs/src/torcs/src/torcsclient.cpp @@ -0,0 +1,130 @@ +#include "torcsclient.h" + +#include +#include +#include +#include +#include +#include + +using namespace torcs; + +TorcsClient::TorcsClient() +{ + std::cout << "TORCS Client!" << std::endl; + + void *shm = NULL; + int shmid; + + shmid = shmget((key_t)4567, sizeof(struct shared_use_st), 0666); + if(shmid < 0) + { + std::cerr << "Failed to get shared memory!" << std::endl; + return; + } + + shm = shmat(shmid, 0, 0); + if(shm == (void*)-1) + { + std::cerr << "Failed to shmat()!" << std::endl; + return; + } + + connected = true; + + std::cout << "Started shared memory at " << std::hex << shm << std::endl; + + shared = (struct shared_use_st*)shm; + shared->written = 0; + shared->control = 0; + shared->pause = 0; + shared->fast = 0.0; + + shared->dist_L = 0.0; + shared->dist_R = 0.0; + + shared->toMarking_L = 0.0; + shared->toMarking_M = 0.0; + shared->toMarking_R = 0.0; + + shared->dist_LL = 0.0; + shared->dist_MM = 0.0; + shared->dist_RR = 0.0; + + shared->toMarking_LL = 0.0; + shared->toMarking_ML = 0.0; + shared->toMarking_MR = 0.0; + shared->toMarking_RR = 0.0; + + shared->toMiddle = 0.0; + shared->angle = 0.0; + shared->speed = 0.0; + + shared->steerCmd = 0.0; + shared->accelCmd = 0.0; + shared->brakeCmd = 0.0; +} + +std::unique_ptr TorcsClient::getScreenshot() +{ + assert(connected); + + while(shared->written == 0) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + auto result = std::make_unique(); + memcpy(result->data(), shared->data, result->size()); + shared->written = 0; + + return result; +} + +void TorcsClient::sendCommand(Command cmd) +{ + assert(connected); + + while (shared->control == 0) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + shared->control = 0; + //shared->accelCmd = 10.0; + //shared->steerCmd = 1.0; +} + +/*void writeScreenshot(shared_use_st* shared, unsigned int& num) +{ + std::cout << "Read image." << std::endl; + + FIBITMAP* newShot = FreeImage_Allocate(IMAGE_WIDTH, IMAGE_HEIGHT, 24); + if (!newShot) + { + std::cerr << "Failed to allocate freeimage file." << std::endl; + FreeImage_Unload(newShot); + return; + } + + BYTE *bits = (BYTE*)FreeImage_GetBits(newShot); + + for (int h = 0; h < IMAGE_HEIGHT; h++) { + BYTE *pixel = (BYTE*)bits; + + for (int w = 0; w < IMAGE_WIDTH; w++) { + pixel[FI_RGBA_RED] = shared->data[((h)*IMAGE_WIDTH+w)*3+0]; + pixel[FI_RGBA_GREEN] = shared->data[((h)*IMAGE_WIDTH+w)*3+1]; + pixel[FI_RGBA_BLUE] = shared->data[((h)*IMAGE_WIDTH+w)*3+2]; + pixel += 3; + } + bits += 3*IMAGE_WIDTH; + } + + FreeImage_Save(FIF_BMP, newShot, (std::string("shot-")+std::to_string(num)).c_str()); + FreeImage_Unload(newShot); + + shared->control = 0; + shared->written = 0; + std::this_thread::sleep_for(std::chrono::seconds(2));std::cout << "Read image." << std::endl; +}*/ + diff --git a/TorcsEMAMGenerator/src/main/resources/torcs/src/torcs/src/torcsclient.h b/TorcsEMAMGenerator/src/main/resources/torcs/src/torcs/src/torcsclient.h new file mode 100644 index 0000000..30c6e4d --- /dev/null +++ b/TorcsEMAMGenerator/src/main/resources/torcs/src/torcs/src/torcsclient.h @@ -0,0 +1,68 @@ +#pragma once + +#include +#include +#include + +namespace torcs +{ + +constexpr auto IMAGE_WIDTH = 640; +constexpr auto IMAGE_HEIGHT = 480; +constexpr auto IMAGE_CHANNELS = 3; +constexpr auto IMAGE_SIZE_BYTES = IMAGE_WIDTH*IMAGE_HEIGHT*IMAGE_CHANNELS; + +struct shared_use_st +{ + int written = 0; //a label, if 1: available to read, if 0: available to write + uint8_t data[IMAGE_SIZE_BYTES]; // image data field + int control = 0; + int pause = 0; + double fast = 0.0; + + double dist_L = 0.0; + double dist_R = 0.0; + + double toMarking_L = 0.0; + double toMarking_M = 0.0; + double toMarking_R = 0.0; + + double dist_LL = 0.0; + double dist_MM = 0.0; + double dist_RR = 0.0; + + double toMarking_LL = 0.0; + double toMarking_ML = 0.0; + double toMarking_MR = 0.0; + double toMarking_RR = 0.0; + + double toMiddle = 0.0; + double angle = 0.0; + double speed = 0.0; + + double steerCmd = 0.0; + double accelCmd = 0.0; + double brakeCmd = 0.0; +}; + +struct Command +{ + +}; + +using DataContainer = std::array; + +class TorcsClient +{ +public: + TorcsClient(); + std::unique_ptr getScreenshot(); + void sendCommand(Command cmd); + +private: + struct shared_use_st *shared; + bool connected = false; +}; + +} + diff --git a/run_all.sh b/run_all.sh deleted file mode 100755 index d9f7a4c..0000000 --- a/run_all.sh +++ /dev/null @@ -1,10 +0,0 @@ -%!/bin/bash - -xterm -T DriverController -e TorcsEMAMGenerator/target/generated-sources-torcs-dl/Drivercontroller/build/coordinator/Coordinator_dp_drivercontroller & - -xterm -T SafetyController -e TorcsEMAMGenerator/target/generated-sources-torcs-dl/Safetycontroller/build/coordinator/Coordinator_dp_safetycontroller & - -xterm -T FeatureExtraction -e TorcsEMAMGenerator/target/generated-sources-torcs-dl/Featureextraction/build/coordinator/Coordinator_dp_featureextraction & - -xterm -T ImagePreprocessing -e TorcsEMAMGenerator/target/generated-sources-torcs-dl/Imagepreprocessing/build/coordinator/Coordinator_dp_imagepreprocessing & - -- GitLab