From 0a35c4964ebac79737cc94260128d926a225bac2 Mon Sep 17 00:00:00 2001
From: cs-99 <ckhuemonma@web.de>
Date: Fri, 30 Jul 2021 17:58:54 +0200
Subject: [PATCH]

---
 CMakeLists.txt            |   4 +
 action/Trigger.action     |   1 +
 garage_actionlib.xml      | 158 +++++++++++
 launch/tron_server.launch |   4 +
 src/garage_server.cpp     |   3 +-
 src/tron_adapter.cpp      | 533 ++++++++++++++++++++++++++++++++++++++
 6 files changed, 702 insertions(+), 1 deletion(-)
 create mode 100644 garage_actionlib.xml
 create mode 100644 launch/tron_server.launch
 create mode 100644 src/tron_adapter.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6ad2b79..22919bc 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -145,6 +145,7 @@ include_directories(
 # add_executable(${PROJECT_NAME}_node src/actionlib_example_node.cpp)
 add_executable(${PROJECT_NAME}_garage_server src/garage_server.cpp)
 add_executable(${PROJECT_NAME}_garage_client src/garage_client.cpp)
+add_executable(${PROJECT_NAME}_tron_adapter src/tron_adapter.cpp)
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
@@ -163,6 +164,9 @@ target_link_libraries(
 target_link_libraries(
  ${PROJECT_NAME}_garage_client ${catkin_LIBRARIES}
 )
+target_link_libraries(
+ ${PROJECT_NAME}_tron_adapter ${catkin_LIBRARIES}
+)
 
 #############
 ## Install ##
diff --git a/action/Trigger.action b/action/Trigger.action
index a727a04..0674e77 100644
--- a/action/Trigger.action
+++ b/action/Trigger.action
@@ -1,5 +1,6 @@
 bool open #goal
 ---
 bool open #result
+int32 position
 ---
 int32 position #feedback
diff --git a/garage_actionlib.xml b/garage_actionlib.xml
new file mode 100644
index 0000000..7ee98a8
--- /dev/null
+++ b/garage_actionlib.xml
@@ -0,0 +1,158 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!DOCTYPE nta PUBLIC '-//Uppaal Team//DTD Flat System 1.1//EN' 'http://www.it.uu.se/research/group/darts/uppaal/flat-1_2.dtd'>
+<nta>
+	<declaration>// Place global declarations here
+chan oeffnen, schliessen;
+broadcast chan position;
+broadcast chan fertig;
+int akt_position = 0;</declaration>
+	<template>
+		<name x="5" y="5">Tor</name>
+		<declaration>// Place local declarations here.
+clock zeit;
+int lokal;</declaration>
+		<location id="id0" x="-144" y="-76">
+			<name x="-212" y="-127">geschlossen</name>
+			<label kind="invariant" x="-238" y="-110">akt_position == 0</label>
+		</location>
+		<location id="id1" x="195" y="-229">
+			<name x="110" y="-246">oeffnend</name>
+		</location>
+		<location id="id2" x="510" y="-51">
+			<name x="476" y="-110">offen</name>
+			<label kind="invariant" x="476" y="-93">akt_position == 1000</label>
+		</location>
+		<location id="id3" x="204" y="85">
+			<name x="102" y="85">schliessend</name>
+		</location>
+		<init ref="id0"/>
+		<transition>
+			<source ref="id0"/>
+			<target ref="id3"/>
+			<label kind="synchronisation" x="-391" y="0">schliessen?</label>
+			<nail x="-280" y="-76"/>
+			<nail x="-280" y="93"/>
+		</transition>
+		<transition>
+			<source ref="id2"/>
+			<target ref="id1"/>
+			<label kind="synchronisation" x="663" y="-195">oeffnen?</label>
+			<nail x="646" y="-51"/>
+			<nail x="646" y="-238"/>
+		</transition>
+		<transition>
+			<source ref="id3"/>
+			<target ref="id1"/>
+			<label kind="synchronisation" x="280" y="-85">oeffnen?</label>
+			<nail x="280" y="-68"/>
+		</transition>
+		<transition>
+			<source ref="id1"/>
+			<target ref="id3"/>
+			<label kind="synchronisation" x="34" y="-110">schliessen?</label>
+			<nail x="119" y="-68"/>
+		</transition>
+		<transition>
+			<source ref="id3"/>
+			<target ref="id3"/>
+			<label kind="select" x="59" y="170">i : int[0,1000]</label>
+			<label kind="guard" x="59" y="187">akt_position - i &gt;= 0</label>
+			<label kind="synchronisation" x="59" y="153">position!</label>
+			<label kind="assignment" x="59" y="204">akt_position = akt_position - i</label>
+			<nail x="280" y="178"/>
+			<nail x="144" y="178"/>
+		</transition>
+		<transition>
+			<source ref="id1"/>
+			<target ref="id1"/>
+			<label kind="select" x="305" y="-356">i : int[0,1000]</label>
+			<label kind="guard" x="305" y="-339">akt_position + i &lt;= 1000</label>
+			<label kind="synchronisation" x="305" y="-373">position!</label>
+			<label kind="assignment" x="305" y="-322">akt_position = akt_position +i</label>
+			<nail x="126" y="-335"/>
+			<nail x="126" y="-335"/>
+			<nail x="270" y="-335"/>
+		</transition>
+		<transition>
+			<source ref="id3"/>
+			<target ref="id0"/>
+			<label kind="synchronisation" x="-143" y="0">fertig!</label>
+		</transition>
+		<transition>
+			<source ref="id2"/>
+			<target ref="id3"/>
+			<label kind="synchronisation" x="331" y="25">schliessen?</label>
+		</transition>
+		<transition>
+			<source ref="id1"/>
+			<target ref="id2"/>
+			<label kind="synchronisation" x="289" y="-195">fertig!</label>
+		</transition>
+		<transition>
+			<source ref="id0"/>
+			<target ref="id1"/>
+			<label kind="synchronisation" x="-76" y="-187">oeffnen?</label>
+		</transition>
+	</template>
+	<template>
+		<name>Schluessel</name>
+		<declaration>clock zeit;</declaration>
+		<location id="id4" x="-1045" y="-722">
+		</location>
+		<init ref="id4"/>
+		<transition>
+			<source ref="id4"/>
+			<target ref="id4"/>
+			<label kind="guard" x="-1215" y="-765">zeit &gt; 10</label>
+			<label kind="synchronisation" x="-1207" y="-739">schliessen!</label>
+			<label kind="assignment" x="-1224" y="-714">zeit = 0</label>
+			<nail x="-1215" y="-688"/>
+			<nail x="-1045" y="-578"/>
+			<nail x="-1045" y="-578"/>
+		</transition>
+		<transition>
+			<source ref="id4"/>
+			<target ref="id4"/>
+			<label kind="guard" x="-935" y="-850">zeit &gt; 10</label>
+			<label kind="synchronisation" x="-977" y="-867">oeffnen!</label>
+			<label kind="assignment" x="-909" y="-824">zeit = 0</label>
+			<nail x="-1038" y="-863"/>
+			<nail x="-860" y="-769"/>
+		</transition>
+	</template>
+	<system>// Place template instantiations here.
+// List one or more processes to be composed into a system.
+system Tor, Schluessel;
+    </system>
+	<queries>
+		<query>
+			<formula>A[] not deadlock
+</formula>
+			<comment></comment>
+		</query>
+		<query>
+			<formula>A[] (akt_position &gt; -1)</formula>
+			<comment></comment>
+		</query>
+		<query>
+			<formula>A[] (akt_position &lt; 1001)</formula>
+			<comment></comment>
+		</query>
+		<query>
+			<formula>E&lt;&gt; Tor.offen</formula>
+			<comment></comment>
+		</query>
+		<query>
+			<formula>E&lt;&gt; Tor.geschlossen</formula>
+			<comment></comment>
+		</query>
+		<query>
+			<formula>E&lt;&gt; Tor.oeffnend</formula>
+			<comment></comment>
+		</query>
+		<query>
+			<formula>E&lt;&gt; Tor.schliessend</formula>
+			<comment></comment>
+		</query>
+	</queries>
+</nta>
diff --git a/launch/tron_server.launch b/launch/tron_server.launch
new file mode 100644
index 0000000..8e7547f
--- /dev/null
+++ b/launch/tron_server.launch
@@ -0,0 +1,4 @@
+<launch>
+    <node name="tron_adapter" pkg="actionlib_example" type="actionlib_example_tron_adapter" respawn="false" output="screen"/>
+    <node name="actionlib_server" pkg="actionlib_example" type="actionlib_example_garage_server" respawn="false" output="screen"/>
+</launch>
diff --git a/src/garage_server.cpp b/src/garage_server.cpp
index d6cdb33..53f92fe 100644
--- a/src/garage_server.cpp
+++ b/src/garage_server.cpp
@@ -39,11 +39,12 @@ class Garage {
                 rate.sleep();
                 if (as.isPreemptRequested() || !ros::ok() || as.isNewGoalAvailable()) {
                     ROS_INFO("server preempted");
-                    as.setPreempted();
+                    as.setPreempted(res, "preempted");
                     return;
                 }
                 if (current_position == to_reach) {
                     res.open = to_reach == OPEN;
+                    res.position = current_position;
                     ROS_INFO("server goal reached");
                     as.setSucceeded(res);
                     return;
diff --git a/src/tron_adapter.cpp b/src/tron_adapter.cpp
new file mode 100644
index 0000000..c6c4c1e
--- /dev/null
+++ b/src/tron_adapter.cpp
@@ -0,0 +1,533 @@
+#include <ros/ros.h>
+#include <stdio.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <std_msgs/Int32.h>
+#include <boost/make_shared.hpp>
+#include <memory>
+#include <actionlib_example/TriggerActionGoal.h>
+#include <actionlib_example/TriggerActionFeedback.h>
+#include <actionlib_example/TriggerAction.h>
+
+const std::string IP = "127.0.0.1";
+const uint16_t PORT = 8080;
+
+typedef uint8_t byte;
+
+// some helper functions -----------------------------------------------
+
+// log bytes sent/received
+inline void byte_info(const byte* buf, int buf_length, bool send=true){
+    std::stringstream strstr;
+    strstr << (send ? "sending" : "received") << " bytes:"; 
+    for (int i = 0; i < buf_length; i++) strstr << " " << (int)buf[i];
+    ROS_INFO(strstr.str().c_str());
+}
+
+// gets count bytes from socket file descriptor (with timeout)
+const double SECONDS_BEFORE_TIMEOUT = 30;
+std::unique_ptr<byte[]> get_bytes_socket(int fd, int count){
+    std::unique_ptr<byte[]> arr = std::make_unique<byte[]>(count);
+    int already_read = 0;
+    ros::Time start_time = ros::Time::now();
+    int success = 0;
+    while (already_read < count) {
+        success = recv(fd, &arr[already_read], count-already_read, MSG_DONTWAIT);
+        if (success == 0) throw "connection was closed by TRON";
+        if (success == -1){
+            if (ros::Time::now().toSec() - start_time.toSec() >= SECONDS_BEFORE_TIMEOUT)
+                throw "timeout while reading bytes from socket file descriptor";
+            continue;
+        }
+        already_read += success; // read returns number of bytes read
+    }
+    byte_info(arr.get(), already_read, false);
+    return arr; // no explicit move needed since return value is rvalue
+};
+
+inline int32_t network_bytes_to_int_32(byte *buf){
+    uint32_t h = ntohl(*reinterpret_cast<uint32_t*>(buf));
+    return *reinterpret_cast<int32_t*>(&h);
+}
+
+inline uint16_t network_bytes_to_uint_16(byte *buf) {
+    return ntohs(*reinterpret_cast<uint16_t*>(buf));
+}
+
+// wrapping get_bytes_socket for converting to 32 bit integer
+int32_t get_int_socket(int fd) {
+    auto ack = get_bytes_socket(fd, 4);
+    return network_bytes_to_int_32(ack.get());
+}
+
+// converts num to network order and adds it to byte array starting from index
+void add_int32_in_network_order(int32_t num, byte *buf, int index){
+    uint32_t n = htonl(*reinterpret_cast<uint32_t*>(&num));
+    byte* bytes = reinterpret_cast<byte*>(&n);
+    buf[index] = bytes[0];
+    buf[++index] = bytes[1];
+    buf[++index] = bytes[2];
+    buf[++index] = bytes[3];
+}
+
+// wraps write() for printing and throwing on errors
+inline void send_bytes(int fd, const byte *buf, int length){
+    byte_info(buf, length);
+    int ret = write(fd, (void*) buf, length);
+    if (ret < 0) throw "writing failed";
+}
+
+// returns false if nothing more to read and true if 4 bytes are read successfully
+// used to reduce overhead in testing phase
+inline bool read_4_bytes_nonblock(int fd, byte *buf) {
+    int bytes_recv = recv(fd, buf, 4, MSG_DONTWAIT);
+    if (bytes_recv == -1) return false; // nothing more to read
+    if (bytes_recv == 0) throw "connection was closed";
+    if (bytes_recv != 4) throw "could not read full 4 bytes";
+    byte_info(buf, 4, false);
+    return true;
+}
+
+// returns file descriptor
+int create_connected_socket(std::string IP, uint16_t port){
+    int socketfd;
+    if ((socketfd = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
+        throw "failed to create socket";
+    }
+    ROS_INFO("socket created successfully");
+
+    struct sockaddr_in addr;
+    addr.sin_family = AF_INET;
+    addr.sin_port = htons(port);
+    {
+        int x = inet_pton(AF_INET, IP.c_str(), &addr.sin_addr);
+        if (x != 1) {
+            throw "IP could not be converted";
+        }
+    }
+    if (connect(socketfd, (struct sockaddr*)&addr, sizeof(sockaddr_in)) < 0) {
+        throw "failed to connect";
+    }
+    ROS_INFO("successfully connected");
+    return socketfd;
+}
+
+// consts for TRON ----------------------------------------------------------
+
+const byte GET_ERROR_MSG = 127;
+const byte DECL_CHAN_INPUT = 1;
+const byte DECL_CHAN_OUTPUT = 2;
+const byte ADD_VAR_TO_INPUT = 3;
+const byte ADD_VAR_TO_OUTPUT = 4;
+const byte SET_TIME_UNIT = 5;
+const byte SET_TIMEOUT = 6;
+const byte REQUEST_START = 64;
+const byte ANSWER_START = 0;
+const int32_t ACK_SINGLE = 1 << 31; // 32 Bit int with most significant bit set to 1
+
+// global vars -------------------------------------------------------------
+
+// ROS uses little endian for its messages
+const bool SYS_IS_BIG_ENDIAN = htonl(47) == 47;
+int socket_fd;
+struct Channel {
+    std::string name;
+    int32_t identifier;
+    bool is_input;
+    std::vector<std::string> vars; // associated variables in Uppaal
+    Channel(std::string name, int32_t id, bool is_input) : name(name), identifier(id), is_input(is_input){};
+};
+
+// used in struct Mapping for convenience
+std::unique_ptr<Channel> send_channel_decl_msg(bool is_input, std::string name);
+void add_var_to_channel(Channel& chan, bool is_input, std::string var);
+struct Mapping {
+    std::string topic; // ROS topic name
+
+    // offset in byte for next value (from last value) 
+    std::vector<int> byte_offset;
+
+    // custom conversion function used if data is not int32 and therefore
+    // need to be converted to an int32 first
+    // nullptr if not used
+    // needs to make sure count of bytes used gets added to *int (parameter)
+    // to indicate beginning of next field
+    std::vector<int32_t (*)(byte*, int*)> converters_to_TRON;
+    // similarly the conversion to a topic field takes the value from TRON,
+    // a pointer to the buffer at which to add the bytes and
+    // a pointer to an integer to increment by the amount of bytes added
+    std::vector<void (*)(int32_t, byte*, int*)> converters_to_topics;
+
+    Channel channel = Channel("", 0, true); // TRON channel
+    /* note: all vectors need to have the same size 
+    example: 
+        topic = "example_topic"
+        byte_offset = [4, 2]
+        converters = *some pointer converting 8 bytes to int32, nullptr
+        channel = struct Channel "example channel"
+
+    This example maps any message from topic "example_topic" to "example_channel".
+    8 Bytes starting from index 4 are converted via the given pointer 
+    and mapped to the first variable of the corresponding channel.
+    The conversion function also adds 8 to the current position, 
+    thus the next field starts 8 bytes + 2 bytes for offset after the last 
+    known position, which was 4.
+    Because the second converter points to nullptr, it is considered an int32,
+    which is mapped to the second variable of the channel. 
+    The next position is incremented by 4 if there is no conversion function given.
+    Note that the first 4 Bytes of the topics message are ignored in this example.
+    */
+
+    // Callback used if TRON sends some message
+    void(*input_callback)(Mapping& map, int32_t*) = nullptr;
+
+    Mapping(std::string topic, std::string channel, bool channelIsInput){
+        this->topic = topic;
+        this->channel = *send_channel_decl_msg(channelIsInput, channel).get();
+    }
+
+    void add_var_to_mapping(std::string name_tron, int byte_offset, 
+                         int32_t (*conv_to_TRON)(byte*, int*) = nullptr,
+                         void (*conv_to_topic)(int32_t, byte*, int*) = nullptr){
+        add_var_to_channel(this->channel, this->channel.is_input, name_tron);
+        this->byte_offset.push_back(byte_offset);
+        this->converters_to_TRON.push_back(conv_to_TRON);
+        this->converters_to_topics.push_back(conv_to_topic);
+    }
+};
+std::list<Mapping> mappings;
+std::vector<ros::Publisher> input_publishers;
+
+template <class T>
+void publish_to_topic(std::string topic, boost::shared_ptr<T> ptr) {
+    for (ros::Publisher& pub : input_publishers)
+    if (pub.getTopic() == topic) {
+        pub.publish(ptr);
+        return;
+    }
+    throw "did not find publisher for topic";
+}
+
+// callback publishing to topics like defined in mapping, used for input_callback
+template<class T>
+void mapping_callback_to_topic(Mapping& map, int32_t* vars){
+    boost::shared_ptr<T> shared_ptr = boost::make_shared<T>();
+    byte* ptr = reinterpret_cast<byte*>(shared_ptr.get());
+    int next_pos = 0;
+    for (int i = 0; i < map.channel.vars.size(); i++){
+        int32_t val = vars[i];
+        next_pos += map.byte_offset[i];
+        if (map.converters_to_topics[i] == nullptr) {
+            byte* val_bytes;
+            if (SYS_IS_BIG_ENDIAN){
+                uint32_t switched_byte_order = htonl(*reinterpret_cast<uint32_t*>(&val));
+                val = *reinterpret_cast<int32_t*>(&switched_byte_order);
+            } 
+            val_bytes = reinterpret_cast<byte*>(&val);
+            ptr[next_pos++] = val_bytes[0];
+            ptr[next_pos++] = val_bytes[1];
+            ptr[next_pos++] = val_bytes[2];
+            ptr[next_pos++] = val_bytes[3];
+        } else {
+            map.converters_to_topics[i](val, &ptr[next_pos], &next_pos);
+        }
+    }
+    publish_to_topic<T>(map.topic, shared_ptr);
+}
+
+// subscribers already have callbacks
+std::vector<ros::Subscriber> output_subscribers;
+
+// keep track of acknowledgements that are missing
+/* note: since communication is asynchronous this value can only be
+compared reliably with 0 after testing is terminated */
+/* note: TRON only acknowledges if virtual clock is used, which it is not (yet) */
+int acks_missing = 0; 
+
+void get_error_msg(int32_t errorcode) {
+    ROS_WARN("got error, trying to get corresponding message");
+    byte get_err_msg_msg[5];
+    get_err_msg_msg[0] = GET_ERROR_MSG; // get error msg code
+    add_int32_in_network_order(errorcode, get_err_msg_msg, 1);
+    send_bytes(socket_fd, get_err_msg_msg, 5); // connection is closed after this write?
+    byte err_msg_length = get_bytes_socket(socket_fd, 1)[0];
+    auto err_msg = get_bytes_socket(socket_fd, err_msg_length);
+    std::string msg_str = std::string(reinterpret_cast<char*>(err_msg.get()), (size_t) err_msg_length);
+    ROS_FATAL("TRON sent error message: %s", msg_str.c_str());
+    throw "got error from TRON";
+}
+
+void add_var_to_channel(Channel& chan, bool is_input, std::string var) {
+    byte msg[6 + var.length()];
+    msg[0] = is_input ? ADD_VAR_TO_INPUT : ADD_VAR_TO_OUTPUT;
+    add_int32_in_network_order(chan.identifier, msg, 1);
+    msg[5] = (byte) var.length();
+    for (int i = 0; i < var.length(); i++) msg[6+i] = var.at(i);
+    ROS_INFO("attaching variable %s to channel %s", var.c_str(), chan.name.c_str());
+    send_bytes(socket_fd, msg, 6 + var.length());
+    int32_t ack = get_int_socket(socket_fd);
+    if (ack < 0) get_error_msg(ack);
+    ROS_INFO("success: attached variable");
+    chan.vars.push_back(var);
+}
+
+std::unique_ptr<Channel> send_channel_decl_msg(bool is_input, std::string name) {
+    // prepare packet
+    size_t msg_length = 2 + name.length();
+    byte msg[msg_length];
+    msg[0] = is_input ? DECL_CHAN_INPUT : DECL_CHAN_OUTPUT;
+    msg[1] = name.length();
+    for (int i = 2, c = 0; i < msg_length; i++, c++) msg[i] = name[c];
+
+    // send packet
+    ROS_INFO("declaring channel %s as %s", name.c_str(), (is_input ? "input" : "output"));
+    byte_info(msg, msg_length);
+    write(socket_fd, (void*) msg, msg_length);
+
+    // get answer from TRON
+    int32_t channel_identifier = get_int_socket(socket_fd);
+    if (channel_identifier < 0) { // error handling
+        get_error_msg(channel_identifier);
+    }
+    if (channel_identifier == 0) throw "did not get channel identifier";
+    // assigned channel ID successfully
+    ROS_INFO("success: identifier for channel %s is %i", name.c_str(), channel_identifier);
+    return std::make_unique<Channel>(name, channel_identifier, is_input);
+}
+
+void set_time_unit_and_timeout(uint64_t microseconds, int32_t timeout){
+    byte msg[9];
+    msg[0] = SET_TIME_UNIT;
+    byte *microseconds_bytes = reinterpret_cast<byte*>(&microseconds);
+
+    // htonl does not exist for long int
+    if (SYS_IS_BIG_ENDIAN) {
+        for (int i = 0; i < 8; i++) msg[i+1] = microseconds_bytes[i];
+    } else {
+        for (int i = 0; i < 8; i++) msg[i+1] = microseconds_bytes[7-i];
+    }
+    ROS_INFO("setting time unit: %li microseconds", microseconds);
+    send_bytes(socket_fd, msg, 9);
+    int32_t ack = get_int_socket(socket_fd);
+    if (ack != 0) get_error_msg(ack);
+    ROS_INFO("success: set time unit");
+
+    msg[0] = SET_TIMEOUT;
+    add_int32_in_network_order(timeout, msg, 1);
+    ROS_INFO("setting timeout to %i units", timeout);
+    send_bytes(socket_fd, msg, 5);
+    ack = get_int_socket(socket_fd);
+    if (ack != 0) get_error_msg(ack);
+    ROS_INFO("success: set timeout");
+}
+ 
+void request_start() {
+    /* documentation confuses codes for start and getErrorMessage, actually used:
+    64 is start
+    127 is gerErrorMessage */
+    ROS_INFO("requesting start");
+    byte start = REQUEST_START;
+    send_bytes(socket_fd, &start, 1);
+    byte answer = get_bytes_socket(socket_fd, 1)[0];
+    if (answer != ANSWER_START) throw "starting failed";
+    ROS_INFO("success: starting test phase");
+}
+
+void report_now(Channel& chan, uint16_t var_count, int32_t *vars){
+    std::unique_ptr<byte[]> msg = std::make_unique<byte[]>(6 + 4 * var_count);
+
+    add_int32_in_network_order(chan.identifier, msg.get(), 0);
+
+    unsigned short var_count_network_order = htons(var_count);
+    byte *var_count_bytes = reinterpret_cast<byte*>(&var_count_network_order);
+    msg[4] = var_count_bytes[0];
+    msg[5] = var_count_bytes[1];
+
+    for (unsigned short i = 0; i < var_count; i++) 
+        add_int32_in_network_order(vars[i], msg.get(), 6 + i * 4);
+
+    ROS_INFO("sending to output channel %s", chan.name.c_str());
+    if (var_count == 0) ROS_INFO("no variables attached");
+    for (unsigned short i = 0; i < var_count; i++)
+        ROS_INFO("attached value %i to variable %s", vars[i], chan.vars[i].c_str());
+    send_bytes(socket_fd, msg.get(), 6 + 4 * var_count);
+    acks_missing++;
+}
+
+// callback reports to TRON like defined in mappings
+template<class T>
+void mappings_callback_to_TRON(const ros::MessageEvent<T>& event){
+    std::string topic = event.getConnectionHeader().at("topic");
+    byte *bytes = reinterpret_cast<byte*>(event.getMessage().get());
+    for (Mapping& mapping : mappings) {
+        if (mapping.topic.c_str() == topic && !mapping.channel.is_input) {
+            int var_count = mapping.byte_offset.size();
+            int32_t vars[var_count * 4];
+            int next_pos = 0;
+            for (int i = 0; i < var_count; i++) {
+                next_pos += mapping.byte_offset[i];
+                if (mapping.converters_to_TRON[i] == nullptr) {
+                    if (SYS_IS_BIG_ENDIAN)
+                        vars[i] = network_bytes_to_int_32(&bytes[next_pos]);
+                    else vars[i] = *reinterpret_cast<int32_t*>(&bytes[next_pos]);
+                    next_pos += 4;
+                    continue;
+                } else {
+                    vars[i] = mapping.converters_to_TRON[i](&bytes[next_pos], &next_pos);
+                }
+            }
+            report_now(mapping.channel, var_count, vars);
+        }
+    }
+}
+
+void send_goal (Mapping& map, int32_t* val, bool open){
+    auto shared_ptr = boost::make_shared<actionlib_example::TriggerActionGoal>();
+    shared_ptr->goal.open = open;
+    publish_to_topic<actionlib_example::TriggerActionGoal>(map.topic, shared_ptr);
+}
+void send_goal_open(Mapping& map, int32_t* val) {send_goal(map, val, true);}
+void send_goal_close(Mapping& map, int32_t* val) {send_goal(map, val, false);}
+void feedback_callback(const boost::shared_ptr<actionlib_example::TriggerActionFeedback> ptr){
+    for (Mapping& map : mappings) {
+        if (map.channel.name == "position" && map.channel.is_input == false)
+            report_now(map.channel, 1, &ptr->feedback.position);
+    }
+}
+void result_callback(const boost::shared_ptr<actionlib_example::TriggerActionResult> ptr){
+    for (Mapping& map : mappings) {
+        if (map.channel.name == "fertig" && map.channel.is_input == false
+           && ptr->status.status == 3) // 3 means succeeded, could alternively be implemented in uppaal model
+            report_now(map.channel, 1, &ptr->result.position);
+    }
+}
+
+void configuration_phase(ros::NodeHandle& nh){
+    /* note: for configuration phase maximum message length is 256 Bytes, 
+    therefore heap allocation can be avoided most of the time in called functions */
+    
+    // note: since we are not an actual client (or server) 
+    // we need to use the high level packet::*Action* messages.
+    // custom callbacks are implemented in order to not worry about header size
+    Mapping map = Mapping("/garage_server/goal", "oeffnen", true);
+    map.input_callback = send_goal_open;
+    mappings.push_back(map);
+
+    map = Mapping("/garage_server/goal", "schliessen", true);
+    map.input_callback = send_goal_close;
+    mappings.push_back(map);
+
+    input_publishers.push_back(nh.advertise<actionlib_example::TriggerActionGoal>("/garage_server/goal", 100));
+
+    map = Mapping("/garage_server/result", "fertig", false);
+    map.add_var_to_mapping("akt_position", 0);
+    mappings.push_back(map);
+    output_subscribers.push_back(nh.subscribe("/garage_server/result", 100, result_callback));
+
+    map = Mapping("/garage_server/feedback", "position", false);
+    map.add_var_to_mapping("akt_position", 0);
+    mappings.push_back(map);
+    output_subscribers.push_back(nh.subscribe("/garage_server/feedback", 100, 
+            feedback_callback));
+    // not obvious in documentation: local variables are not supported
+    // add_var_to_channel(socketfd, "ausloesen", "lokal");
+    uint64_t microseconds = 1000000; // one second
+    // documentation states 2 signed integers are used for some reason
+    set_time_unit_and_timeout(microseconds, 100);
+
+    // wait till subscribers initialized
+    for (ros::Publisher& pub : input_publishers) {
+        while (pub.getNumSubscribers() == 0) { ros::Duration(1).sleep(); };
+    }
+}
+
+void process_TRONs_msgs(){
+    /* note: TRONs communication after start is not guaranteed to be synchronous,
+    thus incoming messages must be checked for their content */
+    while (true){
+        // get 4 bytes at a time as an int32
+        byte bytes[4];
+        if (!read_4_bytes_nonblock(socket_fd, bytes)) 
+            break; // no bytes left to process
+        int32_t next = network_bytes_to_int_32(bytes);
+
+        // bytes are acknowledgement
+        if (next == ACK_SINGLE) {
+            if (--acks_missing < 0) throw "too many acknowledgements";
+            ROS_INFO("got acknowledgement for output");
+            continue;
+        }
+
+        // bytes are channel identifier
+        // find corresponding channel
+        const Channel *chan = nullptr;
+        for (Mapping& map : mappings) if (map.channel.identifier == next) chan = &map.channel;
+        /* note: this only happens if message number 12 in TRON User Manual is received,
+            which should not be the case according to the documentation */
+        if (chan == nullptr) 
+            throw "channel could not be identified";
+
+        ROS_INFO("got channel identifier (%s) for input", chan->name.c_str());
+
+        // channel identified, assuming all following bytes are correct
+
+        // get number of variables
+        recv(socket_fd, bytes, 2, MSG_DONTWAIT);
+        byte_info(bytes, 2, false);
+        uint16_t var_count = network_bytes_to_uint_16(bytes);
+        ROS_INFO("got variable count %i", var_count);
+
+        int32_t vals[var_count];
+        // process variables
+        for (uint16_t i = 0; i < var_count; i++) {
+            recv(socket_fd, bytes, 4, MSG_DONTWAIT);
+            next = network_bytes_to_int_32(bytes);
+            std::string var = chan->vars[i];
+            ROS_INFO("got variable number %i:  value of %s is %i", i+1, var.c_str(), next);
+            vals[i] = next;
+        }
+
+        for (Mapping& map : mappings)
+            if (map.channel.name == chan->name && map.channel.is_input)
+                for (ros::Publisher& pub : input_publishers){
+                    if (pub.getTopic() == map.topic) {
+                        if (map.input_callback != nullptr)
+                            map.input_callback(map, vals);  
+                        else throw "no callback declared";  
+                        break;
+                    }
+                }
+
+        // send acknowledgement
+        add_int32_in_network_order(ACK_SINGLE, bytes, 0);
+        ROS_INFO("sending acknowledgement");
+        send_bytes(socket_fd, bytes, 4);
+    }
+}
+
+int main(int argc, char**argv){
+    ros::init(argc, argv, "TRON dapter");
+    ros::NodeHandle nh;
+
+    try {
+        socket_fd = create_connected_socket(IP, PORT);
+
+        configuration_phase(nh);
+        
+        request_start(); 
+
+        // testing phase loop
+        ros::Rate test_phase_freq(1);
+        ros::AsyncSpinner spinner(4);
+        spinner.start();
+        while (ros::ok()) {
+            //ros::spinOnce();
+            process_TRONs_msgs();
+            test_phase_freq.sleep();
+        } 
+    } catch (const char* err){
+        ROS_FATAL("shutting down: %s", err);
+        ros::shutdown();
+    }
+}
\ No newline at end of file
-- 
GitLab