diff --git a/CMakeLists.txt b/CMakeLists.txt index 22919bcb228f17102a227ae4a9622d213d894233..0a36c6eda875b3b1aa4130111547442e95c6542f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -125,7 +125,7 @@ catkin_package( ## Specify additional locations of header files ## Your package locations should be listed before other locations include_directories( -# include + include ${catkin_INCLUDE_DIRS} ) @@ -145,7 +145,7 @@ include_directories( # add_executable(${PROJECT_NAME}_node src/actionlib_example_node.cpp) add_executable(${PROJECT_NAME}_garage_server src/garage_server.cpp) add_executable(${PROJECT_NAME}_garage_client src/garage_client.cpp) -add_executable(${PROJECT_NAME}_tron_adapter src/tron_adapter.cpp) +add_executable(${PROJECT_NAME}_garage_adapter src/garage_adapter.cpp) ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the @@ -157,6 +157,8 @@ add_executable(${PROJECT_NAME}_tron_adapter src/tron_adapter.cpp) ## same as for the library above # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +add_library(tron_adapter src/tron_adapter.cpp) ## Specify libraries to link a library or executable target against target_link_libraries( ${PROJECT_NAME}_garage_server ${catkin_LIBRARIES} @@ -165,7 +167,7 @@ target_link_libraries( ${PROJECT_NAME}_garage_client ${catkin_LIBRARIES} ) target_link_libraries( - ${PROJECT_NAME}_tron_adapter ${catkin_LIBRARIES} + ${PROJECT_NAME}_garage_adapter ${catkin_LIBRARIES} tron_adapter ) ############# diff --git a/include/tron_adapter.h b/include/tron_adapter.h new file mode 100644 index 0000000000000000000000000000000000000000..b353d738323be99c07dbcfe0301e3dc47bd7ec99 --- /dev/null +++ b/include/tron_adapter.h @@ -0,0 +1,330 @@ +#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 + 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 + // 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 for topic"; + } + + /** + * 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 + diff --git a/launch/tron_server.launch b/launch/tron_server.launch index 8e7547fbdf81e513e541699dc158945e5532fcae..1a681103796e135f5efc363d7cbf15638ec6e74f 100644 --- a/launch/tron_server.launch +++ b/launch/tron_server.launch @@ -1,4 +1,4 @@ <launch> - <node name="tron_adapter" pkg="actionlib_example" type="actionlib_example_tron_adapter" respawn="false" output="screen"/> + <node name="tron_adapter" pkg="actionlib_example" type="actionlib_example_garage_adapter" respawn="false" output="screen"/> <node name="actionlib_server" pkg="actionlib_example" type="actionlib_example_garage_server" respawn="false" output="screen"/> </launch> diff --git a/src/garage_adapter.cpp b/src/garage_adapter.cpp new file mode 100644 index 0000000000000000000000000000000000000000..44920de90ef26c7fcfc557a8cc16c9f2d34dc6f6 --- /dev/null +++ b/src/garage_adapter.cpp @@ -0,0 +1,92 @@ +#include <ros/ros.h> +#include <actionlib_example/TriggerAction.h> +#include "tron_adapter.h" + +TRON_Adapter adapter; + +void send_goal (Mapping& map, int32_t* val, bool open){ + auto shared_ptr = boost::make_shared<actionlib_example::TriggerActionGoal>(); + shared_ptr->goal.open = open; + adapter.publish_to_topic<actionlib_example::TriggerActionGoal>(map.topic, shared_ptr); +} +void send_goal_open(Mapping& map, int32_t* val) {send_goal(map, val, true);} +void send_goal_close(Mapping& map, int32_t* val) {send_goal(map, val, false);} +void feedback_callback(const boost::shared_ptr<actionlib_example::TriggerActionFeedback> ptr){ + for (Mapping& map : adapter.mappings) { + if (map.channel.name == "position" && map.channel.is_input == false) + adapter.report_now(map.channel, 1, &ptr->feedback.position); + } +} +void result_callback(const boost::shared_ptr<actionlib_example::TriggerActionResult> ptr){ + for (Mapping& map : adapter.mappings) { + if (map.channel.name == "fertig" && map.channel.is_input == false) + adapter.report_now(map.channel, 1, &ptr->result.position); + } +} + +void configuration_phase(ros::NodeHandle& nh){ + /* note: for configuration phase maximum message length is 256 Bytes, + therefore heap allocation can be avoided most of the time in called functions */ + + // note: since we are not an actual client (or server) + // we need to use the high level packet::*Action* messages. + // custom callbacks are implemented in order to not worry about header size + Mapping map = adapter.createMapping("/garage_server/goal", "oeffnen", true); + map.input_callback = send_goal_open; + adapter.mappings.push_back(map); + + map = adapter.createMapping("/garage_server/goal", "schliessen", true); + map.input_callback = send_goal_close; + adapter.mappings.push_back(map); + + adapter.input_publishers.push_back(nh.advertise<actionlib_example::TriggerActionGoal>("/garage_server/goal", 1)); + + map = adapter.createMapping("/garage_server/result", "fertig", false); + adapter.add_var_to_mapping(map, "akt_position"); + adapter.mappings.push_back(map); + adapter.output_subscribers.push_back(nh.subscribe("/garage_server/result", 100, result_callback)); + + map = adapter.createMapping("/garage_server/feedback", "position", false); + adapter.add_var_to_mapping(map, "akt_position"); + adapter.mappings.push_back(map); + adapter.output_subscribers.push_back(nh.subscribe("/garage_server/feedback", 100, + feedback_callback)); + // not obvious in documentation: local variables are not supported + uint64_t microseconds = 1000000; // one second + // documentation states 2 signed integers are used for some reason + adapter.set_time_unit_and_timeout(microseconds, 300); + + // wait till subscribers initialized + for (ros::Publisher& pub : adapter.input_publishers) { + while (pub.getNumSubscribers() == 0) { ros::Duration(1).sleep(); }; + } +} + +int main(int argc, char**argv){ + ros::init(argc, argv, "TRON dapter"); + ros::NodeHandle nh; + + try { + const std::string IP = "127.0.0.1"; + const uint16_t PORT = 8080; + adapter = TRON_Adapter(IP, PORT); + + configuration_phase(nh); + + adapter.request_start(); + + // testing phase loop + ros::Rate test_phase_freq(60); + while (ros::ok()) { + ros::spinOnce(); + adapter.process_TRONs_msgs(); + test_phase_freq.sleep(); + } + } catch (const char* err){ + ROS_FATAL("shutting down: %s", err); + ros::shutdown(); + } catch (...){ + ROS_FATAL("shutting down"); + ros::shutdown(); + } +} diff --git a/src/tron_adapter.cpp b/src/tron_adapter.cpp index b8e54aae4078af2d8bc634805eb0cda6d2bb53ca..995eac4554b95ef5a4d60a41013ed1ef4ffee198 100644 --- a/src/tron_adapter.cpp +++ b/src/tron_adapter.cpp @@ -3,22 +3,18 @@ #include <sys/socket.h> #include <netinet/in.h> #include <arpa/inet.h> -#include <std_msgs/Int32.h> #include <boost/make_shared.hpp> #include <memory> -#include <actionlib_example/TriggerActionGoal.h> -#include <actionlib_example/TriggerActionFeedback.h> -#include <actionlib_example/TriggerAction.h> - -const std::string IP = "127.0.0.1"; -const uint16_t PORT = 8080; +#include <algorithm> // for std::find +#include <fcntl.h> +#include "tron_adapter.h" typedef uint8_t byte; // some helper functions ----------------------------------------------- -// log bytes sent/received -inline void byte_info(const byte* buf, int buf_length, bool send=true){ +// logs bytes +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]; @@ -75,7 +71,7 @@ void add_int32_in_network_order(int32_t num, byte *buf, int index){ inline void send_bytes(int fd, const byte *buf, int length){ byte_info(buf, length); int ret = write(fd, (void*) buf, length); - if (ret < 0) throw "writing failed"; + if (ret < 0) throw "sending failed"; } // returns false if nothing more to read and true if 4 bytes are read successfully @@ -113,144 +109,36 @@ int create_connected_socket(std::string IP, uint16_t port){ return socketfd; } -// consts for TRON ---------------------------------------------------------- - -const byte GET_ERROR_MSG = 127; -const byte DECL_CHAN_INPUT = 1; -const byte DECL_CHAN_OUTPUT = 2; -const byte ADD_VAR_TO_INPUT = 3; -const byte ADD_VAR_TO_OUTPUT = 4; -const byte SET_TIME_UNIT = 5; -const byte SET_TIMEOUT = 6; -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 ------------------------------------------------------------- - // 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(); - } - 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; - -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 for topic"; +TRON_Adapter::TRON_Adapter(std::string IP, uint16_t PORT) { + socket_fd = create_connected_socket(IP, PORT); } -// 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); - } - } - publish_to_topic<T>(map.topic, shared_ptr); +Mapping TRON_Adapter::createMapping(std::string topic, std::string channel, bool channelIsInput){ + Mapping map; + map.topic = topic; + map.channel = *send_channel_decl_msg(channelIsInput, channel).get(); + return map; } -// subscribers already have callbacks -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 (yet) */ -int acks_missing = 0; +void TRON_Adapter::add_var_to_mapping(Mapping& map, 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(map.channel, map.channel.is_input, name_tron); + 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); +} -void get_error_msg(int32_t errorcode) { +void TRON_Adapter::get_error_msg(int32_t errorcode) { ROS_WARN("got error, trying to get corresponding message"); byte get_err_msg_msg[5]; - get_err_msg_msg[0] = GET_ERROR_MSG; // get error msg code + get_err_msg_msg[0] = GET_ERROR_MSG; add_int32_in_network_order(errorcode, get_err_msg_msg, 1); - send_bytes(socket_fd, get_err_msg_msg, 5); // connection is closed after this write? + send_bytes(socket_fd, get_err_msg_msg, 5); byte err_msg_length = get_bytes_socket(socket_fd, 1)[0]; auto err_msg = get_bytes_socket(socket_fd, err_msg_length); std::string msg_str = std::string(reinterpret_cast<char*>(err_msg.get()), (size_t) err_msg_length); @@ -258,7 +146,16 @@ void get_error_msg(int32_t errorcode) { throw "got error from TRON"; } -void add_var_to_channel(Channel& chan, bool is_input, std::string var) { +void TRON_Adapter::add_var_to_channel(Channel& chan, bool is_input, std::string var) { + bool var_already_declared = false; + for (Mapping& map : mappings) + if (chan.name == map.channel.name) + if (std::find(map.channel.vars.begin(), map.channel.vars.end(), var) != map.channel.vars.end()) var_already_declared = true; + if (var_already_declared) { + ROS_INFO("variable %s was already declared to channel %s", var.c_str(), chan.name.c_str()); + chan.vars.push_back(var); + return; + } byte msg[6 + var.length()]; msg[0] = is_input ? ADD_VAR_TO_INPUT : ADD_VAR_TO_OUTPUT; add_int32_in_network_order(chan.identifier, msg, 1); @@ -272,7 +169,7 @@ void add_var_to_channel(Channel& chan, bool is_input, std::string var) { chan.vars.push_back(var); } -std::unique_ptr<Channel> send_channel_decl_msg(bool is_input, std::string name) { +std::unique_ptr<Channel> TRON_Adapter::send_channel_decl_msg(bool is_input, std::string name) { // prepare packet size_t msg_length = 2 + name.length(); byte msg[msg_length]; @@ -282,8 +179,7 @@ std::unique_ptr<Channel> send_channel_decl_msg(bool is_input, std::string name) // send packet ROS_INFO("declaring channel %s as %s", name.c_str(), (is_input ? "input" : "output")); - byte_info(msg, msg_length); - write(socket_fd, (void*) msg, msg_length); + send_bytes(socket_fd, msg, msg_length); // get answer from TRON int32_t channel_identifier = get_int_socket(socket_fd); @@ -296,7 +192,7 @@ std::unique_ptr<Channel> send_channel_decl_msg(bool is_input, std::string name) return std::make_unique<Channel>(name, channel_identifier, is_input); } -void set_time_unit_and_timeout(uint64_t microseconds, int32_t timeout){ +void TRON_Adapter::set_time_unit_and_timeout(uint64_t microseconds, int32_t timeout){ byte msg[9]; msg[0] = SET_TIME_UNIT; byte *microseconds_bytes = reinterpret_cast<byte*>(µseconds); @@ -322,7 +218,7 @@ void set_time_unit_and_timeout(uint64_t microseconds, int32_t timeout){ ROS_INFO("success: set timeout"); } -void request_start() { +void TRON_Adapter::request_start() { /* documentation confuses codes for start and getErrorMessage, actually used: 64 is start 127 is gerErrorMessage */ @@ -334,7 +230,7 @@ void request_start() { ROS_INFO("success: starting test phase"); } -void report_now(Channel& chan, uint16_t var_count, int32_t *vars){ +void TRON_Adapter::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); @@ -355,93 +251,16 @@ void report_now(Channel& chan, uint16_t var_count, int32_t *vars){ acks_missing++; } -// callback reports to TRON like defined in mappings -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 (SYS_IS_BIG_ENDIAN) - 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); +void TRON_Adapter::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); + return; } - } + throw "could not report to channel"; } -void send_goal (Mapping& map, int32_t* val, bool open){ - auto shared_ptr = boost::make_shared<actionlib_example::TriggerActionGoal>(); - shared_ptr->goal.open = open; - publish_to_topic<actionlib_example::TriggerActionGoal>(map.topic, shared_ptr); -} -void send_goal_open(Mapping& map, int32_t* val) {send_goal(map, val, true);} -void send_goal_close(Mapping& map, int32_t* val) {send_goal(map, val, false);} -void feedback_callback(const boost::shared_ptr<actionlib_example::TriggerActionFeedback> ptr){ - for (Mapping& map : mappings) { - if (map.channel.name == "position" && map.channel.is_input == false) - report_now(map.channel, 1, &ptr->feedback.position); - } -} -void result_callback(const boost::shared_ptr<actionlib_example::TriggerActionResult> ptr){ - for (Mapping& map : mappings) { - if (map.channel.name == "fertig" && map.channel.is_input == false) - report_now(map.channel, 1, &ptr->result.position); - } -} - -void configuration_phase(ros::NodeHandle& nh){ - /* note: for configuration phase maximum message length is 256 Bytes, - therefore heap allocation can be avoided most of the time in called functions */ - - // note: since we are not an actual client (or server) - // we need to use the high level packet::*Action* messages. - // custom callbacks are implemented in order to not worry about header size - Mapping map = Mapping("/garage_server/goal", "oeffnen", true); - map.input_callback = send_goal_open; - mappings.push_back(map); - - map = Mapping("/garage_server/goal", "schliessen", true); - map.input_callback = send_goal_close; - mappings.push_back(map); - - input_publishers.push_back(nh.advertise<actionlib_example::TriggerActionGoal>("/garage_server/goal", 100)); - - map = Mapping("/garage_server/result", "fertig", false); - map.add_var_to_mapping("akt_position", 0); - mappings.push_back(map); - output_subscribers.push_back(nh.subscribe("/garage_server/result", 100, result_callback)); - - map = Mapping("/garage_server/feedback", "position", false); - map.add_var_to_mapping("akt_position", 0); - mappings.push_back(map); - output_subscribers.push_back(nh.subscribe("/garage_server/feedback", 100, - feedback_callback)); - // not obvious in documentation: local variables are not supported - // add_var_to_channel(socketfd, "ausloesen", "lokal"); - uint64_t microseconds = 1000000; // one second - // documentation states 2 signed integers are used for some reason - set_time_unit_and_timeout(microseconds, 300); - - // wait till subscribers initialized - for (ros::Publisher& pub : input_publishers) { - while (pub.getNumSubscribers() == 0) { ros::Duration(1).sleep(); }; - } -} - -void process_TRONs_msgs(){ +void TRON_Adapter::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){ @@ -488,15 +307,12 @@ void process_TRONs_msgs(){ } for (Mapping& map : mappings) - if (map.channel.name == chan->name && map.channel.is_input) - for (ros::Publisher& pub : input_publishers){ - if (pub.getTopic() == map.topic) { - if (map.input_callback != nullptr) - map.input_callback(map, vals); - else throw "no callback declared"; - break; - } - } + 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); @@ -505,28 +321,4 @@ void process_TRONs_msgs(){ } } -int main(int argc, char**argv){ - ros::init(argc, argv, "TRON dapter"); - ros::NodeHandle nh; - - try { - socket_fd = create_connected_socket(IP, PORT); - - configuration_phase(nh); - - request_start(); - - // testing phase loop - ros::Rate test_phase_freq(1); - ros::AsyncSpinner spinner(4); - spinner.start(); - while (ros::ok()) { - ros::spinOnce(); - process_TRONs_msgs(); - test_phase_freq.sleep(); - } - } catch (const char* err){ - ROS_FATAL("shutting down: %s", err); - ros::shutdown(); - } -} \ No newline at end of file +