Commit 92c1176f authored by Svetlana's avatar Svetlana

added torcs client, build and run scripts.

parent 4be0024d
......@@ -44,6 +44,7 @@ import java.util.LinkedList;
import java.util.List;
import de.monticore.lang.monticar.generator.order.simulator.AbstractSymtab;
import org.apache.commons.io.FileUtils;
public class Experiments {
......@@ -57,11 +58,21 @@ public class Experiments {
generateForEmamComponent("Imagepreprocessing");
generateForEmamComponent("Safetycontroller");
generateForEmamComponent("Drivercontroller");
generateForEmadlComponent("Featureextraction");
generateForEmamComponent("Featureextraction");
//generateForEmadlComponent("Featureextraction");
try {
Files.copy(Paths.get(RESOURCES_PATH+"scripts/build_all.sh"), Paths.get(TARGET_PATH_GENERATED+"build_all.sh"), StandardCopyOption.REPLACE_EXISTING);
Runtime.getRuntime().exec("chmod u+x "+TARGET_PATH_GENERATED+"build_all.sh");
Files.copy(Paths.get(RESOURCES_PATH+"scripts/run_all.sh"), Paths.get(TARGET_PATH_GENERATED+"run_all.sh"), StandardCopyOption.REPLACE_EXISTING);
Runtime.getRuntime().exec("chmod u+x "+TARGET_PATH_GENERATED+"run_all.sh");
FileUtils.copyDirectory(new File(RESOURCES_PATH+"torcs"), new File(TARGET_PATH_GENERATED+"torcs"));
Runtime.getRuntime().exec("chmod u+x "+TARGET_PATH_GENERATED+"torcs/build.sh");
Runtime.getRuntime().exec("chmod u+x "+TARGET_PATH_GENERATED+"torcs/run_all.sh");
FileUtils.copyDirectory(new File(RESOURCES_PATH+"dpnet"), new File(TARGET_PATH_GENERATED+"model/dpnet"));
} catch (IOException e) {
System.err.println("Failed to copy build and start scripts.");
e.printStackTrace();
......
......@@ -6,7 +6,9 @@ ports
out Q(0:1)^{3,1} commandsOut;
implementation Math {
commandsOut=affordanceIn;
commandsOut(1,1)=0.5; // steering
commandsOut(2,1)=0.8; // acceleration
commandsOut(3,1)=0.0; // brake
}
}
......@@ -3,5 +3,5 @@ conforms to de.monticore.lang.monticar.generator.roscpp.RosToEmamTagSchema;
tags Drivercontroller {
tag drivercontroller.affordanceIn with RosConnection = {topic=(/affordance,std_msgs/Float64MultiArray)};
tag drivercontroller.commandsOut with RosConnection = {topic=(/commands,std_msgs/Float64MultiArray)};
tag drivercontroller.commandsOut with RosConnection = {topic=(/commands,std_msgs/Float32MultiArray)};
}
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
}
}
package dp;
component Featureextraction {
ports
in Z(0:255)^{640, 480} imageIn,
out Q(0:1)^{13,1} affordanceOut;
implementation Math {
affordanceOut=[0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0];
}
//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
//}
}
......@@ -3,5 +3,5 @@ conforms to de.monticore.lang.monticar.generator.roscpp.RosToEmamTagSchema;
tags Featureextraction {
tag featureextraction.imageIn with RosConnection = {topic=(/images,std_msgs/Int8MultiArray)};
tag featureextraction.affordanceOut with RosConnection = {topic=(/affordance,std_msgs/Int8MultiArray)};
tag featureextraction.affordanceOut with RosConnection = {topic=(/affordance,std_msgs/Float64MultiArray)};
}
......@@ -32,6 +32,10 @@ cmake ..
make -j
cd ../..
echo "Building TORCS adapters"
cd torcs
./build.sh
echo
echo "Well done!"
echo "Run ./run_all.sh to run all."
......
%!/bin/bash
xterm -T ROSCORE -e roscore &
echo "Waiting for ROSCORE to start..."
sleep 2
xterm -T TORCSDriver -e torcs/src/driver/build/driver &
xterm -T TORCSCamera -e torcs/src/camera/build/camera &
xterm -T DriverController -e Drivercontroller/build/coordinator/Coordinator_dp_drivercontroller &
xterm -T SafetyController -e Safetycontroller/build/coordinator/Coordinator_dp_safetycontroller &
......
%!/bin/bash
source devel/setup.bash
xterm -T driver -e src/driver/build/driver &
xterm -T camera -e src/camera/build/camera &
#include "ros/ros.h"
#include "std_msgs/Float64MultiArray.h"
#include "std_msgs/Int8MultiArray.h"
#include "torcsclient.h"
#include <sstream>
......@@ -8,7 +8,7 @@ int main(int argc, char **argv)
{
ros::init(argc, argv, "camera");
ros::NodeHandle n;
ros::Publisher chatter_pub = n.advertise<std_msgs::Float64MultiArray>("/camera", 1000);
ros::Publisher chatter_pub = n.advertise<std_msgs::Int8MultiArray>("/camera", 1000);
ros::Rate loop_rate(10);
int count = 0;
......@@ -16,8 +16,8 @@ int main(int argc, char **argv)
while (ros::ok())
{
std_msgs::Float64MultiArray::Ptr msg(new std_msgs::Float64MultiArray);
msg->data.resize(torcs::IMAGE_SIZE_BYTES);
std_msgs::Int8MultiArray::Ptr msg(new std_msgs::Int8MultiArray);
msg->data.resize(torcs::IMAGE_SIZE_BYTES);
const auto imageData = client.getScreenshot();
memcpy(msg->data.data(), imageData->data(), imageData->size());
......
......@@ -89,6 +89,10 @@ void TorcsClient::sendCommand(Command cmd)
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
shared->accelCmd = cmd.accelCmd;
shared->steerCmd = cmd.steerCmd;
shared->brakeCmd = cmd.brakeCmd;
shared->control = 0;
//shared->accelCmd = 10.0;
//shared->steerCmd = 1.0;
......
......@@ -47,7 +47,14 @@ struct shared_use_st
struct Command
{
Command(float accel, float steer, float brakec)
: accelCmd(accel), steerCmd(steer), brakeCmd(brakec)
{
}
float accelCmd;
float steerCmd;
float brakeCmd;
};
using DataContainer = std::array<uint8_t, IMAGE_SIZE_BYTES>;
......
#include "ros/ros.h"
#include "std_msgs/Float64MultiArray.h"
#include "std_msgs/Float32MultiArray.h"
#include "torcsclient.h"
#include <memory>
std::unique_ptr<torcs::TorcsClient> client;
void chatterCallback(const std_msgs::Float64MultiArray::ConstPtr& msg)
void chatterCallback(const std_msgs::Float32MultiArray::ConstPtr& msg)
{
ROS_INFO("Received command: %f", msg->data[0]);
client->sendCommand(torcs::Command());
ROS_INFO("Received command. accel: %f, steer: %f, break: %f", msg->data[0], msg->data[1], msg->data[2]);
client->sendCommand(torcs::Command(msg->data[0], msg->data[1], msg->data[2]));
}
int main(int argc, char **argv)
......@@ -20,7 +20,7 @@ int main(int argc, char **argv)
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("/commands", 1000, chatterCallback);
ros::Subscriber sub = n.subscribe("/commands", 100, chatterCallback);
ros::spin();
return 0;
......
......@@ -89,6 +89,10 @@ void TorcsClient::sendCommand(Command cmd)
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
shared->accelCmd = cmd.accelCmd;
shared->steerCmd = cmd.steerCmd;
shared->brakeCmd = cmd.brakeCmd;
shared->control = 0;
//shared->accelCmd = 10.0;
//shared->steerCmd = 1.0;
......
......@@ -47,7 +47,14 @@ struct shared_use_st
struct Command
{
Command(float accel, float steer, float brakec)
: accelCmd(accel), steerCmd(steer), brakeCmd(brakec)
{
}
float accelCmd;
float steerCmd;
float brakeCmd;
};
using DataContainer = std::array<uint8_t, IMAGE_SIZE_BYTES>;
......
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)
<?xml version="1.0"?>
<package>
<name>torcs</name>
<version>0.0.0</version>
<description>The torcs package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="svetlana.pavlitskaya@rwth-aachen.de">Sveta</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/torcs</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *_depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<!-- <run_depend>message_runtime</run_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>std_msgs</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
#ifndef HELPERA_H
#define HELPERA_H
#define _GLIBCXX_USE_CXX11_ABI 0
#include <iostream>
#include "armadillo.h"
#include <stdarg.h>
#include <initializer_list>
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<A.n_cols;++i){
result(i, 0) = getIndexForClusterCentroids(A, i, centroids);
}
return result;
}
static int getIndexForClusterCentroids(mat A, int colIndex, mat centroids){
int index=1;
double lowestDistance=getEuclideanDistance(A, colIndex, centroids, 0);
for(int i=1;i<centroids.n_cols;++i){
double curDistance=getEuclideanDistance(A, colIndex, centroids, i);
if(curDistance<lowestDistance){
lowestDistance=curDistance;
index=i+1;
}
}
return index;
}
static double getEuclideanDistance(mat A, int colIndexA, mat B, int colIndexB){
double distance=0;
for(int i=0;i<A.n_rows;++i){
double elementA=A(i,colIndexA);
double elementB=B(i,colIndexB);
double diff=elementA-elementB;
distance+=diff*diff;
}
return sqrt(distance);
}
static mat getSqrtMat(mat A){
cx_mat result=sqrtmat(A);
return real(result);
}
static mat invertDiagMatrix(mat A){
for(int i=0;i<A.n_rows;++i){
double curVal = A(i,i);
A(i,i) = 1/curVal;
}
return A;
}
};
#endif
#ifndef TESTS_A_COMPA
#define TESTS_A_COMPA
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
#include "armadillo.h"
using namespace arma;
class tests_a_compA{
public:
double rosIn;
double noRosIn;
double rosOut;
double noRosOut;
void init()
{
}
void execute()