No commit message

No commit message

main.cpp

@@ -5,7 +5,21 @@
 #include <netinet/in.h>
 #include <arpa/inet.h>
 
-typedef unsigned char byte;
+typedef uint8_t byte;
+
+// 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 = -2147483648; // 32 Bit int with most significant bit set to 1
+
+
 // function declarations --------------------------------------------------------
 
 // gets count bytes from socket file descriptor
@@ -14,25 +28,25 @@ std::unique_ptr<byte[]> get_bytes_socket(int fd, int count);
 // log bytes sent/received
 void byte_info(const byte *msg, int msg_length, bool send=true); 
 
-// wrapping get_bytes_socket for converting to integer
-int get_int_socket(int fd);
+// wrapping get_bytes_socket for converting to 32 bit integer
+int32_t get_int_socket(int fd);
 
 // retrieves error message and prints it, then throws
-void get_error_msg(int fd, int errorcode); // needs fix see implementation
+void get_error_msg(int fd, int32_t errorcode); // needs fix see implementation
 
 // converts num to network order and adds it to msg starting from index
-void add_int32_in_network_order(int num, byte *msg, int index);
+void add_int32_in_network_order(int32_t num, byte *msg, int index);
 
 // wraps write() for printing and throwing on errors
 void send_bytes(int fd, const byte* bytes, int length);
 
-void set_time_unit_and_timeout(int fd, unsigned long int microseconds, int timeout);
+void set_time_unit_and_timeout(int fd, uint64_t microseconds, int32_t timeout);
 
 // attaches UPPAAL variable called var to channel
 void add_var_to_channel(int fd, std::string channel, bool is_input, std::string var);
 
 // reports to var_count variables to channel named chan_name
-void report_now(int fd, std::string chan_name, unsigned short var_count, int *vars);
+void report_now(int fd, std::string chan_name, unsigned short var_count, int32_t *vars);
 
 // throws if not answered with byte 0
 void request_start(int fd);
@@ -40,10 +54,10 @@ void request_start(int fd);
 
 const double SECONDS_BEFORE_TIMEOUT = 30;
 struct Channel {
-    int identifier;
+    int32_t identifier;
     bool is_input;
     std::vector<std::string> vars; // associated variables in Uppaal
-    Channel(int id, bool is_input) : identifier(id), is_input(is_input), vars(){};
+    Channel(int32_t id, bool is_input) : identifier(id), is_input(is_input){};
     Channel() = default; // default constructor needed for std::map
 };
 std::map<std::string, Channel> channels;
@@ -67,14 +81,22 @@ std::unique_ptr<byte[]> get_bytes_socket(int fd, int count){
     return arr; // no explicit move needed since return value is rvalue
 };
 
-int get_int_socket(int fd) {
+int32_t bytes_to_int_32(byte *ptr){
+    uint32_t h = ntohl(*reinterpret_cast<uint32_t*>(ptr));
+    return *reinterpret_cast<int32_t*>(&h);
+}
+
+uint16_t bytes_to_uint_16(byte *ptr) {
+    return ntohs(*reinterpret_cast<uint16_t*>(ptr));
+}
+
+int32_t get_int_socket(int fd) {
     auto ack = get_bytes_socket(fd, 4);
-    unsigned int h = ntohl(*reinterpret_cast<unsigned int*>(ack.get()));
-    return *reinterpret_cast<int*>(&h); // reinterpret unsigned int as int
+    return bytes_to_int_32(ack.get());
 }
 
-void add_int32_in_network_order(int num, byte *msg, int index){
-    unsigned int n = htonl(*reinterpret_cast<unsigned int*>(&num));
+void add_int32_in_network_order(int32_t num, byte *msg, int index){
+    uint32_t n = htonl(*reinterpret_cast<uint32_t*>(&num));
     byte* bytes = reinterpret_cast<byte*>(&n);
     msg[index] = bytes[0];
     msg[++index] = bytes[1];
@@ -95,10 +117,10 @@ void send_bytes(int fd, const byte* bytes, int length){
     if (ret < 0) throw "writing failed";
 }
 
-void get_error_msg(int fd, int errorcode) {
+void get_error_msg(int fd, int32_t errorcode) {
     ROS_WARN("got error, trying to get corresponding message");
     std::unique_ptr<byte[]> get_err_msg_msg = std::make_unique<byte[]>(5);
-    get_err_msg_msg[0] = 127; // get error msg code
+    get_err_msg_msg[0] = GET_ERROR_MSG; // get error msg code
     add_int32_in_network_order(errorcode, get_err_msg_msg.get(), 1);
     send_bytes(fd, get_err_msg_msg.get(), 5); // connection is closed after this write?
     byte err_msg_length = get_bytes_socket(fd, 1)[0];
@@ -111,14 +133,14 @@ void get_error_msg(int fd, int errorcode) {
 void add_var_to_channel(int fd, std::string channel, bool is_input, std::string var) {
     std::unique_ptr<byte[]> msg = std::make_unique<byte[]>(6 + var.length());
     if (channels.find(channel) == channels.end()) throw "channel not declared";
-    Channel chan = channels.at(channel);
-    msg[0] = is_input ? 3 : 4;
+    Channel& chan = channels.at(channel);
+    msg[0] = is_input ? ADD_VAR_TO_INPUT : ADD_VAR_TO_OUTPUT;
     add_int32_in_network_order(chan.identifier, msg.get(), 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(), channel.c_str());
     send_bytes(fd, msg.get(), 6 + var.length());
-    int ack = get_int_socket(fd);
+    int32_t ack = get_int_socket(fd);
     if (ack < 0) get_error_msg(fd, ack);
     ROS_INFO("success: attached variable");
     chan.vars.push_back(var);
@@ -128,7 +150,7 @@ void send_channel_decl_msg(int fd, bool is_input, std::string name) {
     // prepare packet
     size_t msg_length = 2 + name.length();
     std::unique_ptr<byte[]> msg = std::make_unique<byte[]>(msg_length);
-    msg[0] = is_input ? 1 : 2;
+    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];
 
@@ -138,7 +160,7 @@ void send_channel_decl_msg(int fd, bool is_input, std::string name) {
     write(fd, (void*) msg.get(), msg_length);
 
     // get answer from TRON
-    int channel_identifier = get_int_socket(fd);
+    int32_t channel_identifier = get_int_socket(fd);
     if (channel_identifier < 0) { // error handling
         get_error_msg(fd, channel_identifier);
     }
@@ -148,9 +170,9 @@ void send_channel_decl_msg(int fd, bool is_input, std::string name) {
     channels[name] = Channel(channel_identifier, is_input);
 }
 
-void set_time_unit_and_timeout(int fd, unsigned long int microseconds, int timeout){
+void set_time_unit_and_timeout(int fd, unsigned long long microseconds, int32_t timeout){
     std::unique_ptr<byte[]> msg = std::make_unique<byte[]>(9);
-    msg[0] = 5;
+    msg[0] = SET_TIME_UNIT;
     byte *microseconds_bytes = reinterpret_cast<byte*>(&microseconds);
 
     // htonl does not exist for long int
@@ -162,12 +184,12 @@ void set_time_unit_and_timeout(int fd, unsigned long int microseconds, int timeo
     }
     ROS_INFO("setting time unit: %i microseconds", microseconds);
     send_bytes(fd, msg.get(), 9);
-    int ack = get_int_socket(fd);
+    int32_t ack = get_int_socket(fd);
     if (ack != 0) get_error_msg(fd, ack);
     ROS_INFO("success: set time unit");
 
     msg.reset(new byte[5]);
-    msg[0] = 6;
+    msg[0] = SET_TIMEOUT;
     add_int32_in_network_order(timeout, msg.get(), 1);
     ROS_INFO("setting timeout to %i units", timeout);
     send_bytes(fd, msg.get(), 5);
@@ -178,15 +200,15 @@ void set_time_unit_and_timeout(int fd, unsigned long int microseconds, int timeo
 
 void request_start(int fd) {
     ROS_INFO("requesting start");
-    byte start = 64;
+    byte start = REQUEST_START;
     send_bytes(fd, &start, 1);
     byte answer = get_bytes_socket(fd, 1)[0];
-    if (answer != 0) throw "starting failed";
+    if (answer != ANSWER_START) throw "starting failed";
     ROS_INFO("success: starting test phase");
 }
 
 // nach input verarbeitung weiter machen
-void report_now(int fd, std::string chan_name, unsigned short var_count, int *vars){
+void report_now(int fd, std::string chan_name, uint16_t var_count, int32_t *vars){
     std::unique_ptr<byte[]> msg = std::make_unique<byte[]>(6 + 4 * var_count);
     if (channels.find(chan_name) == channels.end()) throw "channel not declared";
     Channel chan = channels.at(chan_name);
@@ -237,13 +259,13 @@ int main(int argc, char**argv){
     ROS_INFO("successfully connected to TRON");
     try {   
         send_channel_decl_msg(socketfd, true, "ausloesen");
-        //add_var_to_channel(socketfd, "ausloesen", true, "global");
+        add_var_to_channel(socketfd, "ausloesen", true, "zahl");
         send_channel_decl_msg(socketfd, false, "position");
         add_var_to_channel(socketfd, "position", false, "zahl");
         // nicht in dokumentation: lokale variablen nicht möglich
         //add_var_to_channel(socketfd, "ausloesen", "lokal");
 
-        unsigned long int microseconds = 1000000; // one second
+        unsigned long long microseconds = 1000000; // one second
         // Dokumentation arbeitet mit 2 unsigned ints, äußerst mystisch
         set_time_unit_and_timeout(socketfd, microseconds, 10);
 
@@ -252,13 +274,60 @@ int main(int argc, char**argv){
         // 127 ist gerErrorMessage
         request_start(socketfd); // communication not synchronous anymore after start
 
+        // listen to topics
+        int acks_needed = 0;
+        ros::Rate rate(1);
+        while (ros::ok()) {
+            while (true){ // checking for messages arrived
+                // get 4 bytes at a time
+                std::unique_ptr<byte[]> bytes = std::make_unique<byte[]>(4);
+                int bytes_recv = recv(socketfd, bytes.get(), 4, MSG_DONTWAIT);
+                if (bytes_recv == -1) break; // nothing more to read
+                if (bytes_recv == 0) throw "connection was closed";
+                if (bytes_recv != 4) throw "could not read full ack or chanID";
+                byte_info(bytes.get(), 4, false);
+                int32_t next = bytes_to_int_32(bytes.get());
+                // if those 4 bytes are acknowledgement decrement amount of acks needed;
+                if (next == ACK_SINGLE) {
+                    if (--acks_needed < 0) throw "too many acknowledgements";
+                    ROS_INFO("got acknowledgement for output");
+                    continue;
+                }
+                std::string chan_name;
+                for (std::pair<const std::string, Channel>& pair : channels) {
+                    if (pair.second.identifier == next) chan_name = pair.first;
+                }
+                if (chan_name.empty()) throw "channel could not be identified";
+                ROS_INFO("got channel identifier (%s) for input", chan_name.c_str());
+                // channel identified, assuming following bytes are correct
+                bytes = std::make_unique<byte[]>(2);
+                recv(socketfd, bytes.get(), 2, MSG_DONTWAIT);
+                byte_info(bytes.get(), 2, false);
+                uint16_t var_count = bytes_to_uint_16(bytes.get());
+                ROS_INFO("got variable count %i", var_count);
+                for (uint16_t i = 0; i < var_count; i++) {
+                    bytes = std::make_unique<byte[]>(4);
+                    recv(socketfd, bytes.get(), 4, MSG_DONTWAIT);
+                    next = bytes_to_int_32(bytes.get());
+                    Channel& c = channels.at(chan_name);
+                    std::string var = channels.at(chan_name).vars[i];
+                    ROS_INFO("got variable number %i:  value of %s is %i", i+1, channels.at(chan_name).vars[i].c_str(), next);
+                    // TODO transfer message to topic
+                }
+                bytes = std::make_unique<byte[]>(4);
+                add_int32_in_network_order(ACK_SINGLE, bytes.get(), 0);
+                ROS_INFO("sending acknowledgement");
+                send_bytes(socketfd, bytes.get(), 4);
+            }
+            ros::spinOnce();
+            rate.sleep();
+        }
+
         // erst nach ausloesen input wäre akzeptierbar
-        int zahl = 4;
+        int32_t zahl = 4;
         //report_now(socketfd, "position", 1, &zahl);
     } catch (const char* err){
         ROS_FATAL("shutting down: %s", err);
         ros::shutdown();
     }
-
-    ros::Duration(100).sleep();
 }
\ No newline at end of file