From ccc465d6a490ce06af6b869d581e67c6423c1afd Mon Sep 17 00:00:00 2001
From: CS <christoph.schroeter1@mailbox.tu-dresden.de>
Date: Fri, 13 Aug 2021 01:32:50 +0200
Subject: [PATCH]

---
 include/tron_adapter.h         | 144 ++++++++++++++++++++++
 scripts/analyze_test_output.sh |  59 +++++++++
 scripts/tron_testing_auto.sh   |   6 +-
 src/tron_adapter.cpp           | 214 +++++++++++++--------------------
 4 files changed, 288 insertions(+), 135 deletions(-)
 create mode 100644 include/tron_adapter.h
 create mode 100644 scripts/analyze_test_output.sh

diff --git a/include/tron_adapter.h b/include/tron_adapter.h
new file mode 100644
index 0000000..41bed64
--- /dev/null
+++ b/include/tron_adapter.h
@@ -0,0 +1,144 @@
+#ifndef TRON_ADAPTER
+#define TRON_ADAPTER
+
+#include <ros/ros.h>
+
+typedef uint8_t byte;
+
+// some networking helper functions ---------------------------------------------------
+
+// logs bytes via ROS_INFO when sending or receiving them
+inline void byte_info(const byte* buf, int buf_length, bool send=true);
+
+// reads count bytes from filedescripter fd
+std::unique_ptr<byte[]> get_bytes_socket(int fd, int count);
+
+// converts bytes to host order and casts to int32
+inline int32_t network_bytes_to_int_32(byte *buf);
+
+// converts bytes to host order and casts to uint16
+inline uint16_t network_bytes_to_uint_16(byte *buf);
+
+// reads 4 bytes from socket and returns them as an int (converted to host order before)
+int32_t get_int_socket(int fd);
+
+// converts num to network order and sets the bytes with byte-offset index from buf
+void add_int32_in_network_order(int32_t num, byte *buf, int index);
+
+// wraps system byte sending for automated logging
+inline void send_bytes(int fd, const byte *buf, int length);
+
+// reads 4 bytes into buf from filedescriptor
+inline bool read_4_bytes_nonblock(int fd, byte *buf);
+
+int create_connected_socket(std::string IP, uint16_t port);
+
+// ----------------------------------------------------------------------------------------
+// TRON message codes
+extern const byte GET_ERROR_MSG;
+extern const byte DECL_CHAN_INPUT;
+extern const byte DECL_CHAN_OUTPUT;
+extern const byte ADD_VAR_TO_INPUT;
+extern const byte ADD_VAR_TO_OUTPUT;
+extern const byte SET_TIME_UNIT;
+extern const byte SET_TIMEOUT;
+extern const byte REQUEST_START;
+extern const byte ANSWER_START;
+extern const int32_t ACK_SINGLE;
+
+// Socket kept global
+extern int socket_fd;
+
+// represents a channel in Uppaal
+struct Channel {
+    std::string name;
+    int32_t identifier;
+    bool is_input;
+    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){};
+};
+
+// represents a Mapping of one topic and one channel
+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_to_TRON;
+    // similarly the conversion to a topic field takes the value from TRON,
+    // a pointer to the buffer at which to add the bytes and
+    // a pointer to an integer to increment by the amount of bytes added
+    std::vector<void (*)(int32_t, byte*, int*)> converters_to_topics;
+
+    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 = 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.
+    */
+
+    // Callback used if TRON sends some message
+    void(*input_callback)(Mapping& map, int32_t*) = nullptr;
+
+    Mapping(std::string topic, std::string channel, bool channelIsInput);
+
+    // adds variable to channel and sets position and conversion parameters
+    void add_var_to_mapping(std::string name_tron, int byte_offset = 0, 
+                         int32_t (*conv_to_TRON)(byte*, int*) = nullptr,
+                         void (*conv_to_topic)(int32_t, byte*, int*) = nullptr);
+};
+
+// mappings, publishers and subscribers kept global
+extern std::list<Mapping> mappings;
+extern std::vector<ros::Publisher> input_publishers;
+extern std::vector<ros::Subscriber> output_subscribers;
+
+// publishes to fitting publisher from list
+template <class T>
+void publish_to_topic(std::string topic, boost::shared_ptr<T> ptr);
+
+template<class T>
+void mapping_callback_to_topic(Mapping& map, int32_t* vars);
+
+void get_error_msg(int32_t errorcode);
+
+void add_var_to_channel(Channel& chan, bool is_input, std::string var);
+
+std::unique_ptr<Channel> send_channel_decl_msg(bool is_input, std::string name);
+
+void set_time_unit_and_timeout(uint64_t microseconds, int32_t timeout);
+
+void request_start();
+
+void report_now(Channel& chan, uint16_t var_count = 0, int32_t *vars = nullptr);
+
+void report_now(std::string chan, uint16_t var_count = 0, int32_t *vars = nullptr);
+
+template<class T>
+void mappings_callback_to_TRON(const ros::MessageEvent<T>& event);
+
+void process_TRONs_msgs();
+
+void configuration_phase(ros::NodeHandle& nh);
+
+#endif //TRON_ADAPTER
+
diff --git a/scripts/analyze_test_output.sh b/scripts/analyze_test_output.sh
new file mode 100644
index 0000000..28d4d02
--- /dev/null
+++ b/scripts/analyze_test_output.sh
@@ -0,0 +1,59 @@
+#!/bin/bash
+
+fn="test_output"
+max_test_num=100
+found_succ=0
+found_shut_down=0
+found_fail=0
+success_nums=()
+shut_down_nums=()
+fail_nums=()
+
+for ((i=1;i<=$max_test_num;i++)); do
+	found=false
+	echo "reading file $i"
+	while read line; do
+	
+		if [[ $line == *"Cobot.finished_success"* ]]; then
+		  found=true
+		  ((found_succ++))
+		  success_nums+=($i)
+		  break
+		fi
+		
+		if [[ $line == *"Cobot.shut_down"* ]]; then
+		  found=true
+		  ((found_shut_down++))
+		  shut_down_nums+=($i)
+		  break
+		fi
+		
+	done < "$fn$i"
+	
+	if ! $found; then
+		((found_fail++))
+		fail_nums+=($i)
+	fi
+done
+
+echo "success: $found_succ"
+nums_str=""
+for i in ${success_nums[@]}; do
+nums_str="$nums_str $i"
+done
+echo "success_nums:$nums_str"
+
+echo "shut_down: $found_shut_down"
+nums_str=""
+for i in ${shut_down_nums[@]}; do
+nums_str="$nums_str $i"
+done
+echo "shut_down_nums:$nums_str"
+
+nums_str=""
+echo "fails: $found_fail"
+for i in ${fail_nums[@]}; do
+nums_str="$nums_str $i"
+done
+echo "fail_nums:$nums_str"
+	
diff --git a/scripts/tron_testing_auto.sh b/scripts/tron_testing_auto.sh
index 84ca1d1..7237f31 100755
--- a/scripts/tron_testing_auto.sh
+++ b/scripts/tron_testing_auto.sh
@@ -1,7 +1,7 @@
 #!/bin/bash
 
 start_dir=$PWD
-test_count=50
+test_count=100
 ros_workspace_location="/home/cs/Schreibtisch/panda_gazebo_workspace"
 package_name="cobot_1"
 roslaunch_filename="cobot1_mock_test.launch"
@@ -15,7 +15,7 @@ source devel/setup.bash
 
 tron_args=( # do not set -o here
     "-I SocketAdapter" 
-    "-v 26"
+    "-v 10"
     "-q"
     $model_location # needs to be specified before port
     "-- 8080"
@@ -31,7 +31,7 @@ done
 echo "args given to TRON: "
 echo $tron_args_str
 
-for ((i=29;i<=test_count;i++)); do 
+for ((i=1;i<=test_count;i++)); do 
     tron_args_str_output="-o $start_dir/test_output$i $tron_args_str"
     $tron_location $tron_args_str_output & tron_pid=$!
     roslaunch $package_name $roslaunch_filename & roslaunch_pid=$!
diff --git a/src/tron_adapter.cpp b/src/tron_adapter.cpp
index ec1eb0b..434d027 100644
--- a/src/tron_adapter.cpp
+++ b/src/tron_adapter.cpp
@@ -19,6 +19,7 @@
 #include <tf2/LinearMath/Vector3.h>
 #include <algorithm> // for std::find
 #include <cmath>
+#include "tron_adapter.h"
 
 const std::string IP = "127.0.0.1";
 const uint16_t PORT = 8080;
@@ -28,7 +29,7 @@ typedef uint8_t byte;
 // some helper functions -----------------------------------------------
 
 // log bytes sent/received
-inline void byte_info(const byte* buf, int buf_length, bool send=true){
+inline void byte_info(const byte* buf, int buf_length, bool send){
     std::stringstream strstr;
     strstr << (send ? "sending" : "received") << " bytes:"; 
     for (int i = 0; i < buf_length; i++) strstr << " " << (int)buf[i];
@@ -124,7 +125,6 @@ int create_connected_socket(std::string IP, uint16_t port){
 }
 
 // consts for TRON ----------------------------------------------------------
-
 const byte GET_ERROR_MSG = 127;
 const byte DECL_CHAN_INPUT = 1;
 const byte DECL_CHAN_OUTPUT = 2;
@@ -136,79 +136,29 @@ const byte REQUEST_START = 64;
 const byte ANSWER_START = 0;
 const int32_t ACK_SINGLE = 1 << 31; // 32 Bit int with most significant bit set to 1
 
-// global vars -------------------------------------------------------------
-
+// Socket kept global
+int socket_fd;
 // ROS uses little endian for its messages
 const bool SYS_IS_BIG_ENDIAN = htonl(47) == 47;
-int socket_fd;
-struct Channel {
-    std::string name;
-    int32_t identifier;
-    bool is_input;
-    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){};
-};
 
-// 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
-
-    // 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_to_TRON;
-    // similarly the conversion to a topic field takes the value from TRON,
-    // a pointer to the buffer at which to add the bytes and
-    // a pointer to an integer to increment by the amount of bytes added
-    std::vector<void (*)(int32_t, byte*, int*)> converters_to_topics;
-
-    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 = 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.
-    */
-
-    // 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();
-    }
+Mapping::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 = 0, 
-                         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;
 
+void Mapping::add_var_to_mapping(std::string name_tron, int byte_offset, 
+                         int32_t (*conv_to_TRON)(byte*, int*),
+                         void (*conv_to_topic)(int32_t, byte*, int*)){
+    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);
+}
+
+
 template <class T>
 void publish_to_topic(std::string topic, boost::shared_ptr<T> ptr) {
     for (ros::Publisher& pub : input_publishers)
@@ -351,7 +301,7 @@ void request_start() {
     ROS_INFO("success: starting test phase");
 }
 
-void report_now(Channel& chan, uint16_t var_count = 0, int32_t *vars = nullptr){
+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);
 
     add_int32_in_network_order(chan.identifier, msg.get(), 0);
@@ -372,7 +322,7 @@ void report_now(Channel& chan, uint16_t var_count = 0, int32_t *vars = nullptr){
     acks_missing++;
 }
 
-void report_now(std::string chan, uint16_t var_count = 0, int32_t *vars = nullptr){
+void report_now(std::string chan, uint16_t var_count, int32_t *vars){
     for (Mapping& map : mappings)
         if (map.channel.name == chan && !map.channel.is_input) {
             report_now(map.channel, var_count, vars);
@@ -408,6 +358,67 @@ void mappings_callback_to_TRON(const ros::MessageEvent<T>& event){
     }
 }
 
+void process_TRONs_msgs(){
+    /* note: TRONs communication after start is not guaranteed to be synchronous,
+    thus incoming messages must be checked for their content */
+    while (true){
+        // get 4 bytes at a time as an int32
+        byte bytes[4];
+        if (!read_4_bytes_nonblock(socket_fd, bytes)) 
+            break; // no bytes left to process
+        int32_t next = network_bytes_to_int_32(bytes);
+
+        // bytes are acknowledgement
+        if (next == ACK_SINGLE) {
+            if (--acks_missing < 0) throw "too many acknowledgements";
+            ROS_INFO("got acknowledgement for output");
+            continue;
+        }
+
+        // bytes are channel identifier
+        // find corresponding channel
+        const Channel *chan = nullptr;
+        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) 
+            throw "channel could not be identified";
+
+        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 = network_bytes_to_uint_16(bytes);
+        ROS_INFO("got variable count %i", var_count);
+
+        int32_t vals[var_count];
+        // process variables
+        for (uint16_t i = 0; i < var_count; i++) {
+            recv(socket_fd, bytes, 4, MSG_DONTWAIT);
+            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);
+            vals[i] = next;
+        }
+
+        for (Mapping& map : mappings)
+            if (map.channel.name == chan->name && map.channel.is_input) {
+                if (map.input_callback != nullptr)
+                    map.input_callback(map, vals);  
+                else throw "no callback declared";
+            }
+                          
+
+        // send acknowledgement
+        add_int32_in_network_order(ACK_SINGLE, bytes, 0);
+        ROS_INFO("sending acknowledgement");
+        send_bytes(socket_fd, bytes, 4);
+    }
+}
+
 // ---------------------------------------------------------------------------------------------------------------------------------------
 // custom from here
 
@@ -620,7 +631,7 @@ void configuration_phase(ros::NodeHandle& nh){
         map.add_var_to_mapping(s);
         mappings.push_back(map);
     }
-    output_subscribers.push_back(nh.subscribe("/gazebo/link_states", 1, on_gazebo_link_states));
+    output_subscribers.push_back(nh.subscribe("/gazebo/link_states", 10, on_gazebo_link_states));
 
     // used to end testing when model is done
     map = Mapping("test_done_dummy", "test_done", true);
@@ -641,67 +652,6 @@ void configuration_phase(ros::NodeHandle& nh){
     }
 }
 
-void process_TRONs_msgs(){
-    /* note: TRONs communication after start is not guaranteed to be synchronous,
-    thus incoming messages must be checked for their content */
-    while (true){
-        // get 4 bytes at a time as an int32
-        byte bytes[4];
-        if (!read_4_bytes_nonblock(socket_fd, bytes)) 
-            break; // no bytes left to process
-        int32_t next = network_bytes_to_int_32(bytes);
-
-        // bytes are acknowledgement
-        if (next == ACK_SINGLE) {
-            if (--acks_missing < 0) throw "too many acknowledgements";
-            ROS_INFO("got acknowledgement for output");
-            continue;
-        }
-
-        // bytes are channel identifier
-        // find corresponding channel
-        const Channel *chan = nullptr;
-        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) 
-            throw "channel could not be identified";
-
-        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 = network_bytes_to_uint_16(bytes);
-        ROS_INFO("got variable count %i", var_count);
-
-        int32_t vals[var_count];
-        // process variables
-        for (uint16_t i = 0; i < var_count; i++) {
-            recv(socket_fd, bytes, 4, MSG_DONTWAIT);
-            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);
-            vals[i] = next;
-        }
-
-        for (Mapping& map : mappings)
-            if (map.channel.name == chan->name && map.channel.is_input) {
-                if (map.input_callback != nullptr)
-                    map.input_callback(map, vals);  
-                else throw "no callback declared";
-            }
-                          
-
-        // send acknowledgement
-        add_int32_in_network_order(ACK_SINGLE, bytes, 0);
-        ROS_INFO("sending acknowledgement");
-        send_bytes(socket_fd, bytes, 4);
-    }
-}
-
 int main(int argc, char**argv){
     ros::init(argc, argv, "TRON adapter");
     ros::NodeHandle nh;
-- 
GitLab