diff --git a/launch/simulation_rosrag_test.launch b/launch/simulation_rosrag_test.launch
new file mode 100644
index 0000000000000000000000000000000000000000..3cd50d6ca14b3114acce7fca2f983f53c341ead5
--- /dev/null
+++ b/launch/simulation_rosrag_test.launch
@@ -0,0 +1,65 @@
+<launch>
+    <param name="robot_description" command="$(find xacro)/xacro $(find franka_description)/robots/panda_arm_hand.urdf.xacro" />
+
+    <!-- GAZEBO arguments -->
+    <arg name="paused" default="false" />
+    <arg name="use_sim_time" default="true" />
+    <arg name="gui" default="true" />
+    <arg name="headless" default="false" />
+    <arg name="debug" default="false" />
+    <arg name="load_gripper" default="true" />
+
+    <!--launch GAZEBO with own world configuration -->
+    <include file="$(find gazebo_ros)/launch/empty_world.launch">
+        <!-- <arg name="world_name" value="$(find robotics_assisted_tomography)/worlds/tomography.world"/> -->
+        <arg name="debug" value="$(arg debug)" />
+        <arg name="gui" value="$(arg gui)" />
+        <arg name="paused" value="$(arg paused)" />
+        <arg name="use_sim_time" value="$(arg use_sim_time)" />
+        <arg name="headless" value="$(arg headless)" />
+    </include>
+
+    <node name="rqt_console" pkg="rqt_console" type="rqt_console" />
+    <!-- <node name="rqt_logger_level" pkg="rqt_logger_level" type="rqt_logger_level" /> -->
+
+    <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model panda" />
+
+    <!-- Load joint controller configurations from YAML file to parameter server -->
+    <rosparam file="$(find panda_simulation)/config/panda_control.yaml" command="load" />
+
+    <!-- load the controllers -->
+    <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller panda_arm_controller" />
+    <node if="$(arg load_gripper)" name="controller_spawner_hand" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="panda_hand_controller" />
+
+
+    <!-- convert joint states to TF transforms for rviz, etc -->
+    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
+
+    <include file="$(find panda_moveit_config)/launch/planning_context.launch">
+        <arg name="load_robot_description" value="true" />
+        <arg name="load_gripper" value="$(arg load_gripper)" />
+    </include>
+    <include file="$(find panda_moveit_config)/launch/move_group.launch">
+        <arg name="load_gripper" value="$(arg load_gripper)" />
+    </include>
+    <group if="$(arg gui)">
+        <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
+    </group>
+
+    <node name="joint_state_desired_publisher" pkg="topic_tools" type="relay" args="joint_states joint_states_desired" />
+
+    <!-- launch robot control node for moveit motion planning -->
+    <node pkg="panda_simulation" type="robot_control_node" name="robot_control_node" output="screen" />
+
+    <!-- load (not start!) custom joint position controller -->
+    <node pkg="controller_manager" type="spawner" name="joint_position_launcher" args="--stopped joint_position_controller" />
+
+    <!-- run custom node for automatic intialization -->
+    <node pkg="panda_simulation" type="robot_state_initializer_node" name="robot_state_initializer_node" />
+
+    <node name="TimedCartesianPlanner" pkg="panda_simulation" type="TimedCartesianPlanner" respawn="false" output="screen"/>
+    <node name="MqttToRosNode" pkg="panda_simulation" type="MqttToRosNode" respawn="false" output="screen"/>
+    <node name="MqttRosConnectionTestNode" pkg="panda_simulation" type="MqttRosConnectionTestNode" respawn="false" output="screen"/>
+    <node name="RobotStateProvider" pkg="panda_simulation" type="RobotStateProvider" respawn="false" output="screen"/>
+
+</launch>
diff --git a/src/MqttRosConnectionTestNode.cpp b/src/MqttRosConnectionTestNode.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..78134d9cd2f079be7e1b753d0e22db19b3b2c839
--- /dev/null
+++ b/src/MqttRosConnectionTestNode.cpp
@@ -0,0 +1,167 @@
+//
+// Created by sebastian on 31.03.20.
+//
+#include "ros/ros.h"
+#include <gazebo_msgs/LinkStates.h>
+#include <mqtt/client.h>
+#include "string.h"
+#include "robotconfig.pb.h"
+#include "dataconfig.pb.h"
+#include "mem_persistence.cpp"
+
+using namespace std;
+using namespace std::chrono;
+
+/*
+ * mqtt-topics for all links
+ * ground, link0, link1, link2, link3, link4, link5, link6, link7 (end-effector), left finger, right finder
+ */
+std::string topics[11] = {"panda_link_0", "panda_link_1", "panda_link_2", "panda_link_3",
+                          "panda_link_4", "panda_link_5", "panda_link_6", "panda_link_7",
+                          "panda_link_8", "panda_link_9", "panda_link_10"};
+
+const string SERVER_ADDRESS	{ "tcp://localhost:1883" };
+const string CLIENT_ID		{ "ros_mqtt_tester" };
+
+bool mqttSetup = false;
+bool isInitialized = false;
+
+const int QOS = 1;
+
+mem_persistence persist;
+mqtt::client client("tcp://localhost:1883", "robot_state_provider_mqtt_test", &persist);
+
+void setupMqtt() {
+
+    if (!mqttSetup) {
+        std::cout << "MQTT: Initializing." << std::endl;
+
+        mqtt::connect_options connOpts;
+        connOpts.set_keep_alive_interval(20);
+        connOpts.set_clean_session(true);
+        std::cout << "MQTT: Initialized." << std::endl;
+
+        std::cout << "MQTT: Connecting." << std::endl;
+        client.connect(connOpts);
+        std::cout << "MQTT: Connected." << std::endl;
+        mqttSetup = true;
+    }
+}
+
+bool try_reconnect(mqtt::client& cli)
+{
+    constexpr int N_ATTEMPT = 30;
+
+    for (int i=0; i<N_ATTEMPT && !cli.is_connected(); ++i) {
+        try {
+            cli.reconnect();
+            return true;
+        }
+        catch (const mqtt::exception&) {
+            this_thread::sleep_for(seconds(1));
+        }
+    }
+    return false;
+}
+
+
+void testConfig(ros::NodeHandle n) {
+
+    std::cout << ">>>>>>>>>>>> STARTING TEST <<<<<<<<<<<<<" << std::endl;
+    setupMqtt();
+
+    config::DataConfig dc;
+
+    dc.set_enableposition(true);
+    dc.set_enableorientation(true);
+    dc.set_enabletwistangular(true);
+    dc.set_enabletwistlinear(true);
+    dc.set_publishrate(1000);
+
+    std::string mqtt_msg;
+    dc.SerializeToString(&mqtt_msg);
+
+    auto pubmsg = mqtt::make_message("dataconfig", mqtt_msg);
+    pubmsg->set_qos(QOS);
+    client.publish(pubmsg);
+
+}
+
+void receiveMqttMessages(ros::NodeHandle n)
+{
+    mqtt::connect_options connOpts;
+    mqtt::client cli(SERVER_ADDRESS, CLIENT_ID);
+
+    connOpts.set_keep_alive_interval(20);
+    connOpts.set_clean_session(true);
+    try {
+        ROS_INFO_STREAM("TEST Connecting to the MQTT server.");
+        mqtt::connect_response rsp = cli.connect(connOpts);
+        ROS_INFO_STREAM("TEST Connected to the MQTT server.");
+
+        if (!rsp.is_session_present()) {
+            ROS_INFO_STREAM("TEST Subscribing to topics.");
+            cli.subscribe("nodetest", 1);
+            ROS_INFO_STREAM("TEST Subscribed to topics.");
+        }else {
+            ROS_WARN_STREAM("TEST Session already present. Skipping subscribe.");
+        }
+
+        // Consume messages
+        while (true) {
+
+            auto msg = cli.consume_message();
+
+            if (!msg) {
+                if (!cli.is_connected()) {
+                    ROS_ERROR_STREAM("TEST Lost connection. Attempting reconnect");
+                    if (try_reconnect(cli)) {
+                        cli.subscribe("nodetest", 1);
+                        ROS_INFO_STREAM("TEST Reconnected");
+                        continue;
+                    }
+                    else {
+                        ROS_ERROR_STREAM("TEST Reconnect failed.");
+                    }
+                }
+                else {
+                    ROS_ERROR_STREAM("TEST An error occurred retrieving messages.");
+                }
+                break;
+            }
+
+            if (msg->get_topic() == "nodetest")
+            {
+                testConfig(n);
+            }
+        }
+    }
+    catch (const mqtt::exception& exc) {
+        ROS_ERROR_STREAM("TEST: " << exc.what());
+        cli.disconnect();
+        return;
+    }
+
+    cli.disconnect();
+}
+
+int main(int argc, char **argv) {
+    ros::init(argc, argv, "robot_state_provider_test");
+    ros::NodeHandle n;
+    n.getParam("ready", isInitialized);
+
+    while (ros::ok()) {
+
+        // make sure the robot is initialized
+        while (!isInitialized) {
+            n.getParam("tud_planner_ready", isInitialized);
+        }
+
+        receiveMqttMessages(n);
+        ros::spin();
+    }
+
+    // disconnect from mqtt
+    client.disconnect();
+    return 0;
+}
\ No newline at end of file
diff --git a/src/MqttToRosNode.cpp b/src/MqttToRosNode.cpp
index 3743875c9b871259f8ea67480a612704eb422994..ac714abdc4368ea21640e7e3114109be51266820 100644
--- a/src/MqttToRosNode.cpp
+++ b/src/MqttToRosNode.cpp
@@ -1,19 +1,20 @@
 #include <iostream>
-#include <cstdlib>
 #include <string>
 #include <cstring>
-#include <cctype>
 #include <thread>
 #include <chrono>
 #include "mqtt/client.h"
 
 #include "ros/ros.h"
 
+#include "robotconfig.pb.h"
+#include "dataconfig.pb.h"
+
 using namespace std;
 using namespace std::chrono;
 
 const string SERVER_ADDRESS	{ "tcp://localhost:1883" };
-const string CLIENT_ID		{ "sync_consume_cpp2" };
+const string CLIENT_ID		{ "ros_mqtt_consumer" };
 
 // --------------------------------------------------------------------------
 // Simple function to manually reconnect a client.
@@ -36,17 +37,29 @@ bool try_reconnect(mqtt::client& cli)
 
 /////////////////////////////////////////////////////////////////////////////
 
-void handleCommand(auto msg)
+void handleRobotConfig(config::RobotConfig robotConfig, ros::NodeHandle n)
 {
-    // TBD
+    std::cout << "Retrieved new target-speed: " << robotConfig.speed() << std::endl;
+    n.setParam("robot_speed_factor", robotConfig.speed());
 }
 
-void handleConfig(auto msg)
+void handleDataConfig(config::DataConfig dataConfig, ros::NodeHandle n)
 {
-    // TBD
+    std::cout << "Retrieved new data-config: -- publish-rate: " << dataConfig.publishrate()
+                                        << " -- enablePosition: " << dataConfig.enableposition()
+                                        << " -- enableOrientation: " << dataConfig.enableorientation()
+                                        << " -- enableTwistLinear: " << dataConfig.enabletwistlinear()
+                                        << " -- enableTwistAngular: " << dataConfig.has_enabletwistangular()
+                                        << std::endl;
+
+    n.setParam("data_publish_rate", dataConfig.publishrate());
+    n.setParam("data_enable_position", dataConfig.enableposition());
+    n.setParam("data_enable_orientation", dataConfig.enableorientation());
+    n.setParam("data_enable_twist_linear", dataConfig.enabletwistlinear());
+    n.setParam("data_enable_twist_angular", dataConfig.has_enabletwistangular());
 }
 
-void receiveMqttMessages()
+void receiveMqttMessages(ros::NodeHandle n)
 {
     mqtt::connect_options connOpts;
     mqtt::client cli(SERVER_ADDRESS, CLIENT_ID);
@@ -54,7 +67,7 @@ void receiveMqttMessages()
     connOpts.set_keep_alive_interval(20);
     connOpts.set_clean_session(true);
 
-    const vector<string> TOPICS { "config", "command" };
+    const vector<string> TOPICS { "robotconfig", "dataconfig" };
     const vector<int> QOS { 0, 1 };
 
     try {
@@ -93,21 +106,27 @@ void receiveMqttMessages()
                 break;
             }
 
-            if (msg->get_topic() == "command")
+            if (msg->get_topic() == "robotconfig")
             {
-                handleCommand(msg);
+                const std::string rc_payload = msg->get_payload_str();
+                config::RobotConfig robotConfig;
+                robotConfig.ParseFromString(rc_payload);
+                handleRobotConfig(robotConfig, n);
             }
 
-            if (msg->get_topic() == "config")
+            if (msg->get_topic() == "dataconfig")
             {
-                handleConfig(msg);
+                const std::string dc_payload = msg->get_payload_str();
+                config::DataConfig dataConfig;
+                dataConfig.ParseFromString(dc_payload);
+                handleDataConfig(dataConfig, n);
             }
 
-            ROS_INFO_STREAM("NEW MESSAGE: " << msg->get_topic() << ": " << msg->to_string());
+            //ROS_INFO_STREAM("NEW MESSAGE: " << msg->get_topic() << ": " << msg->to_string());
         }
     }
     catch (const mqtt::exception& exc) {
-        ROS_ERROR_STREAM(exc.what());
+        ROS_ERROR_STREAM("MQTT2ROS " << exc.what());
         cli.disconnect();
         return;
     }
@@ -120,7 +139,7 @@ int main(int argc, char* argv[])
     ros::init(argc, argv, "mqtt_listener");
     ros::NodeHandle n;
 
-    receiveMqttMessages();
+    receiveMqttMessages(n);
 
     ros::spin();
     return 0;
diff --git a/src/RobotStateProvider.cpp b/src/RobotStateProvider.cpp
index cb46a3bfd64ffbda53b2ea4f4bb5897d597195a9..40a12c363742d7f6adfc5935cfc87a2079bcd6e6 100644
--- a/src/RobotStateProvider.cpp
+++ b/src/RobotStateProvider.cpp
@@ -8,30 +8,15 @@
 #include "linkstate.pb.h"
 #include "mem_persistence.cpp"
 
-// CONFIGURATION START
-// ^^^^^^^^^^^^^^^^^^^
-
-// configure the number of not redirected messages between redirected messages
-const int message_redirect_rate = 100;
-
-// values to export
-bool export_position = true;
-bool export_orientation = true;
-bool export_twist_linear = true;
-bool export_twist_angular = true;
-
 bool export_to_log = false;
 
-// CONFIGURATION END
-// ^^^^^^^^^^^^^^^^^
-
 /*
  * mqtt-topics for all links
  * ground, link0, link1, link2, link3, link4, link5, link6, link7 (end-effector), left finger, right finder
  */
-std::string topics [11] = {"panda_link_0","panda_link_1","panda_link_2" ,"panda_link_3",
-                           "panda_link_4","panda_link_5","panda_link_6","panda_link_7",
-                           "panda_link_8","panda_link_9","panda_link_10"};
+std::string topics[11] = {"panda_link_0", "panda_link_1", "panda_link_2", "panda_link_3",
+                          "panda_link_4", "panda_link_5", "panda_link_6", "panda_link_7",
+                          "panda_link_8", "panda_link_9", "panda_link_10"};
 
 bool mqttSetup = false;
 bool isInitialized = false;
@@ -46,6 +31,19 @@ mqtt::client client("tcp://localhost:1883", "robot_state_provider_mqtt", &persis
  * logs to its own file in /.ros/logs (configured in launch-file)
  */
 void exportMessageToLog(const gazebo_msgs::LinkStates &msg) {
+
+    bool export_position = false;
+    bool export_orientation = false;
+    bool export_twist_linear = false;
+    bool export_twist_angular = false;
+
+    ros::NodeHandle n;
+
+    n.getParam("data_enable_position", export_position);
+    n.getParam("data_enable_orientation", export_orientation);
+    n.getParam("data_enable_twist_linear", export_twist_linear);
+    n.getParam("data_enable_twist_angular", export_twist_angular);
+
     ROS_INFO_STREAM("<<< LINK NAMES >>>");
 
     for (int i = 0; i < msg.name.size(); i++) {
@@ -96,6 +94,21 @@ void exportMessageToLog(const gazebo_msgs::LinkStates &msg) {
 
 panda::PandaLinkState buildLinkStateMessage(const gazebo_msgs::LinkStates &msg, int link_number) {
 
+    bool export_position = false;
+    bool export_orientation = false;
+    bool export_twist_linear = false;
+    bool export_twist_angular = false;
+
+    ros::NodeHandle n;
+
+    n.getParam("data_enable_position", export_position);
+    n.getParam("data_enable_orientation", export_orientation);
+    n.getParam("data_enable_twist_linear", export_twist_linear);
+    n.getParam("data_enable_twist_angular", export_twist_angular);
+
+    //std::cout << "Building LinkStateMessage with pos: " << export_position << " ori: " << export_orientation << " twistlin: "
+    //       << export_twist_linear << " twistang: " << export_twist_angular << std::endl;
+
     panda::PandaLinkState pls_msg;
 
     pls_msg.set_name(msg.name.at(link_number));
@@ -139,16 +152,16 @@ panda::PandaLinkState buildLinkStateMessage(const gazebo_msgs::LinkStates &msg,
 void setupMqtt() {
 
     if (!mqttSetup) {
-        std::cout << "Initializing..." << std::endl;
+        std::cout << "MQTT: Initializing." << std::endl;
 
         mqtt::connect_options connOpts;
         connOpts.set_keep_alive_interval(20);
         connOpts.set_clean_session(true);
-        std::cout << "...OK" << std::endl;
+        std::cout << "MQTT: Initialized." << std::endl;
 
-        std::cout << "\nConnecting..." << std::endl;
+        std::cout << "MQTT: Connecting." << std::endl;
         client.connect(connOpts);
-        std::cout << "...OK" << std::endl;
+        std::cout << "MQTT: Connected." << std::endl;
         mqttSetup = true;
     }
 }
@@ -158,20 +171,33 @@ void sendMqttMessages(const gazebo_msgs::LinkStates &msg) {
 
     setupMqtt();
 
-    for(int i = 0; i < msg.name.size(); i++)
-    {
-        panda::PandaLinkState pls_msg = buildLinkStateMessage(msg, i);
-        std::string mqtt_msg;
-        pls_msg.SerializeToString(&mqtt_msg);
+    try {
+        for (int i = 0; i < msg.name.size(); i++) {
+            panda::PandaLinkState pls_msg = buildLinkStateMessage(msg, i);
+            std::string mqtt_msg;
+            pls_msg.SerializeToString(&mqtt_msg);
 
-        //std::cout << "SENDING MESSAGE" << std::endl;
-        auto pubmsg = mqtt::make_message(topics[i], mqtt_msg);
-        pubmsg->set_qos(QOS);
-        client.publish(pubmsg);
+            //std::cout << "SENDING MESSAGE TO " << topics[i] << std::endl;
+            auto pubmsg = mqtt::make_message(topics[i], mqtt_msg);
+            pubmsg->set_qos(QOS);
+            client.publish(pubmsg);
+        }
+    }
+    // happens at lib paho sometimes when multiple instances of an mqtt-client are publishing at the same time
+    catch (const mqtt::exception &exc) {
+        ROS_ERROR_STREAM("RSP: " << exc.what());
+        // causes automatic reconnect
+        mqttSetup = false;
+        return;
     }
 }
 
 void providerCallback(const gazebo_msgs::LinkStates &msg) {
+
+    ros::NodeHandle n;
+    int message_redirect_rate = 200;
+    n.getParam("data_publish_rate", message_redirect_rate);
+
     if (current_redirect != 0 && current_redirect != message_redirect_rate) {
         // std::cout << "Ignoring message (redirect: " << current_redirect << " )" << std::endl;
         current_redirect++;
@@ -179,9 +205,9 @@ void providerCallback(const gazebo_msgs::LinkStates &msg) {
     }
 
     if (current_redirect == 0 || current_redirect == message_redirect_rate) {
-        //std::cout << "Redirecting message:" << msg.pose.at(1).position << std::endl;
+        //std::cout << "Sending LinkStates with rate: " << message_redirect_rate << std::endl;
 
-        if(export_to_log){
+        if (export_to_log) {
             exportMessageToLog(msg);
         }
 
@@ -196,11 +222,12 @@ void providerCallback(const gazebo_msgs::LinkStates &msg) {
 }
 
 int main(int argc, char **argv) {
-    ros::init(argc, argv, "listener");
+    ros::init(argc, argv, "robot_state_provider");
     ros::NodeHandle n;
     n.getParam("ready", isInitialized);
 
     while (ros::ok()) {
+
         // make sure the robot is initialized
         while (!isInitialized) {
             //std::cout << "Waiting" << std::endl;
diff --git a/src/dataconfig.pb.cc b/src/dataconfig.pb.cc
new file mode 100644
index 0000000000000000000000000000000000000000..759e606c910b90f6fef3b455ab6949ee1904795c
--- /dev/null
+++ b/src/dataconfig.pb.cc
@@ -0,0 +1,673 @@
+// Generated by the protocol buffer compiler.  DO NOT EDIT!
+// source: dataconfig.proto
+
+#define INTERNAL_SUPPRESS_PROTOBUF_FIELD_DEPRECATION
+#include "dataconfig.pb.h"
+
+#include <algorithm>
+
+#include <google/protobuf/stubs/common.h>
+#include <google/protobuf/stubs/port.h>
+#include <google/protobuf/stubs/once.h>
+#include <google/protobuf/io/coded_stream.h>
+#include <google/protobuf/wire_format_lite_inl.h>
+#include <google/protobuf/descriptor.h>
+#include <google/protobuf/generated_message_reflection.h>
+#include <google/protobuf/reflection_ops.h>
+#include <google/protobuf/wire_format.h>
+// @@protoc_insertion_point(includes)
+
+namespace config {
+
+namespace {
+
+const ::google::protobuf::Descriptor* DataConfig_descriptor_ = NULL;
+const ::google::protobuf::internal::GeneratedMessageReflection*
+  DataConfig_reflection_ = NULL;
+
+}  // namespace
+
+
+void protobuf_AssignDesc_dataconfig_2eproto() GOOGLE_ATTRIBUTE_COLD;
+void protobuf_AssignDesc_dataconfig_2eproto() {
+  protobuf_AddDesc_dataconfig_2eproto();
+  const ::google::protobuf::FileDescriptor* file =
+    ::google::protobuf::DescriptorPool::generated_pool()->FindFileByName(
+      "dataconfig.proto");
+  GOOGLE_CHECK(file != NULL);
+  DataConfig_descriptor_ = file->message_type(0);
+  static const int DataConfig_offsets_[5] = {
+    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(DataConfig, enableposition_),
+    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(DataConfig, enableorientation_),
+    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(DataConfig, enabletwistlinear_),
+    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(DataConfig, enabletwistangular_),
+    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(DataConfig, publishrate_),
+  };
+  DataConfig_reflection_ =
+    ::google::protobuf::internal::GeneratedMessageReflection::NewGeneratedMessageReflection(
+      DataConfig_descriptor_,
+      DataConfig::default_instance_,
+      DataConfig_offsets_,
+      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(DataConfig, _has_bits_[0]),
+      -1,
+      -1,
+      sizeof(DataConfig),
+      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(DataConfig, _internal_metadata_),
+      -1);
+}
+
+namespace {
+
+GOOGLE_PROTOBUF_DECLARE_ONCE(protobuf_AssignDescriptors_once_);
+inline void protobuf_AssignDescriptorsOnce() {
+  ::google::protobuf::GoogleOnceInit(&protobuf_AssignDescriptors_once_,
+                 &protobuf_AssignDesc_dataconfig_2eproto);
+}
+
+void protobuf_RegisterTypes(const ::std::string&) GOOGLE_ATTRIBUTE_COLD;
+void protobuf_RegisterTypes(const ::std::string&) {
+  protobuf_AssignDescriptorsOnce();
+  ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
+      DataConfig_descriptor_, &DataConfig::default_instance());
+}
+
+}  // namespace
+
+void protobuf_ShutdownFile_dataconfig_2eproto() {
+  delete DataConfig::default_instance_;
+  delete DataConfig_reflection_;
+}
+
+void protobuf_AddDesc_dataconfig_2eproto() GOOGLE_ATTRIBUTE_COLD;
+void protobuf_AddDesc_dataconfig_2eproto() {
+  static bool already_here = false;
+  if (already_here) return;
+  already_here = true;
+  GOOGLE_PROTOBUF_VERIFY_VERSION;
+
+  ::google::protobuf::DescriptorPool::InternalAddGeneratedFile(
+    "\n\020dataconfig.proto\022\006config\"\213\001\n\nDataConfi"
+    "g\022\026\n\016enablePosition\030\001 \002(\010\022\031\n\021enableOrien"
+    "tation\030\002 \002(\010\022\031\n\021enableTwistLinear\030\003 \002(\010\022"
+    "\032\n\022enableTwistAngular\030\004 \002(\010\022\023\n\013publishRa"
+    "te\030\005 \002(\005", 168);
+  ::google::protobuf::MessageFactory::InternalRegisterGeneratedFile(
+    "dataconfig.proto", &protobuf_RegisterTypes);
+  DataConfig::default_instance_ = new DataConfig();
+  DataConfig::default_instance_->InitAsDefaultInstance();
+  ::google::protobuf::internal::OnShutdown(&protobuf_ShutdownFile_dataconfig_2eproto);
+}
+
+// Force AddDescriptors() to be called at static initialization time.
+struct StaticDescriptorInitializer_dataconfig_2eproto {
+  StaticDescriptorInitializer_dataconfig_2eproto() {
+    protobuf_AddDesc_dataconfig_2eproto();
+  }
+} static_descriptor_initializer_dataconfig_2eproto_;
+
+// ===================================================================
+
+#if !defined(_MSC_VER) || _MSC_VER >= 1900
+const int DataConfig::kEnablePositionFieldNumber;
+const int DataConfig::kEnableOrientationFieldNumber;
+const int DataConfig::kEnableTwistLinearFieldNumber;
+const int DataConfig::kEnableTwistAngularFieldNumber;
+const int DataConfig::kPublishRateFieldNumber;
+#endif  // !defined(_MSC_VER) || _MSC_VER >= 1900
+
+DataConfig::DataConfig()
+  : ::google::protobuf::Message(), _internal_metadata_(NULL) {
+  SharedCtor();
+  // @@protoc_insertion_point(constructor:config.DataConfig)
+}
+
+void DataConfig::InitAsDefaultInstance() {
+}
+
+DataConfig::DataConfig(const DataConfig& from)
+  : ::google::protobuf::Message(),
+    _internal_metadata_(NULL) {
+  SharedCtor();
+  MergeFrom(from);
+  // @@protoc_insertion_point(copy_constructor:config.DataConfig)
+}
+
+void DataConfig::SharedCtor() {
+  _cached_size_ = 0;
+  enableposition_ = false;
+  enableorientation_ = false;
+  enabletwistlinear_ = false;
+  enabletwistangular_ = false;
+  publishrate_ = 0;
+  ::memset(_has_bits_, 0, sizeof(_has_bits_));
+}
+
+DataConfig::~DataConfig() {
+  // @@protoc_insertion_point(destructor:config.DataConfig)
+  SharedDtor();
+}
+
+void DataConfig::SharedDtor() {
+  if (this != default_instance_) {
+  }
+}
+
+void DataConfig::SetCachedSize(int size) const {
+  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
+  _cached_size_ = size;
+  GOOGLE_SAFE_CONCURRENT_WRITES_END();
+}
+const ::google::protobuf::Descriptor* DataConfig::descriptor() {
+  protobuf_AssignDescriptorsOnce();
+  return DataConfig_descriptor_;
+}
+
+const DataConfig& DataConfig::default_instance() {
+  if (default_instance_ == NULL) protobuf_AddDesc_dataconfig_2eproto();
+  return *default_instance_;
+}
+
+DataConfig* DataConfig::default_instance_ = NULL;
+
+DataConfig* DataConfig::New(::google::protobuf::Arena* arena) const {
+  DataConfig* n = new DataConfig;
+  if (arena != NULL) {
+    arena->Own(n);
+  }
+  return n;
+}
+
+void DataConfig::Clear() {
+// @@protoc_insertion_point(message_clear_start:config.DataConfig)
+#if defined(__clang__)
+#define ZR_HELPER_(f) \
+  _Pragma("clang diagnostic push") \
+  _Pragma("clang diagnostic ignored \"-Winvalid-offsetof\"") \
+  __builtin_offsetof(DataConfig, f) \
+  _Pragma("clang diagnostic pop")
+#else
+#define ZR_HELPER_(f) reinterpret_cast<char*>(\
+  &reinterpret_cast<DataConfig*>(16)->f)
+#endif
+
+#define ZR_(first, last) do {\
+  ::memset(&first, 0,\
+           ZR_HELPER_(last) - ZR_HELPER_(first) + sizeof(last));\
+} while (0)
+
+  if (_has_bits_[0 / 32] & 31u) {
+    ZR_(enableposition_, publishrate_);
+  }
+
+#undef ZR_HELPER_
+#undef ZR_
+
+  ::memset(_has_bits_, 0, sizeof(_has_bits_));
+  if (_internal_metadata_.have_unknown_fields()) {
+    mutable_unknown_fields()->Clear();
+  }
+}
+
+bool DataConfig::MergePartialFromCodedStream(
+    ::google::protobuf::io::CodedInputStream* input) {
+#define DO_(EXPRESSION) if (!GOOGLE_PREDICT_TRUE(EXPRESSION)) goto failure
+  ::google::protobuf::uint32 tag;
+  // @@protoc_insertion_point(parse_start:config.DataConfig)
+  for (;;) {
+    ::std::pair< ::google::protobuf::uint32, bool> p = input->ReadTagWithCutoff(127);
+    tag = p.first;
+    if (!p.second) goto handle_unusual;
+    switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
+      // required bool enablePosition = 1;
+      case 1: {
+        if (tag == 8) {
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   bool, ::google::protobuf::internal::WireFormatLite::TYPE_BOOL>(
+                 input, &enableposition_)));
+          set_has_enableposition();
+        } else {
+          goto handle_unusual;
+        }
+        if (input->ExpectTag(16)) goto parse_enableOrientation;
+        break;
+      }
+
+      // required bool enableOrientation = 2;
+      case 2: {
+        if (tag == 16) {
+         parse_enableOrientation:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   bool, ::google::protobuf::internal::WireFormatLite::TYPE_BOOL>(
+                 input, &enableorientation_)));
+          set_has_enableorientation();
+        } else {
+          goto handle_unusual;
+        }
+        if (input->ExpectTag(24)) goto parse_enableTwistLinear;
+        break;
+      }
+
+      // required bool enableTwistLinear = 3;
+      case 3: {
+        if (tag == 24) {
+         parse_enableTwistLinear:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   bool, ::google::protobuf::internal::WireFormatLite::TYPE_BOOL>(
+                 input, &enabletwistlinear_)));
+          set_has_enabletwistlinear();
+        } else {
+          goto handle_unusual;
+        }
+        if (input->ExpectTag(32)) goto parse_enableTwistAngular;
+        break;
+      }
+
+      // required bool enableTwistAngular = 4;
+      case 4: {
+        if (tag == 32) {
+         parse_enableTwistAngular:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   bool, ::google::protobuf::internal::WireFormatLite::TYPE_BOOL>(
+                 input, &enabletwistangular_)));
+          set_has_enabletwistangular();
+        } else {
+          goto handle_unusual;
+        }
+        if (input->ExpectTag(40)) goto parse_publishRate;
+        break;
+      }
+
+      // required int32 publishRate = 5;
+      case 5: {
+        if (tag == 40) {
+         parse_publishRate:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
+                 input, &publishrate_)));
+          set_has_publishrate();
+        } else {
+          goto handle_unusual;
+        }
+        if (input->ExpectAtEnd()) goto success;
+        break;
+      }
+
+      default: {
+      handle_unusual:
+        if (tag == 0 ||
+            ::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) {
+          goto success;
+        }
+        DO_(::google::protobuf::internal::WireFormat::SkipField(
+              input, tag, mutable_unknown_fields()));
+        break;
+      }
+    }
+  }
+success:
+  // @@protoc_insertion_point(parse_success:config.DataConfig)
+  return true;
+failure:
+  // @@protoc_insertion_point(parse_failure:config.DataConfig)
+  return false;
+#undef DO_
+}
+
+void DataConfig::SerializeWithCachedSizes(
+    ::google::protobuf::io::CodedOutputStream* output) const {
+  // @@protoc_insertion_point(serialize_start:config.DataConfig)
+  // required bool enablePosition = 1;
+  if (has_enableposition()) {
+    ::google::protobuf::internal::WireFormatLite::WriteBool(1, this->enableposition(), output);
+  }
+
+  // required bool enableOrientation = 2;
+  if (has_enableorientation()) {
+    ::google::protobuf::internal::WireFormatLite::WriteBool(2, this->enableorientation(), output);
+  }
+
+  // required bool enableTwistLinear = 3;
+  if (has_enabletwistlinear()) {
+    ::google::protobuf::internal::WireFormatLite::WriteBool(3, this->enabletwistlinear(), output);
+  }
+
+  // required bool enableTwistAngular = 4;
+  if (has_enabletwistangular()) {
+    ::google::protobuf::internal::WireFormatLite::WriteBool(4, this->enabletwistangular(), output);
+  }
+
+  // required int32 publishRate = 5;
+  if (has_publishrate()) {
+    ::google::protobuf::internal::WireFormatLite::WriteInt32(5, this->publishrate(), output);
+  }
+
+  if (_internal_metadata_.have_unknown_fields()) {
+    ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
+        unknown_fields(), output);
+  }
+  // @@protoc_insertion_point(serialize_end:config.DataConfig)
+}
+
+::google::protobuf::uint8* DataConfig::InternalSerializeWithCachedSizesToArray(
+    bool deterministic, ::google::protobuf::uint8* target) const {
+  // @@protoc_insertion_point(serialize_to_array_start:config.DataConfig)
+  // required bool enablePosition = 1;
+  if (has_enableposition()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteBoolToArray(1, this->enableposition(), target);
+  }
+
+  // required bool enableOrientation = 2;
+  if (has_enableorientation()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteBoolToArray(2, this->enableorientation(), target);
+  }
+
+  // required bool enableTwistLinear = 3;
+  if (has_enabletwistlinear()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteBoolToArray(3, this->enabletwistlinear(), target);
+  }
+
+  // required bool enableTwistAngular = 4;
+  if (has_enabletwistangular()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteBoolToArray(4, this->enabletwistangular(), target);
+  }
+
+  // required int32 publishRate = 5;
+  if (has_publishrate()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(5, this->publishrate(), target);
+  }
+
+  if (_internal_metadata_.have_unknown_fields()) {
+    target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
+        unknown_fields(), target);
+  }
+  // @@protoc_insertion_point(serialize_to_array_end:config.DataConfig)
+  return target;
+}
+
+int DataConfig::RequiredFieldsByteSizeFallback() const {
+// @@protoc_insertion_point(required_fields_byte_size_fallback_start:config.DataConfig)
+  int total_size = 0;
+
+  if (has_enableposition()) {
+    // required bool enablePosition = 1;
+    total_size += 1 + 1;
+  }
+
+  if (has_enableorientation()) {
+    // required bool enableOrientation = 2;
+    total_size += 1 + 1;
+  }
+
+  if (has_enabletwistlinear()) {
+    // required bool enableTwistLinear = 3;
+    total_size += 1 + 1;
+  }
+
+  if (has_enabletwistangular()) {
+    // required bool enableTwistAngular = 4;
+    total_size += 1 + 1;
+  }
+
+  if (has_publishrate()) {
+    // required int32 publishRate = 5;
+    total_size += 1 +
+      ::google::protobuf::internal::WireFormatLite::Int32Size(
+        this->publishrate());
+  }
+
+  return total_size;
+}
+int DataConfig::ByteSize() const {
+// @@protoc_insertion_point(message_byte_size_start:config.DataConfig)
+  int total_size = 0;
+
+  if (((_has_bits_[0] & 0x0000001f) ^ 0x0000001f) == 0) {  // All required fields are present.
+    // required bool enablePosition = 1;
+    total_size += 1 + 1;
+
+    // required bool enableOrientation = 2;
+    total_size += 1 + 1;
+
+    // required bool enableTwistLinear = 3;
+    total_size += 1 + 1;
+
+    // required bool enableTwistAngular = 4;
+    total_size += 1 + 1;
+
+    // required int32 publishRate = 5;
+    total_size += 1 +
+      ::google::protobuf::internal::WireFormatLite::Int32Size(
+        this->publishrate());
+
+  } else {
+    total_size += RequiredFieldsByteSizeFallback();
+  }
+  if (_internal_metadata_.have_unknown_fields()) {
+    total_size +=
+      ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
+        unknown_fields());
+  }
+  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
+  _cached_size_ = total_size;
+  GOOGLE_SAFE_CONCURRENT_WRITES_END();
+  return total_size;
+}
+
+void DataConfig::MergeFrom(const ::google::protobuf::Message& from) {
+// @@protoc_insertion_point(generalized_merge_from_start:config.DataConfig)
+  if (GOOGLE_PREDICT_FALSE(&from == this)) {
+    ::google::protobuf::internal::MergeFromFail(__FILE__, __LINE__);
+  }
+  const DataConfig* source = 
+      ::google::protobuf::internal::DynamicCastToGenerated<const DataConfig>(
+          &from);
+  if (source == NULL) {
+  // @@protoc_insertion_point(generalized_merge_from_cast_fail:config.DataConfig)
+    ::google::protobuf::internal::ReflectionOps::Merge(from, this);
+  } else {
+  // @@protoc_insertion_point(generalized_merge_from_cast_success:config.DataConfig)
+    MergeFrom(*source);
+  }
+}
+
+void DataConfig::MergeFrom(const DataConfig& from) {
+// @@protoc_insertion_point(class_specific_merge_from_start:config.DataConfig)
+  if (GOOGLE_PREDICT_FALSE(&from == this)) {
+    ::google::protobuf::internal::MergeFromFail(__FILE__, __LINE__);
+  }
+  if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
+    if (from.has_enableposition()) {
+      set_enableposition(from.enableposition());
+    }
+    if (from.has_enableorientation()) {
+      set_enableorientation(from.enableorientation());
+    }
+    if (from.has_enabletwistlinear()) {
+      set_enabletwistlinear(from.enabletwistlinear());
+    }
+    if (from.has_enabletwistangular()) {
+      set_enabletwistangular(from.enabletwistangular());
+    }
+    if (from.has_publishrate()) {
+      set_publishrate(from.publishrate());
+    }
+  }
+  if (from._internal_metadata_.have_unknown_fields()) {
+    mutable_unknown_fields()->MergeFrom(from.unknown_fields());
+  }
+}
+
+void DataConfig::CopyFrom(const ::google::protobuf::Message& from) {
+// @@protoc_insertion_point(generalized_copy_from_start:config.DataConfig)
+  if (&from == this) return;
+  Clear();
+  MergeFrom(from);
+}
+
+void DataConfig::CopyFrom(const DataConfig& from) {
+// @@protoc_insertion_point(class_specific_copy_from_start:config.DataConfig)
+  if (&from == this) return;
+  Clear();
+  MergeFrom(from);
+}
+
+bool DataConfig::IsInitialized() const {
+  if ((_has_bits_[0] & 0x0000001f) != 0x0000001f) return false;
+
+  return true;
+}
+
+void DataConfig::Swap(DataConfig* other) {
+  if (other == this) return;
+  InternalSwap(other);
+}
+void DataConfig::InternalSwap(DataConfig* other) {
+  std::swap(enableposition_, other->enableposition_);
+  std::swap(enableorientation_, other->enableorientation_);
+  std::swap(enabletwistlinear_, other->enabletwistlinear_);
+  std::swap(enabletwistangular_, other->enabletwistangular_);
+  std::swap(publishrate_, other->publishrate_);
+  std::swap(_has_bits_[0], other->_has_bits_[0]);
+  _internal_metadata_.Swap(&other->_internal_metadata_);
+  std::swap(_cached_size_, other->_cached_size_);
+}
+
+::google::protobuf::Metadata DataConfig::GetMetadata() const {
+  protobuf_AssignDescriptorsOnce();
+  ::google::protobuf::Metadata metadata;
+  metadata.descriptor = DataConfig_descriptor_;
+  metadata.reflection = DataConfig_reflection_;
+  return metadata;
+}
+
+#if PROTOBUF_INLINE_NOT_IN_HEADERS
+// DataConfig
+
+// required bool enablePosition = 1;
+bool DataConfig::has_enableposition() const {
+  return (_has_bits_[0] & 0x00000001u) != 0;
+}
+void DataConfig::set_has_enableposition() {
+  _has_bits_[0] |= 0x00000001u;
+}
+void DataConfig::clear_has_enableposition() {
+  _has_bits_[0] &= ~0x00000001u;
+}
+void DataConfig::clear_enableposition() {
+  enableposition_ = false;
+  clear_has_enableposition();
+}
+ bool DataConfig::enableposition() const {
+  // @@protoc_insertion_point(field_get:config.DataConfig.enablePosition)
+  return enableposition_;
+}
+ void DataConfig::set_enableposition(bool value) {
+  set_has_enableposition();
+  enableposition_ = value;
+  // @@protoc_insertion_point(field_set:config.DataConfig.enablePosition)
+}
+
+// required bool enableOrientation = 2;
+bool DataConfig::has_enableorientation() const {
+  return (_has_bits_[0] & 0x00000002u) != 0;
+}
+void DataConfig::set_has_enableorientation() {
+  _has_bits_[0] |= 0x00000002u;
+}
+void DataConfig::clear_has_enableorientation() {
+  _has_bits_[0] &= ~0x00000002u;
+}
+void DataConfig::clear_enableorientation() {
+  enableorientation_ = false;
+  clear_has_enableorientation();
+}
+ bool DataConfig::enableorientation() const {
+  // @@protoc_insertion_point(field_get:config.DataConfig.enableOrientation)
+  return enableorientation_;
+}
+ void DataConfig::set_enableorientation(bool value) {
+  set_has_enableorientation();
+  enableorientation_ = value;
+  // @@protoc_insertion_point(field_set:config.DataConfig.enableOrientation)
+}
+
+// required bool enableTwistLinear = 3;
+bool DataConfig::has_enabletwistlinear() const {
+  return (_has_bits_[0] & 0x00000004u) != 0;
+}
+void DataConfig::set_has_enabletwistlinear() {
+  _has_bits_[0] |= 0x00000004u;
+}
+void DataConfig::clear_has_enabletwistlinear() {
+  _has_bits_[0] &= ~0x00000004u;
+}
+void DataConfig::clear_enabletwistlinear() {
+  enabletwistlinear_ = false;
+  clear_has_enabletwistlinear();
+}
+ bool DataConfig::enabletwistlinear() const {
+  // @@protoc_insertion_point(field_get:config.DataConfig.enableTwistLinear)
+  return enabletwistlinear_;
+}
+ void DataConfig::set_enabletwistlinear(bool value) {
+  set_has_enabletwistlinear();
+  enabletwistlinear_ = value;
+  // @@protoc_insertion_point(field_set:config.DataConfig.enableTwistLinear)
+}
+
+// required bool enableTwistAngular = 4;
+bool DataConfig::has_enabletwistangular() const {
+  return (_has_bits_[0] & 0x00000008u) != 0;
+}
+void DataConfig::set_has_enabletwistangular() {
+  _has_bits_[0] |= 0x00000008u;
+}
+void DataConfig::clear_has_enabletwistangular() {
+  _has_bits_[0] &= ~0x00000008u;
+}
+void DataConfig::clear_enabletwistangular() {
+  enabletwistangular_ = false;
+  clear_has_enabletwistangular();
+}
+ bool DataConfig::enabletwistangular() const {
+  // @@protoc_insertion_point(field_get:config.DataConfig.enableTwistAngular)
+  return enabletwistangular_;
+}
+ void DataConfig::set_enabletwistangular(bool value) {
+  set_has_enabletwistangular();
+  enabletwistangular_ = value;
+  // @@protoc_insertion_point(field_set:config.DataConfig.enableTwistAngular)
+}
+
+// required int32 publishRate = 5;
+bool DataConfig::has_publishrate() const {
+  return (_has_bits_[0] & 0x00000010u) != 0;
+}
+void DataConfig::set_has_publishrate() {
+  _has_bits_[0] |= 0x00000010u;
+}
+void DataConfig::clear_has_publishrate() {
+  _has_bits_[0] &= ~0x00000010u;
+}
+void DataConfig::clear_publishrate() {
+  publishrate_ = 0;
+  clear_has_publishrate();
+}
+ ::google::protobuf::int32 DataConfig::publishrate() const {
+  // @@protoc_insertion_point(field_get:config.DataConfig.publishRate)
+  return publishrate_;
+}
+ void DataConfig::set_publishrate(::google::protobuf::int32 value) {
+  set_has_publishrate();
+  publishrate_ = value;
+  // @@protoc_insertion_point(field_set:config.DataConfig.publishRate)
+}
+
+#endif  // PROTOBUF_INLINE_NOT_IN_HEADERS
+
+// @@protoc_insertion_point(namespace_scope)
+
+}  // namespace config
+
+// @@protoc_insertion_point(global_scope)
diff --git a/src/dataconfig.pb.h b/src/dataconfig.pb.h
new file mode 100644
index 0000000000000000000000000000000000000000..3b5a05c344cc376a2598a1ee02b4e995a1a137dc
--- /dev/null
+++ b/src/dataconfig.pb.h
@@ -0,0 +1,313 @@
+// Generated by the protocol buffer compiler.  DO NOT EDIT!
+// source: dataconfig.proto
+
+#ifndef PROTOBUF_dataconfig_2eproto__INCLUDED
+#define PROTOBUF_dataconfig_2eproto__INCLUDED
+
+#include <string>
+
+#include <google/protobuf/stubs/common.h>
+
+#if GOOGLE_PROTOBUF_VERSION < 3000000
+#error This file was generated by a newer version of protoc which is
+#error incompatible with your Protocol Buffer headers.  Please update
+#error your headers.
+#endif
+#if 3000000 < GOOGLE_PROTOBUF_MIN_PROTOC_VERSION
+#error This file was generated by an older version of protoc which is
+#error incompatible with your Protocol Buffer headers.  Please
+#error regenerate this file with a newer version of protoc.
+#endif
+
+#include <google/protobuf/arena.h>
+#include <google/protobuf/arenastring.h>
+#include <google/protobuf/generated_message_util.h>
+#include <google/protobuf/metadata.h>
+#include <google/protobuf/message.h>
+#include <google/protobuf/repeated_field.h>
+#include <google/protobuf/extension_set.h>
+#include <google/protobuf/unknown_field_set.h>
+// @@protoc_insertion_point(includes)
+
+namespace config {
+
+// Internal implementation detail -- do not call these.
+void protobuf_AddDesc_dataconfig_2eproto();
+void protobuf_AssignDesc_dataconfig_2eproto();
+void protobuf_ShutdownFile_dataconfig_2eproto();
+
+class DataConfig;
+
+// ===================================================================
+
+class DataConfig : public ::google::protobuf::Message /* @@protoc_insertion_point(class_definition:config.DataConfig) */ {
+ public:
+  DataConfig();
+  virtual ~DataConfig();
+
+  DataConfig(const DataConfig& from);
+
+  inline DataConfig& operator=(const DataConfig& from) {
+    CopyFrom(from);
+    return *this;
+  }
+
+  inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
+    return _internal_metadata_.unknown_fields();
+  }
+
+  inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
+    return _internal_metadata_.mutable_unknown_fields();
+  }
+
+  static const ::google::protobuf::Descriptor* descriptor();
+  static const DataConfig& default_instance();
+
+  void Swap(DataConfig* other);
+
+  // implements Message ----------------------------------------------
+
+  inline DataConfig* New() const { return New(NULL); }
+
+  DataConfig* New(::google::protobuf::Arena* arena) const;
+  void CopyFrom(const ::google::protobuf::Message& from);
+  void MergeFrom(const ::google::protobuf::Message& from);
+  void CopyFrom(const DataConfig& from);
+  void MergeFrom(const DataConfig& from);
+  void Clear();
+  bool IsInitialized() const;
+
+  int ByteSize() const;
+  bool MergePartialFromCodedStream(
+      ::google::protobuf::io::CodedInputStream* input);
+  void SerializeWithCachedSizes(
+      ::google::protobuf::io::CodedOutputStream* output) const;
+  ::google::protobuf::uint8* InternalSerializeWithCachedSizesToArray(
+      bool deterministic, ::google::protobuf::uint8* output) const;
+  ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const {
+    return InternalSerializeWithCachedSizesToArray(false, output);
+  }
+  int GetCachedSize() const { return _cached_size_; }
+  private:
+  void SharedCtor();
+  void SharedDtor();
+  void SetCachedSize(int size) const;
+  void InternalSwap(DataConfig* other);
+  private:
+  inline ::google::protobuf::Arena* GetArenaNoVirtual() const {
+    return _internal_metadata_.arena();
+  }
+  inline void* MaybeArenaPtr() const {
+    return _internal_metadata_.raw_arena_ptr();
+  }
+  public:
+
+  ::google::protobuf::Metadata GetMetadata() const;
+
+  // nested types ----------------------------------------------------
+
+  // accessors -------------------------------------------------------
+
+  // required bool enablePosition = 1;
+  bool has_enableposition() const;
+  void clear_enableposition();
+  static const int kEnablePositionFieldNumber = 1;
+  bool enableposition() const;
+  void set_enableposition(bool value);
+
+  // required bool enableOrientation = 2;
+  bool has_enableorientation() const;
+  void clear_enableorientation();
+  static const int kEnableOrientationFieldNumber = 2;
+  bool enableorientation() const;
+  void set_enableorientation(bool value);
+
+  // required bool enableTwistLinear = 3;
+  bool has_enabletwistlinear() const;
+  void clear_enabletwistlinear();
+  static const int kEnableTwistLinearFieldNumber = 3;
+  bool enabletwistlinear() const;
+  void set_enabletwistlinear(bool value);
+
+  // required bool enableTwistAngular = 4;
+  bool has_enabletwistangular() const;
+  void clear_enabletwistangular();
+  static const int kEnableTwistAngularFieldNumber = 4;
+  bool enabletwistangular() const;
+  void set_enabletwistangular(bool value);
+
+  // required int32 publishRate = 5;
+  bool has_publishrate() const;
+  void clear_publishrate();
+  static const int kPublishRateFieldNumber = 5;
+  ::google::protobuf::int32 publishrate() const;
+  void set_publishrate(::google::protobuf::int32 value);
+
+  // @@protoc_insertion_point(class_scope:config.DataConfig)
+ private:
+  inline void set_has_enableposition();
+  inline void clear_has_enableposition();
+  inline void set_has_enableorientation();
+  inline void clear_has_enableorientation();
+  inline void set_has_enabletwistlinear();
+  inline void clear_has_enabletwistlinear();
+  inline void set_has_enabletwistangular();
+  inline void clear_has_enabletwistangular();
+  inline void set_has_publishrate();
+  inline void clear_has_publishrate();
+
+  // helper for ByteSize()
+  int RequiredFieldsByteSizeFallback() const;
+
+  ::google::protobuf::internal::InternalMetadataWithArena _internal_metadata_;
+  ::google::protobuf::uint32 _has_bits_[1];
+  mutable int _cached_size_;
+  bool enableposition_;
+  bool enableorientation_;
+  bool enabletwistlinear_;
+  bool enabletwistangular_;
+  ::google::protobuf::int32 publishrate_;
+  friend void  protobuf_AddDesc_dataconfig_2eproto();
+  friend void protobuf_AssignDesc_dataconfig_2eproto();
+  friend void protobuf_ShutdownFile_dataconfig_2eproto();
+
+  void InitAsDefaultInstance();
+  static DataConfig* default_instance_;
+};
+// ===================================================================
+
+
+// ===================================================================
+
+#if !PROTOBUF_INLINE_NOT_IN_HEADERS
+// DataConfig
+
+// required bool enablePosition = 1;
+inline bool DataConfig::has_enableposition() const {
+  return (_has_bits_[0] & 0x00000001u) != 0;
+}
+inline void DataConfig::set_has_enableposition() {
+  _has_bits_[0] |= 0x00000001u;
+}
+inline void DataConfig::clear_has_enableposition() {
+  _has_bits_[0] &= ~0x00000001u;
+}
+inline void DataConfig::clear_enableposition() {
+  enableposition_ = false;
+  clear_has_enableposition();
+}
+inline bool DataConfig::enableposition() const {
+  // @@protoc_insertion_point(field_get:config.DataConfig.enablePosition)
+  return enableposition_;
+}
+inline void DataConfig::set_enableposition(bool value) {
+  set_has_enableposition();
+  enableposition_ = value;
+  // @@protoc_insertion_point(field_set:config.DataConfig.enablePosition)
+}
+
+// required bool enableOrientation = 2;
+inline bool DataConfig::has_enableorientation() const {
+  return (_has_bits_[0] & 0x00000002u) != 0;
+}
+inline void DataConfig::set_has_enableorientation() {
+  _has_bits_[0] |= 0x00000002u;
+}
+inline void DataConfig::clear_has_enableorientation() {
+  _has_bits_[0] &= ~0x00000002u;
+}
+inline void DataConfig::clear_enableorientation() {
+  enableorientation_ = false;
+  clear_has_enableorientation();
+}
+inline bool DataConfig::enableorientation() const {
+  // @@protoc_insertion_point(field_get:config.DataConfig.enableOrientation)
+  return enableorientation_;
+}
+inline void DataConfig::set_enableorientation(bool value) {
+  set_has_enableorientation();
+  enableorientation_ = value;
+  // @@protoc_insertion_point(field_set:config.DataConfig.enableOrientation)
+}
+
+// required bool enableTwistLinear = 3;
+inline bool DataConfig::has_enabletwistlinear() const {
+  return (_has_bits_[0] & 0x00000004u) != 0;
+}
+inline void DataConfig::set_has_enabletwistlinear() {
+  _has_bits_[0] |= 0x00000004u;
+}
+inline void DataConfig::clear_has_enabletwistlinear() {
+  _has_bits_[0] &= ~0x00000004u;
+}
+inline void DataConfig::clear_enabletwistlinear() {
+  enabletwistlinear_ = false;
+  clear_has_enabletwistlinear();
+}
+inline bool DataConfig::enabletwistlinear() const {
+  // @@protoc_insertion_point(field_get:config.DataConfig.enableTwistLinear)
+  return enabletwistlinear_;
+}
+inline void DataConfig::set_enabletwistlinear(bool value) {
+  set_has_enabletwistlinear();
+  enabletwistlinear_ = value;
+  // @@protoc_insertion_point(field_set:config.DataConfig.enableTwistLinear)
+}
+
+// required bool enableTwistAngular = 4;
+inline bool DataConfig::has_enabletwistangular() const {
+  return (_has_bits_[0] & 0x00000008u) != 0;
+}
+inline void DataConfig::set_has_enabletwistangular() {
+  _has_bits_[0] |= 0x00000008u;
+}
+inline void DataConfig::clear_has_enabletwistangular() {
+  _has_bits_[0] &= ~0x00000008u;
+}
+inline void DataConfig::clear_enabletwistangular() {
+  enabletwistangular_ = false;
+  clear_has_enabletwistangular();
+}
+inline bool DataConfig::enabletwistangular() const {
+  // @@protoc_insertion_point(field_get:config.DataConfig.enableTwistAngular)
+  return enabletwistangular_;
+}
+inline void DataConfig::set_enabletwistangular(bool value) {
+  set_has_enabletwistangular();
+  enabletwistangular_ = value;
+  // @@protoc_insertion_point(field_set:config.DataConfig.enableTwistAngular)
+}
+
+// required int32 publishRate = 5;
+inline bool DataConfig::has_publishrate() const {
+  return (_has_bits_[0] & 0x00000010u) != 0;
+}
+inline void DataConfig::set_has_publishrate() {
+  _has_bits_[0] |= 0x00000010u;
+}
+inline void DataConfig::clear_has_publishrate() {
+  _has_bits_[0] &= ~0x00000010u;
+}
+inline void DataConfig::clear_publishrate() {
+  publishrate_ = 0;
+  clear_has_publishrate();
+}
+inline ::google::protobuf::int32 DataConfig::publishrate() const {
+  // @@protoc_insertion_point(field_get:config.DataConfig.publishRate)
+  return publishrate_;
+}
+inline void DataConfig::set_publishrate(::google::protobuf::int32 value) {
+  set_has_publishrate();
+  publishrate_ = value;
+  // @@protoc_insertion_point(field_set:config.DataConfig.publishRate)
+}
+
+#endif  // !PROTOBUF_INLINE_NOT_IN_HEADERS
+
+// @@protoc_insertion_point(namespace_scope)
+
+}  // namespace config
+
+// @@protoc_insertion_point(global_scope)
+
+#endif  // PROTOBUF_dataconfig_2eproto__INCLUDED
diff --git a/src/robotconfig.pb.cc b/src/robotconfig.pb.cc
new file mode 100644
index 0000000000000000000000000000000000000000..5471f097f90e228b06101149825bd04ed3bd73e0
--- /dev/null
+++ b/src/robotconfig.pb.cc
@@ -0,0 +1,374 @@
+// Generated by the protocol buffer compiler.  DO NOT EDIT!
+// source: robotconfig.proto
+
+#define INTERNAL_SUPPRESS_PROTOBUF_FIELD_DEPRECATION
+#include "robotconfig.pb.h"
+
+#include <algorithm>
+
+#include <google/protobuf/stubs/common.h>
+#include <google/protobuf/stubs/port.h>
+#include <google/protobuf/stubs/once.h>
+#include <google/protobuf/io/coded_stream.h>
+#include <google/protobuf/wire_format_lite_inl.h>
+#include <google/protobuf/descriptor.h>
+#include <google/protobuf/generated_message_reflection.h>
+#include <google/protobuf/reflection_ops.h>
+#include <google/protobuf/wire_format.h>
+// @@protoc_insertion_point(includes)
+
+namespace config {
+
+namespace {
+
+const ::google::protobuf::Descriptor* RobotConfig_descriptor_ = NULL;
+const ::google::protobuf::internal::GeneratedMessageReflection*
+  RobotConfig_reflection_ = NULL;
+
+}  // namespace
+
+
+void protobuf_AssignDesc_robotconfig_2eproto() GOOGLE_ATTRIBUTE_COLD;
+void protobuf_AssignDesc_robotconfig_2eproto() {
+  protobuf_AddDesc_robotconfig_2eproto();
+  const ::google::protobuf::FileDescriptor* file =
+    ::google::protobuf::DescriptorPool::generated_pool()->FindFileByName(
+      "robotconfig.proto");
+  GOOGLE_CHECK(file != NULL);
+  RobotConfig_descriptor_ = file->message_type(0);
+  static const int RobotConfig_offsets_[1] = {
+    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RobotConfig, speed_),
+  };
+  RobotConfig_reflection_ =
+    ::google::protobuf::internal::GeneratedMessageReflection::NewGeneratedMessageReflection(
+      RobotConfig_descriptor_,
+      RobotConfig::default_instance_,
+      RobotConfig_offsets_,
+      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RobotConfig, _has_bits_[0]),
+      -1,
+      -1,
+      sizeof(RobotConfig),
+      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RobotConfig, _internal_metadata_),
+      -1);
+}
+
+namespace {
+
+GOOGLE_PROTOBUF_DECLARE_ONCE(protobuf_AssignDescriptors_once_);
+inline void protobuf_AssignDescriptorsOnce() {
+  ::google::protobuf::GoogleOnceInit(&protobuf_AssignDescriptors_once_,
+                 &protobuf_AssignDesc_robotconfig_2eproto);
+}
+
+void protobuf_RegisterTypes(const ::std::string&) GOOGLE_ATTRIBUTE_COLD;
+void protobuf_RegisterTypes(const ::std::string&) {
+  protobuf_AssignDescriptorsOnce();
+  ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
+      RobotConfig_descriptor_, &RobotConfig::default_instance());
+}
+
+}  // namespace
+
+void protobuf_ShutdownFile_robotconfig_2eproto() {
+  delete RobotConfig::default_instance_;
+  delete RobotConfig_reflection_;
+}
+
+void protobuf_AddDesc_robotconfig_2eproto() GOOGLE_ATTRIBUTE_COLD;
+void protobuf_AddDesc_robotconfig_2eproto() {
+  static bool already_here = false;
+  if (already_here) return;
+  already_here = true;
+  GOOGLE_PROTOBUF_VERIFY_VERSION;
+
+  ::google::protobuf::DescriptorPool::InternalAddGeneratedFile(
+    "\n\021robotconfig.proto\022\006config\"\034\n\013RobotConf"
+    "ig\022\r\n\005speed\030\001 \002(\001", 57);
+  ::google::protobuf::MessageFactory::InternalRegisterGeneratedFile(
+    "robotconfig.proto", &protobuf_RegisterTypes);
+  RobotConfig::default_instance_ = new RobotConfig();
+  RobotConfig::default_instance_->InitAsDefaultInstance();
+  ::google::protobuf::internal::OnShutdown(&protobuf_ShutdownFile_robotconfig_2eproto);
+}
+
+// Force AddDescriptors() to be called at static initialization time.
+struct StaticDescriptorInitializer_robotconfig_2eproto {
+  StaticDescriptorInitializer_robotconfig_2eproto() {
+    protobuf_AddDesc_robotconfig_2eproto();
+  }
+} static_descriptor_initializer_robotconfig_2eproto_;
+
+// ===================================================================
+
+#if !defined(_MSC_VER) || _MSC_VER >= 1900
+const int RobotConfig::kSpeedFieldNumber;
+#endif  // !defined(_MSC_VER) || _MSC_VER >= 1900
+
+RobotConfig::RobotConfig()
+  : ::google::protobuf::Message(), _internal_metadata_(NULL) {
+  SharedCtor();
+  // @@protoc_insertion_point(constructor:config.RobotConfig)
+}
+
+void RobotConfig::InitAsDefaultInstance() {
+}
+
+RobotConfig::RobotConfig(const RobotConfig& from)
+  : ::google::protobuf::Message(),
+    _internal_metadata_(NULL) {
+  SharedCtor();
+  MergeFrom(from);
+  // @@protoc_insertion_point(copy_constructor:config.RobotConfig)
+}
+
+void RobotConfig::SharedCtor() {
+  _cached_size_ = 0;
+  speed_ = 0;
+  ::memset(_has_bits_, 0, sizeof(_has_bits_));
+}
+
+RobotConfig::~RobotConfig() {
+  // @@protoc_insertion_point(destructor:config.RobotConfig)
+  SharedDtor();
+}
+
+void RobotConfig::SharedDtor() {
+  if (this != default_instance_) {
+  }
+}
+
+void RobotConfig::SetCachedSize(int size) const {
+  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
+  _cached_size_ = size;
+  GOOGLE_SAFE_CONCURRENT_WRITES_END();
+}
+const ::google::protobuf::Descriptor* RobotConfig::descriptor() {
+  protobuf_AssignDescriptorsOnce();
+  return RobotConfig_descriptor_;
+}
+
+const RobotConfig& RobotConfig::default_instance() {
+  if (default_instance_ == NULL) protobuf_AddDesc_robotconfig_2eproto();
+  return *default_instance_;
+}
+
+RobotConfig* RobotConfig::default_instance_ = NULL;
+
+RobotConfig* RobotConfig::New(::google::protobuf::Arena* arena) const {
+  RobotConfig* n = new RobotConfig;
+  if (arena != NULL) {
+    arena->Own(n);
+  }
+  return n;
+}
+
+void RobotConfig::Clear() {
+// @@protoc_insertion_point(message_clear_start:config.RobotConfig)
+  speed_ = 0;
+  ::memset(_has_bits_, 0, sizeof(_has_bits_));
+  if (_internal_metadata_.have_unknown_fields()) {
+    mutable_unknown_fields()->Clear();
+  }
+}
+
+bool RobotConfig::MergePartialFromCodedStream(
+    ::google::protobuf::io::CodedInputStream* input) {
+#define DO_(EXPRESSION) if (!GOOGLE_PREDICT_TRUE(EXPRESSION)) goto failure
+  ::google::protobuf::uint32 tag;
+  // @@protoc_insertion_point(parse_start:config.RobotConfig)
+  for (;;) {
+    ::std::pair< ::google::protobuf::uint32, bool> p = input->ReadTagWithCutoff(127);
+    tag = p.first;
+    if (!p.second) goto handle_unusual;
+    switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
+      // required double speed = 1;
+      case 1: {
+        if (tag == 9) {
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
+                 input, &speed_)));
+          set_has_speed();
+        } else {
+          goto handle_unusual;
+        }
+        if (input->ExpectAtEnd()) goto success;
+        break;
+      }
+
+      default: {
+      handle_unusual:
+        if (tag == 0 ||
+            ::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) {
+          goto success;
+        }
+        DO_(::google::protobuf::internal::WireFormat::SkipField(
+              input, tag, mutable_unknown_fields()));
+        break;
+      }
+    }
+  }
+success:
+  // @@protoc_insertion_point(parse_success:config.RobotConfig)
+  return true;
+failure:
+  // @@protoc_insertion_point(parse_failure:config.RobotConfig)
+  return false;
+#undef DO_
+}
+
+void RobotConfig::SerializeWithCachedSizes(
+    ::google::protobuf::io::CodedOutputStream* output) const {
+  // @@protoc_insertion_point(serialize_start:config.RobotConfig)
+  // required double speed = 1;
+  if (has_speed()) {
+    ::google::protobuf::internal::WireFormatLite::WriteDouble(1, this->speed(), output);
+  }
+
+  if (_internal_metadata_.have_unknown_fields()) {
+    ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
+        unknown_fields(), output);
+  }
+  // @@protoc_insertion_point(serialize_end:config.RobotConfig)
+}
+
+::google::protobuf::uint8* RobotConfig::InternalSerializeWithCachedSizesToArray(
+    bool deterministic, ::google::protobuf::uint8* target) const {
+  // @@protoc_insertion_point(serialize_to_array_start:config.RobotConfig)
+  // required double speed = 1;
+  if (has_speed()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(1, this->speed(), target);
+  }
+
+  if (_internal_metadata_.have_unknown_fields()) {
+    target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
+        unknown_fields(), target);
+  }
+  // @@protoc_insertion_point(serialize_to_array_end:config.RobotConfig)
+  return target;
+}
+
+int RobotConfig::ByteSize() const {
+// @@protoc_insertion_point(message_byte_size_start:config.RobotConfig)
+  int total_size = 0;
+
+  // required double speed = 1;
+  if (has_speed()) {
+    total_size += 1 + 8;
+  }
+  if (_internal_metadata_.have_unknown_fields()) {
+    total_size +=
+      ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
+        unknown_fields());
+  }
+  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
+  _cached_size_ = total_size;
+  GOOGLE_SAFE_CONCURRENT_WRITES_END();
+  return total_size;
+}
+
+void RobotConfig::MergeFrom(const ::google::protobuf::Message& from) {
+// @@protoc_insertion_point(generalized_merge_from_start:config.RobotConfig)
+  if (GOOGLE_PREDICT_FALSE(&from == this)) {
+    ::google::protobuf::internal::MergeFromFail(__FILE__, __LINE__);
+  }
+  const RobotConfig* source = 
+      ::google::protobuf::internal::DynamicCastToGenerated<const RobotConfig>(
+          &from);
+  if (source == NULL) {
+  // @@protoc_insertion_point(generalized_merge_from_cast_fail:config.RobotConfig)
+    ::google::protobuf::internal::ReflectionOps::Merge(from, this);
+  } else {
+  // @@protoc_insertion_point(generalized_merge_from_cast_success:config.RobotConfig)
+    MergeFrom(*source);
+  }
+}
+
+void RobotConfig::MergeFrom(const RobotConfig& from) {
+// @@protoc_insertion_point(class_specific_merge_from_start:config.RobotConfig)
+  if (GOOGLE_PREDICT_FALSE(&from == this)) {
+    ::google::protobuf::internal::MergeFromFail(__FILE__, __LINE__);
+  }
+  if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
+    if (from.has_speed()) {
+      set_speed(from.speed());
+    }
+  }
+  if (from._internal_metadata_.have_unknown_fields()) {
+    mutable_unknown_fields()->MergeFrom(from.unknown_fields());
+  }
+}
+
+void RobotConfig::CopyFrom(const ::google::protobuf::Message& from) {
+// @@protoc_insertion_point(generalized_copy_from_start:config.RobotConfig)
+  if (&from == this) return;
+  Clear();
+  MergeFrom(from);
+}
+
+void RobotConfig::CopyFrom(const RobotConfig& from) {
+// @@protoc_insertion_point(class_specific_copy_from_start:config.RobotConfig)
+  if (&from == this) return;
+  Clear();
+  MergeFrom(from);
+}
+
+bool RobotConfig::IsInitialized() const {
+  if ((_has_bits_[0] & 0x00000001) != 0x00000001) return false;
+
+  return true;
+}
+
+void RobotConfig::Swap(RobotConfig* other) {
+  if (other == this) return;
+  InternalSwap(other);
+}
+void RobotConfig::InternalSwap(RobotConfig* other) {
+  std::swap(speed_, other->speed_);
+  std::swap(_has_bits_[0], other->_has_bits_[0]);
+  _internal_metadata_.Swap(&other->_internal_metadata_);
+  std::swap(_cached_size_, other->_cached_size_);
+}
+
+::google::protobuf::Metadata RobotConfig::GetMetadata() const {
+  protobuf_AssignDescriptorsOnce();
+  ::google::protobuf::Metadata metadata;
+  metadata.descriptor = RobotConfig_descriptor_;
+  metadata.reflection = RobotConfig_reflection_;
+  return metadata;
+}
+
+#if PROTOBUF_INLINE_NOT_IN_HEADERS
+// RobotConfig
+
+// required double speed = 1;
+bool RobotConfig::has_speed() const {
+  return (_has_bits_[0] & 0x00000001u) != 0;
+}
+void RobotConfig::set_has_speed() {
+  _has_bits_[0] |= 0x00000001u;
+}
+void RobotConfig::clear_has_speed() {
+  _has_bits_[0] &= ~0x00000001u;
+}
+void RobotConfig::clear_speed() {
+  speed_ = 0;
+  clear_has_speed();
+}
+ double RobotConfig::speed() const {
+  // @@protoc_insertion_point(field_get:config.RobotConfig.speed)
+  return speed_;
+}
+ void RobotConfig::set_speed(double value) {
+  set_has_speed();
+  speed_ = value;
+  // @@protoc_insertion_point(field_set:config.RobotConfig.speed)
+}
+
+#endif  // PROTOBUF_INLINE_NOT_IN_HEADERS
+
+// @@protoc_insertion_point(namespace_scope)
+
+}  // namespace config
+
+// @@protoc_insertion_point(global_scope)
diff --git a/src/robotconfig.pb.h b/src/robotconfig.pb.h
new file mode 100644
index 0000000000000000000000000000000000000000..8fec74078316ec27dd474d105ed44b5ca14b8d45
--- /dev/null
+++ b/src/robotconfig.pb.h
@@ -0,0 +1,174 @@
+// Generated by the protocol buffer compiler.  DO NOT EDIT!
+// source: robotconfig.proto
+
+#ifndef PROTOBUF_robotconfig_2eproto__INCLUDED
+#define PROTOBUF_robotconfig_2eproto__INCLUDED
+
+#include <string>
+
+#include <google/protobuf/stubs/common.h>
+
+#if GOOGLE_PROTOBUF_VERSION < 3000000
+#error This file was generated by a newer version of protoc which is
+#error incompatible with your Protocol Buffer headers.  Please update
+#error your headers.
+#endif
+#if 3000000 < GOOGLE_PROTOBUF_MIN_PROTOC_VERSION
+#error This file was generated by an older version of protoc which is
+#error incompatible with your Protocol Buffer headers.  Please
+#error regenerate this file with a newer version of protoc.
+#endif
+
+#include <google/protobuf/arena.h>
+#include <google/protobuf/arenastring.h>
+#include <google/protobuf/generated_message_util.h>
+#include <google/protobuf/metadata.h>
+#include <google/protobuf/message.h>
+#include <google/protobuf/repeated_field.h>
+#include <google/protobuf/extension_set.h>
+#include <google/protobuf/unknown_field_set.h>
+// @@protoc_insertion_point(includes)
+
+namespace config {
+
+// Internal implementation detail -- do not call these.
+void protobuf_AddDesc_robotconfig_2eproto();
+void protobuf_AssignDesc_robotconfig_2eproto();
+void protobuf_ShutdownFile_robotconfig_2eproto();
+
+class RobotConfig;
+
+// ===================================================================
+
+class RobotConfig : public ::google::protobuf::Message /* @@protoc_insertion_point(class_definition:config.RobotConfig) */ {
+ public:
+  RobotConfig();
+  virtual ~RobotConfig();
+
+  RobotConfig(const RobotConfig& from);
+
+  inline RobotConfig& operator=(const RobotConfig& from) {
+    CopyFrom(from);
+    return *this;
+  }
+
+  inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
+    return _internal_metadata_.unknown_fields();
+  }
+
+  inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
+    return _internal_metadata_.mutable_unknown_fields();
+  }
+
+  static const ::google::protobuf::Descriptor* descriptor();
+  static const RobotConfig& default_instance();
+
+  void Swap(RobotConfig* other);
+
+  // implements Message ----------------------------------------------
+
+  inline RobotConfig* New() const { return New(NULL); }
+
+  RobotConfig* New(::google::protobuf::Arena* arena) const;
+  void CopyFrom(const ::google::protobuf::Message& from);
+  void MergeFrom(const ::google::protobuf::Message& from);
+  void CopyFrom(const RobotConfig& from);
+  void MergeFrom(const RobotConfig& from);
+  void Clear();
+  bool IsInitialized() const;
+
+  int ByteSize() const;
+  bool MergePartialFromCodedStream(
+      ::google::protobuf::io::CodedInputStream* input);
+  void SerializeWithCachedSizes(
+      ::google::protobuf::io::CodedOutputStream* output) const;
+  ::google::protobuf::uint8* InternalSerializeWithCachedSizesToArray(
+      bool deterministic, ::google::protobuf::uint8* output) const;
+  ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const {
+    return InternalSerializeWithCachedSizesToArray(false, output);
+  }
+  int GetCachedSize() const { return _cached_size_; }
+  private:
+  void SharedCtor();
+  void SharedDtor();
+  void SetCachedSize(int size) const;
+  void InternalSwap(RobotConfig* other);
+  private:
+  inline ::google::protobuf::Arena* GetArenaNoVirtual() const {
+    return _internal_metadata_.arena();
+  }
+  inline void* MaybeArenaPtr() const {
+    return _internal_metadata_.raw_arena_ptr();
+  }
+  public:
+
+  ::google::protobuf::Metadata GetMetadata() const;
+
+  // nested types ----------------------------------------------------
+
+  // accessors -------------------------------------------------------
+
+  // required double speed = 1;
+  bool has_speed() const;
+  void clear_speed();
+  static const int kSpeedFieldNumber = 1;
+  double speed() const;
+  void set_speed(double value);
+
+  // @@protoc_insertion_point(class_scope:config.RobotConfig)
+ private:
+  inline void set_has_speed();
+  inline void clear_has_speed();
+
+  ::google::protobuf::internal::InternalMetadataWithArena _internal_metadata_;
+  ::google::protobuf::uint32 _has_bits_[1];
+  mutable int _cached_size_;
+  double speed_;
+  friend void  protobuf_AddDesc_robotconfig_2eproto();
+  friend void protobuf_AssignDesc_robotconfig_2eproto();
+  friend void protobuf_ShutdownFile_robotconfig_2eproto();
+
+  void InitAsDefaultInstance();
+  static RobotConfig* default_instance_;
+};
+// ===================================================================
+
+
+// ===================================================================
+
+#if !PROTOBUF_INLINE_NOT_IN_HEADERS
+// RobotConfig
+
+// required double speed = 1;
+inline bool RobotConfig::has_speed() const {
+  return (_has_bits_[0] & 0x00000001u) != 0;
+}
+inline void RobotConfig::set_has_speed() {
+  _has_bits_[0] |= 0x00000001u;
+}
+inline void RobotConfig::clear_has_speed() {
+  _has_bits_[0] &= ~0x00000001u;
+}
+inline void RobotConfig::clear_speed() {
+  speed_ = 0;
+  clear_has_speed();
+}
+inline double RobotConfig::speed() const {
+  // @@protoc_insertion_point(field_get:config.RobotConfig.speed)
+  return speed_;
+}
+inline void RobotConfig::set_speed(double value) {
+  set_has_speed();
+  speed_ = value;
+  // @@protoc_insertion_point(field_set:config.RobotConfig.speed)
+}
+
+#endif  // !PROTOBUF_INLINE_NOT_IN_HEADERS
+
+// @@protoc_insertion_point(namespace_scope)
+
+}  // namespace config
+
+// @@protoc_insertion_point(global_scope)
+
+#endif  // PROTOBUF_robotconfig_2eproto__INCLUDED