No commit message

No commit message

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

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