Skip to content
Snippets Groups Projects
Select Git revision
  • 345c31de4060a6c96ccac962d54505a1bfb767af
  • master default protected
2 results

tron_adapter.h

  • tron_adapter.h 12.54 KiB
    #ifndef TRON_ADAPTER
    #define TRON_ADAPTER
    
    #include <ros/ros.h>
    #include <arpa/inet.h>
    
    typedef uint8_t byte;
    
    // some networking helper functions ---------------------------------------------------
    
    /**
     * logs bytes
     * @param buf start of byte array
     * @param buf_length how many bytes to log
     * @param send true if message gets send, false if received
     */
    inline void byte_info(const byte* buf, int buf_length, bool send=true);
    
    /**
     * reads bytes from file descriptor 
     * @param fd file descriptor
     * @param count how many bytes to read
     * @return byte array read
     */
    std::unique_ptr<byte[]> get_bytes_socket(int fd, int count);
    
    /**
     * converts bytes to host order and casts to int32
     * @param buf start of 4 bytes to convert
     */
    inline int32_t network_bytes_to_int_32(byte *buf);
    
    /**
     * converts bytes to host order and casts to uint16
     * @param buf start of 2 bytes to convert
     */
    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)
     * @param fd file descriptor
     */
    int32_t get_int_socket(int fd);
    
    /**
     * converts int32 to network order and sets it converted to network order into byte array
     * @param num number to convert
     * @param buf start adress of array to add to
     * @param index wanted starting index of bytes in array
     */
    void add_int32_in_network_order(int32_t num, byte *buf, int index);
    
    /**
     * wraps systems byte-sending for automated logging
     * @param fd file descriptor
     * @param buf start adress of byte array to send
     * @param length how many bytes to send
     */
    inline void send_bytes(int fd, const byte *buf, int length);
    
    /**
     * reads 4 bytes instantly, throws if not successfull
     * @param fd file descriptor
     * @param buf start adress to read into
     */
    inline bool read_4_bytes_nonblock(int fd, byte *buf);
    
    /**
     * constructs socket connected to given parameters
     * @param IP IPv4 adress to connect to
     * @param port port to connect to
     */
    int create_connected_socket(std::string IP, uint16_t port);
    
    // ----------------------------------------------------------------------------------------
    
    struct Channel {
        /**
         * represents a channel
         * @param name name of channel in Uppaal
         * @param identifier identifier for TRON
         * @param is_input channel used as input or output
         * @param vars names of variables appended to this 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){};
    };
    
    struct Mapping {
        /**
         * represents a Mapping of one topic and one channel
         * @param topic name of topic in ROS
         * @param channel associated channel
         * @param input_callback callback used if channel is input
         * @param byte_offset offsets of bytes to map (from end of last field)
         * @param converters_to_TRON converters if variable is not int32, nullptr if not used
         * @param converters_to_topics converters if variable is not int32, nullptr if not used
         */
        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
        boost::function<void(Mapping& map, int32_t*)> input_callback = 0;
    
        // 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
        // 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 instance of TRON_Adapter
         * @param IP IPv4 address to connect to
         * @param PORT port to connect to
         */
        TRON_Adapter(std::string IP, uint16_t PORT);
    
        /**
         * used only to be able to declare adapters as global variables without initialising
         */
        TRON_Adapter() = default;
    
        /** 
         * declares used channel to TRON and returns Mapping to add variables;
         * when done with adding variables, add the Mapping instance to mappings list
         * @param topic name of the topic
         * @param channel name of channel in Uppaal model
         * @param channelIsInput is channel used as input or output
         */
        Mapping createMapping(std::string topic, std::string channel, bool channelIsInput);
    
        /** 
         * declares variable to mapping calling add_var_to_channel and adds given values to corresponding lists
         * @param map mapping to append variable to
         * @param name_tron name of variable as in Uppaal model
         * @param byte_offset offset from ending of last field
         * @param conv_to_TRON converter from byte* to int32_t in order to send to TRON, needs to increase given int* by count of bytes used
         * @param conv_to_topic conv_to_TRON reversed for messaging topic
         */
        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 variables to TRON
         * @param microseconds how many microseconds represent one time unit
         * @param timeout how many time units pass until timeout
         */
        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();
    
        /**
         * report to TRON
         * @param chan channel to activate
         * @param var_count how many variables to attach
         * @param vars pointer to int32 array with variables to attach
         */
        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();
    
        /**
         * gets and logs error message from TRON, then throws since proceeding would lead to more errors
         * @param errorcode code received from TRON
         */
        void get_error_msg(int32_t errorcode);
    
        /**
         * declares channel to TRON and constructs corresponding instance
         * @param is_input is channel used as input or output
         * @param name name of channel in Uppaal model
         */
        std::unique_ptr<Channel> send_channel_decl_msg(bool is_input, std::string name);
    
        /**
        * declares variable being attached to channel for TRON
         * @param chan channel to associated with variable
         * @param is_input is channel used as input or output
         * @param var name of int variable in Uppaal model
         */
        void add_var_to_channel(Channel& chan, bool is_input, std::string var);
    
        /**
         * searches publisher list for given topic and publishes to it
         * @param topic name of the topic to publish to
         * @param ptr pointer to message
         */
        template <class T>
        void publish_to_topic(std::string topic, boost::shared_ptr<T> ptr) {
            for (ros::Publisher& pub : input_publishers)
                if (pub.getTopic() == topic) {
                    pub.publish(ptr);
                    return;
                }
            throw "did not find publisher";
        }
    
        /**
         * callback reports to topic like defined in given byte-mapping
         * note: needs to be specified for each mapping
         * @param map mapping associated with this callback
         * @param vars variables received from TRON
         */
        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 (htonl(47) == 47){
                        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);
                }
            }
            publish_to_topic<T>(map.topic, shared_ptr);
        }
     
        /**
         * callback reports to TRON like defined in byte-mappings
         * note: this callback reports to all channels associated with calling topic
         * @param event message event received from topic
         */
        template<class T>
        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) {
                if (mapping.topic.c_str() == topic && !mapping.channel.is_input) {
                    int var_count = mapping.byte_offset.size();
                    int32_t vars[var_count * 4];
                    int next_pos = 0;
                    for (int i = 0; i < var_count; i++) {
                        next_pos += mapping.byte_offset[i];
                        if (mapping.converters_to_TRON[i] == nullptr) {
                            if (htonl(47) == 47)
                                vars[i] = network_bytes_to_int_32(&bytes[next_pos]);
                            else vars[i] = *reinterpret_cast<int32_t*>(&bytes[next_pos]);
                            next_pos += 4;
                            continue;
                        } else {
                            vars[i] = mapping.converters_to_TRON[i](&bytes[next_pos], &next_pos);
                        }
                    }
                    report_now(mapping.channel, var_count, vars);
                }
            }
        }
    };
    
    #endif //TRON_ADAPTER