tron_adapter.h

#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);

// ----------------------------------------------------------------------------------------

// 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

    Channel channel = Channel("", 0, true); // TRON channel
    // Callback used if TRON sends some message
    // output callbacks must be specified when subscribing to topics
    void(*input_callback)(Mapping& map, int32_t*) = nullptr;

    // note: since ROS messages can be complex, using byte positions might be cumbersome to use
    // it is easy to implement custom callbacks using ROS message types
    // (with publish_to_topic for inputs and report_now for outputs)

    // 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;

    /* 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.
    */
};

// sockets file descriptor, mappings, publishers and subscribers kept in struct
struct TRON_Adapter {
    // TRON message codes
    static const byte GET_ERROR_MSG = 127;
    static const byte DECL_CHAN_INPUT = 1;
    static const byte DECL_CHAN_OUTPUT = 2;
    static const byte ADD_VAR_TO_INPUT = 3;
    static const byte ADD_VAR_TO_OUTPUT = 4;
    static const byte SET_TIME_UNIT = 5;
    static const byte SET_TIMEOUT = 6;
    static const byte REQUEST_START = 64;
    static const byte ANSWER_START = 0;
    static const int32_t ACK_SINGLE = 1 << 31; // 32 Bit int with most significant bit set to 1
    int socket_fd = -1;
    std::list<Mapping> mappings;
    std::vector<ros::Publisher> input_publishers;
    std::vector<ros::Subscriber> output_subscribers;
    // 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, which it is not */
    int acks_missing = 0; 

    // creates connected socket and sets socket_fd
    TRON_Adapter(std::string IP, uint16_t PORT);

    TRON_Adapter() = default; // do not use; needed to be able to globally declare adapter instance

    // declares used channel to TRON and returns Mapping to add variables
    // when done with adding variables, add the Mapping instance to mappings list
    Mapping createMapping(std::string topic, std::string channel, bool channelIsInput);

    // declares used variable to TRON and adds given values to corresponding lists
    void add_var_to_mapping(Mapping& map, 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);


    // sends time unit and timeout to TRON
    void set_time_unit_and_timeout(uint64_t microseconds, int32_t timeout);

    // sends REQUEST_START to TRON ans waits until receiving ANSWER_START (with timeout)
    void request_start();

    // reports to channel
    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);

    // starts callback for every message received from TRON
    void process_TRONs_msgs();

    // template callbacks for using specified byte positions
    template<class T>
    void mapping_callback_to_topic(Mapping& map, int32_t* vars);
    template<class T>
    void mappings_callback_to_TRON(const ros::MessageEvent<T>& event);

    // publishes to fitting publisher from list
    template <class T>
    void publish_to_topic(std::string topic, boost::shared_ptr<T> ptr);

    // gets and logs error message from TRON, then throws since proceeding would lead to more errors
    void get_error_msg(int32_t errorcode);

    // declares channel to TRON and constructs Channel instance
    std::unique_ptr<Channel> send_channel_decl_msg(bool is_input, std::string name);

    // declares var to channel
    void add_var_to_channel(Channel& chan, bool is_input, std::string var);
};

#endif //TRON_ADAPTER