No commit message

No commit message

main.cpp

@@ -135,8 +135,10 @@ struct Channel {
     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){};
 };
-std::vector<Channel> channels;
 
+// 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
 
@@ -154,13 +156,13 @@ struct Mapping {
     // a pointer to an integer to increment by the amount of bytes added
     std::vector<void (*)(int32_t, byte*, int*)> converters_to_topics;
 
-    int channel_index; // TRON channel
+    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 = pointer to struct Channel "example channel"
+        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 
@@ -173,14 +175,58 @@ struct Mapping {
     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;
 
+    // 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;
-// custom callbacks for TRONs messages taking channel name, pointer to values and publisher to publish to
-// key is pair of topic name and channel name in this order
-// not using custom fallbacks leads to mapping_callback_to_topics being used
-std::map<std::pair<std::string, std::string>, void(*)(std::string, int32_t*, ros::Publisher&)> input_callbacks;
+// 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);
+        }
+    }
+    for (ros::Publisher& pub : input_publishers)
+        if (pub.getTopic() == map.topic) {
+            pub.publish(shared_ptr);
+            return;
+        }
+    throw "did not find publisher for topic";
+}
+
 // subscribers already have callbacks
 std::vector<ros::Subscriber> output_subscribers;
 
@@ -190,17 +236,6 @@ 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; 
 
-// specific functions -----------------------------------------------------
-
-int get_channel_index(std::string chan_name) {
-    int i = 0;
-    for (Channel& cur_chan : channels) {
-        if (cur_chan.name == chan_name) return i;
-        i++;
-    }
-    throw "channel not declared";
-}
-
 void get_error_msg(int32_t errorcode) {
     ROS_WARN("got error, trying to get corresponding message");
     byte get_err_msg_msg[5];
@@ -214,14 +249,13 @@ void get_error_msg(int32_t errorcode) {
     throw "got error from TRON";
 }
 
-void add_var_to_channel(std::string channel, bool is_input, std::string var) {
+void add_var_to_channel(Channel& chan, bool is_input, std::string var) {
     byte msg[6 + var.length()];
-    Channel& chan = channels[get_channel_index(channel)];
     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(), channel.c_str());
+    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);
@@ -229,7 +263,7 @@ void add_var_to_channel(std::string channel, bool is_input, std::string var) {
     chan.vars.push_back(var);
 }
 
-void send_channel_decl_msg(bool is_input, std::string name) {
+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];
@@ -250,7 +284,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.push_back(Channel(name, channel_identifier, is_input));
+    return std::make_unique<Channel>(name, channel_identifier, is_input);
 }
 
 void set_time_unit_and_timeout(uint64_t microseconds, int32_t timeout){
@@ -291,13 +325,10 @@ void request_start() {
     ROS_INFO("success: starting test phase");
 }
 
-void report_now(std::string chan_name, uint16_t var_count, int32_t *vars){
+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);
-    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);
@@ -307,41 +338,17 @@ void report_now(std::string chan_name, uint16_t var_count, int32_t *vars){
     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());
+    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++;
 }
 
-void init_topic_channel_mapping(const std::string topic, const std::string channel){
-    Mapping map;
-    map.topic = topic;
-    map.channel_index = get_channel_index(channel);
-    mappings.push_back(map);
-}
-
-void add_var_to_mapping(std::string topic, std::string channel, int byte_offset, 
-                         int32_t (*conv_to_TRON)(byte*, int*) = nullptr,
-                         void (*conv_to_topic)(int32_t, byte*, int*) = nullptr) {
-    int index = -1;
-    for (int i = 0; i < mappings.size(); i++) {
-        Mapping& map = mappings[i];
-        if (channels[map.channel_index].name == channel && map.topic == topic) index = i;
-    }
-    if (index < 0) throw "did not find mapping";
-
-    Mapping& map = mappings[index];
-    map.byte_offset.push_back(byte_offset);
-    map.converters_to_TRON.push_back(conv_to_TRON);
-    map.converters_to_topics.push_back(conv_to_topic);
-}
-
-
 // callback reports to TRON like defined in mappings
 template<class T>
-void mapping_callback_to_TRON(const ros::MessageEvent<T>& event){
+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) {
@@ -361,38 +368,7 @@ void mapping_callback_to_TRON(const ros::MessageEvent<T>& event){
                     vars[i] = mapping.converters_to_TRON[i](&bytes[next_pos], &next_pos);
                 }
             }
-            report_now(channels[mapping.channel_index].name, var_count, vars);
-        }
-    }
-}
-
-// callback publishing to topics like defined in mappings
-template<class T>
-void mapping_callback_to_topics(std::string channel, int32_t* vars, ros::Publisher& pub){
-    for (Mapping& mapping : mappings) {
-        if (channels[mapping.channel_index].name == channel) {
-            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 < channels[mapping.channel_index].vars.size(); i++){
-                int32_t val = vars[i];
-                next_pos += mapping.byte_offset[i];
-                if (mapping.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 {
-                    mapping.converters_to_topics[i](val, &ptr[next_pos], &next_pos);
-                }
-            }
-            pub.publish(shared_ptr);
+            report_now(mapping.channel, var_count, vars);
         }
     }
 }
@@ -406,21 +382,18 @@ void configuration_phase(ros::NodeHandle& nh){
     //            add callback for mapping (key is pair of topic and channel name)
     // for output: add subscribers to output_subscribers
     
-    send_channel_decl_msg(true, "ausloesen");
-    init_topic_channel_mapping("/command", "ausloesen");
-    add_var_to_channel("ausloesen", true, "zahl");
-    add_var_to_mapping("/command", "ausloesen", 0);
+    Mapping map = Mapping("/command", "ausloesen", true);
+    map.add_var_to_mapping("zahl", 0);
+    map.input_callback = mapping_callback_to_topic<std_msgs::Int32>;
+    mappings.push_back(map);
     input_publishers.push_back(nh.advertise<std_msgs::Int32>("/command", 1));
-    input_callbacks[std::pair<std::string, std::string>("/command", "ausloesen")] 
-        = mapping_callback_to_topics<std_msgs::Int32>;
     // output channels: declare, init mapping, add vars to TRON channel and mapping
     // add subscriber to list
-    send_channel_decl_msg(false, "position");
-    init_topic_channel_mapping("/position", "position");
-    add_var_to_channel("position", false, "zahl");
-    add_var_to_mapping("/position", "position", 0);
+    map = Mapping("/position", "position", false);
+    map.add_var_to_mapping("zahl", 0);
+    mappings.push_back(map);
     output_subscribers.push_back(nh.subscribe("/position", 1, 
-            mapping_callback_to_TRON<std_msgs::Int32>));
+            mappings_callback_to_TRON<std_msgs::Int32>));
     // not obvious in documentation: local variables are not supported
     // add_var_to_channel(socketfd, "ausloesen", "lokal");
     uint64_t microseconds = 1000000; // one second
@@ -453,7 +426,7 @@ void process_TRONs_msgs(){
         // bytes are channel identifier
         // find corresponding channel
         const Channel *chan = nullptr;
-        for (Channel& cur_chan : channels) if (cur_chan.identifier == next) chan = &cur_chan;
+        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) 
@@ -480,15 +453,12 @@ void process_TRONs_msgs(){
         }
 
         for (Mapping& map : mappings)
-            if (channels[map.channel_index].name == chan->name)
+            if (map.channel.name == chan->name)
                 for (ros::Publisher& pub : input_publishers){
                     if (pub.getTopic() == map.topic) {
-                        if (input_callbacks.find(
-                            std::pair<std::string, std::string>(map.topic, channels[map.channel_index].name))
-                            != input_callbacks.end())
-                            input_callbacks.at(std::pair<std::string, std::string>(map.topic, channels[map.channel_index].name))
-                            (chan->name, vals, pub);  
-                        else throw "did not find callback";  
+                        if (map.input_callback != nullptr)
+                            map.input_callback(map, vals);  
+                        else throw "no callback declared";  
                         break;
                     }
                 }