diff --git a/garage.cpp b/garage.cpp new file mode 100644 index 0000000000000000000000000000000000000000..fb2a33765d7e81b4bf87f635b71ac559595c1f75 --- /dev/null +++ b/garage.cpp @@ -0,0 +1,20 @@ +#include <ros/ros.h> +#include <std_msgs/Int32.h> +#include <std_msgs/String.h> + +void callback(std_msgs::Int32ConstPtr& ptr) { + +} + +int main(int argc, char**argv){ + ros::init(argc, argv, "garage"); + ros::NodeHandle nh; + ros::Publisher pub = nh.advertise<std_msgs::Int32>("position", 1); + if (!pub) throw "publisher failed"; + std_msgs::Int32 x; + x.data = 6; + while(pub.getNumSubscribers() == 0) {}; + ros::Duration(3).sleep(); + pub.publish(x); + ros::Duration(5).sleep(); +} \ No newline at end of file diff --git a/main.cpp b/main.cpp index a1c74095b8be7d2601f224fd4ee1b8196e338962..df0d42d3b34f0fd21ed4610236e8206f90fc3372 100644 --- a/main.cpp +++ b/main.cpp @@ -38,19 +38,19 @@ std::unique_ptr<byte[]> get_bytes_socket(int fd, int count){ return arr; // no explicit move needed since return value is rvalue }; -inline int32_t bytes_to_int_32(byte *buf){ +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 bytes_to_uint_16(byte *buf) { +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 bytes_to_int_32(ack.get()); + return network_bytes_to_int_32(ack.get()); } // converts num to network order and adds it to byte array starting from index @@ -81,6 +81,30 @@ inline bool read_4_bytes_nonblock(int fd, byte *buf) { 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; @@ -96,24 +120,70 @@ const int32_t ACK_SINGLE = 1 << 31; // 32 Bit int with most significant bit set // global vars ------------------------------------------------------------- +// ROS uses little endian for its messages +const bool SYS_IS_BIG_ENDIAN = htonl(47) == 47; + struct Channel { + std::string name; int32_t identifier; bool is_input; std::vector<std::string> vars; // associated variables in Uppaal - Channel(int32_t id, bool is_input) : identifier(id), is_input(is_input){}; - Channel() = default; // default constructor needed for std::map + Channel(std::string name, int32_t id, bool is_input) : name(name), identifier(id), is_input(is_input){}; }; -std::map<std::string, Channel> channels; +std::vector<Channel> channels; + +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; + + Channel *chan; // 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 = pointer to 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. + */ +}; +std::vector<Mapping> mappings; int socket_fd; // 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 */ int acks_missing = 0; // tron specific functions ----------------------------------------------------- +Channel* get_chan(std::string chan_name) { + Channel *chan = nullptr; + for (Channel& cur_chan : channels) if (cur_chan.name == chan_name) chan = &cur_chan; + if (chan == nullptr) throw "channel not declared"; + return chan; +} + void get_error_msg(int32_t errorcode) { ROS_WARN("got error, trying to get corresponding message"); byte get_err_msg_msg[5]; @@ -129,10 +199,9 @@ void get_error_msg(int32_t errorcode) { void add_var_to_channel(std::string channel, bool is_input, std::string var) { byte msg[6 + var.length()]; - if (channels.find(channel) == channels.end()) throw "channel not declared"; - Channel& chan = channels.at(channel); + Channel *chan = get_chan(channel); msg[0] = is_input ? ADD_VAR_TO_INPUT : ADD_VAR_TO_OUTPUT; - add_int32_in_network_order(chan.identifier, msg, 1); + 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(), channel.c_str()); @@ -140,7 +209,7 @@ void add_var_to_channel(std::string channel, bool is_input, std::string var) { 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); + chan->vars.push_back(var); } void send_channel_decl_msg(bool is_input, std::string name) { @@ -164,7 +233,7 @@ void send_channel_decl_msg(bool is_input, std::string name) { 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); + channels.push_back(Channel(name, channel_identifier, is_input)); } void set_time_unit_and_timeout(uint64_t microseconds, int32_t timeout){ @@ -173,8 +242,7 @@ void set_time_unit_and_timeout(uint64_t microseconds, int32_t timeout){ byte *microseconds_bytes = reinterpret_cast<byte*>(µseconds); // htonl does not exist for long int - const bool IS_BIG_ENDIAN = htonl(47) == 47; - if (IS_BIG_ENDIAN) { + 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]; @@ -208,10 +276,11 @@ void request_start() { void report_now(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); + Channel *chan = nullptr; + for (Channel& cur_chan : channels) if (cur_chan.name == chan_name) chan = &cur_chan; + if (chan == nullptr) throw "channel not declared"; - add_int32_in_network_order(chan.identifier, msg.get(), 0); + 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); @@ -224,33 +293,35 @@ void report_now(std::string chan_name, uint16_t var_count, int32_t *vars){ 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()); + 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++; } -// 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"); +void init_topic_channel_mapping(const std::string topic, const std::string channel){ + Mapping map; + map.topic = topic; + map.chan = get_chan(channel); + mappings.push_back(map); +} - 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"; +void add_var_to_mapping(int mapping_index, int byte_offset, + int32_t (*conv)(byte*, int*) = nullptr) { + Mapping& map = mappings[mapping_index]; + map.byte_offset.push_back(byte_offset); + map.converters.push_back(conv); +} + +// this function is a little cumbersome to use +// recommended to use init_channel_topic_mapping and add_var_to_mapping instead +void create_channel_topic_mapping_with_vars(const std::string topic, + const std::string channel, + std::vector<int> byte_offset, + std::vector<int32_t (*)(byte*, int*)> conv){ + init_topic_channel_mapping(topic, channel); + for (int i = 0; i < byte_offset.size(); i++) { + add_var_to_mapping(mappings.size(), byte_offset[i], conv[i]); } - ROS_INFO("successfully connected"); - return socketfd; } void configuration_phase(){ @@ -258,8 +329,11 @@ void configuration_phase(){ therefore heap allocation can be avoided most of the time in called functions */ send_channel_decl_msg(true, "ausloesen"); add_var_to_channel("ausloesen", true, "zahl"); + send_channel_decl_msg(false, "position"); + init_topic_channel_mapping("/position", "position"); add_var_to_channel("position", false, "zahl"); + add_var_to_mapping(0, 0); // not obvious in documentation: local variables are not supported //add_var_to_channel(socketfd, "ausloesen", "lokal"); uint64_t microseconds = 1000000; // one second @@ -267,7 +341,6 @@ void configuration_phase(){ set_time_unit_and_timeout(microseconds, 100); } -// TODO implement callbacks/topic messages void process_TRONs_msgs(){ /* note: TRONs communication after start is not guaranteed to be synchronous, thus incoming messages must be checked for their content */ @@ -276,7 +349,7 @@ void process_TRONs_msgs(){ byte bytes[4]; if (!read_4_bytes_nonblock(socket_fd, bytes)) break; // no bytes left to process - int32_t next = bytes_to_int_32(bytes); + int32_t next = network_bytes_to_int_32(bytes); // bytes are acknowledgement if (next == ACK_SINGLE) { @@ -287,33 +360,29 @@ void process_TRONs_msgs(){ // bytes are channel identifier // find corresponding channel - 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()) - /* note: this only happens if message number 12 in TRON User Manual is received, + Channel *chan = nullptr; + for (Channel& cur_chan : channels) if (cur_chan.identifier == next) chan = &cur_chan; + /* 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()); + + 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 = bytes_to_uint_16(bytes); + uint16_t var_count = network_bytes_to_uint_16(bytes); ROS_INFO("got variable count %i", var_count); // process variables for (uint16_t i = 0; i < var_count; i++) { recv(socket_fd, bytes, 4, MSG_DONTWAIT); - next = bytes_to_int_32(bytes); - 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 possibly via custom callback + 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); } // send acknowledgement @@ -323,16 +392,40 @@ void process_TRONs_msgs(){ } } -void testCallback(const std_msgs::Int32::ConstPtr& msg){ - int32_t x = msg->data; - report_now("position", 1, &x); +// callback reports to TRON like defined in mappings ----------------------------- +template<class T> +void mapCallback(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) { + int var_count = mapping.byte_offset.size(); + int32_t vars[var_count * 4]; + int last_pos = 0; + for (int i = 0; i < var_count; i++) { + last_pos += mapping.byte_offset[i]; + if (mapping.converters[i] == nullptr) { + if (SYS_IS_BIG_ENDIAN) + vars[i] = network_bytes_to_int_32(&bytes[last_pos]); + else vars[i] = *reinterpret_cast<int32_t*>(&bytes[last_pos]); + last_pos += 4; + continue; + } else { + vars[i] = mapping.converters[i](&bytes[last_pos], &last_pos); + } + } + report_now(mapping.chan->name, var_count, vars); + } + } } +// place custom callbacks here (using report_now) + +// ---------------------------------------------------------------- + int main(int argc, char**argv){ ros::init(argc, argv, "TRON dapter"); ros::NodeHandle nh; - ros::Publisher pub = nh.advertise<std_msgs::Int32>("test_topic", 1); - if (!pub) throw "publisher nicht erstellt"; try { const std::string IP = "127.0.0.1"; @@ -341,20 +434,15 @@ int main(int argc, char**argv){ configuration_phase(); - // subscribe to topics and add callbacks which use report_now------------------------------------------- - ros::Subscriber sub = nh.subscribe("test_topic", 1, testCallback); + // subscribe to topics and add callbacks which use report_now + ros::Subscriber sub = nh.subscribe("position", 1, mapCallback<std_msgs::Int32>); // ----------------------------------------------------------- request_start(); // testing phase loop ros::Rate test_phase_freq(1); - int y = 2; while (ros::ok()) { - std_msgs::Int32 x; - x.data = 6; - if (y==2) pub.publish(x); - y++; // TRON does not seem to send acknowledgements back process_TRONs_msgs(); ros::spinOnce(); test_phase_freq.sleep();