From 3ab080e755d5076124f19c126bb859cfffebae56 Mon Sep 17 00:00:00 2001
From: CS <christoph.schroeter1@mailbox.tu-dresden.de>
Date: Sat, 14 Aug 2021 19:13:05 +0200
Subject: [PATCH]

---
 CMakeLists.txt                 |   6 +-
 include/tron_adapter.h         | 115 +++++----
 launch/cobot1_mock_test.launch |   2 +-
 src/cobot_adapter.cpp          | 274 ++++++++++++++++++++
 src/tron_adapter.cpp           | 443 ++++++---------------------------
 5 files changed, 420 insertions(+), 420 deletions(-)
 create mode 100644 src/cobot_adapter.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index aaedd57..03b84db 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -65,12 +65,12 @@ include_directories(
         ${CMAKE_CURRENT_BINARY_DIR}
         ${CMAKE_CURRENT_SOURCE_DIR}
 )
-
+add_library(tron_adapter src/tron_adapter.cpp)
 
 add_executable(CobotController src/CobotController.cpp src/Cobot.cpp )
-add_executable(TRON_Adapter src/tron_adapter.cpp)
+add_executable(TRON_Adapter src/cobot_adapter.cpp)
 add_dependencies(CobotController ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Add cmake target dependencies of the executable
 target_link_libraries(CobotController ${catkin_LIBRARIES})
-target_link_libraries(TRON_Adapter ${catkin_LIBRARIES})
\ No newline at end of file
+target_link_libraries(TRON_Adapter ${catkin_LIBRARIES} tron_adapter)
\ No newline at end of file
diff --git a/include/tron_adapter.h b/include/tron_adapter.h
index 41bed64..41a44cc 100644
--- a/include/tron_adapter.h
+++ b/include/tron_adapter.h
@@ -34,20 +34,6 @@ inline bool read_4_bytes_nonblock(int fd, byte *buf);
 int create_connected_socket(std::string IP, uint16_t port);
 
 // ----------------------------------------------------------------------------------------
-// TRON message codes
-extern const byte GET_ERROR_MSG;
-extern const byte DECL_CHAN_INPUT;
-extern const byte DECL_CHAN_OUTPUT;
-extern const byte ADD_VAR_TO_INPUT;
-extern const byte ADD_VAR_TO_OUTPUT;
-extern const byte SET_TIME_UNIT;
-extern const byte SET_TIMEOUT;
-extern const byte REQUEST_START;
-extern const byte ANSWER_START;
-extern const int32_t ACK_SINGLE;
-
-// Socket kept global
-extern int socket_fd;
 
 // represents a channel in Uppaal
 struct Channel {
@@ -62,9 +48,17 @@ struct 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
@@ -76,7 +70,6 @@ struct Mapping {
     // 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"
@@ -95,50 +88,78 @@ struct Mapping {
     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);
-
-    // adds variable to channel and sets position and conversion parameters
-    void add_var_to_mapping(std::string name_tron, int byte_offset = 0, 
+// 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);
-};
 
-// mappings, publishers and subscribers kept global
-extern std::list<Mapping> mappings;
-extern std::vector<ros::Publisher> input_publishers;
-extern std::vector<ros::Subscriber> output_subscribers;
 
-// publishes to fitting publisher from list
-template <class T>
-void publish_to_topic(std::string topic, boost::shared_ptr<T> ptr);
+    // sends time unit and timeout to TRON
+    void set_time_unit_and_timeout(uint64_t microseconds, int32_t timeout);
 
-template<class T>
-void mapping_callback_to_topic(Mapping& map, int32_t* vars);
+    // sends REQUEST_START to TRON ans waits until receiving ANSWER_START (with timeout)
+    void request_start();
 
-void get_error_msg(int32_t errorcode);
+    // 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);
 
-void add_var_to_channel(Channel& chan, bool is_input, std::string var);
+    // starts callback for every message received from TRON
+    void process_TRONs_msgs();
 
-std::unique_ptr<Channel> send_channel_decl_msg(bool is_input, std::string name);
+    // 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);
 
-void set_time_unit_and_timeout(uint64_t microseconds, int32_t timeout);
+    // publishes to fitting publisher from list
+    template <class T>
+    void publish_to_topic(std::string topic, boost::shared_ptr<T> ptr);
 
-void request_start();
+    // gets and logs error message from TRON, then throws since proceeding would lead to more errors
+    void get_error_msg(int32_t errorcode);
 
-void report_now(Channel& chan, uint16_t var_count = 0, int32_t *vars = nullptr);
+    // declares channel to TRON and constructs Channel instance
+    std::unique_ptr<Channel> send_channel_decl_msg(bool is_input, std::string name);
 
-void report_now(std::string chan, uint16_t var_count = 0, int32_t *vars = nullptr);
-
-template<class T>
-void mappings_callback_to_TRON(const ros::MessageEvent<T>& event);
-
-void process_TRONs_msgs();
-
-void configuration_phase(ros::NodeHandle& nh);
+    // declares var to channel
+    void add_var_to_channel(Channel& chan, bool is_input, std::string var);
+};
 
 #endif //TRON_ADAPTER
 
diff --git a/launch/cobot1_mock_test.launch b/launch/cobot1_mock_test.launch
index 2eaf8f0..59b49aa 100644
--- a/launch/cobot1_mock_test.launch
+++ b/launch/cobot1_mock_test.launch
@@ -1,7 +1,7 @@
 <launch>
 
     <include file="$(find panda_simulation)/launch/simulation.launch">
-        <arg name="gui" value="true"/>
+        <arg name="gui" value="false"/>
         <arg name="slow_simulation" value="true" />
     </include>
 
diff --git a/src/cobot_adapter.cpp b/src/cobot_adapter.cpp
new file mode 100644
index 0000000..f3f1111
--- /dev/null
+++ b/src/cobot_adapter.cpp
@@ -0,0 +1,274 @@
+#include <cooperation_msgs/PressureMsg.h>
+#include <moveit_msgs/PickupAction.h>
+#include <moveit_msgs/PlaceAction.h>
+#include <moveit_msgs/MoveItErrorCodes.h>
+#include <moveit_msgs/MoveGroupAction.h>
+#include <control_msgs/FollowJointTrajectoryAction.h>
+#include <gazebo_msgs/LinkStates.h>
+#include <geometry_msgs/Pose.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/LinearMath/Vector3.h>
+#include <algorithm> // for std::find
+#include <cmath>
+#include "tron_adapter.h"
+
+TRON_Adapter adapter;
+int32_t retry_count; // set in configuration_phase
+
+void on_pressure(const cooperation_msgs::PressureMsg::ConstPtr &msg) { 
+    if (msg->pressed)
+        adapter.report_now("pressed", 1, &retry_count);
+}
+
+void on_pickup_result(const moveit_msgs::PickupActionResult::ConstPtr &msg) {
+    if (msg->result.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
+        adapter.report_now("pickup_success");
+    else adapter.report_now("pickup_fail");
+}
+
+void on_place_result(const moveit_msgs::PlaceActionResult::ConstPtr &msg) {
+    if (msg->result.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
+        adapter.report_now("place_success");
+    else adapter.report_now("place_fail");
+}
+
+void on_move_result(const control_msgs::FollowJointTrajectoryActionResult::ConstPtr &msg){
+    if (msg->result.error_code == control_msgs::FollowJointTrajectoryResult::SUCCESSFUL)
+        adapter.report_now("move_success");
+    else adapter.report_now("move_fail");
+}
+
+void on_move_goal(const moveit_msgs::MoveGroupActionGoal::ConstPtr &msg){
+    int32_t is_orientation_constraint_set;
+    if (msg->goal.request.path_constraints.orientation_constraints.empty())
+        is_orientation_constraint_set = 0;
+    else is_orientation_constraint_set = 1;
+    adapter.report_now("goal", 1, &is_orientation_constraint_set);
+}
+
+void on_pickup_goal(const moveit_msgs::PickupActionGoal::ConstPtr &msg){
+    int32_t is_orientation_constraint_set;
+    if (msg->goal.path_constraints.orientation_constraints.empty())
+        is_orientation_constraint_set = 0;
+    else is_orientation_constraint_set = 1;
+    adapter.report_now("goal", 1, &is_orientation_constraint_set);
+}
+
+void on_place_goal(const moveit_msgs::PlaceActionGoal::ConstPtr &msg) {
+    int32_t is_orientation_constraint_set;
+    if (msg->goal.path_constraints.orientation_constraints.empty())
+        is_orientation_constraint_set = 0;
+    else is_orientation_constraint_set = 1;
+    adapter.report_now("goal", 1, &is_orientation_constraint_set);
+}
+
+double get_pos_diff(geometry_msgs::Point p1, geometry_msgs::Point p2) {
+    double x_d = p1.x - p2.x;
+    double y_d = p1.y - p2.y;
+    double z_d = p1.z - p2.z;
+    return std::sqrt(x_d*x_d + y_d*y_d + z_d*z_d);
+}
+// factor 100 means centimeters
+int32_t get_pos_diff_int32(geometry_msgs::Point p1, geometry_msgs::Point p2, int32_t factor = 100) {
+    double pos_diff = get_pos_diff(p1, p2);
+    return std::round(pos_diff * 100);
+}
+double get_ang_diff_ignoring_yaw(geometry_msgs::Quaternion q_msg1, geometry_msgs::Quaternion q_msg2) {
+    tf2::Quaternion q1, q2, q_diff;
+    tf2::fromMsg(q_msg1, q1);
+    tf2::fromMsg(q_msg2, q2);
+    q_diff = q1 * q2.inverse();
+    tf2::Matrix3x3 matrix(q_diff);
+    double roll, pitch, yaw;
+    matrix.getRPY(roll, pitch, yaw);
+    q_diff.setRPY(roll, pitch, 0); // set yaw to zero since it doesnt matter for glass or bottle
+    return q_diff.getAngleShortestPath();
+}
+int32_t get_ang_diff_ignoring_yaw_int32_deg(geometry_msgs::Quaternion q_msg1, geometry_msgs::Quaternion q_msg2){
+    return std::round(get_ang_diff_ignoring_yaw(q_msg1, q_msg2) * 180 / M_PI);
+}
+
+const std::vector<std::string> tron_pos_vars_ordered = {
+    "bottle_diff_to_start_pos",
+    "bottle_diff_to_start_ang",
+    "bottle_diff_to_target_pos",
+    "bottle_diff_to_target_ang",
+    "glass_diff_to_start_pos",
+    "glass_diff_to_start_ang",
+    "glass_diff_to_target_pos",
+    "glass_diff_to_target_ang"
+};
+std::vector<geometry_msgs::Pose> pose_to_compare_against; // set in configuration_phase
+bool gazebo_initialized = false;
+void on_gazebo_link_states(const gazebo_msgs::LinkStates::ConstPtr &msg){
+    int bottle_index = -1;
+    int glass_index = -1;
+    for (int j = 0; j < msg->name.size(); j++) {
+        if (msg->name[j] == "bottle::link") bottle_index = j;
+        if (msg->name[j] == "glass::link") glass_index = j;
+    }
+    if (bottle_index == -1 || glass_index == -1) return;
+    int32_t vars[tron_pos_vars_ordered.size()];
+    vars[0] = get_pos_diff_int32(msg->pose[bottle_index].position, pose_to_compare_against[0].position);
+    vars[1] = get_ang_diff_ignoring_yaw_int32_deg(msg->pose[bottle_index].orientation, pose_to_compare_against[0].orientation);
+    vars[2] = get_pos_diff_int32(msg->pose[bottle_index].position, pose_to_compare_against[1].position);
+    vars[3] = get_ang_diff_ignoring_yaw_int32_deg(msg->pose[bottle_index].orientation, pose_to_compare_against[1].orientation);
+    vars[4] = get_pos_diff_int32(msg->pose[glass_index].position, pose_to_compare_against[2].position);
+    vars[5] = get_ang_diff_ignoring_yaw_int32_deg(msg->pose[glass_index].orientation, pose_to_compare_against[2].orientation);
+    vars[6] = get_pos_diff_int32(msg->pose[glass_index].position, pose_to_compare_against[3].position);
+    vars[7] = get_ang_diff_ignoring_yaw_int32_deg(msg->pose[glass_index].orientation, pose_to_compare_against[3].orientation);
+    for (Mapping& map : adapter.mappings) {
+        if (map.topic == "/gazebo/link_states" && !map.channel.is_input) {
+            for (int i = 0; i < tron_pos_vars_ordered.size(); i++) {
+                if (tron_pos_vars_ordered[i] == map.channel.vars[0]) {
+                    if (gazebo_initialized) {
+                        adapter.report_now(map.channel.name, 1, &vars[i]);
+                    }      
+                }
+            }
+            
+        }
+    }
+    if (!gazebo_initialized) {
+        int init_diff = 1; // position difference needed to assume gazebo is initialized
+        if (vars[0] > init_diff) return; // bottle init
+        if (vars[4] > init_diff) return; // glass init
+        gazebo_initialized = true;
+    }
+}
+
+//intentionally send not expected output to end TRON testing
+void on_test_done(Mapping& map, int32_t *ptr){
+    adapter.report_now("intentional_fail");
+}
+
+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 */
+
+    nh.getParam("/planning/retries", retry_count);
+
+    geometry_msgs::Pose pose;
+    // bottle start
+    nh.getParam("/object/bottle/x", pose.position.x);
+    nh.getParam("/object/bottle/y", pose.position.y);
+    nh.getParam("/object/bottle/z", pose.position.z);
+    pose.orientation.w = 1.0;
+    pose_to_compare_against.push_back(pose);
+    // bottle target
+    nh.getParam("/object/bottle/place/x", pose.position.x);
+    nh.getParam("/object/bottle/place/y", pose.position.y);
+    nh.getParam("/object/bottle/place/z", pose.position.z);
+    pose.orientation.w = 1.0;
+    pose_to_compare_against.push_back(pose);
+    // glass start
+    nh.getParam("/object/glass/x", pose.position.x);
+    nh.getParam("/object/glass/y", pose.position.y);
+    nh.getParam("/object/glass/z", pose.position.z);
+    pose.orientation.w = 1.0;
+    pose_to_compare_against.push_back(pose);
+    // glass target
+    nh.getParam("/object/glass/place/x", pose.position.x);
+    nh.getParam("/object/glass/place/y", pose.position.y);
+    nh.getParam("/object/glass/place/z", pose.position.z);
+    pose.orientation.w = 1.0;
+    pose_to_compare_against.push_back(pose);
+
+
+    // 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("/pressure", "pressed", false);
+    adapter.add_var_to_mapping(map, "init_retry_count");
+    adapter.mappings.push_back(map);
+    adapter.output_subscribers.push_back(nh.subscribe("/pressure", 10, on_pressure));
+    
+    map = adapter.createMapping("/pickup/result", "pickup_fail", false);
+    adapter.mappings.push_back(map);
+    map = adapter.createMapping("/pickup/result", "pickup_success", false);
+    adapter.mappings.push_back(map);
+    adapter.output_subscribers.push_back(nh.subscribe("/pickup/result", 10, on_pickup_result));
+
+    map = adapter.createMapping("/place/result", "place_fail", false);
+    adapter.mappings.push_back(map);
+    map = adapter.createMapping("/place/result", "place_success", false);
+    adapter.mappings.push_back(map);
+    adapter.output_subscribers.push_back(nh.subscribe("/place/result", 10, on_place_result));
+
+    // there is a separate controller for hand movement, this one is for arm movement
+    map = adapter.createMapping("/position_joint_trajectory_controller/follow_joint_trajectory/result", "move_fail", false);
+    adapter.mappings.push_back(map);
+    map = adapter.createMapping("/position_joint_trajectory_controller/follow_joint_trajectory/result", "move_success", false);
+    adapter.mappings.push_back(map);
+    adapter.output_subscribers.push_back(nh.subscribe("/position_joint_trajectory_controller/follow_joint_trajectory/result", 10, on_move_result));
+
+    map = adapter.createMapping("/move_group/goal", "goal", false);
+    adapter.add_var_to_mapping(map, "orient_constraint_set");
+    adapter.mappings.push_back(map);
+    adapter.output_subscribers.push_back(nh.subscribe("/move_group/goal", 10, on_move_goal));
+
+    map = adapter.createMapping("/place/goal", "goal", false);
+    adapter.add_var_to_mapping(map, "orient_constraint_set");
+    adapter.mappings.push_back(map);
+    adapter.output_subscribers.push_back(nh.subscribe("/place/goal", 10, on_place_goal));
+
+    map = adapter.createMapping("/pickup/goal", "goal", false);
+    adapter.add_var_to_mapping(map, "orient_constraint_set");
+    adapter.mappings.push_back(map);
+    adapter.output_subscribers.push_back(nh.subscribe("/pickup/goal", 10, on_pickup_goal));
+
+    for (std::string s : tron_pos_vars_ordered) {
+        map = adapter.createMapping("/gazebo/link_states", s+"_update", false);
+        adapter.add_var_to_mapping(map, s);
+        adapter.mappings.push_back(map);
+    }
+    adapter.output_subscribers.push_back(nh.subscribe("/gazebo/link_states", 10, on_gazebo_link_states));
+
+    // used to end testing when model is done
+    map = adapter.createMapping("test_done_dummy", "test_done", true);
+    map.input_callback = on_test_done;
+    adapter.mappings.push_back(map);
+    map = adapter.createMapping("test_done_dummy", "intentional_fail", false);
+    adapter.mappings.push_back(map);
+
+    // 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
+    adapter.set_time_unit_and_timeout(microseconds, 600);
+
+    // 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 adapter");
+    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(10);
+        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 434d027..d6feafa 100644
--- a/src/tron_adapter.cpp
+++ b/src/tron_adapter.cpp
@@ -3,27 +3,12 @@
 #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 <cooperation_msgs/PressureMsg.h>
-#include <moveit_msgs/PickupAction.h>
-#include <moveit_msgs/PlaceAction.h>
-#include <moveit_msgs/MoveItErrorCodes.h>
-#include <moveit_msgs/MoveGroupAction.h>
-#include <control_msgs/FollowJointTrajectoryAction.h>
-#include <gazebo_msgs/LinkStates.h>
-#include <geometry_msgs/Pose.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#include <tf2/LinearMath/Quaternion.h>
-#include <tf2/LinearMath/Vector3.h>
 #include <algorithm> // for std::find
-#include <cmath>
+#include <fcntl.h>
 #include "tron_adapter.h"
 
-const std::string IP = "127.0.0.1";
-const uint16_t PORT = 8080;
-
 typedef uint8_t byte;
 
 // some helper functions -----------------------------------------------
@@ -86,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
@@ -124,43 +109,32 @@ 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
-
-// Socket kept global
-int socket_fd;
 // ROS uses little endian for its messages
 const bool SYS_IS_BIG_ENDIAN = htonl(47) == 47;
 
-Mapping::Mapping(std::string topic, std::string channel, bool channelIsInput){
-    this->topic = topic;
-    this->channel = *send_channel_decl_msg(channelIsInput, channel).get();
+
+TRON_Adapter::TRON_Adapter(std::string IP, uint16_t PORT) {
+    socket_fd = create_connected_socket(IP, PORT);
 }
 
-std::list<Mapping> mappings;
-std::vector<ros::Publisher> input_publishers;
+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;
+}
 
-void Mapping::add_var_to_mapping(std::string name_tron, int byte_offset, 
+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(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);
+    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);
 }
 
-
 template <class T>
-void publish_to_topic(std::string topic, boost::shared_ptr<T> ptr) {
+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);
@@ -169,48 +143,12 @@ void publish_to_topic(std::string topic, boost::shared_ptr<T> ptr) {
     throw "did not find publisher for topic";
 }
 
-// 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);
-}
-
-// 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 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);
@@ -218,7 +156,7 @@ 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 (std::find(map.channel.vars.begin(), map.channel.vars.end(), var) != map.channel.vars.end()) var_already_declared = true;
@@ -239,7 +177,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];
@@ -249,8 +187,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);
@@ -263,7 +200,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*>(&microseconds);
@@ -289,7 +226,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 */
@@ -301,7 +238,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);
@@ -322,7 +259,7 @@ void report_now(Channel& chan, uint16_t var_count, int32_t *vars){
     acks_missing++;
 }
 
-void report_now(std::string chan, uint16_t var_count, int32_t *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);
@@ -331,34 +268,7 @@ void report_now(std::string chan, uint16_t var_count, int32_t *vars){
     throw "could not report to channel";
 }
 
-// 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 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){
@@ -419,261 +329,56 @@ void process_TRONs_msgs(){
     }
 }
 
-// ---------------------------------------------------------------------------------------------------------------------------------------
-// custom from here
-
-int32_t retry_count; // set in configuration_phase
-void on_pressure(const cooperation_msgs::PressureMsg::ConstPtr &msg) { 
-    if (msg->pressed)
-        report_now("pressed", 1, &retry_count);
-}
-
-void on_pickup_result(const moveit_msgs::PickupActionResult::ConstPtr &msg) {
-    if (msg->result.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
-        report_now("pickup_success");
-    else report_now("pickup_fail");
-}
-
-void on_place_result(const moveit_msgs::PlaceActionResult::ConstPtr &msg) {
-    if (msg->result.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
-        report_now("place_success");
-    else report_now("place_fail");
-}
-
-void on_move_result(const control_msgs::FollowJointTrajectoryActionResult::ConstPtr &msg){
-    if (msg->result.error_code == control_msgs::FollowJointTrajectoryResult::SUCCESSFUL)
-        report_now("move_success");
-    else report_now("move_fail");
-}
-
-void on_move_goal(const moveit_msgs::MoveGroupActionGoal::ConstPtr &msg){
-    int32_t is_orientation_constraint_set;
-    if (msg->goal.request.path_constraints.orientation_constraints.empty())
-        is_orientation_constraint_set = 0;
-    else is_orientation_constraint_set = 1;
-    report_now("goal", 1, &is_orientation_constraint_set);
-}
-
-void on_pickup_goal(const moveit_msgs::PickupActionGoal::ConstPtr &msg){
-    int32_t is_orientation_constraint_set;
-    if (msg->goal.path_constraints.orientation_constraints.empty())
-        is_orientation_constraint_set = 0;
-    else is_orientation_constraint_set = 1;
-    report_now("goal", 1, &is_orientation_constraint_set);
-}
-
-void on_place_goal(const moveit_msgs::PlaceActionGoal::ConstPtr &msg) {
-    int32_t is_orientation_constraint_set;
-    if (msg->goal.path_constraints.orientation_constraints.empty())
-        is_orientation_constraint_set = 0;
-    else is_orientation_constraint_set = 1;
-    report_now("goal", 1, &is_orientation_constraint_set);
-}
-
-double get_pos_diff(geometry_msgs::Point p1, geometry_msgs::Point p2) {
-    double x_d = p1.x - p2.x;
-    double y_d = p1.y - p2.y;
-    double z_d = p1.z - p2.z;
-    return std::sqrt(x_d*x_d + y_d*y_d + z_d*z_d);
-}
-// factor 100 means centimeters
-int32_t get_pos_diff_int32(geometry_msgs::Point p1, geometry_msgs::Point p2, int32_t factor = 100) {
-    double pos_diff = get_pos_diff(p1, p2);
-    return std::round(pos_diff * 100);
-}
-double get_ang_diff_ignoring_yaw(geometry_msgs::Quaternion q_msg1, geometry_msgs::Quaternion q_msg2) {
-    tf2::Quaternion q1, q2, q_diff;
-    tf2::fromMsg(q_msg1, q1);
-    tf2::fromMsg(q_msg2, q2);
-    q_diff = q1 * q2.inverse();
-    tf2::Matrix3x3 matrix(q_diff);
-    double roll, pitch, yaw;
-    matrix.getRPY(roll, pitch, yaw);
-    q_diff.setRPY(roll, pitch, 0); // set yaw to zero since it doesnt matter for glass or bottle
-    return q_diff.getAngleShortestPath();
-}
-int32_t get_ang_diff_ignoring_yaw_int32_deg(geometry_msgs::Quaternion q_msg1, geometry_msgs::Quaternion q_msg2){
-    return std::round(get_ang_diff_ignoring_yaw(q_msg1, q_msg2) * 180 / M_PI);
-}
-
-const std::vector<std::string> tron_pos_vars_ordered = {
-    "bottle_diff_to_start_pos",
-    "bottle_diff_to_start_ang",
-    "bottle_diff_to_target_pos",
-    "bottle_diff_to_target_ang",
-    "glass_diff_to_start_pos",
-    "glass_diff_to_start_ang",
-    "glass_diff_to_target_pos",
-    "glass_diff_to_target_ang"
-};
-std::vector<geometry_msgs::Pose> pose_to_compare_against; // set in configuration_phase
-bool gazebo_initialized = false;
-void on_gazebo_link_states(const gazebo_msgs::LinkStates::ConstPtr &msg){
-    int bottle_index = -1;
-    int glass_index = -1;
-    for (int j = 0; j < msg->name.size(); j++) {
-        if (msg->name[j] == "bottle::link") bottle_index = j;
-        if (msg->name[j] == "glass::link") glass_index = j;
-    }
-    if (bottle_index == -1 || glass_index == -1) return;
-    int32_t vars[tron_pos_vars_ordered.size()];
-    vars[0] = get_pos_diff_int32(msg->pose[bottle_index].position, pose_to_compare_against[0].position);
-    vars[1] = get_ang_diff_ignoring_yaw_int32_deg(msg->pose[bottle_index].orientation, pose_to_compare_against[0].orientation);
-    vars[2] = get_pos_diff_int32(msg->pose[bottle_index].position, pose_to_compare_against[1].position);
-    vars[3] = get_ang_diff_ignoring_yaw_int32_deg(msg->pose[bottle_index].orientation, pose_to_compare_against[1].orientation);
-    vars[4] = get_pos_diff_int32(msg->pose[glass_index].position, pose_to_compare_against[2].position);
-    vars[5] = get_ang_diff_ignoring_yaw_int32_deg(msg->pose[glass_index].orientation, pose_to_compare_against[2].orientation);
-    vars[6] = get_pos_diff_int32(msg->pose[glass_index].position, pose_to_compare_against[3].position);
-    vars[7] = get_ang_diff_ignoring_yaw_int32_deg(msg->pose[glass_index].orientation, pose_to_compare_against[3].orientation);
-    for (Mapping& map : mappings) {
-        if (map.topic == "/gazebo/link_states" && !map.channel.is_input) {
-            for (int i = 0; i < tron_pos_vars_ordered.size(); i++) {
-                if (tron_pos_vars_ordered[i] == map.channel.vars[0]) {
-                    if (gazebo_initialized) {
-                        report_now(map.channel.name, 1, &vars[i]);
-                    }      
+// 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);
         }
     }
-    if (!gazebo_initialized) {
-        int init_diff = 1; // position difference needed to assume gazebo is initialized
-        if (vars[0] > init_diff) return; // bottle init
-        if (vars[4] > init_diff) return; // glass init
-        gazebo_initialized = true;
-    }
-}
-
-//intentionally send not expected output to end TRON testing
-void on_test_done(Mapping& map, int32_t *ptr){
-    report_now("intentional_fail");
-}
-
-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 */
-
-    nh.getParam("/planning/retries", retry_count);
-
-
-    geometry_msgs::Pose pose;
-    // bottle start
-    nh.getParam("/object/bottle/x", pose.position.x);
-    nh.getParam("/object/bottle/y", pose.position.y);
-    nh.getParam("/object/bottle/z", pose.position.z);
-    pose.orientation.w = 1.0;
-    pose_to_compare_against.push_back(pose);
-    // bottle target
-    nh.getParam("/object/bottle/place/x", pose.position.x);
-    nh.getParam("/object/bottle/place/y", pose.position.y);
-    nh.getParam("/object/bottle/place/z", pose.position.z);
-    pose.orientation.w = 1.0;
-    pose_to_compare_against.push_back(pose);
-    // glass start
-    nh.getParam("/object/glass/x", pose.position.x);
-    nh.getParam("/object/glass/y", pose.position.y);
-    nh.getParam("/object/glass/z", pose.position.z);
-    pose.orientation.w = 1.0;
-    pose_to_compare_against.push_back(pose);
-    // glass target
-    nh.getParam("/object/glass/place/x", pose.position.x);
-    nh.getParam("/object/glass/place/y", pose.position.y);
-    nh.getParam("/object/glass/place/z", pose.position.z);
-    pose.orientation.w = 1.0;
-    pose_to_compare_against.push_back(pose);
-
-
-    // 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("/pressure", "pressed", false);
-    map.add_var_to_mapping("init_retry_count");
-    mappings.push_back(map);
-    output_subscribers.push_back(nh.subscribe("/pressure", 10, on_pressure));
-    
-    map = Mapping("/pickup/result", "pickup_fail", false);
-    mappings.push_back(map);
-    map = Mapping("/pickup/result", "pickup_success", false);
-    mappings.push_back(map);
-    output_subscribers.push_back(nh.subscribe("/pickup/result", 10, on_pickup_result));
-
-    map = Mapping("/place/result", "place_fail", false);
-    mappings.push_back(map);
-    map = Mapping("/place/result", "place_success", false);
-    mappings.push_back(map);
-    output_subscribers.push_back(nh.subscribe("/place/result", 10, on_place_result));
-
-    // there is a separate controller for hand movement, this one is for arm movement
-    map = Mapping("/position_joint_trajectory_controller/follow_joint_trajectory/result", "move_fail", false);
-    mappings.push_back(map);
-    map = Mapping("/position_joint_trajectory_controller/follow_joint_trajectory/result", "move_success", false);
-    mappings.push_back(map);
-    output_subscribers.push_back(nh.subscribe("/position_joint_trajectory_controller/follow_joint_trajectory/result", 10, on_move_result));
-
-    map = Mapping("/move_group/goal", "goal", false);
-    map.add_var_to_mapping("orient_constraint_set");
-    mappings.push_back(map);
-    output_subscribers.push_back(nh.subscribe("/move_group/goal", 10, on_move_goal));
-
-    map = Mapping("/place/goal", "goal", false);
-    map.add_var_to_mapping("orient_constraint_set");
-    mappings.push_back(map);
-    output_subscribers.push_back(nh.subscribe("/place/goal", 10, on_place_goal));
-
-    map = Mapping("/pickup/goal", "goal", false);
-    map.add_var_to_mapping("orient_constraint_set");
-    mappings.push_back(map);
-    output_subscribers.push_back(nh.subscribe("/pickup/goal", 10, on_pickup_goal));
-
-    for (std::string s : tron_pos_vars_ordered) {
-        map = Mapping("/gazebo/link_states", s+"_update", false);
-        map.add_var_to_mapping(s);
-        mappings.push_back(map);
-    }
-    output_subscribers.push_back(nh.subscribe("/gazebo/link_states", 10, on_gazebo_link_states));
-
-    // used to end testing when model is done
-    map = Mapping("test_done_dummy", "test_done", true);
-    map.input_callback = on_test_done;
-    mappings.push_back(map);
-    map = Mapping("test_done_dummy", "intentional_fail", false);
-    mappings.push_back(map);
-
-    // 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, 600);
-
-    // wait till subscribers initialized
-    for (ros::Publisher& pub : input_publishers) {
-        while (pub.getNumSubscribers() == 0) { ros::Duration(1).sleep(); };
-    }
 }
 
-int main(int argc, char**argv){
-    ros::init(argc, argv, "TRON adapter");
-    ros::NodeHandle nh;
-
-    try {
-        socket_fd = create_connected_socket(IP, PORT);
-
-        configuration_phase(nh);
-        request_start(); 
-        
-        // testing phase loop
-        ros::Rate test_phase_freq(10);
-        while (ros::ok()) {
-            ros::spinOnce();
-            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();
+// 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
-- 
GitLab