diff --git a/include/tron_adapter.h b/include/tron_adapter.h index 41a44cc20c980cf83f636060602226840fd985cf..d99795d5c2d61da033c6a6361c94d0265d749734 100644 --- a/include/tron_adapter.h +++ b/include/tron_adapter.h @@ -2,6 +2,7 @@ #define TRON_ADAPTER #include <ros/ros.h> +#include <arpa/inet.h> typedef uint8_t byte; @@ -141,16 +142,6 @@ struct TRON_Adapter { // 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); @@ -159,6 +150,72 @@ struct TRON_Adapter { // declares var to channel void add_var_to_channel(Channel& chan, bool is_input, std::string var); + + // publishes to fitting publisher from list + 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"; + } + + // template callbacks for using specified byte positions + // 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 (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 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 (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/src/tron_adapter.cpp b/src/tron_adapter.cpp index d6feafa71c7fa20d51aa611f9641e6122f6ecd19..205d6bb66eb7c944c272eb0c695b68f95df943cc 100644 --- a/src/tron_adapter.cpp +++ b/src/tron_adapter.cpp @@ -133,16 +133,6 @@ void TRON_Adapter::add_var_to_mapping(Mapping& map, std::string name_tron, int b map.converters_to_topics.push_back(conv_to_topic); } -template <class T> -void TRON_Adapter::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"; -} - void TRON_Adapter::get_error_msg(int32_t errorcode) { ROS_WARN("got error, trying to get corresponding message"); byte get_err_msg_msg[5]; @@ -329,56 +319,4 @@ void TRON_Adapter::process_TRONs_msgs(){ } } -// callback reports to TRON like defined in mappings -template<class T> -void TRON_Adapter::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); - } - } -} -// callback publishing to topics like defined in mapping, used for input_callback -template<class T> -void TRON_Adapter::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); -} \ No newline at end of file