temporär adapter node hinzugefügt

main.cpp

+#include <ros/ros.h>
+#include <ros/console.h>
+#include <stdio.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+
+typedef unsigned char byte;
+// function declarations --------------------------------------------------------
+
+// gets count bytes from socket file descriptor
+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);
+
+// retrieves error message and prints it, then throws
+void get_error_msg(int fd, int 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);
+
+// 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);
+
+// 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);
+
+// throws if not answered with byte 0
+void request_start(int fd);
+// -------------------------------------------------------------------------------
+
+const double SECONDS_BEFORE_TIMEOUT = 30;
+struct Channel {
+    int 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() = default; // default constructor needed for std::map
+};
+std::map<std::string, Channel> channels;
+
+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
+};
+
+int 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
+}
+
+void add_int32_in_network_order(int num, byte *msg, int index){
+    unsigned int n = htonl(*reinterpret_cast<unsigned int*>(&num));
+    byte* bytes = reinterpret_cast<byte*>(&n);
+    msg[index] = bytes[0];
+    msg[++index] = bytes[1];
+    msg[++index] = bytes[2];
+    msg[++index] = bytes[3];
+}
+
+void byte_info(const byte* msg, int msg_length, bool send){
+    std::stringstream strstr;
+    strstr << (send ? "sending" : "received") << " bytes:"; 
+    for (int i = 0; i < msg_length; i++) strstr << " " << (int)msg[i];
+    ROS_INFO(strstr.str().c_str());
+}
+
+void send_bytes(int fd, const byte* bytes, int length){
+    byte_info(bytes, length);
+    int ret = write(fd, (void*) bytes, length);
+    if (ret < 0) throw "writing failed";
+}
+
+void get_error_msg(int fd, int 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
+    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];
+    auto err_msg = get_bytes_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(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;
+    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);
+    if (ack < 0) get_error_msg(fd, ack);
+    ROS_INFO("success: attached variable");
+    chan.vars.push_back(var);
+}
+
+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[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.get(), msg_length);
+    write(fd, (void*) msg.get(), msg_length);
+
+    // get answer from TRON
+    int channel_identifier = get_int_socket(fd);
+    if (channel_identifier < 0) { // error handling
+        get_error_msg(fd, 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);
+    channels[name] = Channel(channel_identifier, is_input);
+}
+
+void set_time_unit_and_timeout(int fd, unsigned long int microseconds, int timeout){
+    std::unique_ptr<byte[]> msg = std::make_unique<byte[]>(9);
+    msg[0] = 5;
+    byte *microseconds_bytes = reinterpret_cast<byte*>(&microseconds);
+
+    // htonl does not exist for long int
+    const bool IS_BIG_ENDIAN = htonl(47) == 47;
+    if (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: %i microseconds", microseconds);
+    send_bytes(fd, msg.get(), 9);
+    int 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;
+    add_int32_in_network_order(timeout, msg.get(), 1);
+    ROS_INFO("setting timeout to %i units", timeout);
+    send_bytes(fd, msg.get(), 5);
+    ack = get_int_socket(fd);
+    if (ack != 0) get_error_msg(fd, ack);
+    ROS_INFO("success: set timeout");
+}
+
+void request_start(int fd) {
+    ROS_INFO("requesting start");
+    byte start = 64;
+    send_bytes(fd, &start, 1);
+    byte answer = get_bytes_socket(fd, 1)[0];
+    if (answer != 0) 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){
+    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);
+
+    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);
+
+    send_bytes(fd, msg.get(), 6 + 4 * var_count);
+}
+
+int main(int argc, char**argv){
+    ros::init(argc, argv, "TRON dapter");
+    ros::NodeHandle nh;
+
+    const std::string IP = "127.0.0.1";
+    const short PORT = 8080;
+
+    int socketfd;
+    if ((socketfd = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
+        ROS_FATAL("socket could not be created");
+        ros::shutdown();
+    }
+    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) {
+            ROS_INFO("valid internet address");
+        } else {
+            ROS_FATAL("internet address could not be converted");
+            ros::shutdown();
+        }
+    }
+    if (connect(socketfd, (struct sockaddr*)&addr, sizeof(sockaddr_in)) < 0) {
+        ROS_FATAL("connection failed");
+        ros::shutdown();
+    }
+    ROS_INFO("successfully connected to TRON");
+    try {   
+        send_channel_decl_msg(socketfd, true, "ausloesen");
+        //add_var_to_channel(socketfd, "ausloesen", true, "global");
+        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
+        // Dokumentation arbeitet mit 2 unsigned ints, äußerst mystisch
+        set_time_unit_and_timeout(socketfd, microseconds, 10);
+
+        // FALSCH IN DOKUMENTATION: Start und error msg byte codes vertauscht
+        // 64 ist start
+        // 127 ist gerErrorMessage
+        request_start(socketfd); // communication not synchronous anymore after start
+
+        // erst nach ausloesen input wäre akzeptierbar
+        int 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