diff --git a/main.cpp b/main.cpp index 3ed6b9948e87a42dd6439f11687b67ae2bf1ee46..a6243965a90e75616bfe24f9b5b98a47a0e01caf 100644 --- a/main.cpp +++ b/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*>(µseconds); // 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