diff --git a/include/tron_adapter.h b/include/tron_adapter.h
new file mode 100644
index 0000000000000000000000000000000000000000..41bed64155c2c7aab345424c185b02ca02bcd7a3
--- /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 0000000000000000000000000000000000000000..28d4d02095a868f7b2188604dfe48692ca5cefce
--- /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 84ca1d1a02c5bfdfaf71b9690d3ec1a86c06d15c..7237f3157b309e46f71371554e9444d7aefee70a 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 ec1eb0bc7fdec1b6a15ffacb53a12e395a38231b..434d027fdbf3bf357020fcda3b661abda2314f57 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;