[Fawkes Git] branch/timn/robotino-direct: created (0.5.0-3090-gce0a8fc)

Tim Niemueller niemueller at kbsg.rwth-aachen.de
Fri Apr 8 18:14:09 CEST 2016


Changes have been pushed for the project "Fawkes Robotics Software Framework".

Gitweb: http://git.fawkesrobotics.org/fawkes.git
Trac:   http://trac.fawkesrobotics.org

The branch, timn/robotino-direct has been created
        at  ce0a8fcdb3b7ba739e619287bb23064724c179eb (commit)

http://git.fawkesrobotics.org/fawkes.git/timn/robotino-direct

- *Log* ---------------------------------------------------------------
commit 279836c58d9f9425942a78122fbde3807c525cc2
Author:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
AuthorDate: Mon Apr 4 11:45:27 2016 +0200
Commit:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
CommitDate: Fri Apr 8 18:10:03 2016 +0200

    robotino: refactor for replaceable com thread
    
    The RobotinoComThread is now an abstract base class and the actual work
    done in OpenRobotinoComThread. This is to prepare for a direct
    communication implementation.

http://git.fawkesrobotics.org/fawkes.git/commit/279836c
http://trac.fawkesrobotics.org/changeset/279836c

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit b469c1ce3ea76c2ef00d57e795fe1f87d27f9086
Author:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
AuthorDate: Wed Apr 6 10:50:24 2016 +0200
Commit:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
CommitDate: Fri Apr 8 18:10:05 2016 +0200

    libplugin: better error handling on config errors in plugin ctors
    
    If a plugin loaded through a meta plugin threw an exception due to a
    config value missing required in the ctor, it was mis-interpreted as a
    "no meta plugin by this name" causing it to try to load a library of the
    meta plugin name, which again failed. The true cause of the failure was
    thus hidden and hard to debug.
    
    Fix by explicitly checking for the config value first.

http://git.fawkesrobotics.org/fawkes.git/commit/b469c1c
http://trac.fawkesrobotics.org/changeset/b469c1c

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 514564c0606f7286975e801db07066f78bdbdb80
Author:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
AuthorDate: Wed Apr 6 10:52:52 2016 +0200
Commit:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
CommitDate: Fri Apr 8 18:10:05 2016 +0200

    buildsys: add support for Boost version checking

http://git.fawkesrobotics.org/fawkes.git/commit/514564c
http://trac.fawkesrobotics.org/changeset/514564c

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit d766cc12e65515991b3142578a7f7f69193306c6
Author:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
AuthorDate: Wed Apr 6 10:53:49 2016 +0200
Commit:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
CommitDate: Fri Apr 8 18:13:05 2016 +0200

    robotino: add initial direct communication support
    
    This includes basic communication and message generation/parsing.
    Communication is now possible. What remains is retrieving the actual
    information.

http://git.fawkesrobotics.org/fawkes.git/commit/d766cc1
http://trac.fawkesrobotics.org/changeset/d766cc1

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit caca8c7357a982124ee614400290e33af2be3d97
Author:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
AuthorDate: Wed Apr 6 11:29:17 2016 +0200
Commit:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
CommitDate: Fri Apr 8 18:13:05 2016 +0200

    robotino: smarttabify robotino plugin
    
    While this is an annoying commit it makes my future life better.

http://git.fawkesrobotics.org/fawkes.git/commit/caca8c7
http://trac.fawkesrobotics.org/changeset/caca8c7

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 173d02e3ff6460d92909f0ee093dab11467d4d68
Author:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
AuthorDate: Wed Apr 6 23:00:10 2016 +0200
Commit:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
CommitDate: Fri Apr 8 18:13:05 2016 +0200

    robotino: re-factor blackboard access for sensor data
    
    The sensor thread now holds the set of sensor interfaces instead of the
    com thread. For this clear separation we accept the intermediate data
    structure. This eliminates code duplication among different com thread
    implementations.
    
    Along the way, fix a bug that caused the OpenRobotino com thread to run
    much more often (virtually in a busy loop), due to the cycle time being
    specified in milliseconds but the tool requiring microseconds...

http://git.fawkesrobotics.org/fawkes.git/commit/173d02e
http://trac.fawkesrobotics.org/changeset/173d02e

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 2ae9c2b9ff361f8906c09e3f27342c66acb2967a
Author:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
AuthorDate: Wed Apr 6 23:49:03 2016 +0200
Commit:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
CommitDate: Fri Apr 8 18:13:05 2016 +0200

    robotino: remove OpenRobotino dependency from act thread
    
    Requires including omni drive projection and unprojection of velocities.

http://git.fawkesrobotics.org/fawkes.git/commit/2ae9c2b
http://trac.fawkesrobotics.org/changeset/2ae9c2b

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit aade921abb532f9d447855b69bdb0d1d15a3d761
Author:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
AuthorDate: Fri Apr 8 16:54:48 2016 +0200
Commit:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
CommitDate: Fri Apr 8 18:13:06 2016 +0200

    robotino: set bumper emergency stop setting

http://git.fawkesrobotics.org/fawkes.git/commit/aade921
http://trac.fawkesrobotics.org/changeset/aade921

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit ce0a8fcdb3b7ba739e619287bb23064724c179eb
Author:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
AuthorDate: Fri Apr 8 17:01:44 2016 +0200
Commit:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
CommitDate: Fri Apr 8 18:13:06 2016 +0200

    robotino: fill in method bodies, full communication working
    
    This is the first version to fully allow for sensor reading and setting
    motor commands. Joystick control looks just fine.
    
    A modified communication scheme might be useful for more efficient error
    recovery more akin to controld3, i.e., just sending a "ping" message
    and parsing it instead of closing and re-opening the serial connection.
    This would require to remove the coupling of send_and_recv, which seems
    feasible in our case.

http://git.fawkesrobotics.org/fawkes.git/commit/ce0a8fc
http://trac.fawkesrobotics.org/changeset/ce0a8fc

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -


- *Summary* -----------------------------------------------------------
 etc/buildsys/boost.mk                            |   15 +-
 src/libs/plugin/manager.cpp                      |   80 ++-
 src/plugins/robotino/Makefile                    |   45 ++-
 src/plugins/robotino/act_thread.cpp              |  744 +++++++++++---------
 src/plugins/robotino/act_thread.h                |  154 ++---
 src/plugins/robotino/com_thread.cpp              |   51 ++-
 src/plugins/robotino/com_thread.h                |   72 ++-
 src/plugins/robotino/direct_com_message.cpp      |  834 ++++++++++++++++++++++
 src/plugins/robotino/direct_com_message.h        |  219 ++++++
 src/plugins/robotino/direct_com_thread.cpp       |  620 ++++++++++++++++
 src/plugins/robotino/direct_com_thread.h         |  115 +++
 src/plugins/robotino/openrobotino_com_thread.cpp |  709 ++++++++-----------
 src/plugins/robotino/openrobotino_com_thread.h   |  174 ++---
 src/plugins/robotino/robotino.mk                 |   22 +
 src/plugins/robotino/robotino_plugin.cpp         |   43 +-
 src/plugins/robotino/sensor_thread.cpp           |  128 ++++-
 src/plugins/robotino/sensor_thread.h             |   44 +-
 17 files changed, 3037 insertions(+), 1032 deletions(-)
 create mode 100644 src/plugins/robotino/direct_com_message.cpp
 create mode 100644 src/plugins/robotino/direct_com_message.h
 create mode 100644 src/plugins/robotino/direct_com_thread.cpp
 create mode 100644 src/plugins/robotino/direct_com_thread.h


- *Diffs* -------------------------------------------------------------

- *commit* 279836c58d9f9425942a78122fbde3807c525cc2 - - - - - - - - - -
Author:  Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
Date:    Mon Apr 4 11:45:27 2016 +0200
Subject: robotino: refactor for replaceable com thread

 src/plugins/robotino/Makefile                      |    8 +-
 src/plugins/robotino/com_thread.cpp                |  650 ++------------------
 src/plugins/robotino/com_thread.h                  |  128 +----
 ...{com_thread.cpp => openrobotino_com_thread.cpp} |   46 +-
 .../{com_thread.h => openrobotino_com_thread.h}    |   13 +-
 src/plugins/robotino/robotino_plugin.cpp           |   10 +-
 6 files changed, 99 insertions(+), 756 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/robotino/Makefile b/src/plugins/robotino/Makefile
index 5d84925..9f46553 100644
--- a/src/plugins/robotino/Makefile
+++ b/src/plugins/robotino/Makefile
@@ -28,7 +28,7 @@ PRESUBDIRS = interfaces
 LIBS_robotino = m fawkescore fawkesutils fawkesaspects fawkesbaseapp \
 		fawkesblackboard fawkesinterface MotorInterface GripperInterface \
 		BatteryInterface RobotinoSensorInterface IMUInterface
-OBJS_robotino = robotino_plugin.o com_thread.o sensor_thread.o act_thread.o 
+OBJS_robotino = robotino_plugin.o com_thread.o sensor_thread.o act_thread.o
 
 LIBS_robotino_ir_pcl = m fawkescore fawkesutils fawkesaspects fawkestf \
 			 fawkesblackboard fawkesinterface fawkespcl_utils \
@@ -41,16 +41,18 @@ LIBS_robotino_ros_joints = m fawkescore fawkesutils fawkesaspects \
 OBJS_robotino_ros_joints = robotino_ros_joints_plugin.o ros_joints_thread.o
 
 ifeq ($(HAVE_OPENROBOTINO),1)
-  CFLAGS  += $(CFLAGS_OPENROBOTINO)
+  CFLAGS  += -DHAVE_OPENROBOTINO $(CFLAGS_OPENROBOTINO)
   LDFLAGS_robotino += $(LDFLAGS_OPENROBOTINO)
 
+	OBJS_robotino += openrobotino_com_thread.o
+
   CFLAGS_sensor_thread = $(CFLAGS) -Wno-reorder -Wno-unused-function
 
   ifeq ($(HAVE_TF),1)
     CFLAGS_robotino_plugin  = $(CFLAGS) $(CFLAGS_TF)
     CFLAGS_act_thread       = $(CFLAGS_robotino_plugin)
     CFLAGS_sensor_thread   += $(CFLAGS) $(CFLAGS_TF)
-    CFLAGS_com_thread      += $(CFLAGS) $(CFLAGS_TF)
+    CFLAGS_openrobotino_com_thread      += $(CFLAGS) $(CFLAGS_TF)
     LDFLAGS_robotino += $(LDFLAGS_TF)
     LIBS_robotino    += fawkestf
   endif
diff --git a/src/plugins/robotino/com_thread.cpp b/src/plugins/robotino/com_thread.cpp
index 6949195..2457992 100644
--- a/src/plugins/robotino/com_thread.cpp
+++ b/src/plugins/robotino/com_thread.cpp
@@ -1,9 +1,9 @@
 
 /***************************************************************************
- *  com_thread.cpp - Robotino com thread
+ *  com_thread.cpp - Robotino com thread base class
  *
  *  Created: Thu Sep 11 13:18:00 2014
- *  Copyright  2011-2014  Tim Niemueller [www.niemueller.de]
+ *  Copyright  2011-2016  Tim Niemueller [www.niemueller.de]
  ****************************************************************************/
 
 /*  This program is free software; you can redistribute it and/or modify
@@ -20,630 +20,68 @@
  */
 
 #include "com_thread.h"
-#ifdef HAVE_OPENROBOTINO_API_1
-#  include <rec/robotino/com/Com.h>
-#  include <rec/sharedmemory/sharedmemory.h>
-#  include <rec/iocontrol/remotestate/SensorState.h>
-#  include <rec/iocontrol/robotstate/State.h>
-#else
-#  include <rec/robotino/api2/Com.h>
-#  include <rec/robotino/api2/AnalogInputArray.h>
-#  include <rec/robotino/api2/Bumper.h>
-#  include <rec/robotino/api2/DigitalInputArray.h>
-#  include <rec/robotino/api2/DistanceSensorArray.h>
-#  include <rec/robotino/api2/ElectricalGripper.h>
-#  include <rec/robotino/api2/Gyroscope.h>
-#  include <rec/robotino/api2/MotorArray.h>
-#  include <rec/robotino/api2/Odometry.h>
-#  include <rec/robotino/api2/PowerManagement.h>
-#endif
-#include <baseapp/run.h>
-#include <core/threading/mutex.h>
-#include <core/threading/mutex_locker.h>
-#include <utils/math/angle.h>
-#include <utils/time/wait.h>
-#include <tf/types.h>
-
-#include <interfaces/BatteryInterface.h>
-#include <interfaces/RobotinoSensorInterface.h>
-#include <interfaces/IMUInterface.h>
-
-#include <unistd.h>
 
 using namespace fawkes;
 
-#define NUM_IR_SENSORS 9
-
-
-/** @class RobotinoComThread "sensor_thread.h"
- * Thread to communicate with Robotino API.
+/** @class RobotinoComThread "com_thread.h"
+ * Virtual base class for thread that communicates with a Robotino.
+ * A communication thread is always continuous and must communicate at the
+ * required pace. It provides hook for sensor and act threads.
  * @author Tim Niemueller
- */
-
-/** Constructor. */
-RobotinoComThread::RobotinoComThread()
-  : Thread("RobotinoComThread", Thread::OPMODE_CONTINUOUS)
-{
-#ifdef HAVE_OPENROBOTINO_API_1
-  com_ = this;
-#endif
-}
-
-
-/** Destructor. */
-RobotinoComThread::~RobotinoComThread()
-{
-}
-
-
-void
-RobotinoComThread::init()
-{
-  cfg_hostname_ = config->get_string("/hardware/robotino/hostname");
-  cfg_quit_on_disconnect_ = config->get_bool("/hardware/robotino/quit_on_disconnect");
-  cfg_enable_gyro_ = config->get_bool("/hardware/robotino/gyro/enable");
-  cfg_imu_iface_id_ = config->get_string("/hardware/robotino/gyro/interface_id");
-  cfg_sensor_update_cycle_time_ =
-    config->get_uint("/hardware/robotino/sensor_update_cycle_time");
-  cfg_gripper_enabled_ = config->get_bool("/hardware/robotino/gripper/enable_gripper");
-
-  batt_if_ = NULL;
-  sens_if_ = NULL;
-  imu_if_ = NULL;
-
-  batt_if_ = blackboard->open_for_writing<BatteryInterface>("Robotino");
-  sens_if_ = blackboard->open_for_writing<RobotinoSensorInterface>("Robotino");
-
-  if (cfg_enable_gyro_) {
-    imu_if_ = blackboard->open_for_writing<IMUInterface>(cfg_imu_iface_id_.c_str());
-  }
-
-  // taken from Robotino API2 DistanceSensorImpl.hpp
-  voltage_to_dist_dps_.push_back(std::make_pair(0.3 , 0.41));
-  voltage_to_dist_dps_.push_back(std::make_pair(0.39, 0.35));
-  voltage_to_dist_dps_.push_back(std::make_pair(0.41, 0.30));
-  voltage_to_dist_dps_.push_back(std::make_pair(0.5 , 0.25));
-  voltage_to_dist_dps_.push_back(std::make_pair(0.75, 0.18));
-  voltage_to_dist_dps_.push_back(std::make_pair(0.8 , 0.16));
-  voltage_to_dist_dps_.push_back(std::make_pair(0.95, 0.14));
-  voltage_to_dist_dps_.push_back(std::make_pair(1.05, 0.12));
-  voltage_to_dist_dps_.push_back(std::make_pair(1.3 , 0.10));
-  voltage_to_dist_dps_.push_back(std::make_pair(1.4 , 0.09));
-  voltage_to_dist_dps_.push_back(std::make_pair(1.55, 0.08));
-  voltage_to_dist_dps_.push_back(std::make_pair(1.8 , 0.07));
-  voltage_to_dist_dps_.push_back(std::make_pair(2.35, 0.05));
-  voltage_to_dist_dps_.push_back(std::make_pair(2.55, 0.04));
-
-#ifdef HAVE_OPENROBOTINO_API_1
-  statemem_ =  new rec::sharedmemory::SharedMemory<rec::iocontrol::robotstate::State>
-    (rec::iocontrol::robotstate::State::sharedMemoryKey);
-  state_ = statemem_->getData();
-  state_mutex_ = new Mutex();
-  set_state_ = new rec::iocontrol::remotestate::SetState();
-  set_state_->gripper_isEnabled = cfg_gripper_enabled_;
-  active_state_ = 0;
-#endif
-
-  if (imu_if_) {
-    // Assume that the gyro is the CruizCore XG1010 and thus set data
-    // from datasheet
-    imu_if_->set_linear_acceleration(0, -1.);
-    imu_if_->set_angular_velocity_covariance(8, deg2rad(0.1));
-    imu_if_->write();
-  }
-
-  data_mutex_  = new Mutex();
-  new_data_    = false;
-  last_seqnum_ = 0;
-  time_wait_   = new TimeWait(clock, cfg_sensor_update_cycle_time_);
-
-#ifdef HAVE_OPENROBOTINO_API_1
-  com_->setAddress(cfg_hostname_.c_str());
-  com_->setMinimumUpdateCycleTime(cfg_sensor_update_cycle_time_);
-  com_->connect(/* blocking */ false);
-#else
-  com_ = new rec::robotino::api2::Com("Fawkes");
-  com_->setAddress(cfg_hostname_.c_str());
-  com_->setAutoReconnectEnabled(false);
-  com_->connectToServer(/* blocking */ true);
-
-  analog_inputs_com_  = new rec::robotino::api2::AnalogInputArray();
-  bumper_com_         = new rec::robotino::api2::Bumper();
-  digital_inputs_com_ = new rec::robotino::api2::DigitalInputArray();
-  distances_com_      = new rec::robotino::api2::DistanceSensorArray();
-  gripper_com_        = new rec::robotino::api2::ElectricalGripper();
-  gyroscope_com_      = new rec::robotino::api2::Gyroscope();
-  motors_com_         = new rec::robotino::api2::MotorArray();
-  odom_com_           = new rec::robotino::api2::Odometry();
-  power_com_          = new rec::robotino::api2::PowerManagement();
-
-  analog_inputs_com_->setComId(com_->id());
-  bumper_com_->setComId(com_->id());
-  digital_inputs_com_->setComId(com_->id());
-  distances_com_->setComId(com_->id());
-  gripper_com_->setComId(com_->id());
-  gyroscope_com_->setComId(com_->id());
-  motors_com_->setComId(com_->id());
-  odom_com_->setComId(com_->id());
-  power_com_->setComId(com_->id());
-#endif
-
-}
-
-
-void
-RobotinoComThread::finalize()
-{
-  delete data_mutex_;
-  delete time_wait_;
-  blackboard->close(sens_if_);
-  blackboard->close(batt_if_);
-  blackboard->close(imu_if_);
-#ifdef HAVE_OPENROBOTINO_API_1
-  set_state_->speedSetPoint[0] = 0.;
-  set_state_->speedSetPoint[1] = 0.;
-  set_state_->speedSetPoint[2] = 0.;
-  set_state_->gripper_isEnabled = false;
-  com_->setSetState(*set_state_);
-  usleep(50000);
-  delete set_state_;
-  delete state_mutex_;
-  delete statemem_;
-#else
-  float speeds[3] = { 0, 0, 0 };
-  motors_com_->setSpeedSetPoints(speeds, 3);
-  usleep(50000);
-  delete analog_inputs_com_;
-  delete bumper_com_;
-  delete digital_inputs_com_;
-  delete distances_com_;
-  delete gripper_com_;
-  delete gyroscope_com_;
-  delete motors_com_;
-  delete odom_com_;
-  delete power_com_;
-  delete com_;
-#endif
-}
-
-void
-RobotinoComThread::once()
-{
-  reset_odometry();
-}
-
-void
-RobotinoComThread::loop()
-{
-  time_wait_->mark_start();
-
-  if (com_->isConnected()) {
-    process_sensor_msgs();
-
-    MutexLocker lock(data_mutex_);
-#ifdef HAVE_OPENROBOTINO_API_1
-    process_sensor_state();
-#else
-    process_com();
-#endif
-
-
-#ifdef HAVE_OPENROBOTINO_API_1
-  } else if (com_->connectionState() == rec::robotino::com::Com::NotConnected) {
-#else
-  } else {
-#endif
-    if (cfg_quit_on_disconnect_) {
-      logger->log_warn(name(), "Connection lost, quitting (as per config)");
-      fawkes::runtime::quit();
-    } else {
-      // retry connection
-#ifdef HAVE_OPENROBOTINO_API_1
-      com_->connect(/* blocking */ false);
-#else
-      com_->connectToServer(/* blocking */ true);
-#endif
-    }
-  }
-
-  time_wait_->wait();
-}
-
-
-void
-RobotinoComThread::process_sensor_msgs()
-{
-  // process command messages
-  while (! sens_if_->msgq_empty()) {
-    if (RobotinoSensorInterface::SetBumperEStopEnabledMessage *msg =
-	sens_if_->msgq_first_safe(msg))
-    {
-#ifdef HAVE_OPENROBOTINO_API_1
-      logger->log_info(name(), "%sabling motor on request",
-		       msg->is_enabled() ? "En" : "Dis");
-      state_->emergencyStop.isEnabled = msg->is_enabled();
-#else
-      logger->log_info(name(), "Setting emergency stop not yet supported for API2");
-#endif
-    }
-    sens_if_->msgq_pop();
-  } // while sensor msgq
-}
-
-
-void
-RobotinoComThread::process_sensor_state()
-{
-#ifdef HAVE_OPENROBOTINO_API_1
-  state_mutex_->lock();
-  fawkes::Time sensor_time = times_[active_state_];
-  rec::iocontrol::remotestate::SensorState sensor_state = sensor_states_[active_state_];
-  state_mutex_->unlock();
-
-  if (sensor_state.sequenceNumber != last_seqnum_) {
-    new_data_ = true;
-    last_seqnum_ = sensor_state.sequenceNumber;
-
-    // update sensor values in interface
-    sens_if_->set_mot_velocity(sensor_state.actualVelocity);
-    sens_if_->set_mot_position(sensor_state.actualPosition);
-    sens_if_->set_mot_current(sensor_state.motorCurrent);
-    sens_if_->set_bumper(sensor_state.bumper);
-    sens_if_->set_bumper_estop_enabled(state_->emergencyStop.isEnabled);
-    sens_if_->set_digital_in(sensor_state.dIn);
-    sens_if_->set_analog_in(sensor_state.aIn);
-    if (cfg_enable_gyro_) {
-      if (state_->gyro.port == rec::serialport::UNDEFINED) {
-	if (fabs(imu_if_->angular_velocity(0) + 1.) > 0.00001) {
-	  imu_if_->set_angular_velocity(0, -1.);
-	  imu_if_->set_angular_velocity(2,  0.);
-	  imu_if_->set_orientation(0, -1.);
-	}
-      } else {
-	imu_if_->set_angular_velocity(0, 0.);
-	imu_if_->set_angular_velocity(2, state_->gyro.rate);
-
-	tf::Quaternion q = tf::create_quaternion_from_yaw(state_->gyro.angle);
-	imu_if_->set_orientation(0, q.x());
-	imu_if_->set_orientation(1, q.y());
-	imu_if_->set_orientation(2, q.z());
-	imu_if_->set_orientation(3, q.w());
-      }
-    }
-
-    update_distances(sensor_state.distanceSensor);
-
-    batt_if_->set_voltage(roundf(sensor_state.voltage * 1000.));
-    batt_if_->set_current(roundf(sensor_state.current));
-
-    // 21.0V is empty, 26.0V is empty, from OpenRobotino lcdd
-    float soc = (sensor_state.voltage - 21.0f) / 5.f;
-    soc = std::min(1.f, std::max(0.f, soc));
-
-    batt_if_->set_absolute_soc(soc);
-  }
-#endif
-}
-
-
-void
-RobotinoComThread::process_com()
-{
-#ifdef HAVE_OPENROBOTINO_API_2
-  com_->processComEvents();
-
-  double odo_x = 0, odo_y = 0, odo_phi = 0;
-  unsigned int odo_seq = 0;
-
-  odom_com_->readings(&odo_x, &odo_y, &odo_phi, &odo_seq);
-
-  if (odo_seq != last_seqnum_) {  
-    new_data_ = true;
-    last_seqnum_ = odo_seq;
-
-    unsigned int mot_num = motors_com_->numMotors();
-    float mot_act_vel[mot_num];
-    int   mot_act_pos[mot_num];
-    float mot_currents[mot_num];
-    motors_com_->actualVelocities(mot_act_vel);
-    motors_com_->actualPositions(mot_act_pos);
-    motors_com_->motorCurrents(mot_currents);
-
-    bool bumper = bumper_com_->value();
-
-    unsigned int digin_num = digital_inputs_com_->numDigitalInputs();
-    int digin_readings[digin_num];
-    bool digin_bools[digin_num];
-    digital_inputs_com_->values(digin_readings);
-    for (unsigned int i = 0; i < digin_num; ++i)  digin_bools[i] = (digin_readings[i] != 0);
-
-    unsigned int anlgin_num = analog_inputs_com_->numAnalogInputs();
-    float anlgin_readings[anlgin_num];
-    analog_inputs_com_->values(anlgin_readings);
-
-    unsigned int dist_num = distances_com_->numDistanceSensors();
-    // the distance calculation from API2 uses a max value of 0.41,
-    // which breaks the previous behavior of 0.0 for "nothing"
-    // therefore use our API1 conversion routine
-    //float dist_distances[dist_num];
-    //distances_com_->distances(dist_distances);
-    float dist_voltages[dist_num];
-    distances_com_->voltages(dist_voltages);
-
-    if (anlgin_num != sens_if_->maxlenof_analog_in()) {
-      logger->log_warn(name(), "Different number of analog inputs: %u vs. %u",
-		       anlgin_num, sens_if_->maxlenof_analog_in());
-    }
-
-    if (digin_num != sens_if_->maxlenof_digital_in()) {
-      logger->log_warn(name(), "Different number of digital inputs: %u vs. %u",
-		       digin_num, sens_if_->maxlenof_digital_in());
-    }
-
-    if (dist_num != sens_if_->maxlenof_distance()) {
-      logger->log_warn(name(), "Different number of distances: %u vs. %u",
-		       dist_num, sens_if_->maxlenof_distance());
-    }
-
-    float pow_current = power_com_->current() * 1000.; // A -> mA
-    float pow_voltage = power_com_->voltage() * 1000.; // V -> mV
-
-    float gyro_angle  = gyroscope_com_->angle();
-    float gyro_rate   = gyroscope_com_->rate();
-
-    // update sensor values in interface
-    sens_if_->set_mot_velocity(mot_act_vel);
-    sens_if_->set_mot_position(mot_act_pos);
-    sens_if_->set_mot_current(mot_currents);
-    sens_if_->set_bumper(bumper);
-    //sens_if_->set_bumper_estop_enabled(???);
-    sens_if_->set_digital_in(digin_bools);
-    sens_if_->set_analog_in(anlgin_readings);
-    if (cfg_enable_gyro_) {
-      imu_if_->set_angular_velocity(0, 0.);
-      imu_if_->set_angular_velocity(2, gyro_rate);
-
-      tf::Quaternion q = tf::create_quaternion_from_yaw(gyro_angle);
-      imu_if_->set_orientation(0, q.x());
-      imu_if_->set_orientation(1, q.y());
-      imu_if_->set_orientation(2, q.z());
-      imu_if_->set_orientation(3, q.w());
-    }
-
-    //sens_if_->set_distance(dist_distances);
-    update_distances(dist_voltages);
-
-    batt_if_->set_voltage(roundf(pow_voltage));
-    batt_if_->set_current(roundf(pow_current));
-
-    // 22.0V is empty, 24.5V is full, this is just a guess
-    float soc = (power_com_->voltage() - 22.0f) / 2.5f;
-    soc = std::min(1.f, std::max(0.f, soc));
-    batt_if_->set_absolute_soc(soc);
-  }
-#endif
-}
-
-
-/** Trigger writes of blackboard interfaces.
+ *
+ *
+ * @fn void RobotinoComThread::update_bb_sensor() = 0
+ * Trigger writes of blackboard interfaces.
  * This is meant to be called by the sensor thread so that writes to the
  * blackboard happen in the sensor acquisition hook.
- */
-void
-RobotinoComThread::update_bb_sensor()
-{
-  MutexLocker lock(data_mutex_);
-  if (new_data_) {
-    batt_if_->write();
-    sens_if_->write();
-    if (imu_if_)  imu_if_->write();
-    new_data_ = false;
-  }
-}
-
-void
-RobotinoComThread::update_distances(float *voltages)
-{
-  float dist_m[NUM_IR_SENSORS];
-  const size_t num_dps = voltage_to_dist_dps_.size();
-
-  for (int i = 0; i < NUM_IR_SENSORS; ++i) {
-    dist_m[i] = 0.;
-    // find the two enclosing data points
-
-    for (size_t j = 0; j < num_dps - 1; ++j) {
-      // This determines two points, l(eft) and r(ight) that are
-      // defined by voltage (x coord) and distance (y coord). We
-      // assume a linear progression between two adjacent points,
-      // i.e. between l and r. We then do the following:
-      // 1. Find two adjacent voltage values lv and rv where
-      //    the voltage lies inbetween
-      // 2. Interpolate by calculating the line parameters
-      //    m = dd/dv, x = voltage - lv and b = ld.
-      // cf. http://www.acroname.com/robotics/info/articles/irlinear/irlinear.html
-
-      const double lv = voltage_to_dist_dps_[j  ].first;
-      const double rv = voltage_to_dist_dps_[j+1].first;
-
-      if ((voltages[i] >= lv) && (voltages[i] < rv)) {
-        const double ld = voltage_to_dist_dps_[j  ].second;
-        const double rd = voltage_to_dist_dps_[j+1].second;
-
-        double dv = rv - lv;
-        double dd = rd - ld;
-
-        // Linear interpolation between 
-        dist_m[i] = (dd / dv) * (voltages[i] - lv) + ld;
-        break;
-      }
-    }
-  }
-
-  sens_if_->set_distance(dist_m);
-}
-
-#ifdef HAVE_OPENROBOTINO_API_1
-/** Update event. */
-void
-RobotinoComThread::updateEvent()
-{
-  unsigned int next_state = 1 - active_state_;
-  sensor_states_[next_state] = sensorState();
-  times_[next_state].stamp();
-
-  MutexLocker lock(state_mutex_);
-  active_state_ = next_state;
-}
-#endif
-
-
-/** Reset odometry to zero. */
-void
-RobotinoComThread::reset_odometry()
-{
-  if (com_->isConnected()) {
-#ifdef HAVE_OPENROBOTINO_API_1
-    set_state_->setOdometry = true;
-    set_state_->odometryX = set_state_->odometryY = set_state_->odometryPhi = 0.;
-    com_->setSetState(*set_state_);
-    set_state_->setOdometry = false;
-#else
-    odom_com_->set(0., 0., 0., /* blocking */ true);
-#endif
-  }
-}
-
-
-/** Check if we are connected to OpenRobotino.
+ *
+ * @fn void RobotinoComThread::reset_odometry() = 0
+ * Reset odometry to zero.
+ *
+ * @fn bool RobotinoComThread::is_connected() = 0
+ * Check if we are connected to OpenRobotino.
  * @return true if the connection has been established, false otherwise
- */
-bool
-RobotinoComThread::is_connected()
-{
-  return com_->isConnected();
-}
-
-
-/** Get actual velocity.
+ *
+ * @fn void RobotinoComThread::get_act_velocity(float &a1, float &a2, float &a3, unsigned int &seq, fawkes::Time &t) = 0
+ * Get actual velocity.
  * @param a1 upon return contains velocity in RPM for first wheel
  * @param a2 upon return contains velocity in RPM for second wheel
  * @param a3 upon return contains velocity in RPM for third wheel
  * @param seq upon return contains sequence number of latest data
  * @param t upon return contains time of latest data
- */
-void
-RobotinoComThread::get_act_velocity(float &a1, float &a2, float &a3, unsigned int &seq, fawkes::Time &t)
-{
-  MutexLocker lock(data_mutex_);
-
-#ifdef HAVE_OPENROBOTINO_API_1
-  state_mutex_->lock();
-  t = times_[active_state_];
-  rec::iocontrol::remotestate::SensorState sensor_state = sensor_states_[active_state_];
-  state_mutex_->unlock();
-
-  // div by 1000 to convert from mm to m
-  a1 = sensor_state.actualVelocity[0] / 1000.f;
-  a2 = sensor_state.actualVelocity[1] / 1000.f;
-  a3 = sensor_state.actualVelocity[2] / 1000.f;
-  seq = sensor_state.sequenceNumber;
-#else
-  float mot_act_vel[motors_com_->numMotors()];
-  motors_com_->actualVelocities(mot_act_vel);
-
-  double odo_x = 0, odo_y = 0, odo_phi = 0;
-  odom_com_->readings(&odo_x, &odo_y, &odo_phi, &seq);
-
-  a1 = mot_act_vel[0];
-  a2 = mot_act_vel[1];
-  a3 = mot_act_vel[2];
-#endif
-}
-
-
-/** Get current odometry.
- * @param x X coordinate of robot in odometry frame
- * @param y Y coordinate of robot in odometry frame
- * @param phi orientation of robot in odometry frame
- */
-void
-RobotinoComThread::get_odometry(double &x, double &y, double &phi)
-{
-  MutexLocker lock(data_mutex_);
-
-#ifdef HAVE_OPENROBOTINO_API_1
-  state_mutex_->lock();
-  rec::iocontrol::remotestate::SensorState sensor_state = sensor_states_[active_state_];
-  state_mutex_->unlock();
-
-  x   = sensor_state.odometryX / 1000.f;
-  y   = sensor_state.odometryY / 1000.f;
-  phi = deg2rad(sensor_state.odometryPhi);
-
-#else
-  unsigned int seq;
-  odom_com_->readings(&x, &y, &phi, &seq);
-#endif
-}
-
-
-/** Check if gripper is open.
+ *
+ * @fn bool RobotinoComThread::is_gripper_open() = 0
+ * Check if gripper is open.
  * @return true if the gripper is presumably open, false otherwise
- */
-bool
-RobotinoComThread::is_gripper_open()
-{
-  MutexLocker lock(data_mutex_);
-
-#ifdef HAVE_OPENROBOTINO_API_1
-  state_mutex_->lock();
-  rec::iocontrol::remotestate::SensorState sensor_state = sensor_states_[active_state_];
-  state_mutex_->unlock();
-
-  return sensor_state.isGripperOpened;
-#else
-  return gripper_com_->isOpened();
-#endif
-}
-
-
-/** Set speed points for wheels.
+ *
+ * @fn void RobotinoComThread::set_speed_points(float s1, float s2, float s3) = 0
+ * Set speed points for wheels.
  * @param s1 speed point for first wheel in RPM
  * @param s2 speed point for second wheel in RPM
  * @param s3 speed point for third wheel in RPM
+ *
+ * @fn void RobotinoComThread::set_gripper(bool opened) = 0
+ * Open or close gripper.
+ * @param opened true to open gripper, false to close
+ *
+ * @fn void RobotinoComThread::get_odometry(double &x, double &y, double &phi) = 0
+ * Get latest odometry value.
+ * @param x upon return contains x coordinate of odometry
+ * @param y upon return contains y coordinate of odometry
+ * @param phi upon return contains rptation of odometry
  */
-void
-RobotinoComThread::set_speed_points(float s1, float s2, float s3)
-{
-#ifdef HAVE_OPENROBOTINO_API_1
-  set_state_->speedSetPoint[0] = s1;
-  set_state_->speedSetPoint[1] = s2;
-  set_state_->speedSetPoint[2] = s3;
-
-  com_->setSetState(*set_state_);
-#else
-  float speeds[3] = { s1, s2, s3 };
-  motors_com_->setSpeedSetPoints(speeds, 3);
-#endif
 
+/** Constructor.
+ * @param thread_name name of thread
+ */
+RobotinoComThread::RobotinoComThread(const char *thread_name)
+  : Thread(thread_name, Thread::OPMODE_CONTINUOUS)
+{
 }
 
 
-/** Open or close gripper.
- * @param opened true to open gripper, false to close
- */
-void
-RobotinoComThread::set_gripper(bool opened)
+/** Destructor. */
+RobotinoComThread::~RobotinoComThread()
 {
-#ifdef HAVE_OPENROBOTINO_API_1
-  set_state_->gripper_close = ! opened;
-  com_->setSetState(*set_state_);
-#else
-  if (opened) {
-    gripper_com_->open();
-  } else {
-    gripper_com_->close();
-  }
-#endif
-
 }
+
diff --git a/src/plugins/robotino/com_thread.h b/src/plugins/robotino/com_thread.h
index f60feff..d1db566 100644
--- a/src/plugins/robotino/com_thread.h
+++ b/src/plugins/robotino/com_thread.h
@@ -1,8 +1,8 @@
 /***************************************************************************
- *  com_thread.h - Robotino com thread
+ *  com_thread.h - Robotino com thread base class
  *
  *  Created: Thu Sep 11 11:43:42 2014
- *  Copyright  2011-2014  Tim Niemueller [www.niemueller.de]
+ *  Copyright  2011-2016  Tim Niemueller [www.niemueller.de]
  ****************************************************************************/
 
 /*  This program is free software; you can redistribute it and/or modify
@@ -29,40 +29,6 @@
 
 #include <utils/time/time.h>
 
-#ifdef HAVE_OPENROBOTINO_API_1
-#  include <rec/robotino/com/Com.h>
-namespace rec {
-  namespace sharedmemory {
-    template<typename SharedType> class SharedMemory;
-  }
-  namespace iocontrol {
-    namespace robotstate {
-      class State;
-    }
-    namespace remotestate {
-      class SetState;
-    }
-  }
-}
-#else
-namespace rec {
-  namespace robotino {
-    namespace api2 {
-      class Com;
-      class AnalogInputArray;
-      class Bumper;
-      class DigitalInputArray;
-      class DistanceSensorArray;
-      class ElectricalGripper;
-      class Gyroscope;
-      class MotorArray;
-      class Odometry;
-      class PowerManagement;
-    }
-  }
-}
-#endif
-
 namespace fawkes {
   class Mutex;
   class Clock;
@@ -74,92 +40,22 @@ namespace fawkes {
 }
 
 class RobotinoComThread
-: public fawkes::Thread,
-#ifdef HAVE_OPENROBOTINO_API_1
-  public rec::robotino::com::Com,
-#endif
-  public fawkes::LoggingAspect,
-  public fawkes::ConfigurableAspect,
-  public fawkes::ClockAspect,
-  public fawkes::BlackBoardAspect
+: public fawkes::Thread
 {
  public:
-  RobotinoComThread();
+	RobotinoComThread(const char *thread_name);
   virtual ~RobotinoComThread();
 
-  virtual void init();
-  virtual void once();
-  virtual void loop();
-  virtual void finalize();
-
-  void update_bb_sensor();
-
-  bool is_connected();
-
-  void set_gripper(bool opened);
-  bool is_gripper_open();
-  void set_speed_points(float s1, float s2, float s3);
-  void get_act_velocity(float &a1, float &a2, float &a3, unsigned int &seq, fawkes::Time &t);
-  void get_odometry(double &x, double &y, double &phi);
-  void reset_odometry();
-
- /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
- protected: virtual void run() { Thread::run(); }
+  virtual void update_bb_sensor() = 0;
 
- private:
-#ifdef HAVE_OPENROBOTINO_API_1
-  using rec::robotino::com::Com::sensorState;
-  virtual void updateEvent();
-#endif
-  void process_sensor_msgs();
-  void process_sensor_state();
-  void process_com();
-  void update_distances(float *voltages);
-
- private:
-  std::string     cfg_hostname_;
-  bool            cfg_quit_on_disconnect_;
-  bool            cfg_enable_gyro_;
-  std::string     cfg_imu_iface_id_;
-  unsigned int    cfg_sensor_update_cycle_time_;
-  bool            cfg_gripper_enabled_;
-
-  // Voltage to distance data points
-  std::vector<std::pair<double, double> > voltage_to_dist_dps_;
-
-  fawkes::Mutex    *data_mutex_;
-  bool              new_data_;
-  fawkes::TimeWait *time_wait_;
-  unsigned int      last_seqnum_;
-
-  fawkes::BatteryInterface        *batt_if_;
-  fawkes::RobotinoSensorInterface *sens_if_;
-  fawkes::IMUInterface            *imu_if_;
+  virtual bool is_connected() = 0;
 
-#ifdef HAVE_OPENROBOTINO_API_1
-  rec::robotino::com::Com *com_;
-  fawkes::Mutex *state_mutex_;
-  unsigned int active_state_;
-  rec::iocontrol::remotestate::SensorState sensor_states_[2];
-  fawkes::Time times_[2];
-
-  rec::sharedmemory::SharedMemory<rec::iocontrol::robotstate::State> *statemem_;
-  rec::iocontrol::robotstate::State *state_;
-
-  rec::iocontrol::remotestate::SetState *set_state_;
-
-#else
-  rec::robotino::api2::Com                    *com_;
-  rec::robotino::api2::AnalogInputArray       *analog_inputs_com_;
-  rec::robotino::api2::Bumper                 *bumper_com_;
-  rec::robotino::api2::DigitalInputArray      *digital_inputs_com_;
-  rec::robotino::api2::DistanceSensorArray    *distances_com_;
-  rec::robotino::api2::ElectricalGripper      *gripper_com_;
-  rec::robotino::api2::Gyroscope              *gyroscope_com_;
-  rec::robotino::api2::MotorArray             *motors_com_;
-  rec::robotino::api2::Odometry               *odom_com_;
-  rec::robotino::api2::PowerManagement        *power_com_;
-#endif
+  virtual void set_gripper(bool opened) = 0;
+  virtual bool is_gripper_open() = 0;
+  virtual void set_speed_points(float s1, float s2, float s3) = 0;
+  virtual void get_act_velocity(float &a1, float &a2, float &a3, unsigned int &seq, fawkes::Time &t) = 0;
+  virtual void get_odometry(double &x, double &y, double &phi) = 0;
+  virtual void reset_odometry() = 0;
 };
 
 
diff --git a/src/plugins/robotino/robotino_plugin.cpp b/src/plugins/robotino/robotino_plugin.cpp
index adbf6eb..163c092 100644
--- a/src/plugins/robotino/robotino_plugin.cpp
+++ b/src/plugins/robotino/robotino_plugin.cpp
@@ -22,7 +22,9 @@
 
 #include <core/plugin.h>
 
-#include "com_thread.h"
+#ifdef HAVE_OPENROBOTINO
+#  include "openrobotino_com_thread.h"
+#endif
 #include "sensor_thread.h"
 #include "act_thread.h"
 
@@ -40,7 +42,11 @@ class RobotinoPlugin : public fawkes::Plugin
   RobotinoPlugin(Configuration *config)
     : Plugin(config)
   {
-    RobotinoComThread *com_thread = new RobotinoComThread();
+#ifdef HAVE_OPENROBOTINO
+    RobotinoComThread *com_thread = new OpenRobotinoComThread();
+#else
+#  error "No com thread implementation available"
+#endif
     thread_list.push_back(com_thread);
     thread_list.push_back(new RobotinoSensorThread(com_thread));
     thread_list.push_back(new RobotinoActThread(com_thread));

- *commit* b469c1ce3ea76c2ef00d57e795fe1f87d27f9086 - - - - - - - - - -
Author:  Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
Date:    Wed Apr 6 10:50:24 2016 +0200
Subject: libplugin: better error handling on config errors in plugin ctors

 src/libs/plugin/manager.cpp |   80 +++++++++++++++++++++++--------------------
 1 files changed, 43 insertions(+), 37 deletions(-)

_Diff for modified files_:
diff --git a/src/libs/plugin/manager.cpp b/src/libs/plugin/manager.cpp
index b7d8e88..52fd19a 100644
--- a/src/libs/plugin/manager.cpp
+++ b/src/libs/plugin/manager.cpp
@@ -301,43 +301,49 @@ PluginManager::parse_plugin_list(const char *plugin_list)
 void
 PluginManager::load(const char *plugin_list)
 {
-  std::list<std::string> pp = parse_plugin_list(plugin_list);
+	std::list<std::string> pp = parse_plugin_list(plugin_list);
 
-  for (std::list<std::string>::iterator i = pp.begin(); i != pp.end(); ++i) {
-    if ( i->length() == 0 ) continue;
-
-    bool try_real_plugin = true;
-    if ( __meta_plugins.find(*i) == __meta_plugins.end() ) {
-      std::string meta_plugin = __meta_plugin_prefix + *i;
-      try {
-	std::string pset = __config->get_string(meta_plugin.c_str());
-	if (pset.length() == 0) {
-	  throw Exception("Refusing to load an empty meta plugin");
-	}
-	//printf("Going to load meta plugin %s (%s)\n", i->c_str(), pset.c_str());
-	__meta_plugins.lock();
-	// Setting has to happen here, so that a meta plugin will not cause an
-	// endless loop if it references itself!
-	__meta_plugins[*i] = pset;
-	__meta_plugins.unlock();
-	try {
-	  LibLogger::log_info("PluginManager", "Loading plugins %s for meta plugin %s",
-	                      pset.c_str(), i->c_str());
-	  load(pset.c_str());
-	  notify_loaded(i->c_str());
-	} catch (Exception &e) {
-	  e.append("Could not initialize meta plugin %s, aborting loading.", i->c_str());
-	  __meta_plugins.erase_locked(*i);
-	  throw;
-	}
-
-	try_real_plugin = false;
-      } catch (ConfigEntryNotFoundException &e) {
-	// no meta plugin defined by that name
-	//printf("No meta plugin defined with the name %s\n", i->c_str());
-	try_real_plugin = true;
-      }
-    }
+	for (std::list<std::string>::iterator i = pp.begin(); i != pp.end(); ++i) {
+		if ( i->length() == 0 ) continue;
+
+		bool try_real_plugin = true;
+		if ( __meta_plugins.find(*i) == __meta_plugins.end() ) {
+			std::string meta_plugin = __meta_plugin_prefix + *i;
+			bool found_meta = false;
+			std::string pset = "";
+			try {
+				pset = __config->get_string(meta_plugin.c_str());
+				found_meta = true;
+			} catch (ConfigEntryNotFoundException &e) {
+				// no meta plugin defined by that name
+	      //printf("No meta plugin defined with the name %s\n", i->c_str());
+				try_real_plugin = true;
+			}
+
+			if (found_meta) {
+				if (pset.length() == 0) {
+					throw Exception("Refusing to load an empty meta plugin");
+				}
+				//printf("Going to load meta plugin %s (%s)\n", i->c_str(), pset.c_str());
+				__meta_plugins.lock();
+				// Setting has to happen here, so that a meta plugin will not cause an
+				// endless loop if it references itself!
+				__meta_plugins[*i] = pset;
+				__meta_plugins.unlock();
+				try {
+					LibLogger::log_info("PluginManager", "Loading plugins %s for meta plugin %s",
+					                    pset.c_str(), i->c_str());
+					load(pset.c_str());
+					notify_loaded(i->c_str());
+				} catch (Exception &e) {
+					e.append("Could not initialize meta plugin %s, aborting loading.", i->c_str());
+					__meta_plugins.erase_locked(*i);
+					throw;
+				}
+			
+				try_real_plugin = false;
+			}
+		}
 
     if (try_real_plugin &&
 	(find_if(plugins.begin(), plugins.end(), plname_eq(*i)) == plugins.end()))

- *commit* 514564c0606f7286975e801db07066f78bdbdb80 - - - - - - - - - -
Author:  Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
Date:    Wed Apr 6 10:52:52 2016 +0200
Subject: buildsys: add support for Boost version checking

 etc/buildsys/boost.mk |   15 ++++++++++++++-
 1 files changed, 14 insertions(+), 1 deletions(-)

_Diff for modified files_:
diff --git a/etc/buildsys/boost.mk b/etc/buildsys/boost.mk
index a77eace..309e00d 100644
--- a/etc/buildsys/boost.mk
+++ b/etc/buildsys/boost.mk
@@ -26,8 +26,10 @@ BOOST_LIB_DIRS=/usr/lib64 /usr/lib /usr/lib32 \
 BOOST_INCLUDE_DIRS=
 BOOST_LIBRARY_SUFFIXES=-mt NOSUFFIX
 
+boost-find-include = $(firstword $(wildcard $(foreach i,$(BOOST_INCLUDE_DIRS) /usr/include /usr/local/include,$i/boost/$1)))
+boost-have-include = $(if $(call boost-find-include,$1),1)
 boost-have-libfile = $(if $(wildcard $(foreach l,$(BOOST_LIB_DIRS),$(foreach s,$(BOOST_LIBRARY_SUFFIXES),$l/libboost_$1$(subst NOSUFFIX,,$s).$(SOEXT) ))),1)
-boost-have-lib     = $(if $(or $(call boost-have-libfile,$1),$(wildcard $(foreach i,$(BOOST_INCLUDE_DIRS) /usr/include /usr/local/include,$i/boost/$1.hpp))),1)
+boost-have-lib     = $(if $(or $(call boost-have-libfile,$1),$(call boost-have-include,$(1).hpp)),1)
 boost-lib-cflags   = $(addprefix -I,$(wildcard $(BOOST_INCLUDE_DIRS)))
 boost-lib-ldflags  = $(addprefix -lboost_,$(foreach l,$(BOOST_LIB_DIRS),$(foreach s,$(BOOST_LIBRARY_SUFFIXES),$(if $(wildcard $l/libboost_$1$(subst NOSUFFIX,,$s).$(SOEXT)),$1$(subst NOSUFFIX,,$s) ))))
 
@@ -35,4 +37,15 @@ boost-have-libs    = $(if $(strip $(subst 1,,$(foreach l,$1,$(or $(call boost-ha
 boost-libs-cflags  = $(foreach l,$1,$(call boost-lib-cflags,$l))
 boost-libs-ldflags = $(foreach l,$1,$(call boost-lib-ldflags,$l))
 
+ifeq ($(call boost-have-include,version.hpp),1)
+	HAVE_BOOST = 1
+  BOOST_VERSION = $(shell LANG=C grep "define BOOST_VERSION " "$(call boost-find-include,version.hpp)" | awk '{ print $$3 }')
+  BOOST_VERSION_MAJOR = $(shell echo $$(($(BOOST_VERSION) / 100000)))
+  BOOST_VERSION_MINOR = $(shell echo $$(($(BOOST_VERSION) / 100 % 1000)))
+  BOOST_VERSION_PATCH = $(shell echo $$(($(BOOST_VERSION) % 100)))
+endif
+
+boost-version-create = $(shell echo $$(($1 * 100000 + $2 * 1000 + $3)))
+boost-version-atleast = $(shell echo $$(($(BOOST_VERSION) >= $1 * 100000 + $2 * 1000 + $3)))
+
 endif # __buildsys_boost_mk_

- *commit* d766cc12e65515991b3142578a7f7f69193306c6 - - - - - - - - - -
Author:  Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
Date:    Wed Apr 6 10:53:49 2016 +0200
Subject: robotino: add initial direct communication support

 src/plugins/robotino/Makefile               |   45 ++-
 src/plugins/robotino/act_thread.cpp         |   16 +-
 src/plugins/robotino/act_thread.h           |    4 +-
 src/plugins/robotino/direct_com_message.cpp |  834 +++++++++++++++++++++++++++
 src/plugins/robotino/direct_com_message.h   |  219 +++++++
 src/plugins/robotino/direct_com_thread.cpp  |  528 +++++++++++++++++
 src/plugins/robotino/direct_com_thread.h    |  125 ++++
 src/plugins/robotino/robotino.mk            |   22 +
 src/plugins/robotino/robotino_plugin.cpp    |   21 +-
 9 files changed, 1797 insertions(+), 17 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/robotino/Makefile b/src/plugins/robotino/Makefile
index 9f46553..0374f77 100644
--- a/src/plugins/robotino/Makefile
+++ b/src/plugins/robotino/Makefile
@@ -40,12 +40,40 @@ LIBS_robotino_ros_joints = m fawkescore fawkesutils fawkesaspects \
 			   RobotinoSensorInterface
 OBJS_robotino_ros_joints = robotino_ros_joints_plugin.o ros_joints_thread.o
 
+
 ifeq ($(HAVE_OPENROBOTINO),1)
   CFLAGS  += -DHAVE_OPENROBOTINO $(CFLAGS_OPENROBOTINO)
   LDFLAGS_robotino += $(LDFLAGS_OPENROBOTINO)
 
 	OBJS_robotino += openrobotino_com_thread.o
+else
+  WARN_TARGETS += warning_openrobotino
+endif
+
+ifeq ($(HAVE_LIBUDEV)$(HAVE_ROBOTINO_DIRECT_BOOST_LIBS)$(call boost-version-atleast,1,0,48)$(HAVE_CPP11),1111)
+	HAVE_ROBOTINO_DIRECT = 1
+
+  CFLAGS += -DHAVE_ROBOTINO_DIRECT $(CFLAGS_CPP11) $(CFLAGS_LIBUDEV) $(CFLAGS_ROBOTINO_DIRECT_BOOST)
+  LDFLAGS_robotino += $(LDFLAGS_LIBUDEV) $(LDFLAGS_ROBOTINO_DIRECT_BOOST)
+
+	OBJS_robotino += direct_com_thread.o direct_com_message.o
+else
+  ifneq ($(HAVE_LIBUDEV),1)
+    WARN_TARGETS += warning_libudev
+  endif
+  ifneq ($(HAVE_ROBOTINO_DIRECT_BOOST_LIBS),1)
+    WARN_TARGETS_ROBOTINO_DIRECT_BOOST = $(foreach l,$(ROBOTINO_DIRECT_REQ_BOOST_LIBS),$(if $(call boost-have-lib,$l),, warning_robotino_direct_boost_$l))
+	  WARN_TARGETS += $(WARN_TARGETS_ROBOTINO_DIRECT_BOOST)
+  endif
+  ifneq ($(call boost-version-atleast,1,0,48),1)
+    WARN_TARGETS += warning_old_boost
+  endif
+  ifneq ($(HAVE_CPP11),1)
+    WARN_TARGETS += warning_cpp11
+  endif
+endif
 
+ifneq ($(HAVE_OPENROBOTINO)$(HAVE_ROBOTINO_DIRECT),)
   CFLAGS_sensor_thread = $(CFLAGS) -Wno-reorder -Wno-unused-function
 
   ifeq ($(HAVE_TF),1)
@@ -53,6 +81,7 @@ ifeq ($(HAVE_OPENROBOTINO),1)
     CFLAGS_act_thread       = $(CFLAGS_robotino_plugin)
     CFLAGS_sensor_thread   += $(CFLAGS) $(CFLAGS_TF)
     CFLAGS_openrobotino_com_thread      += $(CFLAGS) $(CFLAGS_TF)
+    CFLAGS_direct_com_thread      += $(CFLAGS) $(CFLAGS_TF)
     LDFLAGS_robotino += $(LDFLAGS_TF)
     LIBS_robotino    += fawkestf
   endif
@@ -69,7 +98,7 @@ ifeq ($(HAVE_OPENROBOTINO),1)
     WARN_TARGETS += warning_pcl
   endif
 else
-  WARN_TARGETS += warning_openrobotino
+  WARN_TARGETS += warning_nodriver
 endif
 
 ifeq ($(HAVE_ROS),1)
@@ -93,15 +122,25 @@ ifeq ($(OBJSSUBMAKE),1)
   ifneq ($(WARN_TARGETS),)
 all: $(WARN_TARGETS)
   endif
-.PHONY: warning_openrobotino warning_pcl warning_ros warning_sensor_msgs
+.PHONY: warning_openrobotino warning_pcl warning_ros warning_sensor_msgs $(WARN_TARGETS_ROBOTINO_DIRECT_BOOST) warning_nodriver warning_old_boost warning_cpp11
 warning_openrobotino:
-	$(SILENT)echo -e "$(INDENT_PRINT)--> $(TBROWN)Omitting Robotino support$(TNORMAL) (OpenRobotino API not found)"
+	$(SILENT)echo -e "$(INDENT_PRINT)--> $(TBROWN)Omitting OpenRobotino API support$(TNORMAL) (OpenRobotino API not found)"
+warning_nodriver:
+	$(SILENT)echo -e "$(INDENT_PRINT)--> $(TBROWN)Omitting Robotino platform support$(TNORMAL) (driver prerequisites not met)"
 warning_pcl:
 	$(SILENT)echo -e "$(INDENT_PRINT)--> $(TBROWN)Omitting Robotino PCL plugins$(TNORMAL) (PCL not found)"
 warning_ros:
 	$(SILENT)echo -e "$(INDENT_PRINT)--> $(TBROWN)Omitting Robotino Joints plugin$(TNORMAL) (ROS not found)"
 warning_sensor_msgs:
 	$(SILENT)echo -e "$(INDENT_PRINT)--> $(TBROWN)Omitting Robotino Joints plugin$(TNORMAL) (ROS package sensor_msgs not found)"
+warning_libudev:
+	$(SILENT)echo -e "$(INDENT_PRINT)--> $(TYELLOW)Omitting Robotino direct commnunication support$(TNORMAL) (libudev not available)"
+$(WARN_TARGETS_ROBOTINO_DIRECT_BOOST): warning_robotino_direct_boost_%:
+	$(SILENT)echo -e "$(INDENT_PRINT)--> $(TYELLOW)Omitting Robotino direct commnunication support$(TNORMAL) (Boost library $* not found)"
+warning_old_boost:
+	$(SILENT)echo -e "$(INDENT_PRINT)--> $(TYELLOW)Omitting Robotino direct commnunication support$(TNORMAL) (Boost library too old, require at least 1.0.48, have $(BOOST_VERSION))"
+warning_cpp11:
+	$(SILENT)echo -e "$(INDENT_PRINT)--> $(TYELLOW)Omitting Robotino direct commnunication support$(TNORMAL) (C++11 support required)"
 endif
 
 include $(BUILDSYSDIR)/base.mk
diff --git a/src/plugins/robotino/act_thread.cpp b/src/plugins/robotino/act_thread.cpp
index 223d8d2..b0e7c5f 100644
--- a/src/plugins/robotino/act_thread.cpp
+++ b/src/plugins/robotino/act_thread.cpp
@@ -35,7 +35,7 @@
 #  include <rec/sharedmemory/sharedmemory.h>
 #  include <rec/iocontrol/remotestate/SensorState.h>
 #  include <rec/iocontrol/robotstate/State.h>
-#else
+#elif defined(HAVE_OPENROBOTINO_API_2)
 #  include <rec/robotino/api2/OmniDriveModel.h>
 #endif
 
@@ -69,11 +69,8 @@ RobotinoActThread::init()
   last_msg_time_ = clock->now();
 
 #ifdef HAVE_OPENROBOTINO_API_1
-  statemem_ =  new rec::sharedmemory::SharedMemory<rec::iocontrol::robotstate::State>
-    (rec::iocontrol::robotstate::State::sharedMemoryKey);
-  state_ = statemem_->getData();
   omni_drive_ = new rec::robotino::com::OmniDrive();
-#else
+#elif defined(HAVE_OPENROBOTINO_API_2)
   omni_drive_ = new rec::robotino::api2::OmniDriveModel();
 #endif
 
@@ -144,9 +141,8 @@ RobotinoActThread::finalize()
   com_->set_speed_points(0., 0., 0.);
   com_ = NULL;
   delete odom_time_;
+#if defined(HAVE_OPENROBOTINO_API_1) || defined(HAVE_OPENROBOTINO_API_2)
   delete omni_drive_;
-#ifdef HAVE_OPENROBOTINO_API_1
-  delete statemem_;
 #endif
 }
 
@@ -185,7 +181,7 @@ RobotinoActThread::loop()
       omni_drive_->project(&s1, &s2, &s3,
 			   msg->vx() * 1000., msg->vy() * 1000.,
 			   rad2deg(msg->omega()));
-#else
+#elif defined(HAVE_OPENROBOTINO_API_2)
       omni_drive_->project(&s1, &s2, &s3, msg->vx(), msg->vy(), msg->omega());
 #endif
 
@@ -278,8 +274,10 @@ RobotinoActThread::publish_odometry()
   if (seq != last_seqnum_) {
     last_seqnum_ = seq;
 
-    float vx, vy, omega;
+    float vx = 0., vy = 0., omega = 0.;
+#if defined(HAVE_OPENROBOTINO_API_1) || defined(HAVE_OPENROBOTINO_API_2)
     omni_drive_->unproject(&vx, &vy, &omega, a1, a2, a3);
+#endif
 
     motor_if_->set_vx(vx);
     motor_if_->set_vy(vy);
diff --git a/src/plugins/robotino/act_thread.h b/src/plugins/robotino/act_thread.h
index 013352c..bacd1ae 100644
--- a/src/plugins/robotino/act_thread.h
+++ b/src/plugins/robotino/act_thread.h
@@ -106,9 +106,7 @@ class RobotinoActThread
 
 #ifdef HAVE_OPENROBOTINO_API_1
   rec::robotino::com::OmniDrive  *omni_drive_;
-  rec::sharedmemory::SharedMemory<rec::iocontrol::robotstate::State> *statemem_;
-  rec::iocontrol::robotstate::State *state_;
-#else
+#elif defined(HAVE_OPENROBOTINO_API_2)
   rec::robotino::api2::OmniDriveModel *omni_drive_;
 #endif
 
diff --git a/src/plugins/robotino/robotino.mk b/src/plugins/robotino/robotino.mk
index fcd9593..5401f71 100644
--- a/src/plugins/robotino/robotino.mk
+++ b/src/plugins/robotino/robotino.mk
@@ -13,6 +13,8 @@
 #
 #*****************************************************************************
 
+include $(BUILDSYSDIR)/boost.mk
+
 OPENROBOTINOAPI_DIR=/usr/local/OpenRobotinoAPI
 OPENROBOTINOAPI2_DIR=/usr/local/robotino
 
@@ -35,3 +37,23 @@ else
 			   -lrec_iocontrol_robotstate -lrec_iocontrol_remotestate
   endif
 endif
+
+HAVE_LIBUDEV=$(if $(shell $(PKGCONFIG) --exists 'libudev'; echo $${?/1/}),1,0)
+ifeq ($(HAVE_LIBUDEV),1)
+  CFLAGS_LIBUDEV  = -DHAVE_LIBUDEV $(shell $(PKGCONFIG) --cflags 'libudev')
+  LDFLAGS_LIBUDEV = $(shell $(PKGCONFIG) --libs 'libudev')
+endif
+
+HAVE_LIBUSB=$(if $(shell $(PKGCONFIG) --exists 'libusb-1.0'; echo $${?/1/}),1,0)
+ifeq ($(HAVE_LIBUSB),1)
+  CFLAGS_LIBUSB  = -DHAVE_LIBUSB $(shell $(PKGCONFIG) --cflags 'libusb-1.0')
+  LDFLAGS_LIBUSB = $(shell $(PKGCONFIG) --libs 'libusb-1.0')
+endif
+
+ROBOTINO_DIRECT_REQ_BOOST_LIBS = thread asio system
+HAVE_ROBOTINO_DIRECT_BOOST_LIBS = $(call boost-have-libs,$(ROBOTINO_DIRECT_REQ_BOOST_LIBS))
+ifeq ($(HAVE_ROBOTINO_DIRECT_BOOST_LIBS),1)
+  CFLAGS_ROBOTINO_DIRECT_BOOST  = $(call boost-libs-cflags,$(ROBOTINO_DIRECT_REQ_BOOST_LIBS))
+  LDFLAGS_ROBOTINO_DIRECT_BOOST = $(call boost-libs-ldflags,$(ROBOTINO_DIRECT_REQ_BOOST_LIBS)) -lpthread
+endif
+
diff --git a/src/plugins/robotino/robotino_plugin.cpp b/src/plugins/robotino/robotino_plugin.cpp
index 163c092..1e9e678 100644
--- a/src/plugins/robotino/robotino_plugin.cpp
+++ b/src/plugins/robotino/robotino_plugin.cpp
@@ -25,6 +25,9 @@
 #ifdef HAVE_OPENROBOTINO
 #  include "openrobotino_com_thread.h"
 #endif
+#ifdef HAVE_ROBOTINO_DIRECT
+#  include "direct_com_thread.h"
+#endif
 #include "sensor_thread.h"
 #include "act_thread.h"
 
@@ -42,11 +45,25 @@ class RobotinoPlugin : public fawkes::Plugin
   RobotinoPlugin(Configuration *config)
     : Plugin(config)
   {
+	  std::string cfg_driver = config->get_string("/hardware/robotino/driver");
+	  
+	  RobotinoComThread *com_thread = NULL;
+
+	  if (cfg_driver == "openrobotino") {
 #ifdef HAVE_OPENROBOTINO
-    RobotinoComThread *com_thread = new OpenRobotinoComThread();
+		  com_thread = new OpenRobotinoComThread();
+#else
+		  throw Exception("robotino: driver mode 'openrobotino' not available at compile time");
+#endif
+	  } else if (cfg_driver == "direct") {
+#ifdef HAVE_ROBOTINO_DIRECT
+		  com_thread = new DirectRobotinoComThread();    
 #else
-#  error "No com thread implementation available"
+		  throw Exception("robotino: driver mode 'direct' not available at compile time");
 #endif
+	  } else {
+		  throw Exception("robotino: unknown driver '%s'", cfg_driver.c_str());
+	  }
     thread_list.push_back(com_thread);
     thread_list.push_back(new RobotinoSensorThread(com_thread));
     thread_list.push_back(new RobotinoActThread(com_thread));

- *commit* caca8c7357a982124ee614400290e33af2be3d97 - - - - - - - - - -
Author:  Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
Date:    Wed Apr 6 11:29:17 2016 +0200
Subject: robotino: smarttabify robotino plugin

 src/plugins/robotino/act_thread.cpp              |  630 +++++++++---------
 src/plugins/robotino/act_thread.h                |  152 +++---
 src/plugins/robotino/com_thread.cpp              |    2 +-
 src/plugins/robotino/com_thread.h                |   30 +-
 src/plugins/robotino/openrobotino_com_thread.cpp |  756 +++++++++++-----------
 src/plugins/robotino/openrobotino_com_thread.h   |  180 +++---
 src/plugins/robotino/robotino_plugin.cpp         |   44 +-
 src/plugins/robotino/sensor_thread.cpp           |    8 +-
 src/plugins/robotino/sensor_thread.h             |   30 +-
 9 files changed, 916 insertions(+), 916 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/robotino/act_thread.cpp b/src/plugins/robotino/act_thread.cpp
index b0e7c5f..c2f4399 100644
--- a/src/plugins/robotino/act_thread.cpp
+++ b/src/plugins/robotino/act_thread.cpp
@@ -52,213 +52,213 @@ using namespace fawkes;
  * @param com_thread thread that communicates with the hardware base
  */
 RobotinoActThread::RobotinoActThread(RobotinoComThread *com_thread)
-  : Thread("RobotinoActThread", Thread::OPMODE_WAITFORWAKEUP),
+	: Thread("RobotinoActThread", Thread::OPMODE_WAITFORWAKEUP),
 #ifdef HAVE_TF
-    TransformAspect(TransformAspect::ONLY_PUBLISHER, "Robotino Odometry"),
+	  TransformAspect(TransformAspect::ONLY_PUBLISHER, "Robotino Odometry"),
 #endif
-    BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_ACT)
+	  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_ACT)
 {
-  com_ = com_thread;
+	com_ = com_thread;
 }
 
 
 void
 RobotinoActThread::init()
 {
-  last_seqnum_ = 0;
-  last_msg_time_ = clock->now();
+	last_seqnum_ = 0;
+	last_msg_time_ = clock->now();
 
 #ifdef HAVE_OPENROBOTINO_API_1
-  omni_drive_ = new rec::robotino::com::OmniDrive();
+	omni_drive_ = new rec::robotino::com::OmniDrive();
 #elif defined(HAVE_OPENROBOTINO_API_2)
-  omni_drive_ = new rec::robotino::api2::OmniDriveModel();
+	omni_drive_ = new rec::robotino::api2::OmniDriveModel();
 #endif
 
-  //get config values
-  cfg_deadman_threshold_    = config->get_float("/hardware/robotino/deadman_time_threshold");
-  cfg_gripper_enabled_      = config->get_bool("/hardware/robotino/gripper/enable_gripper");
-  cfg_bumper_estop_enabled_ = config->get_bool("/hardware/robotino/bumper/estop_enabled");
-  cfg_odom_time_offset_     = config->get_float("/hardware/robotino/odometry/time_offset");
-  cfg_odom_frame_           = config->get_string("/hardware/robotino/odometry/frame");
-  cfg_base_frame_           = config->get_string("/hardware/robotino/base_frame");
-  std::string odom_mode     = config->get_string("/hardware/robotino/odometry/mode");
-  cfg_odom_corr_phi_        =
-    config->get_float("/hardware/robotino/odometry/calc/correction/phi");
-  cfg_odom_corr_trans_      =
-    config->get_float("/hardware/robotino/odometry/calc/correction/trans");
-
-  std::string imu_if_id;
-
-  imu_if_ = NULL;
-  if (odom_mode == "copy") {
-    cfg_odom_mode_ = ODOM_COPY;
-  } else if (odom_mode == "calc") {
-    cfg_odom_mode_ = ODOM_CALC;
-    imu_if_id =
-      config->get_string("/hardware/robotino/odometry/calc/imu_interface_id");
-    cfg_imu_deadman_loops_ =
-      config->get_uint("/hardware/robotino/odometry/calc/imu_deadman_loops");
-  } else {
-    throw Exception("Invalid odometry mode '%s', must be calc or copy", odom_mode.c_str());
-  }
-
-  gripper_close_ = false;
-
-  msg_received_ = false;
-  msg_zero_vel_ = false;
-
-  des_vx_    = 0.;
-  des_vy_    = 0.;
-  des_omega_ = 0.;
-
-  odom_x_ = odom_y_ = odom_phi_ = 0.;
-  odom_time_ = new Time(clock);
-
-  motor_if_ = blackboard->open_for_writing<MotorInterface>("Robotino");
-  gripper_if_ = blackboard->open_for_writing<GripperInterface>("Robotino");
-
-  if (cfg_odom_mode_ == ODOM_CALC) {
-    imu_if_ = blackboard->open_for_reading<IMUInterface>(imu_if_id.c_str());
-    imu_if_writer_warning_printed_  = false;
-    imu_if_changed_warning_printed_ = false;
-    imu_if_invquat_warning_printed_ = false;
-    imu_if_nochange_loops_          = 0;
-  }
-
-  //state_->emergencyStop.isEnabled = cfg_bumper_estop_enabled_;
-
-  motor_if_->set_motor_state(MotorInterface::MOTOR_ENABLED);
-  motor_if_->write();
+	//get config values
+	cfg_deadman_threshold_    = config->get_float("/hardware/robotino/deadman_time_threshold");
+	cfg_gripper_enabled_      = config->get_bool("/hardware/robotino/gripper/enable_gripper");
+	cfg_bumper_estop_enabled_ = config->get_bool("/hardware/robotino/bumper/estop_enabled");
+	cfg_odom_time_offset_     = config->get_float("/hardware/robotino/odometry/time_offset");
+	cfg_odom_frame_           = config->get_string("/hardware/robotino/odometry/frame");
+	cfg_base_frame_           = config->get_string("/hardware/robotino/base_frame");
+	std::string odom_mode     = config->get_string("/hardware/robotino/odometry/mode");
+	cfg_odom_corr_phi_        =
+		config->get_float("/hardware/robotino/odometry/calc/correction/phi");
+	cfg_odom_corr_trans_      =
+		config->get_float("/hardware/robotino/odometry/calc/correction/trans");
+
+	std::string imu_if_id;
+
+	imu_if_ = NULL;
+	if (odom_mode == "copy") {
+		cfg_odom_mode_ = ODOM_COPY;
+	} else if (odom_mode == "calc") {
+		cfg_odom_mode_ = ODOM_CALC;
+		imu_if_id =
+			config->get_string("/hardware/robotino/odometry/calc/imu_interface_id");
+		cfg_imu_deadman_loops_ =
+			config->get_uint("/hardware/robotino/odometry/calc/imu_deadman_loops");
+	} else {
+		throw Exception("Invalid odometry mode '%s', must be calc or copy", odom_mode.c_str());
+	}
+
+	gripper_close_ = false;
+
+	msg_received_ = false;
+	msg_zero_vel_ = false;
+
+	des_vx_    = 0.;
+	des_vy_    = 0.;
+	des_omega_ = 0.;
+
+	odom_x_ = odom_y_ = odom_phi_ = 0.;
+	odom_time_ = new Time(clock);
+
+	motor_if_ = blackboard->open_for_writing<MotorInterface>("Robotino");
+	gripper_if_ = blackboard->open_for_writing<GripperInterface>("Robotino");
+
+	if (cfg_odom_mode_ == ODOM_CALC) {
+		imu_if_ = blackboard->open_for_reading<IMUInterface>(imu_if_id.c_str());
+		imu_if_writer_warning_printed_  = false;
+		imu_if_changed_warning_printed_ = false;
+		imu_if_invquat_warning_printed_ = false;
+		imu_if_nochange_loops_          = 0;
+	}
+
+	//state_->emergencyStop.isEnabled = cfg_bumper_estop_enabled_;
+
+	motor_if_->set_motor_state(MotorInterface::MOTOR_ENABLED);
+	motor_if_->write();
 }
 
 
 void
 RobotinoActThread::finalize()
 {
-  blackboard->close(imu_if_);
-  blackboard->close(motor_if_);
-  blackboard->close(gripper_if_);
-  com_->set_speed_points(0., 0., 0.);
-  com_ = NULL;
-  delete odom_time_;
+	blackboard->close(imu_if_);
+	blackboard->close(motor_if_);
+	blackboard->close(gripper_if_);
+	com_->set_speed_points(0., 0., 0.);
+	com_ = NULL;
+	delete odom_time_;
 #if defined(HAVE_OPENROBOTINO_API_1) || defined(HAVE_OPENROBOTINO_API_2)
-  delete omni_drive_;
+	delete omni_drive_;
 #endif
 }
 
 void
 RobotinoActThread::loop()
 {
-  if (! com_->is_connected()) {
-    if (! motor_if_->msgq_empty()) {
-      logger->log_warn(name(), "Motor commands received while not connected");
-      motor_if_->msgq_flush();
-    }
-    if (! gripper_if_->msgq_empty()) {
-      logger->log_warn(name(), "Gripper commands received while not connected");
-      gripper_if_->msgq_flush();
-    }
-    return;
-  }
-
-  bool set_speed_points = false;
-  float s1 = 0., s2 = 0., s3 = 0.;
-  bool reset_odometry = false;
-
-  while (! motor_if_->msgq_empty()) {
-    if (MotorInterface::SetMotorStateMessage *msg = motor_if_->msgq_first_safe(msg))
-    {
-      logger->log_info(name(), "%sabling motor on request",
-		       msg->motor_state() == MotorInterface::MOTOR_ENABLED
-		       ? "En" : "Dis");
-      motor_if_->set_motor_state(msg->motor_state());
-      motor_if_->write();
-    }
-
-    else if (MotorInterface::TransRotMessage *msg = motor_if_->msgq_first_safe(msg))
-    {
+	if (! com_->is_connected()) {
+		if (! motor_if_->msgq_empty()) {
+			logger->log_warn(name(), "Motor commands received while not connected");
+			motor_if_->msgq_flush();
+		}
+		if (! gripper_if_->msgq_empty()) {
+			logger->log_warn(name(), "Gripper commands received while not connected");
+			gripper_if_->msgq_flush();
+		}
+		return;
+	}
+
+	bool set_speed_points = false;
+	float s1 = 0., s2 = 0., s3 = 0.;
+	bool reset_odometry = false;
+
+	while (! motor_if_->msgq_empty()) {
+		if (MotorInterface::SetMotorStateMessage *msg = motor_if_->msgq_first_safe(msg))
+		{
+			logger->log_info(name(), "%sabling motor on request",
+			                 msg->motor_state() == MotorInterface::MOTOR_ENABLED
+			                 ? "En" : "Dis");
+			motor_if_->set_motor_state(msg->motor_state());
+			motor_if_->write();
+		}
+
+		else if (MotorInterface::TransRotMessage *msg = motor_if_->msgq_first_safe(msg))
+		{
 #ifdef HAVE_OPENROBOTINO_API_1
-      omni_drive_->project(&s1, &s2, &s3,
-			   msg->vx() * 1000., msg->vy() * 1000.,
-			   rad2deg(msg->omega()));
+			omni_drive_->project(&s1, &s2, &s3,
+			                     msg->vx() * 1000., msg->vy() * 1000.,
+			                     rad2deg(msg->omega()));
 #elif defined(HAVE_OPENROBOTINO_API_2)
-      omni_drive_->project(&s1, &s2, &s3, msg->vx(), msg->vy(), msg->omega());
+			omni_drive_->project(&s1, &s2, &s3, msg->vx(), msg->vy(), msg->omega());
 #endif
 
-      des_vx_    = msg->vx();
-      des_vy_    = msg->vy();
-      des_omega_ = msg->omega();
+			des_vx_    = msg->vx();
+			des_vy_    = msg->vy();
+			des_omega_ = msg->omega();
 
-      set_speed_points = true;
+			set_speed_points = true;
         
-      last_msg_time_ = clock->now();
-      msg_received_ = true;
+			last_msg_time_ = clock->now();
+			msg_received_ = true;
 	
-      msg_zero_vel_ = (s1 == 0.0 && s2 == 0.0 && s3 == 0.0);
-    }
-
-    else if (motor_if_->msgq_first_is<MotorInterface::ResetOdometryMessage>())
-    {
-      odom_x_ = odom_y_ = odom_phi_ = 0.;
-      if (imu_if_) {
-	imu_if_->read();
-	odom_gyro_origin_ = tf::get_yaw(imu_if_->orientation());
-      }
-
-      reset_odometry = true;
-    }
-
-    motor_if_->msgq_pop();
-  } // while motor msgq
-
-  if (cfg_gripper_enabled_) {
-    bool open_gripper = false, close_gripper = false;
-    while (! gripper_if_->msgq_empty()) {
-      if (gripper_if_->msgq_first_is<GripperInterface::OpenGripperMessage>()) {
-	close_gripper = false;
-	open_gripper  = true;
-      } else if (gripper_if_->msgq_first_is<GripperInterface::CloseGripperMessage>()) {
-	close_gripper = true;
-	open_gripper  = false;
-      }
+			msg_zero_vel_ = (s1 == 0.0 && s2 == 0.0 && s3 == 0.0);
+		}
+
+		else if (motor_if_->msgq_first_is<MotorInterface::ResetOdometryMessage>())
+		{
+			odom_x_ = odom_y_ = odom_phi_ = 0.;
+			if (imu_if_) {
+				imu_if_->read();
+				odom_gyro_origin_ = tf::get_yaw(imu_if_->orientation());
+			}
+
+			reset_odometry = true;
+		}
+
+		motor_if_->msgq_pop();
+	} // while motor msgq
+
+	if (cfg_gripper_enabled_) {
+		bool open_gripper = false, close_gripper = false;
+		while (! gripper_if_->msgq_empty()) {
+			if (gripper_if_->msgq_first_is<GripperInterface::OpenGripperMessage>()) {
+				close_gripper = false;
+				open_gripper  = true;
+			} else if (gripper_if_->msgq_first_is<GripperInterface::CloseGripperMessage>()) {
+				close_gripper = true;
+				open_gripper  = false;
+			}
       
-      gripper_if_->msgq_pop(); 
-    }
-
-    if (close_gripper || open_gripper) {
-      gripper_close_ = close_gripper;
-      com_->set_gripper(open_gripper);
-    }
-  } else {
-    if (! gripper_if_->msgq_empty())  gripper_if_->msgq_flush();
-  }
-
-  // deadman switch to set the velocities to zero if no new message arrives
-  double diff =  ( clock->now() - (&last_msg_time_) );
-  if (diff >= cfg_deadman_threshold_ && msg_received_ && !msg_zero_vel_) {
-    logger->log_error(name(), "Time-Gap between TransRotMsgs too large "
-		      "(%f sec.), motion planner alive?", diff);
-    s1 = s2 = s3 = 0.;
-    set_speed_points = true;
-    msg_received_ = false;
-  }
-
-  if (motor_if_->motor_state() == MotorInterface::MOTOR_DISABLED) {
-    if (set_speed_points && ((s1 != 0.0) || (s2 != 0.0) || (s3 != 0.0))) {
-      logger->log_warn(name(), "Motor command received while disabled, ignoring");
-    }
-    s1 = s2 = s3 = 0.;
-    set_speed_points = true;
-  }
-
-  if (set_speed_points)  com_->set_speed_points(s1, s2, s3);
-  if (reset_odometry)    com_->reset_odometry();
-
-  publish_odometry();
-
-  if (cfg_gripper_enabled_) {
-    publish_gripper();
-  }
+			gripper_if_->msgq_pop(); 
+		}
+
+		if (close_gripper || open_gripper) {
+			gripper_close_ = close_gripper;
+			com_->set_gripper(open_gripper);
+		}
+	} else {
+		if (! gripper_if_->msgq_empty())  gripper_if_->msgq_flush();
+	}
+
+	// deadman switch to set the velocities to zero if no new message arrives
+	double diff =  ( clock->now() - (&last_msg_time_) );
+	if (diff >= cfg_deadman_threshold_ && msg_received_ && !msg_zero_vel_) {
+		logger->log_error(name(), "Time-Gap between TransRotMsgs too large "
+		                  "(%f sec.), motion planner alive?", diff);
+		s1 = s2 = s3 = 0.;
+		set_speed_points = true;
+		msg_received_ = false;
+	}
+
+	if (motor_if_->motor_state() == MotorInterface::MOTOR_DISABLED) {
+		if (set_speed_points && ((s1 != 0.0) || (s2 != 0.0) || (s3 != 0.0))) {
+			logger->log_warn(name(), "Motor command received while disabled, ignoring");
+		}
+		s1 = s2 = s3 = 0.;
+		set_speed_points = true;
+	}
+
+	if (set_speed_points)  com_->set_speed_points(s1, s2, s3);
+	if (reset_odometry)    com_->reset_odometry();
+
+	publish_odometry();
+
+	if (cfg_gripper_enabled_) {
+		publish_gripper();
+	}
 
 }
 
@@ -266,160 +266,160 @@ RobotinoActThread::loop()
 void
 RobotinoActThread::publish_odometry()
 {
-  fawkes::Time sensor_time;
-  float a1 = 0., a2 = 0., a3 = 0.;
-  unsigned int seq = 0;
-  com_->get_act_velocity(a1, a2, a3, seq, sensor_time);
+	fawkes::Time sensor_time;
+	float a1 = 0., a2 = 0., a3 = 0.;
+	unsigned int seq = 0;
+	com_->get_act_velocity(a1, a2, a3, seq, sensor_time);
 
-  if (seq != last_seqnum_) {
-    last_seqnum_ = seq;
+	if (seq != last_seqnum_) {
+		last_seqnum_ = seq;
 
-    float vx = 0., vy = 0., omega = 0.;
+		float vx = 0., vy = 0., omega = 0.;
 #if defined(HAVE_OPENROBOTINO_API_1) || defined(HAVE_OPENROBOTINO_API_2)
-    omni_drive_->unproject(&vx, &vy, &omega, a1, a2, a3);
+		omni_drive_->unproject(&vx, &vy, &omega, a1, a2, a3);
 #endif
 
-    motor_if_->set_vx(vx);
-    motor_if_->set_vy(vy);
-    motor_if_->set_omega(omega);
-
-    motor_if_->set_des_vx(des_vx_);
-    motor_if_->set_des_vy(des_vy_);
-    motor_if_->set_des_omega(des_omega_);
-
-    if (cfg_odom_mode_ == ODOM_COPY) {
-      double x, y, phi;
-      com_->get_odometry(x, y, phi);
-      odom_x_   = x;
-      odom_y_   = y;
-      odom_phi_ = phi;
-    } else {
-      float diff_sec = sensor_time - odom_time_;
-      *odom_time_ = sensor_time;
-
-      // velocity-based method
-      if (imu_if_ && imu_if_->has_writer()) {
-	imu_if_->read();
-	if (imu_if_->changed()) {
-	  //float imu_age = now - imu_if_->timestamp();
-	  //logger->log_debug(name(), "IMU age: %f sec", imu_age);
-	  float *ori_q = imu_if_->orientation();
-	  try {
-	    tf::Quaternion q(ori_q[0], ori_q[1], ori_q[2], ori_q[3]);
-	    tf::assert_quaternion_valid(q);
-
-	    // reset no change loop count
-	    imu_if_nochange_loops_ = 0;
-
-	    if (imu_if_writer_warning_printed_ ||
-		imu_if_invquat_warning_printed_ ||
-		imu_if_changed_warning_printed_)
-	    {
-	      float old_odom_gyro_origin = odom_gyro_origin_;
-
-	      // adjust gyro angle for continuity if we used
-	      // wheel odometry for some time
-	      // Note that we use the _updated_ odometry, i.e.  we
-	      // use wheel odometry for the last time frame because
-	      // we do not have any point of reference for the gyro, yet
-	      float wheel_odom_phi = normalize_mirror_rad(odom_phi_ + omega * diff_sec);
-	      odom_gyro_origin_ = tf::get_yaw(q) - wheel_odom_phi;
-
-	      if (imu_if_writer_warning_printed_) {
-		imu_if_writer_warning_printed_ = false;
-		logger->log_info(name(), "IMU writer is back again, "
-				 "adjusted origin to %f (was %f)",
-				 odom_gyro_origin_, old_odom_gyro_origin);
-	      }
-
-	      if (imu_if_changed_warning_printed_) {
-		imu_if_changed_warning_printed_ = false;
-		logger->log_info(name(), "IMU interface changed again, "
-				 "adjusted origin to %f (was %f)",
-				 odom_gyro_origin_, old_odom_gyro_origin);
-	      }
-	      if (imu_if_invquat_warning_printed_) {
-		imu_if_invquat_warning_printed_ = false;
-
-		logger->log_info(name(), "IMU quaternion valid again, "
-				 "adjusted origin to %f (was %f)",
-				 odom_gyro_origin_, old_odom_gyro_origin);
-	      }
-	    }
-
-	    // Yaw taken as asbolute value from IMU, the origin is used
-	    // to smooth recovery of IMU data (see above) or for
-	    // odometry reset requests (see message processing)
-	    odom_phi_ =
-	      normalize_mirror_rad(tf::get_yaw(q) - odom_gyro_origin_);
-
-	  } catch (Exception &e) {
-	    if (! imu_if_invquat_warning_printed_) {
-	      imu_if_invquat_warning_printed_ = true;
-	      logger->log_warn(name(), "Invalid gyro quaternion (%f,%f,%f,%f), "
-			       "falling back to wheel odometry",
-			       ori_q[0], ori_q[1], ori_q[2], ori_q[3]);
-	    }
-	    odom_phi_ =
-	      normalize_mirror_rad(odom_phi_ + omega * diff_sec * cfg_odom_corr_phi_);
-	  }
-	} else {
-	  if (++imu_if_nochange_loops_ > cfg_imu_deadman_loops_) {
-	    if (! imu_if_changed_warning_printed_) {
-	      imu_if_changed_warning_printed_ = true;
-	      logger->log_warn(name(), "IMU interface not changed, "
-			       "falling back to wheel odometry");
-	    }
-	    odom_phi_ =
-	      normalize_mirror_rad(odom_phi_ + omega * diff_sec * cfg_odom_corr_phi_);
-	  } // else use previous odometry yaw value
-	}
-      } else {
-	if (! imu_if_writer_warning_printed_) {
-	  logger->log_warn(name(), "No writer for IMU interface, "
-			   "using wheel odometry only");
-	  imu_if_writer_warning_printed_ = true;
-	}
-
-	odom_phi_ =
-	  normalize_mirror_rad(odom_phi_ + omega * diff_sec * cfg_odom_corr_phi_);
-      }
-
-      odom_x_ += cos(odom_phi_) * vx * diff_sec * cfg_odom_corr_trans_ - sin(odom_phi_) * vy * diff_sec * cfg_odom_corr_trans_;
-      odom_y_ += sin(odom_phi_) * vx * diff_sec * cfg_odom_corr_trans_ + cos(odom_phi_) * vy * diff_sec * cfg_odom_corr_trans_;
-    }
-
-
-    motor_if_->set_odometry_position_x(odom_x_);
-    motor_if_->set_odometry_position_y(odom_y_);
-    motor_if_->set_odometry_orientation(odom_phi_);
-    motor_if_->write();
+		motor_if_->set_vx(vx);
+		motor_if_->set_vy(vy);
+		motor_if_->set_omega(omega);
+
+		motor_if_->set_des_vx(des_vx_);
+		motor_if_->set_des_vy(des_vy_);
+		motor_if_->set_des_omega(des_omega_);
+
+		if (cfg_odom_mode_ == ODOM_COPY) {
+			double x, y, phi;
+			com_->get_odometry(x, y, phi);
+			odom_x_   = x;
+			odom_y_   = y;
+			odom_phi_ = phi;
+		} else {
+			float diff_sec = sensor_time - odom_time_;
+			*odom_time_ = sensor_time;
+
+			// velocity-based method
+			if (imu_if_ && imu_if_->has_writer()) {
+				imu_if_->read();
+				if (imu_if_->changed()) {
+					//float imu_age = now - imu_if_->timestamp();
+					//logger->log_debug(name(), "IMU age: %f sec", imu_age);
+					float *ori_q = imu_if_->orientation();
+					try {
+						tf::Quaternion q(ori_q[0], ori_q[1], ori_q[2], ori_q[3]);
+						tf::assert_quaternion_valid(q);
+
+						// reset no change loop count
+						imu_if_nochange_loops_ = 0;
+
+						if (imu_if_writer_warning_printed_ ||
+						    imu_if_invquat_warning_printed_ ||
+						    imu_if_changed_warning_printed_)
+						{
+							float old_odom_gyro_origin = odom_gyro_origin_;
+
+							// adjust gyro angle for continuity if we used
+							// wheel odometry for some time
+							// Note that we use the _updated_ odometry, i.e.  we
+							// use wheel odometry for the last time frame because
+							// we do not have any point of reference for the gyro, yet
+							float wheel_odom_phi = normalize_mirror_rad(odom_phi_ + omega * diff_sec);
+							odom_gyro_origin_ = tf::get_yaw(q) - wheel_odom_phi;
+
+							if (imu_if_writer_warning_printed_) {
+								imu_if_writer_warning_printed_ = false;
+								logger->log_info(name(), "IMU writer is back again, "
+								                 "adjusted origin to %f (was %f)",
+								                 odom_gyro_origin_, old_odom_gyro_origin);
+							}
+
+							if (imu_if_changed_warning_printed_) {
+								imu_if_changed_warning_printed_ = false;
+								logger->log_info(name(), "IMU interface changed again, "
+								                 "adjusted origin to %f (was %f)",
+								                 odom_gyro_origin_, old_odom_gyro_origin);
+							}
+							if (imu_if_invquat_warning_printed_) {
+								imu_if_invquat_warning_printed_ = false;
+
+								logger->log_info(name(), "IMU quaternion valid again, "
+								                 "adjusted origin to %f (was %f)",
+								                 odom_gyro_origin_, old_odom_gyro_origin);
+							}
+						}
+
+						// Yaw taken as asbolute value from IMU, the origin is used
+						// to smooth recovery of IMU data (see above) or for
+						// odometry reset requests (see message processing)
+						odom_phi_ =
+							normalize_mirror_rad(tf::get_yaw(q) - odom_gyro_origin_);
+
+					} catch (Exception &e) {
+						if (! imu_if_invquat_warning_printed_) {
+							imu_if_invquat_warning_printed_ = true;
+							logger->log_warn(name(), "Invalid gyro quaternion (%f,%f,%f,%f), "
+							                 "falling back to wheel odometry",
+							                 ori_q[0], ori_q[1], ori_q[2], ori_q[3]);
+						}
+						odom_phi_ =
+							normalize_mirror_rad(odom_phi_ + omega * diff_sec * cfg_odom_corr_phi_);
+					}
+				} else {
+					if (++imu_if_nochange_loops_ > cfg_imu_deadman_loops_) {
+						if (! imu_if_changed_warning_printed_) {
+							imu_if_changed_warning_printed_ = true;
+							logger->log_warn(name(), "IMU interface not changed, "
+							                 "falling back to wheel odometry");
+						}
+						odom_phi_ =
+							normalize_mirror_rad(odom_phi_ + omega * diff_sec * cfg_odom_corr_phi_);
+					} // else use previous odometry yaw value
+				}
+			} else {
+				if (! imu_if_writer_warning_printed_) {
+					logger->log_warn(name(), "No writer for IMU interface, "
+					                 "using wheel odometry only");
+					imu_if_writer_warning_printed_ = true;
+				}
+
+				odom_phi_ =
+					normalize_mirror_rad(odom_phi_ + omega * diff_sec * cfg_odom_corr_phi_);
+			}
+
+			odom_x_ += cos(odom_phi_) * vx * diff_sec * cfg_odom_corr_trans_ - sin(odom_phi_) * vy * diff_sec * cfg_odom_corr_trans_;
+			odom_y_ += sin(odom_phi_) * vx * diff_sec * cfg_odom_corr_trans_ + cos(odom_phi_) * vy * diff_sec * cfg_odom_corr_trans_;
+		}
+
+
+		motor_if_->set_odometry_position_x(odom_x_);
+		motor_if_->set_odometry_position_y(odom_y_);
+		motor_if_->set_odometry_orientation(odom_phi_);
+		motor_if_->write();
 
 #ifdef HAVE_TF
-    tf::Transform t(tf::Quaternion(tf::Vector3(0,0,1), odom_phi_),
-		    tf::Vector3(odom_x_, odom_y_, 0.));
-
-    try {
-      tf_publisher->send_transform(t, sensor_time + cfg_odom_time_offset_,
-				   cfg_odom_frame_, cfg_base_frame_);
-    } catch (Exception &e) {
-      logger->log_warn(name(), "Failed to publish odometry transform for "
-		       "(%f,%f,%f), exception follows",
-		       odom_x_, odom_y_, odom_phi_);
-      logger->log_warn(name(), e);
-    }
+		tf::Transform t(tf::Quaternion(tf::Vector3(0,0,1), odom_phi_),
+		                tf::Vector3(odom_x_, odom_y_, 0.));
+
+		try {
+			tf_publisher->send_transform(t, sensor_time + cfg_odom_time_offset_,
+			                             cfg_odom_frame_, cfg_base_frame_);
+		} catch (Exception &e) {
+			logger->log_warn(name(), "Failed to publish odometry transform for "
+			                 "(%f,%f,%f), exception follows",
+			                 odom_x_, odom_y_, odom_phi_);
+			logger->log_warn(name(), e);
+		}
 #endif
-  }
+	}
 }
 
 void
 RobotinoActThread::publish_gripper()
 {
-  if (com_->is_gripper_open()) {
-    gripper_if_->set_gripper_state(GripperInterface::CLOSED);
-    gripper_if_->write();
-  } else {
-    gripper_if_->set_gripper_state(GripperInterface::OPEN);
-    gripper_if_->write();
-  }
+	if (com_->is_gripper_open()) {
+		gripper_if_->set_gripper_state(GripperInterface::CLOSED);
+		gripper_if_->write();
+	} else {
+		gripper_if_->set_gripper_state(GripperInterface::OPEN);
+		gripper_if_->write();
+	}
 }
diff --git a/src/plugins/robotino/act_thread.h b/src/plugins/robotino/act_thread.h
index bacd1ae..ae29e75 100644
--- a/src/plugins/robotino/act_thread.h
+++ b/src/plugins/robotino/act_thread.h
@@ -38,112 +38,112 @@
 
 #ifdef HAVE_OPENROBOTINO_API_1
 namespace rec {
-  namespace robotino {
-    namespace com {
-      class Com;
-      class OmniDrive;
-    }
-  }
-  namespace sharedmemory {
-    template<typename SharedType> class SharedMemory;
-  }
-  namespace iocontrol {
-    namespace robotstate {
-      class State;
-    }
-  }
+	namespace robotino {
+		namespace com {
+			class Com;
+			class OmniDrive;
+		}
+	}
+	namespace sharedmemory {
+		template<typename SharedType> class SharedMemory;
+	}
+	namespace iocontrol {
+		namespace robotstate {
+			class State;
+		}
+	}
 }
 #else
 namespace rec {
-  namespace robotino {
-    namespace api2 {
-      class OmniDriveModel;
-    }
-  }
+	namespace robotino {
+		namespace api2 {
+			class OmniDriveModel;
+		}
+	}
 }
 #endif
 
 namespace fawkes {
-  class MotorInterface;
-  class GripperInterface;
-  class IMUInterface;
+	class MotorInterface;
+	class GripperInterface;
+	class IMUInterface;
 }
 
 class RobotinoComThread;
 
 class RobotinoActThread
 : public fawkes::Thread,
-  public fawkes::LoggingAspect,
-  public fawkes::ConfigurableAspect,
-  public fawkes::ClockAspect,
+	public fawkes::LoggingAspect,
+	public fawkes::ConfigurableAspect,
+	public fawkes::ClockAspect,
 #ifdef HAVE_TF
-  public fawkes::TransformAspect,
+	public fawkes::TransformAspect,
 #endif
-  public fawkes::BlockedTimingAspect,
-  public fawkes::BlackBoardAspect
+	public fawkes::BlockedTimingAspect,
+	public fawkes::BlackBoardAspect
 {
  public:
-  RobotinoActThread(RobotinoComThread *com_thread);
+	RobotinoActThread(RobotinoComThread *com_thread);
 
-  virtual void init();
-  virtual void loop();
-  virtual void finalize();
+	virtual void init();
+	virtual void loop();
+	virtual void finalize();
 
- /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
+	/** Stub to see name in backtrace for easier debugging. @see Thread::run() */
  protected: virtual void run() { Thread::run(); }
 
  private:
-  typedef enum {
-    ODOM_COPY,
-    ODOM_CALC
-  } OdometryMode;
+	typedef enum {
+		ODOM_COPY,
+		ODOM_CALC
+	} OdometryMode;
 
-  void publish_odometry();
-  void publish_gripper();
+	void publish_odometry();
+	void publish_gripper();
 
  private:
-  RobotinoComThread              *com_;
+	RobotinoComThread              *com_;
 
 #ifdef HAVE_OPENROBOTINO_API_1
-  rec::robotino::com::OmniDrive  *omni_drive_;
+	rec::robotino::com::OmniDrive  *omni_drive_;
 #elif defined(HAVE_OPENROBOTINO_API_2)
-  rec::robotino::api2::OmniDriveModel *omni_drive_;
+	rec::robotino::api2::OmniDriveModel *omni_drive_;
 #endif
 
-  unsigned int                    last_seqnum_;
-  fawkes::MotorInterface         *motor_if_;
-  fawkes::GripperInterface       *gripper_if_;
-  fawkes::IMUInterface           *imu_if_;
-  unsigned int                    imu_if_nochange_loops_;
-  bool                            imu_if_writer_warning_printed_;
-  bool                            imu_if_invquat_warning_printed_;
-  bool                            imu_if_changed_warning_printed_;
-  bool        			  msg_received_;
-  bool        			  msg_zero_vel_;
-  fawkes::Time 			  last_msg_time_;
-
-  float        			  cfg_deadman_threshold_;
-  float        			  cfg_odom_time_offset_;
-  bool 				  cfg_gripper_enabled_;
-  std::string                     cfg_odom_frame_;
-  std::string                     cfg_base_frame_;
-  OdometryMode                    cfg_odom_mode_;
-  unsigned int                    cfg_imu_deadman_loops_;
-  float                           cfg_odom_corr_phi_;
-  float                           cfg_odom_corr_trans_;
-  bool                            cfg_bumper_estop_enabled_;
-
-  float                           des_vx_;
-  float                           des_vy_;
-  float                           des_omega_;
-
-  bool                            gripper_close_;
-
-  float                           odom_x_;
-  float                           odom_y_;
-  float                           odom_phi_;
-  float                           odom_gyro_origin_;
-  fawkes::Time                   *odom_time_;
+	unsigned int                    last_seqnum_;
+	fawkes::MotorInterface         *motor_if_;
+	fawkes::GripperInterface       *gripper_if_;
+	fawkes::IMUInterface           *imu_if_;
+	unsigned int                    imu_if_nochange_loops_;
+	bool                            imu_if_writer_warning_printed_;
+	bool                            imu_if_invquat_warning_printed_;
+	bool                            imu_if_changed_warning_printed_;
+	bool        			  msg_received_;
+	bool        			  msg_zero_vel_;
+	fawkes::Time 			  last_msg_time_;
+
+	float        			  cfg_deadman_threshold_;
+	float        			  cfg_odom_time_offset_;
+	bool 				  cfg_gripper_enabled_;
+	std::string                     cfg_odom_frame_;
+	std::string                     cfg_base_frame_;
+	OdometryMode                    cfg_odom_mode_;
+	unsigned int                    cfg_imu_deadman_loops_;
+	float                           cfg_odom_corr_phi_;
+	float                           cfg_odom_corr_trans_;
+	bool                            cfg_bumper_estop_enabled_;
+
+	float                           des_vx_;
+	float                           des_vy_;
+	float                           des_omega_;
+
+	bool                            gripper_close_;
+
+	float                           odom_x_;
+	float                           odom_y_;
+	float                           odom_phi_;
+	float                           odom_gyro_origin_;
+	fawkes::Time                   *odom_time_;
 };
 
 
diff --git a/src/plugins/robotino/com_thread.cpp b/src/plugins/robotino/com_thread.cpp
index 2457992..50e4bfb 100644
--- a/src/plugins/robotino/com_thread.cpp
+++ b/src/plugins/robotino/com_thread.cpp
@@ -75,7 +75,7 @@ using namespace fawkes;
  * @param thread_name name of thread
  */
 RobotinoComThread::RobotinoComThread(const char *thread_name)
-  : Thread(thread_name, Thread::OPMODE_CONTINUOUS)
+	: Thread(thread_name, Thread::OPMODE_CONTINUOUS)
 {
 }
 
diff --git a/src/plugins/robotino/com_thread.h b/src/plugins/robotino/com_thread.h
index d1db566..fa1b589 100644
--- a/src/plugins/robotino/com_thread.h
+++ b/src/plugins/robotino/com_thread.h
@@ -30,13 +30,13 @@
 #include <utils/time/time.h>
 
 namespace fawkes {
-  class Mutex;
-  class Clock;
-  class TimeWait;
+	class Mutex;
+	class Clock;
+	class TimeWait;
 
-  class BatteryInterface;
-  class RobotinoSensorInterface;
-  class IMUInterface;
+	class BatteryInterface;
+	class RobotinoSensorInterface;
+	class IMUInterface;
 }
 
 class RobotinoComThread
@@ -44,18 +44,18 @@ class RobotinoComThread
 {
  public:
 	RobotinoComThread(const char *thread_name);
-  virtual ~RobotinoComThread();
+	virtual ~RobotinoComThread();
 
-  virtual void update_bb_sensor() = 0;
+	virtual void update_bb_sensor() = 0;
 
-  virtual bool is_connected() = 0;
+	virtual bool is_connected() = 0;
 
-  virtual void set_gripper(bool opened) = 0;
-  virtual bool is_gripper_open() = 0;
-  virtual void set_speed_points(float s1, float s2, float s3) = 0;
-  virtual void get_act_velocity(float &a1, float &a2, float &a3, unsigned int &seq, fawkes::Time &t) = 0;
-  virtual void get_odometry(double &x, double &y, double &phi) = 0;
-  virtual void reset_odometry() = 0;
+	virtual void set_gripper(bool opened) = 0;
+	virtual bool is_gripper_open() = 0;
+	virtual void set_speed_points(float s1, float s2, float s3) = 0;
+	virtual void get_act_velocity(float &a1, float &a2, float &a3, unsigned int &seq, fawkes::Time &t) = 0;
+	virtual void get_odometry(double &x, double &y, double &phi) = 0;
+	virtual void reset_odometry() = 0;
 };
 
 
diff --git a/src/plugins/robotino/openrobotino_com_thread.cpp b/src/plugins/robotino/openrobotino_com_thread.cpp
index 1a883fa..f01d4ce 100644
--- a/src/plugins/robotino/openrobotino_com_thread.cpp
+++ b/src/plugins/robotino/openrobotino_com_thread.cpp
@@ -62,10 +62,10 @@ using namespace fawkes;
 
 /** Constructor. */
 OpenRobotinoComThread::OpenRobotinoComThread()
-  : RobotinoComThread("OpenRobotinoComThread")
+	: RobotinoComThread("OpenRobotinoComThread")
 {
 #ifdef HAVE_OPENROBOTINO_API_1
-  com_ = this;
+	com_ = this;
 #endif
 }
 
@@ -79,93 +79,93 @@ OpenRobotinoComThread::~OpenRobotinoComThread()
 void
 OpenRobotinoComThread::init()
 {
-  cfg_hostname_ = config->get_string("/hardware/robotino/hostname");
-  cfg_quit_on_disconnect_ = config->get_bool("/hardware/robotino/quit_on_disconnect");
-  cfg_enable_gyro_ = config->get_bool("/hardware/robotino/gyro/enable");
-  cfg_imu_iface_id_ = config->get_string("/hardware/robotino/gyro/interface_id");
-  cfg_sensor_update_cycle_time_ =
-    config->get_uint("/hardware/robotino/sensor_update_cycle_time");
-  cfg_gripper_enabled_ = config->get_bool("/hardware/robotino/gripper/enable_gripper");
-
-  batt_if_ = NULL;
-  sens_if_ = NULL;
-  imu_if_ = NULL;
-
-  batt_if_ = blackboard->open_for_writing<BatteryInterface>("Robotino");
-  sens_if_ = blackboard->open_for_writing<RobotinoSensorInterface>("Robotino");
-
-  if (cfg_enable_gyro_) {
-    imu_if_ = blackboard->open_for_writing<IMUInterface>(cfg_imu_iface_id_.c_str());
-  }
-
-  // taken from Robotino API2 DistanceSensorImpl.hpp
-  voltage_to_dist_dps_.push_back(std::make_pair(0.3 , 0.41));
-  voltage_to_dist_dps_.push_back(std::make_pair(0.39, 0.35));
-  voltage_to_dist_dps_.push_back(std::make_pair(0.41, 0.30));
-  voltage_to_dist_dps_.push_back(std::make_pair(0.5 , 0.25));
-  voltage_to_dist_dps_.push_back(std::make_pair(0.75, 0.18));
-  voltage_to_dist_dps_.push_back(std::make_pair(0.8 , 0.16));
-  voltage_to_dist_dps_.push_back(std::make_pair(0.95, 0.14));
-  voltage_to_dist_dps_.push_back(std::make_pair(1.05, 0.12));
-  voltage_to_dist_dps_.push_back(std::make_pair(1.3 , 0.10));
-  voltage_to_dist_dps_.push_back(std::make_pair(1.4 , 0.09));
-  voltage_to_dist_dps_.push_back(std::make_pair(1.55, 0.08));
-  voltage_to_dist_dps_.push_back(std::make_pair(1.8 , 0.07));
-  voltage_to_dist_dps_.push_back(std::make_pair(2.35, 0.05));
-  voltage_to_dist_dps_.push_back(std::make_pair(2.55, 0.04));
+	cfg_hostname_ = config->get_string("/hardware/robotino/hostname");
+	cfg_quit_on_disconnect_ = config->get_bool("/hardware/robotino/quit_on_disconnect");
+	cfg_enable_gyro_ = config->get_bool("/hardware/robotino/gyro/enable");
+	cfg_imu_iface_id_ = config->get_string("/hardware/robotino/gyro/interface_id");
+	cfg_sensor_update_cycle_time_ =
+		config->get_uint("/hardware/robotino/sensor_update_cycle_time");
+	cfg_gripper_enabled_ = config->get_bool("/hardware/robotino/gripper/enable_gripper");
+
+	batt_if_ = NULL;
+	sens_if_ = NULL;
+	imu_if_ = NULL;
+
+	batt_if_ = blackboard->open_for_writing<BatteryInterface>("Robotino");
+	sens_if_ = blackboard->open_for_writing<RobotinoSensorInterface>("Robotino");
+
+	if (cfg_enable_gyro_) {
+		imu_if_ = blackboard->open_for_writing<IMUInterface>(cfg_imu_iface_id_.c_str());
+	}
+
+	// taken from Robotino API2 DistanceSensorImpl.hpp
+	voltage_to_dist_dps_.push_back(std::make_pair(0.3 , 0.41));
+	voltage_to_dist_dps_.push_back(std::make_pair(0.39, 0.35));
+	voltage_to_dist_dps_.push_back(std::make_pair(0.41, 0.30));
+	voltage_to_dist_dps_.push_back(std::make_pair(0.5 , 0.25));
+	voltage_to_dist_dps_.push_back(std::make_pair(0.75, 0.18));
+	voltage_to_dist_dps_.push_back(std::make_pair(0.8 , 0.16));
+	voltage_to_dist_dps_.push_back(std::make_pair(0.95, 0.14));
+	voltage_to_dist_dps_.push_back(std::make_pair(1.05, 0.12));
+	voltage_to_dist_dps_.push_back(std::make_pair(1.3 , 0.10));
+	voltage_to_dist_dps_.push_back(std::make_pair(1.4 , 0.09));
+	voltage_to_dist_dps_.push_back(std::make_pair(1.55, 0.08));
+	voltage_to_dist_dps_.push_back(std::make_pair(1.8 , 0.07));
+	voltage_to_dist_dps_.push_back(std::make_pair(2.35, 0.05));
+	voltage_to_dist_dps_.push_back(std::make_pair(2.55, 0.04));
 
 #ifdef HAVE_OPENROBOTINO_API_1
-  statemem_ =  new rec::sharedmemory::SharedMemory<rec::iocontrol::robotstate::State>
-    (rec::iocontrol::robotstate::State::sharedMemoryKey);
-  state_ = statemem_->getData();
-  state_mutex_ = new Mutex();
-  set_state_ = new rec::iocontrol::remotestate::SetState();
-  set_state_->gripper_isEnabled = cfg_gripper_enabled_;
-  active_state_ = 0;
+	statemem_ =  new rec::sharedmemory::SharedMemory<rec::iocontrol::robotstate::State>
+		(rec::iocontrol::robotstate::State::sharedMemoryKey);
+	state_ = statemem_->getData();
+	state_mutex_ = new Mutex();
+	set_state_ = new rec::iocontrol::remotestate::SetState();
+	set_state_->gripper_isEnabled = cfg_gripper_enabled_;
+	active_state_ = 0;
 #endif
 
-  if (imu_if_) {
-    // Assume that the gyro is the CruizCore XG1010 and thus set data
-    // from datasheet
-    imu_if_->set_linear_acceleration(0, -1.);
-    imu_if_->set_angular_velocity_covariance(8, deg2rad(0.1));
-    imu_if_->write();
-  }
+	if (imu_if_) {
+		// Assume that the gyro is the CruizCore XG1010 and thus set data
+		// from datasheet
+		imu_if_->set_linear_acceleration(0, -1.);
+		imu_if_->set_angular_velocity_covariance(8, deg2rad(0.1));
+		imu_if_->write();
+	}
 
-  data_mutex_  = new Mutex();
-  new_data_    = false;
-  last_seqnum_ = 0;
-  time_wait_   = new TimeWait(clock, cfg_sensor_update_cycle_time_);
+	data_mutex_  = new Mutex();
+	new_data_    = false;
+	last_seqnum_ = 0;
+	time_wait_   = new TimeWait(clock, cfg_sensor_update_cycle_time_);
 
 #ifdef HAVE_OPENROBOTINO_API_1
-  com_->setAddress(cfg_hostname_.c_str());
-  com_->setMinimumUpdateCycleTime(cfg_sensor_update_cycle_time_);
-  com_->connect(/* blocking */ false);
+	com_->setAddress(cfg_hostname_.c_str());
+	com_->setMinimumUpdateCycleTime(cfg_sensor_update_cycle_time_);
+	com_->connect(/* blocking */ false);
 #else
-  com_ = new rec::robotino::api2::Com("Fawkes");
-  com_->setAddress(cfg_hostname_.c_str());
-  com_->setAutoReconnectEnabled(false);
-  com_->connectToServer(/* blocking */ true);
-
-  analog_inputs_com_  = new rec::robotino::api2::AnalogInputArray();
-  bumper_com_         = new rec::robotino::api2::Bumper();
-  digital_inputs_com_ = new rec::robotino::api2::DigitalInputArray();
-  distances_com_      = new rec::robotino::api2::DistanceSensorArray();
-  gripper_com_        = new rec::robotino::api2::ElectricalGripper();
-  gyroscope_com_      = new rec::robotino::api2::Gyroscope();
-  motors_com_         = new rec::robotino::api2::MotorArray();
-  odom_com_           = new rec::robotino::api2::Odometry();
-  power_com_          = new rec::robotino::api2::PowerManagement();
-
-  analog_inputs_com_->setComId(com_->id());
-  bumper_com_->setComId(com_->id());
-  digital_inputs_com_->setComId(com_->id());
-  distances_com_->setComId(com_->id());
-  gripper_com_->setComId(com_->id());
-  gyroscope_com_->setComId(com_->id());
-  motors_com_->setComId(com_->id());
-  odom_com_->setComId(com_->id());
-  power_com_->setComId(com_->id());
+	com_ = new rec::robotino::api2::Com("Fawkes");
+	com_->setAddress(cfg_hostname_.c_str());
+	com_->setAutoReconnectEnabled(false);
+	com_->connectToServer(/* blocking */ true);
+
+	analog_inputs_com_  = new rec::robotino::api2::AnalogInputArray();
+	bumper_com_         = new rec::robotino::api2::Bumper();
+	digital_inputs_com_ = new rec::robotino::api2::DigitalInputArray();
+	distances_com_      = new rec::robotino::api2::DistanceSensorArray();
+	gripper_com_        = new rec::robotino::api2::ElectricalGripper();
+	gyroscope_com_      = new rec::robotino::api2::Gyroscope();
+	motors_com_         = new rec::robotino::api2::MotorArray();
+	odom_com_           = new rec::robotino::api2::Odometry();
+	power_com_          = new rec::robotino::api2::PowerManagement();
+
+	analog_inputs_com_->setComId(com_->id());
+	bumper_com_->setComId(com_->id());
+	digital_inputs_com_->setComId(com_->id());
+	distances_com_->setComId(com_->id());
+	gripper_com_->setComId(com_->id());
+	gyroscope_com_->setComId(com_->id());
+	motors_com_->setComId(com_->id());
+	odom_com_->setComId(com_->id());
+	power_com_->setComId(com_->id());
 #endif
 
 }
@@ -174,100 +174,100 @@ OpenRobotinoComThread::init()
 void
 OpenRobotinoComThread::finalize()
 {
-  delete data_mutex_;
-  delete time_wait_;
-  blackboard->close(sens_if_);
-  blackboard->close(batt_if_);
-  blackboard->close(imu_if_);
+	delete data_mutex_;
+	delete time_wait_;
+	blackboard->close(sens_if_);
+	blackboard->close(batt_if_);
+	blackboard->close(imu_if_);
 #ifdef HAVE_OPENROBOTINO_API_1
-  set_state_->speedSetPoint[0] = 0.;
-  set_state_->speedSetPoint[1] = 0.;
-  set_state_->speedSetPoint[2] = 0.;
-  set_state_->gripper_isEnabled = false;
-  com_->setSetState(*set_state_);
-  usleep(50000);
-  delete set_state_;
-  delete state_mutex_;
-  delete statemem_;
+	set_state_->speedSetPoint[0] = 0.;
+	set_state_->speedSetPoint[1] = 0.;
+	set_state_->speedSetPoint[2] = 0.;
+	set_state_->gripper_isEnabled = false;
+	com_->setSetState(*set_state_);
+	usleep(50000);
+	delete set_state_;
+	delete state_mutex_;
+	delete statemem_;
 #else
-  float speeds[3] = { 0, 0, 0 };
-  motors_com_->setSpeedSetPoints(speeds, 3);
-  usleep(50000);
-  delete analog_inputs_com_;
-  delete bumper_com_;
-  delete digital_inputs_com_;
-  delete distances_com_;
-  delete gripper_com_;
-  delete gyroscope_com_;
-  delete motors_com_;
-  delete odom_com_;
-  delete power_com_;
-  delete com_;
+	float speeds[3] = { 0, 0, 0 };
+	motors_com_->setSpeedSetPoints(speeds, 3);
+	usleep(50000);
+	delete analog_inputs_com_;
+	delete bumper_com_;
+	delete digital_inputs_com_;
+	delete distances_com_;
+	delete gripper_com_;
+	delete gyroscope_com_;
+	delete motors_com_;
+	delete odom_com_;
+	delete power_com_;
+	delete com_;
 #endif
 }
 
 void
 OpenRobotinoComThread::once()
 {
-  reset_odometry();
+	reset_odometry();
 }
 
 void
 OpenRobotinoComThread::loop()
 {
-  time_wait_->mark_start();
+	time_wait_->mark_start();
 
-  if (com_->isConnected()) {
-    process_sensor_msgs();
+	if (com_->isConnected()) {
+		process_sensor_msgs();
 
-    MutexLocker lock(data_mutex_);
+		MutexLocker lock(data_mutex_);
 #ifdef HAVE_OPENROBOTINO_API_1
-    process_sensor_state();
+		process_sensor_state();
 #else
-    process_com();
+		process_com();
 #endif
 
 
 #ifdef HAVE_OPENROBOTINO_API_1
-  } else if (com_->connectionState() == rec::robotino::com::Com::NotConnected) {
+	} else if (com_->connectionState() == rec::robotino::com::Com::NotConnected) {
 #else
-  } else {
+	} else {
 #endif
-    if (cfg_quit_on_disconnect_) {
-      logger->log_warn(name(), "Connection lost, quitting (as per config)");
-      fawkes::runtime::quit();
-    } else {
-      // retry connection
+		if (cfg_quit_on_disconnect_) {
+			logger->log_warn(name(), "Connection lost, quitting (as per config)");
+			fawkes::runtime::quit();
+		} else {
+			// retry connection
 #ifdef HAVE_OPENROBOTINO_API_1
-      com_->connect(/* blocking */ false);
+			com_->connect(/* blocking */ false);
 #else
-      com_->connectToServer(/* blocking */ true);
+			com_->connectToServer(/* blocking */ true);
 #endif
-    }
-  }
+		}
+	}
 
-  time_wait_->wait();
+	time_wait_->wait();
 }
 
 
 void
 OpenRobotinoComThread::process_sensor_msgs()
 {
-  // process command messages
-  while (! sens_if_->msgq_empty()) {
-    if (RobotinoSensorInterface::SetBumperEStopEnabledMessage *msg =
-	sens_if_->msgq_first_safe(msg))
-    {
+	// process command messages
+	while (! sens_if_->msgq_empty()) {
+		if (RobotinoSensorInterface::SetBumperEStopEnabledMessage *msg =
+		    sens_if_->msgq_first_safe(msg))
+		{
 #ifdef HAVE_OPENROBOTINO_API_1
-      logger->log_info(name(), "%sabling motor on request",
-		       msg->is_enabled() ? "En" : "Dis");
-      state_->emergencyStop.isEnabled = msg->is_enabled();
+			logger->log_info(name(), "%sabling motor on request",
+			                 msg->is_enabled() ? "En" : "Dis");
+			state_->emergencyStop.isEnabled = msg->is_enabled();
 #else
-      logger->log_info(name(), "Setting emergency stop not yet supported for API2");
+			logger->log_info(name(), "Setting emergency stop not yet supported for API2");
 #endif
-    }
-    sens_if_->msgq_pop();
-  } // while sensor msgq
+		}
+		sens_if_->msgq_pop();
+	} // while sensor msgq
 }
 
 
@@ -275,53 +275,53 @@ void
 OpenRobotinoComThread::process_sensor_state()
 {
 #ifdef HAVE_OPENROBOTINO_API_1
-  state_mutex_->lock();
-  fawkes::Time sensor_time = times_[active_state_];
-  rec::iocontrol::remotestate::SensorState sensor_state = sensor_states_[active_state_];
-  state_mutex_->unlock();
-
-  if (sensor_state.sequenceNumber != last_seqnum_) {
-    new_data_ = true;
-    last_seqnum_ = sensor_state.sequenceNumber;
-
-    // update sensor values in interface
-    sens_if_->set_mot_velocity(sensor_state.actualVelocity);
-    sens_if_->set_mot_position(sensor_state.actualPosition);
-    sens_if_->set_mot_current(sensor_state.motorCurrent);
-    sens_if_->set_bumper(sensor_state.bumper);
-    sens_if_->set_bumper_estop_enabled(state_->emergencyStop.isEnabled);
-    sens_if_->set_digital_in(sensor_state.dIn);
-    sens_if_->set_analog_in(sensor_state.aIn);
-    if (cfg_enable_gyro_) {
-      if (state_->gyro.port == rec::serialport::UNDEFINED) {
-	if (fabs(imu_if_->angular_velocity(0) + 1.) > 0.00001) {
-	  imu_if_->set_angular_velocity(0, -1.);
-	  imu_if_->set_angular_velocity(2,  0.);
-	  imu_if_->set_orientation(0, -1.);
+	state_mutex_->lock();
+	fawkes::Time sensor_time = times_[active_state_];
+	rec::iocontrol::remotestate::SensorState sensor_state = sensor_states_[active_state_];
+	state_mutex_->unlock();
+
+	if (sensor_state.sequenceNumber != last_seqnum_) {
+		new_data_ = true;
+		last_seqnum_ = sensor_state.sequenceNumber;
+
+		// update sensor values in interface
+		sens_if_->set_mot_velocity(sensor_state.actualVelocity);
+		sens_if_->set_mot_position(sensor_state.actualPosition);
+		sens_if_->set_mot_current(sensor_state.motorCurrent);
+		sens_if_->set_bumper(sensor_state.bumper);
+		sens_if_->set_bumper_estop_enabled(state_->emergencyStop.isEnabled);
+		sens_if_->set_digital_in(sensor_state.dIn);
+		sens_if_->set_analog_in(sensor_state.aIn);
+		if (cfg_enable_gyro_) {
+			if (state_->gyro.port == rec::serialport::UNDEFINED) {
+				if (fabs(imu_if_->angular_velocity(0) + 1.) > 0.00001) {
+					imu_if_->set_angular_velocity(0, -1.);
+					imu_if_->set_angular_velocity(2,  0.);
+					imu_if_->set_orientation(0, -1.);
+				}
+			} else {
+				imu_if_->set_angular_velocity(0, 0.);
+				imu_if_->set_angular_velocity(2, state_->gyro.rate);
+
+				tf::Quaternion q = tf::create_quaternion_from_yaw(state_->gyro.angle);
+				imu_if_->set_orientation(0, q.x());
+				imu_if_->set_orientation(1, q.y());
+				imu_if_->set_orientation(2, q.z());
+				imu_if_->set_orientation(3, q.w());
+			}
+		}
+
+		update_distances(sensor_state.distanceSensor);
+
+		batt_if_->set_voltage(roundf(sensor_state.voltage * 1000.));
+		batt_if_->set_current(roundf(sensor_state.current));
+
+		// 21.0V is empty, 26.0V is empty, from OpenRobotino lcdd
+		float soc = (sensor_state.voltage - 21.0f) / 5.f;
+		soc = std::min(1.f, std::max(0.f, soc));
+
+		batt_if_->set_absolute_soc(soc);
 	}
-      } else {
-	imu_if_->set_angular_velocity(0, 0.);
-	imu_if_->set_angular_velocity(2, state_->gyro.rate);
-
-	tf::Quaternion q = tf::create_quaternion_from_yaw(state_->gyro.angle);
-	imu_if_->set_orientation(0, q.x());
-	imu_if_->set_orientation(1, q.y());
-	imu_if_->set_orientation(2, q.z());
-	imu_if_->set_orientation(3, q.w());
-      }
-    }
-
-    update_distances(sensor_state.distanceSensor);
-
-    batt_if_->set_voltage(roundf(sensor_state.voltage * 1000.));
-    batt_if_->set_current(roundf(sensor_state.current));
-
-    // 21.0V is empty, 26.0V is empty, from OpenRobotino lcdd
-    float soc = (sensor_state.voltage - 21.0f) / 5.f;
-    soc = std::min(1.f, std::max(0.f, soc));
-
-    batt_if_->set_absolute_soc(soc);
-  }
 #endif
 }
 
@@ -330,97 +330,97 @@ void
 OpenRobotinoComThread::process_com()
 {
 #ifdef HAVE_OPENROBOTINO_API_2
-  com_->processComEvents();
-
-  double odo_x = 0, odo_y = 0, odo_phi = 0;
-  unsigned int odo_seq = 0;
-
-  odom_com_->readings(&odo_x, &odo_y, &odo_phi, &odo_seq);
-
-  if (odo_seq != last_seqnum_) {  
-    new_data_ = true;
-    last_seqnum_ = odo_seq;
-
-    unsigned int mot_num = motors_com_->numMotors();
-    float mot_act_vel[mot_num];
-    int   mot_act_pos[mot_num];
-    float mot_currents[mot_num];
-    motors_com_->actualVelocities(mot_act_vel);
-    motors_com_->actualPositions(mot_act_pos);
-    motors_com_->motorCurrents(mot_currents);
-
-    bool bumper = bumper_com_->value();
-
-    unsigned int digin_num = digital_inputs_com_->numDigitalInputs();
-    int digin_readings[digin_num];
-    bool digin_bools[digin_num];
-    digital_inputs_com_->values(digin_readings);
-    for (unsigned int i = 0; i < digin_num; ++i)  digin_bools[i] = (digin_readings[i] != 0);
-
-    unsigned int anlgin_num = analog_inputs_com_->numAnalogInputs();
-    float anlgin_readings[anlgin_num];
-    analog_inputs_com_->values(anlgin_readings);
-
-    unsigned int dist_num = distances_com_->numDistanceSensors();
-    // the distance calculation from API2 uses a max value of 0.41,
-    // which breaks the previous behavior of 0.0 for "nothing"
-    // therefore use our API1 conversion routine
-    //float dist_distances[dist_num];
-    //distances_com_->distances(dist_distances);
-    float dist_voltages[dist_num];
-    distances_com_->voltages(dist_voltages);
-
-    if (anlgin_num != sens_if_->maxlenof_analog_in()) {
-      logger->log_warn(name(), "Different number of analog inputs: %u vs. %u",
-		       anlgin_num, sens_if_->maxlenof_analog_in());
-    }
-
-    if (digin_num != sens_if_->maxlenof_digital_in()) {
-      logger->log_warn(name(), "Different number of digital inputs: %u vs. %u",
-		       digin_num, sens_if_->maxlenof_digital_in());
-    }
-
-    if (dist_num != sens_if_->maxlenof_distance()) {
-      logger->log_warn(name(), "Different number of distances: %u vs. %u",
-		       dist_num, sens_if_->maxlenof_distance());
-    }
-
-    float pow_current = power_com_->current() * 1000.; // A -> mA
-    float pow_voltage = power_com_->voltage() * 1000.; // V -> mV
-
-    float gyro_angle  = gyroscope_com_->angle();
-    float gyro_rate   = gyroscope_com_->rate();
-
-    // update sensor values in interface
-    sens_if_->set_mot_velocity(mot_act_vel);
-    sens_if_->set_mot_position(mot_act_pos);
-    sens_if_->set_mot_current(mot_currents);
-    sens_if_->set_bumper(bumper);
-    //sens_if_->set_bumper_estop_enabled(???);
-    sens_if_->set_digital_in(digin_bools);
-    sens_if_->set_analog_in(anlgin_readings);
-    if (cfg_enable_gyro_) {
-      imu_if_->set_angular_velocity(0, 0.);
-      imu_if_->set_angular_velocity(2, gyro_rate);
-
-      tf::Quaternion q = tf::create_quaternion_from_yaw(gyro_angle);
-      imu_if_->set_orientation(0, q.x());
-      imu_if_->set_orientation(1, q.y());
-      imu_if_->set_orientation(2, q.z());
-      imu_if_->set_orientation(3, q.w());
-    }
-
-    //sens_if_->set_distance(dist_distances);
-    update_distances(dist_voltages);
-
-    batt_if_->set_voltage(roundf(pow_voltage));
-    batt_if_->set_current(roundf(pow_current));
-
-    // 22.0V is empty, 24.5V is full, this is just a guess
-    float soc = (power_com_->voltage() - 22.0f) / 2.5f;
-    soc = std::min(1.f, std::max(0.f, soc));
-    batt_if_->set_absolute_soc(soc);
-  }
+	com_->processComEvents();
+
+	double odo_x = 0, odo_y = 0, odo_phi = 0;
+	unsigned int odo_seq = 0;
+
+	odom_com_->readings(&odo_x, &odo_y, &odo_phi, &odo_seq);
+
+	if (odo_seq != last_seqnum_) {  
+		new_data_ = true;
+		last_seqnum_ = odo_seq;
+
+		unsigned int mot_num = motors_com_->numMotors();
+		float mot_act_vel[mot_num];
+		int   mot_act_pos[mot_num];
+		float mot_currents[mot_num];
+		motors_com_->actualVelocities(mot_act_vel);
+		motors_com_->actualPositions(mot_act_pos);
+		motors_com_->motorCurrents(mot_currents);
+
+		bool bumper = bumper_com_->value();
+
+		unsigned int digin_num = digital_inputs_com_->numDigitalInputs();
+		int digin_readings[digin_num];
+		bool digin_bools[digin_num];
+		digital_inputs_com_->values(digin_readings);
+		for (unsigned int i = 0; i < digin_num; ++i)  digin_bools[i] = (digin_readings[i] != 0);
+
+		unsigned int anlgin_num = analog_inputs_com_->numAnalogInputs();
+		float anlgin_readings[anlgin_num];
+		analog_inputs_com_->values(anlgin_readings);
+
+		unsigned int dist_num = distances_com_->numDistanceSensors();
+		// the distance calculation from API2 uses a max value of 0.41,
+		// which breaks the previous behavior of 0.0 for "nothing"
+		// therefore use our API1 conversion routine
+		//float dist_distances[dist_num];
+		//distances_com_->distances(dist_distances);
+		float dist_voltages[dist_num];
+		distances_com_->voltages(dist_voltages);
+
+		if (anlgin_num != sens_if_->maxlenof_analog_in()) {
+			logger->log_warn(name(), "Different number of analog inputs: %u vs. %u",
+			                 anlgin_num, sens_if_->maxlenof_analog_in());
+		}
+
+		if (digin_num != sens_if_->maxlenof_digital_in()) {
+			logger->log_warn(name(), "Different number of digital inputs: %u vs. %u",
+			                 digin_num, sens_if_->maxlenof_digital_in());
+		}
+
+		if (dist_num != sens_if_->maxlenof_distance()) {
+			logger->log_warn(name(), "Different number of distances: %u vs. %u",
+			                 dist_num, sens_if_->maxlenof_distance());
+		}
+
+		float pow_current = power_com_->current() * 1000.; // A -> mA
+		float pow_voltage = power_com_->voltage() * 1000.; // V -> mV
+
+		float gyro_angle  = gyroscope_com_->angle();
+		float gyro_rate   = gyroscope_com_->rate();
+
+		// update sensor values in interface
+		sens_if_->set_mot_velocity(mot_act_vel);
+		sens_if_->set_mot_position(mot_act_pos);
+		sens_if_->set_mot_current(mot_currents);
+		sens_if_->set_bumper(bumper);
+		//sens_if_->set_bumper_estop_enabled(???);
+		sens_if_->set_digital_in(digin_bools);
+		sens_if_->set_analog_in(anlgin_readings);
+		if (cfg_enable_gyro_) {
+			imu_if_->set_angular_velocity(0, 0.);
+			imu_if_->set_angular_velocity(2, gyro_rate);
+
+			tf::Quaternion q = tf::create_quaternion_from_yaw(gyro_angle);
+			imu_if_->set_orientation(0, q.x());
+			imu_if_->set_orientation(1, q.y());
+			imu_if_->set_orientation(2, q.z());
+			imu_if_->set_orientation(3, q.w());
+		}
+
+		//sens_if_->set_distance(dist_distances);
+		update_distances(dist_voltages);
+
+		batt_if_->set_voltage(roundf(pow_voltage));
+		batt_if_->set_current(roundf(pow_current));
+
+		// 22.0V is empty, 24.5V is full, this is just a guess
+		float soc = (power_com_->voltage() - 22.0f) / 2.5f;
+		soc = std::min(1.f, std::max(0.f, soc));
+		batt_if_->set_absolute_soc(soc);
+	}
 #endif
 }
 
@@ -432,54 +432,54 @@ OpenRobotinoComThread::process_com()
 void
 OpenRobotinoComThread::update_bb_sensor()
 {
-  MutexLocker lock(data_mutex_);
-  if (new_data_) {
-    batt_if_->write();
-    sens_if_->write();
-    if (imu_if_)  imu_if_->write();
-    new_data_ = false;
-  }
+	MutexLocker lock(data_mutex_);
+	if (new_data_) {
+		batt_if_->write();
+		sens_if_->write();
+		if (imu_if_)  imu_if_->write();
+		new_data_ = false;
+	}
 }
 
 void
 OpenRobotinoComThread::update_distances(float *voltages)
 {
-  float dist_m[NUM_IR_SENSORS];
-  const size_t num_dps = voltage_to_dist_dps_.size();
-
-  for (int i = 0; i < NUM_IR_SENSORS; ++i) {
-    dist_m[i] = 0.;
-    // find the two enclosing data points
-
-    for (size_t j = 0; j < num_dps - 1; ++j) {
-      // This determines two points, l(eft) and r(ight) that are
-      // defined by voltage (x coord) and distance (y coord). We
-      // assume a linear progression between two adjacent points,
-      // i.e. between l and r. We then do the following:
-      // 1. Find two adjacent voltage values lv and rv where
-      //    the voltage lies inbetween
-      // 2. Interpolate by calculating the line parameters
-      //    m = dd/dv, x = voltage - lv and b = ld.
-      // cf. http://www.acroname.com/robotics/info/articles/irlinear/irlinear.html
-
-      const double lv = voltage_to_dist_dps_[j  ].first;
-      const double rv = voltage_to_dist_dps_[j+1].first;
-
-      if ((voltages[i] >= lv) && (voltages[i] < rv)) {
-        const double ld = voltage_to_dist_dps_[j  ].second;
-        const double rd = voltage_to_dist_dps_[j+1].second;
-
-        double dv = rv - lv;
-        double dd = rd - ld;
-
-        // Linear interpolation between 
-        dist_m[i] = (dd / dv) * (voltages[i] - lv) + ld;
-        break;
-      }
-    }
-  }
-
-  sens_if_->set_distance(dist_m);
+	float dist_m[NUM_IR_SENSORS];
+	const size_t num_dps = voltage_to_dist_dps_.size();
+
+	for (int i = 0; i < NUM_IR_SENSORS; ++i) {
+		dist_m[i] = 0.;
+		// find the two enclosing data points
+
+		for (size_t j = 0; j < num_dps - 1; ++j) {
+			// This determines two points, l(eft) and r(ight) that are
+			// defined by voltage (x coord) and distance (y coord). We
+			// assume a linear progression between two adjacent points,
+			// i.e. between l and r. We then do the following:
+			// 1. Find two adjacent voltage values lv and rv where
+			//    the voltage lies inbetween
+			// 2. Interpolate by calculating the line parameters
+			//    m = dd/dv, x = voltage - lv and b = ld.
+			// cf. http://www.acroname.com/robotics/info/articles/irlinear/irlinear.html
+
+			const double lv = voltage_to_dist_dps_[j  ].first;
+			const double rv = voltage_to_dist_dps_[j+1].first;
+
+			if ((voltages[i] >= lv) && (voltages[i] < rv)) {
+				const double ld = voltage_to_dist_dps_[j  ].second;
+				const double rd = voltage_to_dist_dps_[j+1].second;
+
+				double dv = rv - lv;
+				double dd = rd - ld;
+
+				// Linear interpolation between 
+				dist_m[i] = (dd / dv) * (voltages[i] - lv) + ld;
+				break;
+			}
+		}
+	}
+
+	sens_if_->set_distance(dist_m);
 }
 
 #ifdef HAVE_OPENROBOTINO_API_1
@@ -487,12 +487,12 @@ OpenRobotinoComThread::update_distances(float *voltages)
 void
 OpenRobotinoComThread::updateEvent()
 {
-  unsigned int next_state = 1 - active_state_;
-  sensor_states_[next_state] = sensorState();
-  times_[next_state].stamp();
+	unsigned int next_state = 1 - active_state_;
+	sensor_states_[next_state] = sensorState();
+	times_[next_state].stamp();
 
-  MutexLocker lock(state_mutex_);
-  active_state_ = next_state;
+	MutexLocker lock(state_mutex_);
+	active_state_ = next_state;
 }
 #endif
 
@@ -501,16 +501,16 @@ OpenRobotinoComThread::updateEvent()
 void
 OpenRobotinoComThread::reset_odometry()
 {
-  if (com_->isConnected()) {
+	if (com_->isConnected()) {
 #ifdef HAVE_OPENROBOTINO_API_1
-    set_state_->setOdometry = true;
-    set_state_->odometryX = set_state_->odometryY = set_state_->odometryPhi = 0.;
-    com_->setSetState(*set_state_);
-    set_state_->setOdometry = false;
+		set_state_->setOdometry = true;
+		set_state_->odometryX = set_state_->odometryY = set_state_->odometryPhi = 0.;
+		com_->setSetState(*set_state_);
+		set_state_->setOdometry = false;
 #else
-    odom_com_->set(0., 0., 0., /* blocking */ true);
+		odom_com_->set(0., 0., 0., /* blocking */ true);
 #endif
-  }
+	}
 }
 
 
@@ -520,7 +520,7 @@ OpenRobotinoComThread::reset_odometry()
 bool
 OpenRobotinoComThread::is_connected()
 {
-  return com_->isConnected();
+	return com_->isConnected();
 }
 
 
@@ -534,29 +534,29 @@ OpenRobotinoComThread::is_connected()
 void
 OpenRobotinoComThread::get_act_velocity(float &a1, float &a2, float &a3, unsigned int &seq, fawkes::Time &t)
 {
-  MutexLocker lock(data_mutex_);
+	MutexLocker lock(data_mutex_);
 
 #ifdef HAVE_OPENROBOTINO_API_1
-  state_mutex_->lock();
-  t = times_[active_state_];
-  rec::iocontrol::remotestate::SensorState sensor_state = sensor_states_[active_state_];
-  state_mutex_->unlock();
-
-  // div by 1000 to convert from mm to m
-  a1 = sensor_state.actualVelocity[0] / 1000.f;
-  a2 = sensor_state.actualVelocity[1] / 1000.f;
-  a3 = sensor_state.actualVelocity[2] / 1000.f;
-  seq = sensor_state.sequenceNumber;
+	state_mutex_->lock();
+	t = times_[active_state_];
+	rec::iocontrol::remotestate::SensorState sensor_state = sensor_states_[active_state_];
+	state_mutex_->unlock();
+
+	// div by 1000 to convert from mm to m
+	a1 = sensor_state.actualVelocity[0] / 1000.f;
+	a2 = sensor_state.actualVelocity[1] / 1000.f;
+	a3 = sensor_state.actualVelocity[2] / 1000.f;
+	seq = sensor_state.sequenceNumber;
 #else
-  float mot_act_vel[motors_com_->numMotors()];
-  motors_com_->actualVelocities(mot_act_vel);
+	float mot_act_vel[motors_com_->numMotors()];
+	motors_com_->actualVelocities(mot_act_vel);
 
-  double odo_x = 0, odo_y = 0, odo_phi = 0;
-  odom_com_->readings(&odo_x, &odo_y, &odo_phi, &seq);
+	double odo_x = 0, odo_y = 0, odo_phi = 0;
+	odom_com_->readings(&odo_x, &odo_y, &odo_phi, &seq);
 
-  a1 = mot_act_vel[0];
-  a2 = mot_act_vel[1];
-  a3 = mot_act_vel[2];
+	a1 = mot_act_vel[0];
+	a2 = mot_act_vel[1];
+	a3 = mot_act_vel[2];
 #endif
 }
 
@@ -569,20 +569,20 @@ OpenRobotinoComThread::get_act_velocity(float &a1, float &a2, float &a3, unsigne
 void
 OpenRobotinoComThread::get_odometry(double &x, double &y, double &phi)
 {
-  MutexLocker lock(data_mutex_);
+	MutexLocker lock(data_mutex_);
 
 #ifdef HAVE_OPENROBOTINO_API_1
-  state_mutex_->lock();
-  rec::iocontrol::remotestate::SensorState sensor_state = sensor_states_[active_state_];
-  state_mutex_->unlock();
+	state_mutex_->lock();
+	rec::iocontrol::remotestate::SensorState sensor_state = sensor_states_[active_state_];
+	state_mutex_->unlock();
 
-  x   = sensor_state.odometryX / 1000.f;
-  y   = sensor_state.odometryY / 1000.f;
-  phi = deg2rad(sensor_state.odometryPhi);
+	x   = sensor_state.odometryX / 1000.f;
+	y   = sensor_state.odometryY / 1000.f;
+	phi = deg2rad(sensor_state.odometryPhi);
 
 #else
-  unsigned int seq;
-  odom_com_->readings(&x, &y, &phi, &seq);
+	unsigned int seq;
+	odom_com_->readings(&x, &y, &phi, &seq);
 #endif
 }
 
@@ -593,16 +593,16 @@ OpenRobotinoComThread::get_odometry(double &x, double &y, double &phi)
 bool
 OpenRobotinoComThread::is_gripper_open()
 {
-  MutexLocker lock(data_mutex_);
+	MutexLocker lock(data_mutex_);
 
 #ifdef HAVE_OPENROBOTINO_API_1
-  state_mutex_->lock();
-  rec::iocontrol::remotestate::SensorState sensor_state = sensor_states_[active_state_];
-  state_mutex_->unlock();
+	state_mutex_->lock();
+	rec::iocontrol::remotestate::SensorState sensor_state = sensor_states_[active_state_];
+	state_mutex_->unlock();
 
-  return sensor_state.isGripperOpened;
+	return sensor_state.isGripperOpened;
 #else
-  return gripper_com_->isOpened();
+	return gripper_com_->isOpened();
 #endif
 }
 
@@ -616,14 +616,14 @@ void
 OpenRobotinoComThread::set_speed_points(float s1, float s2, float s3)
 {
 #ifdef HAVE_OPENROBOTINO_API_1
-  set_state_->speedSetPoint[0] = s1;
-  set_state_->speedSetPoint[1] = s2;
-  set_state_->speedSetPoint[2] = s3;
+	set_state_->speedSetPoint[0] = s1;
+	set_state_->speedSetPoint[1] = s2;
+	set_state_->speedSetPoint[2] = s3;
 
-  com_->setSetState(*set_state_);
+	com_->setSetState(*set_state_);
 #else
-  float speeds[3] = { s1, s2, s3 };
-  motors_com_->setSpeedSetPoints(speeds, 3);
+	float speeds[3] = { s1, s2, s3 };
+	motors_com_->setSpeedSetPoints(speeds, 3);
 #endif
 
 }
@@ -636,14 +636,14 @@ void
 OpenRobotinoComThread::set_gripper(bool opened)
 {
 #ifdef HAVE_OPENROBOTINO_API_1
-  set_state_->gripper_close = ! opened;
-  com_->setSetState(*set_state_);
+	set_state_->gripper_close = ! opened;
+	com_->setSetState(*set_state_);
 #else
-  if (opened) {
-    gripper_com_->open();
-  } else {
-    gripper_com_->close();
-  }
+	if (opened) {
+		gripper_com_->open();
+	} else {
+		gripper_com_->close();
+	}
 #endif
 
 }
diff --git a/src/plugins/robotino/openrobotino_com_thread.h b/src/plugins/robotino/openrobotino_com_thread.h
index 0f01703..34a4344 100644
--- a/src/plugins/robotino/openrobotino_com_thread.h
+++ b/src/plugins/robotino/openrobotino_com_thread.h
@@ -33,133 +33,133 @@
 #ifdef HAVE_OPENROBOTINO_API_1
 #  include <rec/robotino/com/Com.h>
 namespace rec {
-  namespace sharedmemory {
-    template<typename SharedType> class SharedMemory;
-  }
-  namespace iocontrol {
-    namespace robotstate {
-      class State;
-    }
-    namespace remotestate {
-      class SetState;
-    }
-  }
+	namespace sharedmemory {
+		template<typename SharedType> class SharedMemory;
+	}
+	namespace iocontrol {
+		namespace robotstate {
+			class State;
+		}
+		namespace remotestate {
+			class SetState;
+		}
+	}
 }
 #else
 namespace rec {
-  namespace robotino {
-    namespace api2 {
-      class Com;
-      class AnalogInputArray;
-      class Bumper;
-      class DigitalInputArray;
-      class DistanceSensorArray;
-      class ElectricalGripper;
-      class Gyroscope;
-      class MotorArray;
-      class Odometry;
-      class PowerManagement;
-    }
-  }
+	namespace robotino {
+		namespace api2 {
+			class Com;
+			class AnalogInputArray;
+			class Bumper;
+			class DigitalInputArray;
+			class DistanceSensorArray;
+			class ElectricalGripper;
+			class Gyroscope;
+			class MotorArray;
+			class Odometry;
+			class PowerManagement;
+		}
+	}
 }
 #endif
 
 namespace fawkes {
-  class Mutex;
-  class Clock;
-  class TimeWait;
+	class Mutex;
+	class Clock;
+	class TimeWait;
 
-  class BatteryInterface;
-  class RobotinoSensorInterface;
-  class IMUInterface;
+	class BatteryInterface;
+	class RobotinoSensorInterface;
+	class IMUInterface;
 }
 
 class OpenRobotinoComThread
 : public RobotinoComThread,
 #ifdef HAVE_OPENROBOTINO_API_1
-  public rec::robotino::com::Com,
+	public rec::robotino::com::Com,
 #endif
-  public fawkes::LoggingAspect,
-  public fawkes::ConfigurableAspect,
-  public fawkes::ClockAspect,
-  public fawkes::BlackBoardAspect
+	public fawkes::LoggingAspect,
+	public fawkes::ConfigurableAspect,
+	public fawkes::ClockAspect,
+	public fawkes::BlackBoardAspect
 {
  public:
-  OpenRobotinoComThread();
-  virtual ~OpenRobotinoComThread();
+	OpenRobotinoComThread();
+	virtual ~OpenRobotinoComThread();
 
-  virtual void init();
-  virtual void once();
-  virtual void loop();
-  virtual void finalize();
+	virtual void init();
+	virtual void once();
+	virtual void loop();
+	virtual void finalize();
 
-  void update_bb_sensor();
+	void update_bb_sensor();
 
-  bool is_connected();
+	bool is_connected();
 
-  void set_gripper(bool opened);
-  bool is_gripper_open();
-  void set_speed_points(float s1, float s2, float s3);
-  void get_act_velocity(float &a1, float &a2, float &a3, unsigned int &seq, fawkes::Time &t);
-  void get_odometry(double &x, double &y, double &phi);
-  void reset_odometry();
+	void set_gripper(bool opened);
+	bool is_gripper_open();
+	void set_speed_points(float s1, float s2, float s3);
+	void get_act_velocity(float &a1, float &a2, float &a3, unsigned int &seq, fawkes::Time &t);
+	void get_odometry(double &x, double &y, double &phi);
+	void reset_odometry();
 
- /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
+	/** Stub to see name in backtrace for easier debugging. @see Thread::run() */
  protected: virtual void run() { Thread::run(); }
 
  private:
 #ifdef HAVE_OPENROBOTINO_API_1
-  using rec::robotino::com::Com::sensorState;
-  virtual void updateEvent();
+	using rec::robotino::com::Com::sensorState;
+	virtual void updateEvent();
 #endif
-  void process_sensor_msgs();
-  void process_sensor_state();
-  void process_com();
-  void update_distances(float *voltages);
+	void process_sensor_msgs();
+	void process_sensor_state();
+	void process_com();
+	void update_distances(float *voltages);
 
  private:
-  std::string     cfg_hostname_;
-  bool            cfg_quit_on_disconnect_;
-  bool            cfg_enable_gyro_;
-  std::string     cfg_imu_iface_id_;
-  unsigned int    cfg_sensor_update_cycle_time_;
-  bool            cfg_gripper_enabled_;
+	std::string     cfg_hostname_;
+	bool            cfg_quit_on_disconnect_;
+	bool            cfg_enable_gyro_;
+	std::string     cfg_imu_iface_id_;
+	unsigned int    cfg_sensor_update_cycle_time_;
+	bool            cfg_gripper_enabled_;
 
-  // Voltage to distance data points
-  std::vector<std::pair<double, double> > voltage_to_dist_dps_;
+	// Voltage to distance data points
+	std::vector<std::pair<double, double> > voltage_to_dist_dps_;
 
-  fawkes::Mutex    *data_mutex_;
-  bool              new_data_;
-  fawkes::TimeWait *time_wait_;
-  unsigned int      last_seqnum_;
+	fawkes::Mutex    *data_mutex_;
+	bool              new_data_;
+	fawkes::TimeWait *time_wait_;
+	unsigned int      last_seqnum_;
 
-  fawkes::BatteryInterface        *batt_if_;
-  fawkes::RobotinoSensorInterface *sens_if_;
-  fawkes::IMUInterface            *imu_if_;
+	fawkes::BatteryInterface        *batt_if_;
+	fawkes::RobotinoSensorInterface *sens_if_;
+	fawkes::IMUInterface            *imu_if_;
 
 #ifdef HAVE_OPENROBOTINO_API_1
-  rec::robotino::com::Com *com_;
-  fawkes::Mutex *state_mutex_;
-  unsigned int active_state_;
-  rec::iocontrol::remotestate::SensorState sensor_states_[2];
-  fawkes::Time times_[2];
+	rec::robotino::com::Com *com_;
+	fawkes::Mutex *state_mutex_;
+	unsigned int active_state_;
+	rec::iocontrol::remotestate::SensorState sensor_states_[2];
+	fawkes::Time times_[2];
 
-  rec::sharedmemory::SharedMemory<rec::iocontrol::robotstate::State> *statemem_;
-  rec::iocontrol::robotstate::State *state_;
+	rec::sharedmemory::SharedMemory<rec::iocontrol::robotstate::State> *statemem_;
+	rec::iocontrol::robotstate::State *state_;
 
-  rec::iocontrol::remotestate::SetState *set_state_;
+	rec::iocontrol::remotestate::SetState *set_state_;
 
 #else
-  rec::robotino::api2::Com                    *com_;
-  rec::robotino::api2::AnalogInputArray       *analog_inputs_com_;
-  rec::robotino::api2::Bumper                 *bumper_com_;
-  rec::robotino::api2::DigitalInputArray      *digital_inputs_com_;
-  rec::robotino::api2::DistanceSensorArray    *distances_com_;
-  rec::robotino::api2::ElectricalGripper      *gripper_com_;
-  rec::robotino::api2::Gyroscope              *gyroscope_com_;
-  rec::robotino::api2::MotorArray             *motors_com_;
-  rec::robotino::api2::Odometry               *odom_com_;
-  rec::robotino::api2::PowerManagement        *power_com_;
+	rec::robotino::api2::Com                    *com_;
+	rec::robotino::api2::AnalogInputArray       *analog_inputs_com_;
+	rec::robotino::api2::Bumper                 *bumper_com_;
+	rec::robotino::api2::DigitalInputArray      *digital_inputs_com_;
+	rec::robotino::api2::DistanceSensorArray    *distances_com_;
+	rec::robotino::api2::ElectricalGripper      *gripper_com_;
+	rec::robotino::api2::Gyroscope              *gyroscope_com_;
+	rec::robotino::api2::MotorArray             *motors_com_;
+	rec::robotino::api2::Odometry               *odom_com_;
+	rec::robotino::api2::PowerManagement        *power_com_;
 #endif
 };
 
diff --git a/src/plugins/robotino/robotino_plugin.cpp b/src/plugins/robotino/robotino_plugin.cpp
index 1e9e678..598ac95 100644
--- a/src/plugins/robotino/robotino_plugin.cpp
+++ b/src/plugins/robotino/robotino_plugin.cpp
@@ -38,36 +38,36 @@ using namespace fawkes;
  */
 class RobotinoPlugin : public fawkes::Plugin
 {
- public:
-  /** Constructor.
-   * @param config Fawkes configuration
-   */
-  RobotinoPlugin(Configuration *config)
-    : Plugin(config)
-  {
-	  std::string cfg_driver = config->get_string("/hardware/robotino/driver");
+public:
+	/** Constructor.
+	 * @param config Fawkes configuration
+	 */
+	RobotinoPlugin(Configuration *config)
+		: Plugin(config)
+	{
+		std::string cfg_driver = config->get_string("/hardware/robotino/driver");
 	  
-	  RobotinoComThread *com_thread = NULL;
+		RobotinoComThread *com_thread = NULL;
 
-	  if (cfg_driver == "openrobotino") {
+		if (cfg_driver == "openrobotino") {
 #ifdef HAVE_OPENROBOTINO
-		  com_thread = new OpenRobotinoComThread();
+			com_thread = new OpenRobotinoComThread();
 #else
-		  throw Exception("robotino: driver mode 'openrobotino' not available at compile time");
+			throw Exception("robotino: driver mode 'openrobotino' not available at compile time");
 #endif
-	  } else if (cfg_driver == "direct") {
+		} else if (cfg_driver == "direct") {
 #ifdef HAVE_ROBOTINO_DIRECT
-		  com_thread = new DirectRobotinoComThread();    
+			com_thread = new DirectRobotinoComThread();    
 #else
-		  throw Exception("robotino: driver mode 'direct' not available at compile time");
+			throw Exception("robotino: driver mode 'direct' not available at compile time");
 #endif
-	  } else {
-		  throw Exception("robotino: unknown driver '%s'", cfg_driver.c_str());
-	  }
-    thread_list.push_back(com_thread);
-    thread_list.push_back(new RobotinoSensorThread(com_thread));
-    thread_list.push_back(new RobotinoActThread(com_thread));
-  }
+		} else {
+			throw Exception("robotino: unknown driver '%s'", cfg_driver.c_str());
+		}
+		thread_list.push_back(com_thread);
+		thread_list.push_back(new RobotinoSensorThread(com_thread));
+		thread_list.push_back(new RobotinoActThread(com_thread));
+	}
 };
 
 PLUGIN_DESCRIPTION("Robotino platform support")
diff --git a/src/plugins/robotino/sensor_thread.cpp b/src/plugins/robotino/sensor_thread.cpp
index 3847368..0b7492d 100644
--- a/src/plugins/robotino/sensor_thread.cpp
+++ b/src/plugins/robotino/sensor_thread.cpp
@@ -35,10 +35,10 @@ using namespace fawkes;
  * @param com_thread communication thread to trigger for writing data
  */
 RobotinoSensorThread::RobotinoSensorThread(RobotinoComThread *com_thread)
-  : Thread("RobotinoSensorThread", Thread::OPMODE_WAITFORWAKEUP),
-    BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE)
+	: Thread("RobotinoSensorThread", Thread::OPMODE_WAITFORWAKEUP),
+	  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE)
 {
-  com_ = com_thread;
+	com_ = com_thread;
 }
 
 
@@ -56,5 +56,5 @@ RobotinoSensorThread::finalize()
 void
 RobotinoSensorThread::loop()
 {
-  com_->update_bb_sensor();
+	com_->update_bb_sensor();
 }
diff --git a/src/plugins/robotino/sensor_thread.h b/src/plugins/robotino/sensor_thread.h
index 06952fd..12e449c 100644
--- a/src/plugins/robotino/sensor_thread.h
+++ b/src/plugins/robotino/sensor_thread.h
@@ -36,35 +36,35 @@
 class RobotinoComThread;
 
 namespace fawkes {
-  class BatteryInterface;
-  class RobotinoSensorInterface;
-  class IMUInterface;
+	class BatteryInterface;
+	class RobotinoSensorInterface;
+	class IMUInterface;
 }
 
 
 class RobotinoSensorThread
 : public fawkes::Thread,
-  public fawkes::BlockedTimingAspect,
-  public fawkes::LoggingAspect,
-  public fawkes::ClockAspect,
-  public fawkes::ConfigurableAspect,
-  public fawkes::BlackBoardAspect
+	public fawkes::BlockedTimingAspect,
+	public fawkes::LoggingAspect,
+	public fawkes::ClockAspect,
+	public fawkes::ConfigurableAspect,
+	public fawkes::BlackBoardAspect
 {
-  friend class RobotinoActThread;
+	friend class RobotinoActThread;
  public:
-  RobotinoSensorThread(RobotinoComThread *com_thread);
+	RobotinoSensorThread(RobotinoComThread *com_thread);
 
-  virtual void init();
-  virtual void loop();
-  virtual void finalize();
+	virtual void init();
+	virtual void loop();
+	virtual void finalize();
 
- /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
+	/** Stub to see name in backtrace for easier debugging. @see Thread::run() */
  protected: virtual void run() { Thread::run(); }
 
  private: // methods
 
  private: // members
-  RobotinoComThread *com_;
+	RobotinoComThread *com_;
 };
 
 

- *commit* 173d02e3ff6460d92909f0ee093dab11467d4d68 - - - - - - - - - -
Author:  Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
Date:    Wed Apr 6 23:00:10 2016 +0200
Subject: robotino: re-factor blackboard access for sensor data

 src/plugins/robotino/com_thread.cpp              |   49 +++-
 src/plugins/robotino/com_thread.h                |   46 +++-
 src/plugins/robotino/direct_com_thread.cpp       |   94 +------
 src/plugins/robotino/direct_com_thread.h         |   34 +--
 src/plugins/robotino/openrobotino_com_thread.cpp |  313 +++++++---------------
 src/plugins/robotino/openrobotino_com_thread.h   |   40 +--
 src/plugins/robotino/sensor_thread.cpp           |  122 +++++++++-
 src/plugins/robotino/sensor_thread.h             |   14 +
 8 files changed, 347 insertions(+), 365 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/robotino/com_thread.cpp b/src/plugins/robotino/com_thread.cpp
index 50e4bfb..8ac72bf 100644
--- a/src/plugins/robotino/com_thread.cpp
+++ b/src/plugins/robotino/com_thread.cpp
@@ -21,6 +21,9 @@
 
 #include "com_thread.h"
 
+#include <core/threading/mutex.h>
+#include <core/threading/mutex_locker.h>
+
 using namespace fawkes;
 
 /** @class RobotinoComThread "com_thread.h"
@@ -30,11 +33,6 @@ using namespace fawkes;
  * @author Tim Niemueller
  *
  *
- * @fn void RobotinoComThread::update_bb_sensor() = 0
- * Trigger writes of blackboard interfaces.
- * This is meant to be called by the sensor thread so that writes to the
- * blackboard happen in the sensor acquisition hook.
- *
  * @fn void RobotinoComThread::reset_odometry() = 0
  * Reset odometry to zero.
  *
@@ -69,19 +67,60 @@ using namespace fawkes;
  * @param x upon return contains x coordinate of odometry
  * @param y upon return contains y coordinate of odometry
  * @param phi upon return contains rptation of odometry
+ *
+ * @fn void RobotinoComThread::set_bumper_estop_enabled(bool enabled) = 0
+ * Enable or disable emergency stop on bumper contact.
+ * @param enabled true to enable, false to disable
  */
 
+/** @class RobotinoComThread::SensorData "com_thread.h"
+ * Struct to exchange data between com and sensor thread.
+ */
+
+/** Constructor. */
+RobotinoComThread::SensorData::SensorData()
+	: seq(0), mot_velocity{0,0,0}, mot_position{0,0,0}, mot_current{0.,0.,0.},
+	  bumper(false), bumper_estop_enabled(false), digital_in{0,0,0,0,0,0,0,0},
+	  analog_in{0.,0.,0.,0.,0.,0.,0.,0.}, bat_voltage(0.), bat_current(0.),
+	  imu_enabled(false), imu_orientation{0.,0.,0.,0.}, imu_angular_velocity{0.,0.,0.},
+	  imu_angular_velocity_covariance{0.,0.,0.,0.,0.,0.,0.,0.,0.},
+	  ir_voltages{0.,0.,0.,0.,0.,0.,0.,0.,0.}
+{
+}
+
 /** Constructor.
  * @param thread_name name of thread
  */
 RobotinoComThread::RobotinoComThread(const char *thread_name)
 	: Thread(thread_name, Thread::OPMODE_CONTINUOUS)
 {
+	data_mutex_  = new Mutex();
+	new_data_    = false;
 }
 
 
 /** Destructor. */
 RobotinoComThread::~RobotinoComThread()
 {
+	delete data_mutex_;
 }
 
+
+/** Get all current sensor data.
+ * @param sensor_data upon return (true) contains the latest available
+ * sensor data
+ * @return true if new data was available and has been stored in \p
+ * sensor_data, false otherwise
+ */
+bool
+RobotinoComThread::get_data(SensorData &sensor_data)
+{
+	MutexLocker lock(data_mutex_);
+	if (new_data_) {
+		sensor_data = data_;
+		new_data_ = false;
+		return true;
+	} else {
+		return false;
+	}
+}
diff --git a/src/plugins/robotino/com_thread.h b/src/plugins/robotino/com_thread.h
index fa1b589..74fef51 100644
--- a/src/plugins/robotino/com_thread.h
+++ b/src/plugins/robotino/com_thread.h
@@ -39,15 +39,44 @@ namespace fawkes {
 	class IMUInterface;
 }
 
+#define NUM_IR_SENSORS 9
+
 class RobotinoComThread
 : public fawkes::Thread
 {
  public:
+	struct SensorData {
+		SensorData();
+
+		/// @cond INTERNAL
+		unsigned int seq;
+
+		float        mot_velocity[3];
+		int32_t      mot_position[3];
+		float        mot_current[3];
+		bool         bumper;
+		bool         bumper_estop_enabled;
+		bool         digital_in[8];
+		float        analog_in[8];
+
+		float        bat_voltage;
+		float        bat_current;
+		float        bat_absolute_soc;
+
+		bool         imu_enabled;
+		float        imu_orientation[4];
+		float        imu_angular_velocity[3];
+		double       imu_angular_velocity_covariance[9];
+
+		float        ir_voltages[NUM_IR_SENSORS];
+		
+		fawkes::Time time;
+		/// @endcond
+	};
+
 	RobotinoComThread(const char *thread_name);
 	virtual ~RobotinoComThread();
 
-	virtual void update_bb_sensor() = 0;
-
 	virtual bool is_connected() = 0;
 
 	virtual void set_gripper(bool opened) = 0;
@@ -56,6 +85,19 @@ class RobotinoComThread
 	virtual void get_act_velocity(float &a1, float &a2, float &a3, unsigned int &seq, fawkes::Time &t) = 0;
 	virtual void get_odometry(double &x, double &y, double &phi) = 0;
 	virtual void reset_odometry() = 0;
+	virtual void set_bumper_estop_enabled(bool enabled) = 0;
+	
+	virtual bool get_data(SensorData &sensor_data);
+
+ protected:
+	/** Mutex to protect data_. Lock whenever accessing it. */
+	fawkes::Mutex    *data_mutex_;
+	/** Data struct that must be updated whenever new data is available. */
+	SensorData        data_;
+	/** Flag to indicate new data, set to true if data_ is modified. */
+	bool              new_data_;
+
+
 };
 
 
diff --git a/src/plugins/robotino/direct_com_thread.cpp b/src/plugins/robotino/direct_com_thread.cpp
index dd8f4b0..2f94b1d 100644
--- a/src/plugins/robotino/direct_com_thread.cpp
+++ b/src/plugins/robotino/direct_com_thread.cpp
@@ -40,9 +40,6 @@
 
 using namespace fawkes;
 
-#define NUM_IR_SENSORS 9
-
-
 /** @class DirectRobotinoComThread "openrobotino_com_thread.h"
  * Thread to communicate with Robotino via OpenRobotino API (v1 or v2).
  * @author Tim Niemueller
@@ -66,50 +63,11 @@ void
 DirectRobotinoComThread::init()
 {
 	cfg_hostname_ = config->get_string("/hardware/robotino/hostname");
-	cfg_quit_on_disconnect_ = config->get_bool("/hardware/robotino/quit_on_disconnect");
 	cfg_enable_gyro_ = config->get_bool("/hardware/robotino/gyro/enable");
-	cfg_imu_iface_id_ = config->get_string("/hardware/robotino/gyro/interface_id");
 	cfg_sensor_update_cycle_time_ =
 		config->get_uint("/hardware/robotino/sensor_update_cycle_time");
 	cfg_gripper_enabled_ = config->get_bool("/hardware/robotino/gripper/enable_gripper");
 
-	batt_if_ = NULL;
-	sens_if_ = NULL;
-	imu_if_ = NULL;
-
-	batt_if_ = blackboard->open_for_writing<BatteryInterface>("Robotino");
-	sens_if_ = blackboard->open_for_writing<RobotinoSensorInterface>("Robotino");
-
-	if (cfg_enable_gyro_) {
-		imu_if_ = blackboard->open_for_writing<IMUInterface>(cfg_imu_iface_id_.c_str());
-	}
-
-	// taken from Robotino API2 DistanceSensorImpl.hpp
-	voltage_to_dist_dps_.push_back(std::make_pair(0.3 , 0.41));
-	voltage_to_dist_dps_.push_back(std::make_pair(0.39, 0.35));
-	voltage_to_dist_dps_.push_back(std::make_pair(0.41, 0.30));
-	voltage_to_dist_dps_.push_back(std::make_pair(0.5 , 0.25));
-	voltage_to_dist_dps_.push_back(std::make_pair(0.75, 0.18));
-	voltage_to_dist_dps_.push_back(std::make_pair(0.8 , 0.16));
-	voltage_to_dist_dps_.push_back(std::make_pair(0.95, 0.14));
-	voltage_to_dist_dps_.push_back(std::make_pair(1.05, 0.12));
-	voltage_to_dist_dps_.push_back(std::make_pair(1.3 , 0.10));
-	voltage_to_dist_dps_.push_back(std::make_pair(1.4 , 0.09));
-	voltage_to_dist_dps_.push_back(std::make_pair(1.55, 0.08));
-	voltage_to_dist_dps_.push_back(std::make_pair(1.8 , 0.07));
-	voltage_to_dist_dps_.push_back(std::make_pair(2.35, 0.05));
-	voltage_to_dist_dps_.push_back(std::make_pair(2.55, 0.04));
-
-	if (imu_if_) {
-		// Assume that the gyro is the CruizCore XG1010 and thus set data
-		// from datasheet
-		imu_if_->set_linear_acceleration(0, -1.);
-		imu_if_->set_angular_velocity_covariance(8, deg2rad(0.1));
-		imu_if_->write();
-	}
-
-	data_mutex_  = new Mutex();
-	new_data_    = false;
 	last_seqnum_ = 0;
 	time_wait_   = new TimeWait(clock, cfg_sensor_update_cycle_time_ * 1000);
 
@@ -135,11 +93,7 @@ DirectRobotinoComThread::init()
 void
 DirectRobotinoComThread::finalize()
 {
-	delete data_mutex_;
 	delete time_wait_;
-	blackboard->close(sens_if_);
-	blackboard->close(batt_if_);
-	blackboard->close(imu_if_);
 }
 
 void
@@ -159,44 +113,6 @@ DirectRobotinoComThread::loop()
 
 
 void
-DirectRobotinoComThread::process_sensor_msgs()
-{
-	// process command messages
-	while (! sens_if_->msgq_empty()) {
-		if (RobotinoSensorInterface::SetBumperEStopEnabledMessage *msg =
-		    sens_if_->msgq_first_safe(msg))
-		{
-#ifdef HAVE_OPENROBOTINO_API_1
-			logger->log_info(name(), "%sabling motor on request",
-			                 msg->is_enabled() ? "En" : "Dis");
-			state_->emergencyStop.isEnabled = msg->is_enabled();
-#else
-			logger->log_info(name(), "Setting emergency stop not yet supported for API2");
-#endif
-		}
-		sens_if_->msgq_pop();
-	} // while sensor msgq
-}
-
-
-/** Trigger writes of blackboard interfaces.
- * This is meant to be called by the sensor thread so that writes to the
- * blackboard happen in the sensor acquisition hook.
- */
-void
-DirectRobotinoComThread::update_bb_sensor()
-{
-	MutexLocker lock(data_mutex_);
-	if (new_data_) {
-		batt_if_->write();
-		sens_if_->write();
-		if (imu_if_)  imu_if_->write();
-		new_data_ = false;
-	}
-}
-
-
-void
 DirectRobotinoComThread::reset_odometry()
 {
 }
@@ -222,7 +138,6 @@ DirectRobotinoComThread::get_odometry(double &x, double &y, double &phi)
 	MutexLocker lock(data_mutex_);
 }
 
-
 bool
 DirectRobotinoComThread::is_gripper_open()
 {
@@ -230,26 +145,27 @@ DirectRobotinoComThread::is_gripper_open()
 	return false;
 }
 
-
 void
 DirectRobotinoComThread::set_speed_points(float s1, float s2, float s3)
 {
 }
 
-
 void
 DirectRobotinoComThread::set_gripper(bool opened)
 {
 }
 
+void
+DirectRobotinoComThread::set_bumper_estop_enabled(bool enabled)
+{
+}
+
 
 std::string
 DirectRobotinoComThread::find_device_udev()
 {
 	std::string cfg_device = "";
 
-        
-
 	// try to find device using udev
 	struct udev *udev;
 	struct udev_enumerate *enumerate;
diff --git a/src/plugins/robotino/direct_com_thread.h b/src/plugins/robotino/direct_com_thread.h
index 4b6b647..bfb6527 100644
--- a/src/plugins/robotino/direct_com_thread.h
+++ b/src/plugins/robotino/direct_com_thread.h
@@ -49,8 +49,7 @@ class DirectRobotinoComThread
 : public RobotinoComThread,
 	public fawkes::LoggingAspect,
 	public fawkes::ConfigurableAspect,
-	public fawkes::ClockAspect,
-	public fawkes::BlackBoardAspect
+	public fawkes::ClockAspect
 {
  public:
 	DirectRobotinoComThread();
@@ -61,16 +60,15 @@ class DirectRobotinoComThread
 	virtual void loop();
 	virtual void finalize();
 
-	void update_bb_sensor();
+	virtual bool is_connected();
 
-	bool is_connected();
-
-	void set_gripper(bool opened);
-	bool is_gripper_open();
-	void set_speed_points(float s1, float s2, float s3);
-	void get_act_velocity(float &a1, float &a2, float &a3, unsigned int &seq, fawkes::Time &t);
-	void get_odometry(double &x, double &y, double &phi);
-	void reset_odometry();
+	virtual void set_gripper(bool opened);
+	virtual bool is_gripper_open();
+	virtual void set_speed_points(float s1, float s2, float s3);
+	virtual void get_act_velocity(float &a1, float &a2, float &a3, unsigned int &seq, fawkes::Time &t);
+	virtual void get_odometry(double &x, double &y, double &phi);
+	virtual void reset_odometry();
+	virtual void set_bumper_estop_enabled(bool enabled);
 
 	/** Stub to see name in backtrace for easier debugging. @see Thread::run() */
  protected: virtual void run() { Thread::run(); }
@@ -81,11 +79,6 @@ class DirectRobotinoComThread
 	void close_device();
 	void check_deadline();
 
-	void process_sensor_msgs();
-	void process_sensor_state();
-	void process_com();
-	void update_distances(float *voltages);
-
 	void read_packet();
 	void send_message(DirectRobotinoComMessage &msg);
 	std::shared_ptr<DirectRobotinoComMessage>
@@ -94,24 +87,15 @@ class DirectRobotinoComThread
  private:
 	std::string     cfg_device_;
 	std::string     cfg_hostname_;
-	bool            cfg_quit_on_disconnect_;
 	bool            cfg_enable_gyro_;
-	std::string     cfg_imu_iface_id_;
 	unsigned int    cfg_sensor_update_cycle_time_;
 	bool            cfg_gripper_enabled_;
 
-	// Voltage to distance data points
-	std::vector<std::pair<double, double> > voltage_to_dist_dps_;
-
 	fawkes::Mutex    *data_mutex_;
 	bool              new_data_;
 	fawkes::TimeWait *time_wait_;
 	unsigned int      last_seqnum_;
 
-	fawkes::BatteryInterface        *batt_if_;
-	fawkes::RobotinoSensorInterface *sens_if_;
-	fawkes::IMUInterface            *imu_if_;
-
 	boost::asio::io_service       io_service_;
 	boost::asio::serial_port      serial_;
 	boost::asio::io_service::work io_service_work_;
diff --git a/src/plugins/robotino/openrobotino_com_thread.cpp b/src/plugins/robotino/openrobotino_com_thread.cpp
index f01d4ce..deac1ac 100644
--- a/src/plugins/robotino/openrobotino_com_thread.cpp
+++ b/src/plugins/robotino/openrobotino_com_thread.cpp
@@ -1,6 +1,6 @@
 
 /***************************************************************************
- *  com_thread.cpp - Robotino com thread
+ *  openrobotino_com_thread.cpp - Robotino com thread
  *
  *  Created: Thu Sep 11 13:18:00 2014
  *  Copyright  2011-2014  Tim Niemueller [www.niemueller.de]
@@ -38,22 +38,16 @@
 #  include <rec/robotino/api2/PowerManagement.h>
 #endif
 #include <baseapp/run.h>
+#include <utils/math/angle.h>
 #include <core/threading/mutex.h>
 #include <core/threading/mutex_locker.h>
-#include <utils/math/angle.h>
 #include <utils/time/wait.h>
 #include <tf/types.h>
 
-#include <interfaces/BatteryInterface.h>
-#include <interfaces/RobotinoSensorInterface.h>
-#include <interfaces/IMUInterface.h>
-
 #include <unistd.h>
 
 using namespace fawkes;
 
-#define NUM_IR_SENSORS 9
-
 
 /** @class OpenRobotinoComThread "openrobotino_com_thread.h"
  * Thread to communicate with Robotino via OpenRobotino API (v1 or v2).
@@ -81,38 +75,10 @@ OpenRobotinoComThread::init()
 {
 	cfg_hostname_ = config->get_string("/hardware/robotino/hostname");
 	cfg_quit_on_disconnect_ = config->get_bool("/hardware/robotino/quit_on_disconnect");
-	cfg_enable_gyro_ = config->get_bool("/hardware/robotino/gyro/enable");
-	cfg_imu_iface_id_ = config->get_string("/hardware/robotino/gyro/interface_id");
 	cfg_sensor_update_cycle_time_ =
 		config->get_uint("/hardware/robotino/sensor_update_cycle_time");
 	cfg_gripper_enabled_ = config->get_bool("/hardware/robotino/gripper/enable_gripper");
-
-	batt_if_ = NULL;
-	sens_if_ = NULL;
-	imu_if_ = NULL;
-
-	batt_if_ = blackboard->open_for_writing<BatteryInterface>("Robotino");
-	sens_if_ = blackboard->open_for_writing<RobotinoSensorInterface>("Robotino");
-
-	if (cfg_enable_gyro_) {
-		imu_if_ = blackboard->open_for_writing<IMUInterface>(cfg_imu_iface_id_.c_str());
-	}
-
-	// taken from Robotino API2 DistanceSensorImpl.hpp
-	voltage_to_dist_dps_.push_back(std::make_pair(0.3 , 0.41));
-	voltage_to_dist_dps_.push_back(std::make_pair(0.39, 0.35));
-	voltage_to_dist_dps_.push_back(std::make_pair(0.41, 0.30));
-	voltage_to_dist_dps_.push_back(std::make_pair(0.5 , 0.25));
-	voltage_to_dist_dps_.push_back(std::make_pair(0.75, 0.18));
-	voltage_to_dist_dps_.push_back(std::make_pair(0.8 , 0.16));
-	voltage_to_dist_dps_.push_back(std::make_pair(0.95, 0.14));
-	voltage_to_dist_dps_.push_back(std::make_pair(1.05, 0.12));
-	voltage_to_dist_dps_.push_back(std::make_pair(1.3 , 0.10));
-	voltage_to_dist_dps_.push_back(std::make_pair(1.4 , 0.09));
-	voltage_to_dist_dps_.push_back(std::make_pair(1.55, 0.08));
-	voltage_to_dist_dps_.push_back(std::make_pair(1.8 , 0.07));
-	voltage_to_dist_dps_.push_back(std::make_pair(2.35, 0.05));
-	voltage_to_dist_dps_.push_back(std::make_pair(2.55, 0.04));
+	cfg_enable_gyro_ = config->get_bool("/hardware/robotino/gyro/enable");
 
 #ifdef HAVE_OPENROBOTINO_API_1
 	statemem_ =  new rec::sharedmemory::SharedMemory<rec::iocontrol::robotstate::State>
@@ -124,19 +90,20 @@ OpenRobotinoComThread::init()
 	active_state_ = 0;
 #endif
 
-	if (imu_if_) {
+	last_seqnum_ = 0;
+	time_wait_   = new TimeWait(clock, cfg_sensor_update_cycle_time_ * 1000);
+
+	if (cfg_enable_gyro_) {
+		data_.imu_enabled = true;
+
+		for (int i = 0; i < 3; ++i)  data_.imu_angular_velocity[i] = 0.;
+		for (int i = 0; i < 4; ++i)  data_.imu_orientation[i] = 0.;
+		for (int i = 0; i < 9; ++i)  data_.imu_angular_velocity_covariance[i] = 0.;
 		// Assume that the gyro is the CruizCore XG1010 and thus set data
 		// from datasheet
-		imu_if_->set_linear_acceleration(0, -1.);
-		imu_if_->set_angular_velocity_covariance(8, deg2rad(0.1));
-		imu_if_->write();
+		data_.imu_angular_velocity_covariance[8] = deg2rad(0.1);
 	}
-
-	data_mutex_  = new Mutex();
-	new_data_    = false;
-	last_seqnum_ = 0;
-	time_wait_   = new TimeWait(clock, cfg_sensor_update_cycle_time_);
-
+	
 #ifdef HAVE_OPENROBOTINO_API_1
 	com_->setAddress(cfg_hostname_.c_str());
 	com_->setMinimumUpdateCycleTime(cfg_sensor_update_cycle_time_);
@@ -174,11 +141,7 @@ OpenRobotinoComThread::init()
 void
 OpenRobotinoComThread::finalize()
 {
-	delete data_mutex_;
 	delete time_wait_;
-	blackboard->close(sens_if_);
-	blackboard->close(batt_if_);
-	blackboard->close(imu_if_);
 #ifdef HAVE_OPENROBOTINO_API_1
 	set_state_->speedSetPoint[0] = 0.;
 	set_state_->speedSetPoint[1] = 0.;
@@ -218,13 +181,11 @@ OpenRobotinoComThread::loop()
 	time_wait_->mark_start();
 
 	if (com_->isConnected()) {
-		process_sensor_msgs();
-
 		MutexLocker lock(data_mutex_);
 #ifdef HAVE_OPENROBOTINO_API_1
-		process_sensor_state();
+		process_api_v1();
 #else
-		process_com();
+		process_api_v2();
 #endif
 
 
@@ -251,28 +212,7 @@ OpenRobotinoComThread::loop()
 
 
 void
-OpenRobotinoComThread::process_sensor_msgs()
-{
-	// process command messages
-	while (! sens_if_->msgq_empty()) {
-		if (RobotinoSensorInterface::SetBumperEStopEnabledMessage *msg =
-		    sens_if_->msgq_first_safe(msg))
-		{
-#ifdef HAVE_OPENROBOTINO_API_1
-			logger->log_info(name(), "%sabling motor on request",
-			                 msg->is_enabled() ? "En" : "Dis");
-			state_->emergencyStop.isEnabled = msg->is_enabled();
-#else
-			logger->log_info(name(), "Setting emergency stop not yet supported for API2");
-#endif
-		}
-		sens_if_->msgq_pop();
-	} // while sensor msgq
-}
-
-
-void
-OpenRobotinoComThread::process_sensor_state()
+OpenRobotinoComThread::process_api_v1()
 {
 #ifdef HAVE_OPENROBOTINO_API_1
 	state_mutex_->lock();
@@ -285,49 +225,54 @@ OpenRobotinoComThread::process_sensor_state()
 		last_seqnum_ = sensor_state.sequenceNumber;
 
 		// update sensor values in interface
-		sens_if_->set_mot_velocity(sensor_state.actualVelocity);
-		sens_if_->set_mot_position(sensor_state.actualPosition);
-		sens_if_->set_mot_current(sensor_state.motorCurrent);
-		sens_if_->set_bumper(sensor_state.bumper);
-		sens_if_->set_bumper_estop_enabled(state_->emergencyStop.isEnabled);
-		sens_if_->set_digital_in(sensor_state.dIn);
-		sens_if_->set_analog_in(sensor_state.aIn);
+		for (int i = 0; i < 3; ++i) {
+			data_.mot_velocity[i] = sensor_state.actualVelocity[i];
+			data_.mot_position[i] = sensor_state.actualPosition[i];
+			data_.mot_current[i] = sensor_state.motorCurrent[i];
+		}
+		data_.bumper = sensor_state.bumper;
+		data_.bumper_estop_enabled = state_->emergencyStop.isEnabled;
+		for (int i = 0; i < 8; ++i) {
+			data_.digital_in[i] = sensor_state.dIn[i];
+			data_.analog_in[i]  = sensor_state.aIn[i];
+		}
 		if (cfg_enable_gyro_) {
 			if (state_->gyro.port == rec::serialport::UNDEFINED) {
-				if (fabs(imu_if_->angular_velocity(0) + 1.) > 0.00001) {
-					imu_if_->set_angular_velocity(0, -1.);
-					imu_if_->set_angular_velocity(2,  0.);
-					imu_if_->set_orientation(0, -1.);
+				if (fabs(data_.imu_angular_velocity[0] + 1.) > 0.00001) {
+					data_.imu_angular_velocity[0] = -1.;
+					data_.imu_angular_velocity[2] =  0.;
+					data_.imu_orientation[0] = -1.;
 				}
 			} else {
-				imu_if_->set_angular_velocity(0, 0.);
-				imu_if_->set_angular_velocity(2, state_->gyro.rate);
+				data_.imu_angular_velocity[0] = 0.;
+				data_.imu_angular_velocity[2] = state_->gyro.rate;
 
 				tf::Quaternion q = tf::create_quaternion_from_yaw(state_->gyro.angle);
-				imu_if_->set_orientation(0, q.x());
-				imu_if_->set_orientation(1, q.y());
-				imu_if_->set_orientation(2, q.z());
-				imu_if_->set_orientation(3, q.w());
+				data_.imu_orientation[0] = q.x();
+				data_.imu_orientation[1] = q.y();
+				data_.imu_orientation[2] = q.z();
+				data_.imu_orientation[3] = q.w();
 			}
 		}
 
-		update_distances(sensor_state.distanceSensor);
+		for (int i = 0; i < NUM_IR_SENSORS; ++i) {
+			data_.ir_voltages[i] = sensor_state.distanceSensor[i];
+		}
 
-		batt_if_->set_voltage(roundf(sensor_state.voltage * 1000.));
-		batt_if_->set_current(roundf(sensor_state.current));
+		data_.bat_voltage = roundf(sensor_state.voltage * 1000.);
+		data_.bat_current = roundf(sensor_state.current);
 
 		// 21.0V is empty, 26.0V is empty, from OpenRobotino lcdd
 		float soc = (sensor_state.voltage - 21.0f) / 5.f;
 		soc = std::min(1.f, std::max(0.f, soc));
-
-		batt_if_->set_absolute_soc(soc);
+		data_.bat_absolute_soc = soc;
 	}
 #endif
 }
 
 
 void
-OpenRobotinoComThread::process_com()
+OpenRobotinoComThread::process_api_v2()
 {
 #ifdef HAVE_OPENROBOTINO_API_2
 	com_->processComEvents();
@@ -341,49 +286,42 @@ OpenRobotinoComThread::process_com()
 		new_data_ = true;
 		last_seqnum_ = odo_seq;
 
-		unsigned int mot_num = motors_com_->numMotors();
-		float mot_act_vel[mot_num];
-		int   mot_act_pos[mot_num];
-		float mot_currents[mot_num];
-		motors_com_->actualVelocities(mot_act_vel);
-		motors_com_->actualPositions(mot_act_pos);
-		motors_com_->motorCurrents(mot_currents);
+		if (motors_com_->numMotors() != 3) {
+			logger->log_error(name(), "Invalid number of motors, got %u, expected 3",
+			                  motors_com_->numMotors());
+			return;
+		}
+		motors_com_->actualVelocities(data_.mot_velocity);
+		motors_com_->actualPositions(data_.mot_position);
+		motors_com_->motorCurrents(data_.mot_current);
 
-		bool bumper = bumper_com_->value();
+		data_.bumper = bumper_com_->value();
 
-		unsigned int digin_num = digital_inputs_com_->numDigitalInputs();
-		int digin_readings[digin_num];
-		bool digin_bools[digin_num];
+		if (digital_inputs_com_->numDigitalInputs() != 8) {
+			logger->log_error(name(), "Invalid number of digital inputs, got %u, expected 8",
+			                  digital_inputs_com_->numDigitalInputs());
+			return;
+		}
+		int digin_readings[8];
 		digital_inputs_com_->values(digin_readings);
-		for (unsigned int i = 0; i < digin_num; ++i)  digin_bools[i] = (digin_readings[i] != 0);
+		for (unsigned int i = 0; i < 8; ++i)  data_.digital_in[i] = (digin_readings[i] != 0);
 
-		unsigned int anlgin_num = analog_inputs_com_->numAnalogInputs();
-		float anlgin_readings[anlgin_num];
-		analog_inputs_com_->values(anlgin_readings);
+		if (analog_inputs_com_->numAnalogInputs() != 8) {
+			logger->log_error(name(), "Invalid number of analog inputs, got %u, expected 8",
+			                  analog_inputs_com_->numAnalogInputs());
+			return;
+		}
+		analog_inputs_com_->values(data_.analog_in);
 
-		unsigned int dist_num = distances_com_->numDistanceSensors();
+		if (distances_com_->numDistanceSensors() != NUM_IR_SENSORS) {
+			logger->log_error(name(), "Invalid number of distance sensors, got %u, expected 9",
+			                  distances_com_->numDistanceSensors());
+			return;
+		}
 		// the distance calculation from API2 uses a max value of 0.41,
 		// which breaks the previous behavior of 0.0 for "nothing"
 		// therefore use our API1 conversion routine
-		//float dist_distances[dist_num];
-		//distances_com_->distances(dist_distances);
-		float dist_voltages[dist_num];
-		distances_com_->voltages(dist_voltages);
-
-		if (anlgin_num != sens_if_->maxlenof_analog_in()) {
-			logger->log_warn(name(), "Different number of analog inputs: %u vs. %u",
-			                 anlgin_num, sens_if_->maxlenof_analog_in());
-		}
-
-		if (digin_num != sens_if_->maxlenof_digital_in()) {
-			logger->log_warn(name(), "Different number of digital inputs: %u vs. %u",
-			                 digin_num, sens_if_->maxlenof_digital_in());
-		}
-
-		if (dist_num != sens_if_->maxlenof_distance()) {
-			logger->log_warn(name(), "Different number of distances: %u vs. %u",
-			                 dist_num, sens_if_->maxlenof_distance());
-		}
+		distances_com_->voltages(data_.ir_voltages);
 
 		float pow_current = power_com_->current() * 1000.; // A -> mA
 		float pow_voltage = power_com_->voltage() * 1000.; // V -> mV
@@ -391,97 +329,27 @@ OpenRobotinoComThread::process_com()
 		float gyro_angle  = gyroscope_com_->angle();
 		float gyro_rate   = gyroscope_com_->rate();
 
-		// update sensor values in interface
-		sens_if_->set_mot_velocity(mot_act_vel);
-		sens_if_->set_mot_position(mot_act_pos);
-		sens_if_->set_mot_current(mot_currents);
-		sens_if_->set_bumper(bumper);
-		//sens_if_->set_bumper_estop_enabled(???);
-		sens_if_->set_digital_in(digin_bools);
-		sens_if_->set_analog_in(anlgin_readings);
-		if (cfg_enable_gyro_) {
-			imu_if_->set_angular_velocity(0, 0.);
-			imu_if_->set_angular_velocity(2, gyro_rate);
-
-			tf::Quaternion q = tf::create_quaternion_from_yaw(gyro_angle);
-			imu_if_->set_orientation(0, q.x());
-			imu_if_->set_orientation(1, q.y());
-			imu_if_->set_orientation(2, q.z());
-			imu_if_->set_orientation(3, q.w());
-		}
+		data_.bumper_estop_enabled = false;
+		data_.imu_angular_velocity[0] = 0.;
+		data_.imu_angular_velocity[2] = gyro_rate;
 
-		//sens_if_->set_distance(dist_distances);
-		update_distances(dist_voltages);
+		tf::Quaternion q = tf::create_quaternion_from_yaw(gyro_angle);
+		data_.imu_orientation[0] = q.x();
+		data_.imu_orientation[1] = q.y();
+		data_.imu_orientation[2] = q.z();
+		data_.imu_orientation[3] = q.w();
 
-		batt_if_->set_voltage(roundf(pow_voltage));
-		batt_if_->set_current(roundf(pow_current));
+		data_.bat_voltage = roundf(pow_voltage);;
+		data_.bat_current = roundf(pow_current);
 
 		// 22.0V is empty, 24.5V is full, this is just a guess
 		float soc = (power_com_->voltage() - 22.0f) / 2.5f;
 		soc = std::min(1.f, std::max(0.f, soc));
-		batt_if_->set_absolute_soc(soc);
+		data_.bat_absolute_soc = soc;
 	}
 #endif
 }
 
-
-/** Trigger writes of blackboard interfaces.
- * This is meant to be called by the sensor thread so that writes to the
- * blackboard happen in the sensor acquisition hook.
- */
-void
-OpenRobotinoComThread::update_bb_sensor()
-{
-	MutexLocker lock(data_mutex_);
-	if (new_data_) {
-		batt_if_->write();
-		sens_if_->write();
-		if (imu_if_)  imu_if_->write();
-		new_data_ = false;
-	}
-}
-
-void
-OpenRobotinoComThread::update_distances(float *voltages)
-{
-	float dist_m[NUM_IR_SENSORS];
-	const size_t num_dps = voltage_to_dist_dps_.size();
-
-	for (int i = 0; i < NUM_IR_SENSORS; ++i) {
-		dist_m[i] = 0.;
-		// find the two enclosing data points
-
-		for (size_t j = 0; j < num_dps - 1; ++j) {
-			// This determines two points, l(eft) and r(ight) that are
-			// defined by voltage (x coord) and distance (y coord). We
-			// assume a linear progression between two adjacent points,
-			// i.e. between l and r. We then do the following:
-			// 1. Find two adjacent voltage values lv and rv where
-			//    the voltage lies inbetween
-			// 2. Interpolate by calculating the line parameters
-			//    m = dd/dv, x = voltage - lv and b = ld.
-			// cf. http://www.acroname.com/robotics/info/articles/irlinear/irlinear.html
-
-			const double lv = voltage_to_dist_dps_[j  ].first;
-			const double rv = voltage_to_dist_dps_[j+1].first;
-
-			if ((voltages[i] >= lv) && (voltages[i] < rv)) {
-				const double ld = voltage_to_dist_dps_[j  ].second;
-				const double rd = voltage_to_dist_dps_[j+1].second;
-
-				double dv = rv - lv;
-				double dd = rd - ld;
-
-				// Linear interpolation between 
-				dist_m[i] = (dd / dv) * (voltages[i] - lv) + ld;
-				break;
-			}
-		}
-	}
-
-	sens_if_->set_distance(dist_m);
-}
-
 #ifdef HAVE_OPENROBOTINO_API_1
 /** Update event. */
 void
@@ -647,3 +515,16 @@ OpenRobotinoComThread::set_gripper(bool opened)
 #endif
 
 }
+
+
+void
+OpenRobotinoComThread::set_bumper_estop_enabled(bool enabled)
+{
+#ifdef HAVE_OPENROBOTINO_API_1
+	logger->log_info(name(), "%sabling bumper estop on request",
+	                 msg->is_enabled() ? "En" : "Dis");
+	state_->emergencyStop.isEnabled = msg->is_enabled();
+#else
+	logger->log_info(name(), "Setting bumper estop not supported for API2");
+#endif
+}
diff --git a/src/plugins/robotino/openrobotino_com_thread.h b/src/plugins/robotino/openrobotino_com_thread.h
index 34a4344..bfc177c 100644
--- a/src/plugins/robotino/openrobotino_com_thread.h
+++ b/src/plugins/robotino/openrobotino_com_thread.h
@@ -81,8 +81,7 @@ class OpenRobotinoComThread
 #endif
 	public fawkes::LoggingAspect,
 	public fawkes::ConfigurableAspect,
-	public fawkes::ClockAspect,
-	public fawkes::BlackBoardAspect
+	public fawkes::ClockAspect
 {
  public:
 	OpenRobotinoComThread();
@@ -93,16 +92,15 @@ class OpenRobotinoComThread
 	virtual void loop();
 	virtual void finalize();
 
-	void update_bb_sensor();
+	virtual bool is_connected();
 
-	bool is_connected();
-
-	void set_gripper(bool opened);
-	bool is_gripper_open();
-	void set_speed_points(float s1, float s2, float s3);
-	void get_act_velocity(float &a1, float &a2, float &a3, unsigned int &seq, fawkes::Time &t);
-	void get_odometry(double &x, double &y, double &phi);
-	void reset_odometry();
+	virtual void set_gripper(bool opened);
+	virtual bool is_gripper_open();
+	virtual void set_speed_points(float s1, float s2, float s3);
+	virtual void get_act_velocity(float &a1, float &a2, float &a3, unsigned int &seq, fawkes::Time &t);
+	virtual void get_odometry(double &x, double &y, double &phi);
+	virtual void reset_odometry();
+	virtual void set_bumper_estop_enabled(bool enabled);
 
 	/** Stub to see name in backtrace for easier debugging. @see Thread::run() */
  protected: virtual void run() { Thread::run(); }
@@ -112,31 +110,19 @@ class OpenRobotinoComThread
 	using rec::robotino::com::Com::sensorState;
 	virtual void updateEvent();
 #endif
-	void process_sensor_msgs();
-	void process_sensor_state();
-	void process_com();
-	void update_distances(float *voltages);
-
+	void process_api_v1();
+	void process_api_v2();
+	
  private:
 	std::string     cfg_hostname_;
 	bool            cfg_quit_on_disconnect_;
-	bool            cfg_enable_gyro_;
-	std::string     cfg_imu_iface_id_;
 	unsigned int    cfg_sensor_update_cycle_time_;
 	bool            cfg_gripper_enabled_;
+	bool            cfg_enable_gyro_;
 
-	// Voltage to distance data points
-	std::vector<std::pair<double, double> > voltage_to_dist_dps_;
-
-	fawkes::Mutex    *data_mutex_;
-	bool              new_data_;
 	fawkes::TimeWait *time_wait_;
 	unsigned int      last_seqnum_;
 
-	fawkes::BatteryInterface        *batt_if_;
-	fawkes::RobotinoSensorInterface *sens_if_;
-	fawkes::IMUInterface            *imu_if_;
-
 #ifdef HAVE_OPENROBOTINO_API_1
 	rec::robotino::com::Com *com_;
 	fawkes::Mutex *state_mutex_;
diff --git a/src/plugins/robotino/sensor_thread.cpp b/src/plugins/robotino/sensor_thread.cpp
index 0b7492d..f136bbe 100644
--- a/src/plugins/robotino/sensor_thread.cpp
+++ b/src/plugins/robotino/sensor_thread.cpp
@@ -22,6 +22,10 @@
 #include "sensor_thread.h"
 #include "com_thread.h"
 
+#include <interfaces/BatteryInterface.h>
+#include <interfaces/RobotinoSensorInterface.h>
+#include <interfaces/IMUInterface.h>
+
 using namespace fawkes;
 
 /** @class RobotinoSensorThread "sensor_thread.h"
@@ -31,6 +35,15 @@ using namespace fawkes;
  * @author Tim Niemueller
  */
 
+/// taken from Robotino API2 DistanceSensorImpl.hpp
+const std::vector<std::pair<double, double> > VOLTAGE_TO_DIST_DPS =
+	{
+		{0.3 , 0.41},	{0.39, 0.35},	{0.41, 0.30},	{0.5 , 0.25},	{0.75, 0.18},
+		{0.8 , 0.16},	{0.95, 0.14},	{1.05, 0.12},	{1.3 , 0.10},	{1.4 , 0.09},
+		{1.55, 0.08},	{1.8 , 0.07},	{2.35, 0.05},	{2.55, 0.04}
+	};
+
+
 /** Constructor.
  * @param com_thread communication thread to trigger for writing data
  */
@@ -45,16 +58,123 @@ RobotinoSensorThread::RobotinoSensorThread(RobotinoComThread *com_thread)
 void
 RobotinoSensorThread::init()
 {
+	cfg_enable_gyro_ = config->get_bool("/hardware/robotino/gyro/enable");
+	cfg_imu_iface_id_ = config->get_string("/hardware/robotino/gyro/interface_id");
+
+	batt_if_ = NULL;
+	sens_if_ = NULL;
+	imu_if_ = NULL;
+
+	batt_if_ = blackboard->open_for_writing<BatteryInterface>("Robotino");
+	sens_if_ = blackboard->open_for_writing<RobotinoSensorInterface>("Robotino");
+
+	if (cfg_enable_gyro_) {
+		imu_if_ = blackboard->open_for_writing<IMUInterface>(cfg_imu_iface_id_.c_str());
+	}
 }
 
 
 void
 RobotinoSensorThread::finalize()
 {
+	blackboard->close(sens_if_);
+	blackboard->close(batt_if_);
+	blackboard->close(imu_if_);
 }
 
 void
 RobotinoSensorThread::loop()
 {
-	com_->update_bb_sensor();
+	process_sensor_msgs();
+
+	RobotinoComThread::SensorData data;
+	if (com_->get_data(data)) {
+		sens_if_->set_mot_velocity(data.mot_velocity);
+		sens_if_->set_mot_position(data.mot_position);
+		sens_if_->set_mot_current(data.mot_current);
+		sens_if_->set_bumper(data.bumper);
+		sens_if_->set_bumper_estop_enabled(data.bumper_estop_enabled);
+		sens_if_->set_digital_in(data.digital_in);
+		sens_if_->set_analog_in(data.analog_in);
+		update_distances(data.ir_voltages);
+		sens_if_->write();
+		
+		batt_if_->set_voltage(data.bat_voltage);
+		batt_if_->set_current(data.bat_current);
+		batt_if_->set_absolute_soc(data.bat_absolute_soc);
+		batt_if_->write();
+		
+		if (cfg_enable_gyro_) {
+			if (data.imu_enabled) {
+				imu_if_->set_angular_velocity(data.imu_angular_velocity);
+				imu_if_->set_angular_velocity_covariance(data.imu_angular_velocity_covariance);
+				imu_if_->set_orientation(data.imu_orientation);
+				imu_if_->write();
+			} else {
+				if (fabs(data.imu_angular_velocity[0] + 1.) > 0.00001) {
+					imu_if_->set_linear_acceleration(0, -1.);
+					imu_if_->set_angular_velocity(0, -1.);
+					imu_if_->set_angular_velocity(2,  0.);
+					imu_if_->set_orientation(0, -1.);
+					imu_if_->write();
+				}
+			}
+		}
+	}
+}
+
+
+void
+RobotinoSensorThread::process_sensor_msgs()
+{
+	// process command messages
+	while (! sens_if_->msgq_empty()) {
+		if (RobotinoSensorInterface::SetBumperEStopEnabledMessage *msg =
+		    sens_if_->msgq_first_safe(msg))
+		{
+			com_->set_bumper_estop_enabled(msg->is_enabled());
+		}
+		sens_if_->msgq_pop();
+	} // while sensor msgq
+}
+
+void
+RobotinoSensorThread::update_distances(float *voltages)
+{
+	float dist_m[NUM_IR_SENSORS];
+	const size_t num_dps = VOLTAGE_TO_DIST_DPS.size();
+
+	for (int i = 0; i < NUM_IR_SENSORS; ++i) {
+		dist_m[i] = 0.;
+		// find the two enclosing data points
+
+		for (size_t j = 0; j < num_dps - 1; ++j) {
+			// This determines two points, l(eft) and r(ight) that are
+			// defined by voltage (x coord) and distance (y coord). We
+			// assume a linear progression between two adjacent points,
+			// i.e. between l and r. We then do the following:
+			// 1. Find two adjacent voltage values lv and rv where
+			//    the voltage lies inbetween
+			// 2. Interpolate by calculating the line parameters
+			//    m = dd/dv, x = voltage - lv and b = ld.
+			// cf. http://www.acroname.com/robotics/info/articles/irlinear/irlinear.html
+
+			const double lv = VOLTAGE_TO_DIST_DPS[j  ].first;
+			const double rv = VOLTAGE_TO_DIST_DPS[j+1].first;
+
+			if ((voltages[i] >= lv) && (voltages[i] < rv)) {
+				const double ld = VOLTAGE_TO_DIST_DPS[j  ].second;
+				const double rd = VOLTAGE_TO_DIST_DPS[j+1].second;
+
+				double dv = rv - lv;
+				double dd = rd - ld;
+
+				// Linear interpolation between 
+				dist_m[i] = (dd / dv) * (voltages[i] - lv) + ld;
+				break;
+			}
+		}
+	}
+
+	sens_if_->set_distance(dist_m);
 }
diff --git a/src/plugins/robotino/sensor_thread.h b/src/plugins/robotino/sensor_thread.h
index 12e449c..e8e95e8 100644
--- a/src/plugins/robotino/sensor_thread.h
+++ b/src/plugins/robotino/sensor_thread.h
@@ -62,9 +62,23 @@ class RobotinoSensorThread
  protected: virtual void run() { Thread::run(); }
 
  private: // methods
+	void process_sensor_msgs();
+	void update_distances(float *voltages);
+
+	// Voltage to distance data points
+	static const std::vector<std::pair<double, double> > voltage_to_dist_dps_;
+
 
  private: // members
 	RobotinoComThread *com_;
+
+	bool            cfg_enable_gyro_;
+	std::string     cfg_imu_iface_id_;
+
+	fawkes::BatteryInterface        *batt_if_;
+	fawkes::RobotinoSensorInterface *sens_if_;
+	fawkes::IMUInterface            *imu_if_;
+
 };
 
 

- *commit* 2ae9c2b9ff361f8906c09e3f27342c66acb2967a - - - - - - - - - -
Author:  Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
Date:    Wed Apr 6 23:49:03 2016 +0200
Subject: robotino: remove OpenRobotino dependency from act thread

 src/plugins/robotino/act_thread.cpp |  117 ++++++++++++++++++++++++++---------
 src/plugins/robotino/act_thread.h   |   41 ++----------
 2 files changed, 94 insertions(+), 64 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/robotino/act_thread.cpp b/src/plugins/robotino/act_thread.cpp
index c2f4399..150c67a 100644
--- a/src/plugins/robotino/act_thread.cpp
+++ b/src/plugins/robotino/act_thread.cpp
@@ -3,7 +3,7 @@
  *  act_thread.cpp - Robotino act thread
  *
  *  Created: Sun Nov 13 16:07:40 2011
- *  Copyright  2011-2014  Tim Niemueller [www.niemueller.de]
+ *  Copyright  2011-2016  Tim Niemueller [www.niemueller.de]
  *             2014       Sebastian Reuter
  *             2014       Tobias Neumann
  ****************************************************************************/
@@ -29,16 +29,6 @@
 #include <interfaces/IMUInterface.h>
 #include <utils/math/angle.h>
 
-#ifdef HAVE_OPENROBOTINO_API_1
-#  include <rec/robotino/com/Com.h>
-#  include <rec/robotino/com/OmniDrive.h>
-#  include <rec/sharedmemory/sharedmemory.h>
-#  include <rec/iocontrol/remotestate/SensorState.h>
-#  include <rec/iocontrol/robotstate/State.h>
-#elif defined(HAVE_OPENROBOTINO_API_2)
-#  include <rec/robotino/api2/OmniDriveModel.h>
-#endif
-
 using namespace fawkes;
 
 /** @class RobotinoActThread "act_thread.h"
@@ -68,12 +58,6 @@ RobotinoActThread::init()
 	last_seqnum_ = 0;
 	last_msg_time_ = clock->now();
 
-#ifdef HAVE_OPENROBOTINO_API_1
-	omni_drive_ = new rec::robotino::com::OmniDrive();
-#elif defined(HAVE_OPENROBOTINO_API_2)
-	omni_drive_ = new rec::robotino::api2::OmniDriveModel();
-#endif
-
 	//get config values
 	cfg_deadman_threshold_    = config->get_float("/hardware/robotino/deadman_time_threshold");
 	cfg_gripper_enabled_      = config->get_bool("/hardware/robotino/gripper/enable_gripper");
@@ -87,6 +71,10 @@ RobotinoActThread::init()
 	cfg_odom_corr_trans_      =
 		config->get_float("/hardware/robotino/odometry/calc/correction/trans");
 
+	cfg_rb_   = config->get_float("/hardware/robotino/motor-layout/rb");
+	cfg_rw_   = config->get_float("/hardware/robotino/motor-layout/rw");
+	cfg_gear_ = config->get_float("/hardware/robotino/motor-layout/gear");
+
 	std::string imu_if_id;
 
 	imu_if_ = NULL;
@@ -141,9 +129,6 @@ RobotinoActThread::finalize()
 	com_->set_speed_points(0., 0., 0.);
 	com_ = NULL;
 	delete odom_time_;
-#if defined(HAVE_OPENROBOTINO_API_1) || defined(HAVE_OPENROBOTINO_API_2)
-	delete omni_drive_;
-#endif
 }
 
 void
@@ -177,13 +162,7 @@ RobotinoActThread::loop()
 
 		else if (MotorInterface::TransRotMessage *msg = motor_if_->msgq_first_safe(msg))
 		{
-#ifdef HAVE_OPENROBOTINO_API_1
-			omni_drive_->project(&s1, &s2, &s3,
-			                     msg->vx() * 1000., msg->vy() * 1000.,
-			                     rad2deg(msg->omega()));
-#elif defined(HAVE_OPENROBOTINO_API_2)
-			omni_drive_->project(&s1, &s2, &s3, msg->vx(), msg->vy(), msg->omega());
-#endif
+			project(&s1, &s2, &s3, msg->vx(), msg->vy(), msg->omega());
 
 			des_vx_    = msg->vx();
 			des_vy_    = msg->vy();
@@ -275,9 +254,7 @@ RobotinoActThread::publish_odometry()
 		last_seqnum_ = seq;
 
 		float vx = 0., vy = 0., omega = 0.;
-#if defined(HAVE_OPENROBOTINO_API_1) || defined(HAVE_OPENROBOTINO_API_2)
-		omni_drive_->unproject(&vx, &vy, &omega, a1, a2, a3);
-#endif
+		unproject(&vx, &vy, &omega, a1, a2, a3);
 
 		motor_if_->set_vx(vx);
 		motor_if_->set_vy(vy);
@@ -423,3 +400,83 @@ RobotinoActThread::publish_gripper()
 		gripper_if_->write();
 	}
 }
+
+/** Project the velocity of the robot in cartesian coordinates to single motor speeds.
+ *
+ * From OpenRobotino API2 (C) REC Robotics Equipment Corporation GmbH, Planegg, Germany.
+ * The code has been released under a 2-clause BSD license.
+ *
+ * @param m1		The resulting speed of motor 1 in rpm
+ * @param m2		The resulting speed of motor 2 in rpm
+ * @param m3		The resulting speed of motor 3 in rpm
+ * @param vx		Velocity in x-direction in m/s
+ * @param vy		Velocity in y-direction in m/s
+ * @param omega	Angular velocity in rad/s
+ */
+//Redistribution and use in source and binary forms, with or without
+//modification, are permitted provided that the following conditions
+//are met:
+//1) Redistributions of source code must retain the above copyright
+//notice, this list of conditions and the following disclaimer.
+//2) Redistributions in binary form must reproduce the above copyright
+//notice, this list of conditions and the following disclaimer in the
+//documentation and/or other materials provided with the distribution.
+//
+//THIS SOFTWARE IS PROVIDED BY REC ROBOTICS EQUIPMENT CORPORATION GMBH
+//"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+//LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+//FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL REC
+//ROBOTICS EQUIPMENT CORPORATION GMBH BE LIABLE FOR ANY DIRECT,
+//INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+//(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+//SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+//HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
+//STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+//ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
+//OF THE POSSIBILITY OF SUCH DAMAGE.
+void
+RobotinoActThread::project( float* m1, float* m2, float* m3, float vx, float vy, float omega ) const
+{
+	//Projection matrix
+	static const double v0[2] = { -0.5 * sqrt( 3.0 ),  0.5 };
+	static const double v1[2] = {  0.0              , -1.0 };
+	static const double v2[2] = {  0.5 * sqrt( 3.0 ),  0.5 };
+
+	//Scale omega with the radius of the robot
+	double vOmegaScaled = cfg_rb_ * (double)omega ;
+
+	//Convert from m/s to RPM
+	const double k = 60.0 * cfg_gear_ / ( 2.0 * M_PI * cfg_rw_ );
+
+	//Compute the desired velocity
+	*m1 = static_cast<float>( ( v0[0] * (double)vx + v0[1] * (double)vy + vOmegaScaled ) * k );
+	*m2 = static_cast<float>( ( v1[0] * (double)vx + v1[1] * (double)vy + vOmegaScaled ) * k );
+	*m3 = static_cast<float>( ( v2[0] * (double)vx + v2[1] * (double)vy + vOmegaScaled ) * k );
+}
+
+/** Project single motor speeds to velocity in cartesian coordinates.
+ *
+ * From OpenRobotino API2 (C) REC Robotics Equipment Corporation GmbH, Planegg, Germany.
+ * The code has been released under a 2-clause BSD license.
+ *
+ * @param vx		The resulting speed in x-direction in m/s
+ * @param vy		The resulting speed in y-direction in m/s
+ * @param omega	The resulting angular velocity in rad/s
+ * @param m1		Speed of motor 1 in rpm
+ * @param m2		Speed of motor 2 in rpm
+ * @param m3		Speed of motor 3 in rpm
+ * @throws		RobotinoException if no valid drive layout parameters are available.
+ */
+void
+RobotinoActThread::unproject( float* vx, float* vy, float* omega, float m1, float m2, float m3 ) const
+{
+	//Convert from RPM to mm/s
+	const double k = 60.0 * cfg_gear_ / ( 2.0 * M_PI * cfg_rw_ );
+
+	*vx = static_cast<float>( ( (double)m3 - (double)m1 ) / sqrt( 3.0 ) / k );
+	*vy = static_cast<float>( 2.0 / 3.0 * ( (double)m1 + 0.5 * ( (double)m3 - (double)m1 ) - (double)m2 ) / k );
+
+	double vw = (double)*vy + (double)m2 / k;
+
+	*omega = static_cast<float>( vw / cfg_rb_ );
+}
diff --git a/src/plugins/robotino/act_thread.h b/src/plugins/robotino/act_thread.h
index ae29e75..6cca010 100644
--- a/src/plugins/robotino/act_thread.h
+++ b/src/plugins/robotino/act_thread.h
@@ -36,33 +36,6 @@
 
 #include <string>
 
-#ifdef HAVE_OPENROBOTINO_API_1
-namespace rec {
-	namespace robotino {
-		namespace com {
-			class Com;
-			class OmniDrive;
-		}
-	}
-	namespace sharedmemory {
-		template<typename SharedType> class SharedMemory;
-	}
-	namespace iocontrol {
-		namespace robotstate {
-			class State;
-		}
-	}
-}
-#else
-namespace rec {
-	namespace robotino {
-		namespace api2 {
-			class OmniDriveModel;
-		}
-	}
-}
-#endif
-
 namespace fawkes {
 	class MotorInterface;
 	class GripperInterface;
@@ -101,15 +74,12 @@ class RobotinoActThread
 	void publish_odometry();
 	void publish_gripper();
 
+	void project(float *m1, float *m2, float *m3, float vx, float vy, float omega) const;
+	void unproject(float *vx, float *vy, float *omega, float m1, float m2, float m3) const;
+	
  private:
 	RobotinoComThread              *com_;
 
-#ifdef HAVE_OPENROBOTINO_API_1
-	rec::robotino::com::OmniDrive  *omni_drive_;
-#elif defined(HAVE_OPENROBOTINO_API_2)
-	rec::robotino::api2::OmniDriveModel *omni_drive_;
-#endif
-
 	unsigned int                    last_seqnum_;
 	fawkes::MotorInterface         *motor_if_;
 	fawkes::GripperInterface       *gripper_if_;
@@ -132,7 +102,10 @@ class RobotinoActThread
 	float                           cfg_odom_corr_phi_;
 	float                           cfg_odom_corr_trans_;
 	bool                            cfg_bumper_estop_enabled_;
-
+	float                           cfg_rb_;
+	float                           cfg_rw_;
+	float                           cfg_gear_;
+	
 	float                           des_vx_;
 	float                           des_vy_;
 	float                           des_omega_;

- *commit* aade921abb532f9d447855b69bdb0d1d15a3d761 - - - - - - - - - -
Author:  Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
Date:    Fri Apr 8 16:54:48 2016 +0200
Subject: robotino: set bumper emergency stop setting

 src/plugins/robotino/act_thread.cpp |    9 +++++++++
 src/plugins/robotino/act_thread.h   |    1 +
 2 files changed, 10 insertions(+), 0 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/robotino/act_thread.cpp b/src/plugins/robotino/act_thread.cpp
index 150c67a..74d71ad 100644
--- a/src/plugins/robotino/act_thread.cpp
+++ b/src/plugins/robotino/act_thread.cpp
@@ -131,6 +131,15 @@ RobotinoActThread::finalize()
 	delete odom_time_;
 }
 
+
+void
+RobotinoActThread::once()
+{
+	try {
+		com_->set_bumper_estop_enabled(cfg_bumper_estop_enabled_);
+	} catch (Exception &e) {}
+}
+
 void
 RobotinoActThread::loop()
 {
diff --git a/src/plugins/robotino/act_thread.h b/src/plugins/robotino/act_thread.h
index 6cca010..2e099e4 100644
--- a/src/plugins/robotino/act_thread.h
+++ b/src/plugins/robotino/act_thread.h
@@ -59,6 +59,7 @@ class RobotinoActThread
 	RobotinoActThread(RobotinoComThread *com_thread);
 
 	virtual void init();
+	virtual void once();
 	virtual void loop();
 	virtual void finalize();
 

- *commit* ce0a8fcdb3b7ba739e619287bb23064724c179eb - - - - - - - - - -
Author:  Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
Date:    Fri Apr 8 17:01:44 2016 +0200
Subject: robotino: fill in method bodies, full communication working

 src/plugins/robotino/direct_com_thread.cpp       |  188 +++++++++++++++++++++-
 src/plugins/robotino/direct_com_thread.h         |   12 +-
 src/plugins/robotino/openrobotino_com_thread.cpp |    2 +
 3 files changed, 193 insertions(+), 9 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/robotino/direct_com_thread.cpp b/src/plugins/robotino/direct_com_thread.cpp
index 2f94b1d..cf67ef9 100644
--- a/src/plugins/robotino/direct_com_thread.cpp
+++ b/src/plugins/robotino/direct_com_thread.cpp
@@ -87,6 +87,8 @@ DirectRobotinoComThread::init()
 	check_deadline();
 
 	open_device();
+	opened_ = true;
+	open_tries_ = 0;
 }
 
 
@@ -99,7 +101,7 @@ DirectRobotinoComThread::finalize()
 void
 DirectRobotinoComThread::once()
 {
-	//reset_odometry();
+	reset_odometry();
 }
 
 void
@@ -107,21 +109,125 @@ DirectRobotinoComThread::loop()
 {
 	time_wait_->mark_start();
 
+	if (opened_) {
+		DirectRobotinoComMessage req;
+		req.add_command(DirectRobotinoComMessage::CMDID_GET_ALL_MOTOR_READINGS);
+		req.add_command(DirectRobotinoComMessage::CMDID_GET_DISTANCE_SENSOR_READINGS);
+		req.add_command(DirectRobotinoComMessage::CMDID_GET_ALL_ANALOG_INPUTS);
+		req.add_command(DirectRobotinoComMessage::CMDID_GET_ALL_DIGITAL_INPUTS);
+		req.add_command(DirectRobotinoComMessage::CMDID_GET_BUMPER);
+		req.add_command(DirectRobotinoComMessage::CMDID_GET_GYRO_Z_ANGLE);
+		// This command is documented in the wiki but does not exist in the
+		// enum and is not handled in the firmware. Instead, the information
+		// is sent with every reply, so we also get it here automatically.
+		// This has been checked in Robotino3 firmware 1.1.1
+		// req.add_command(DirectRobotinoComMessage::CMDID_GET_POWER_SOURCE_READINGS);
+
+		//req.pack();
+		//logger->log_debug(name(), "Req1:\n%s", req.to_string().c_str());
+
+		try {
+			DirectRobotinoComMessage::pointer m = send_and_recv(req);
+	
+			MutexLocker lock(data_mutex_);
+			new_data_ = true;
+			data_.seq += 1;
+
+			process_message(m);
+		} catch (Exception &e) {
+			logger->log_warn(name(), "Transmission error, re-connecting, exception follows");
+			logger->log_warn(name(), e);
+			opened_ = false;
+			open_tries_ = 0;
+			close_device();
+		}
+	} else {
+		try {
+			open_device();
+			opened_ = true;
+			logger->log_info(name(), "Connection re-established after %u tries", open_tries_ + 1);
+		} catch (Exception &e) {
+			open_tries_ += 1;
+			if (open_tries_ >= (1000 / cfg_sensor_update_cycle_time_)) {
+				logger->log_error(name(), "Connection problem to base persists");
+				open_tries_ = 0;
+			}
+		}
+	}
 
 	time_wait_->wait();
 }
 
 
 void
+DirectRobotinoComThread::process_message(DirectRobotinoComMessage::pointer m)
+{
+	DirectRobotinoComMessage::command_id_t msgid;
+	while ((msgid = m->next_command()) != DirectRobotinoComMessage::CMDID_NONE) {
+		//logger->log_info(name(), "Command length: %u", m->command_length());
+
+		if (msgid == DirectRobotinoComMessage::CMDID_ALL_MOTOR_READINGS) {
+			// there are four motors, one of which might be a gripper, therefore skips
+
+			for (int i = 0; i < 3; ++i)  data_.mot_velocity[i] = m->get_int16();
+			m->skip_int16();
+
+			for (int i = 0; i < 3; ++i)  data_.mot_position[i] = m->get_int32();
+			m->skip_int32();
+
+			for (int i = 0; i < 3; ++i)  data_.mot_current[i] = m->get_float();
+
+		} else if (msgid == DirectRobotinoComMessage::CMDID_DISTANCE_SENSOR_READINGS) {
+			for (int i = 0; i < 9; ++i)  data_.ir_voltages[i] = m->get_float();
+
+		} else if (msgid == DirectRobotinoComMessage::CMDID_ALL_ANALOG_INPUTS) {
+			for (int i = 0; i < 8; ++i)  data_.analog_in[i] = m->get_float();
+
+		} else if (msgid == DirectRobotinoComMessage::CMDID_ALL_DIGITAL_INPUTS) {
+			uint8_t value = m->get_uint8();
+			for (int i = 0; i < 8; ++i)  data_.digital_in[i] = (value & (1 << i)) ? true : false;
+
+		} else if (msgid == DirectRobotinoComMessage::CMDID_BUMPER) {
+			data_.bumper = (m->get_uint8() != 0) ? true : false;
+
+		} else if (msgid == DirectRobotinoComMessage::CMDID_POWER_SOURCE_READINGS) {
+			float voltage = m->get_float();
+			float current = m->get_float();
+
+			data_.bat_voltage = voltage * 1000.; // V -> mV
+			data_.bat_current = current * 1000.; // A -> mA
+
+			// 22.0V is empty, 24.5V is full, this is just a guess
+			float soc = (voltage - 22.0f) / 2.5f;
+			soc = std::min(1.f, std::max(0.f, soc));
+			data_.bat_absolute_soc = soc;
+
+		} else if (msgid == DirectRobotinoComMessage::CMDID_CHARGER_ERROR) {
+			uint8_t id = m->get_uint8();
+			uint32_t mtime = m->get_uint32();
+			std::string error = m->get_string();
+			logger->log_warn(name(), "Charger error (ID %u, Time: %u): %s",
+			                 id, mtime, error.c_str());
+		}
+	}
+}
+
+
+void
 DirectRobotinoComThread::reset_odometry()
 {
+	DirectRobotinoComMessage m(DirectRobotinoComMessage::CMDID_SET_ODOMETRY);
+	m.add_float(0.); // X (m)
+	m.add_float(0.); // Y (m)
+	m.add_float(0.); // rot (rad)
+	send_message(m);
 }
 
 
 bool
 DirectRobotinoComThread::is_connected()
 {
-	return false;
+	return serial_.is_open();
 }
 
 
@@ -129,13 +235,23 @@ void
 DirectRobotinoComThread::get_act_velocity(float &a1, float &a2, float &a3, unsigned int &seq, fawkes::Time &t)
 {
 	MutexLocker lock(data_mutex_);
+	a1 = data_.mot_velocity[0];
+	a2 = data_.mot_velocity[1];
+	a3 = data_.mot_velocity[2];
+
+	seq = data_.seq;
+	t   = data_.time;
 }
 
 
 void
 DirectRobotinoComThread::get_odometry(double &x, double &y, double &phi)
 {
-	MutexLocker lock(data_mutex_);
+	DirectRobotinoComMessage req(DirectRobotinoComMessage::CMDID_GET_ODOMETRY);
+	DirectRobotinoComMessage::pointer m = send_and_recv(req);
+	x   = m->get_float();
+	y   = m->get_float();
+	phi = m->get_float();
 }
 
 bool
@@ -148,6 +264,17 @@ DirectRobotinoComThread::is_gripper_open()
 void
 DirectRobotinoComThread::set_speed_points(float s1, float s2, float s3)
 {
+	DirectRobotinoComMessage m;
+	m.add_command(DirectRobotinoComMessage::CMDID_SET_MOTOR_SPEED);
+	m.add_uint8(0);
+	m.add_uint16((uint16_t)roundf(s1));
+	m.add_command(DirectRobotinoComMessage::CMDID_SET_MOTOR_SPEED);
+	m.add_uint8(1);
+	m.add_uint16((uint16_t)roundf(s2));
+	m.add_command(DirectRobotinoComMessage::CMDID_SET_MOTOR_SPEED);
+	m.add_uint8(2);
+	m.add_uint16((uint16_t)roundf(s3));
+	send_message(m);
 }
 
 void
@@ -158,6 +285,9 @@ DirectRobotinoComThread::set_gripper(bool opened)
 void
 DirectRobotinoComThread::set_bumper_estop_enabled(bool enabled)
 {
+	DirectRobotinoComMessage m(DirectRobotinoComMessage::CMDID_SET_EMERGENCY_BUMPER);
+	m.add_uint8(enabled ? 1 : 0);
+	send_message(m);
 }
 
 
@@ -240,6 +370,8 @@ DirectRobotinoComThread::open_device()
 	try {
 		input_buffer_.consume(input_buffer_.size());
 
+		boost::mutex::scoped_lock lock(io_mutex_);
+
 		serial_.open(cfg_device_);
 		//serial_.set_option(boost::asio::serial_port::stop_bits(boost::asio::serial_port::stop_bits::none));
 		serial_.set_option(boost::asio::serial_port::parity(boost::asio::serial_port::parity::none));
@@ -274,6 +406,8 @@ DirectRobotinoComThread::open_device()
 				std::string error = m->get_string();
 				logger->log_warn(name(), "Charger error (ID %u, Time: %u): %s",
 				                 id, mtime, error.c_str());
+				//} else {
+				//logger->log_debug(name(), "  - %u\n", msgid);
 			}
 		}
 		if (hw_version.empty() || sw_version.empty()) {
@@ -289,13 +423,48 @@ DirectRobotinoComThread::open_device()
 void
 DirectRobotinoComThread::close_device()
 {
+	boost::mutex::scoped_lock lock(io_mutex_);
 	serial_.cancel();
 	serial_.close();
 }
 
+
+void
+DirectRobotinoComThread::flush_device()
+{
+	if (serial_.is_open()) {
+		try {
+			boost::system::error_code ec = boost::asio::error::would_block;
+			size_t bytes_read = 0;
+			do {
+				ec = boost::asio::error::would_block;
+				bytes_read = 0;
+
+				deadline_.expires_from_now(boost::posix_time::milliseconds(200));
+				boost::asio::async_read(serial_, input_buffer_,
+				                        boost::asio::transfer_at_least(1),
+				                        (boost::lambda::var(ec) = boost::lambda::_1,
+				                         boost::lambda::var(bytes_read) = boost::lambda::_2));
+
+				do io_service_.run_one(); while (ec == boost::asio::error::would_block);
+
+				if (bytes_read > 0) {
+					logger->log_warn(name(), "Flushing %zu bytes\n", bytes_read);
+				}
+
+			} while (bytes_read > 0);
+			deadline_.expires_from_now(boost::posix_time::pos_infin);
+		} catch (boost::system::system_error &e) {
+			// ignore, just assume done, if there really is an error we'll
+			// catch it later on
+		}
+	}
+}
+
 void
 DirectRobotinoComThread::send_message(DirectRobotinoComMessage &msg)
 {
+	boost::mutex::scoped_lock lock(io_mutex_);
 	boost::asio::write(serial_, boost::asio::const_buffers_1(msg.buffer()));
 }
 
@@ -310,7 +479,8 @@ DirectRobotinoComThread::send_message(DirectRobotinoComMessage &msg)
 std::shared_ptr<DirectRobotinoComMessage>
 DirectRobotinoComThread::send_and_recv(DirectRobotinoComMessage &msg)
 {
-	send_message(msg);
+	boost::mutex::scoped_lock lock(io_mutex_);
+	boost::asio::write(serial_, boost::asio::const_buffers_1(msg.buffer()));
 	read_packet();
 
 	std::shared_ptr<DirectRobotinoComMessage> m =
@@ -360,7 +530,7 @@ DirectRobotinoComThread::read_packet()
 	boost::system::error_code ec = boost::asio::error::would_block;
 	size_t bytes_read = 0;
 
-	deadline_.expires_from_now(boost::posix_time::milliseconds(50));
+	deadline_.expires_from_now(boost::posix_time::milliseconds(200));
 	boost::asio::async_read_until(serial_, input_buffer_, DirectRobotinoComMessage::MSG_HEAD,
 	                              (boost::lambda::var(ec) = boost::lambda::_1,
 	                               boost::lambda::var(bytes_read) = boost::lambda::_2));
@@ -376,6 +546,9 @@ DirectRobotinoComThread::read_packet()
 	}
 
 	// Read all potential junk before the start header
+	if (bytes_read > 1) {
+		logger->log_warn(name(), "Read junk off line");
+	}
 	input_buffer_.consume(bytes_read - 1);
 
 	// read packet length
@@ -419,11 +592,14 @@ DirectRobotinoComThread::read_packet()
 
 	if (ec) {
 		if (ec.value() == boost::system::errc::operation_canceled) {
-			throw Exception("Timeout (3) on initial synchronization");
+			throw Exception("Timeout (3) on initial synchronization (reading %u bytes, have %zu)",
+			                length, input_buffer_.size());
 		} else {
 			throw Exception("Error (3) on initial synchronization: %s", ec.message().c_str());
 		}
 	}
+
+	deadline_.expires_at(boost::posix_time::pos_infin);
 }
 
 
diff --git a/src/plugins/robotino/direct_com_thread.h b/src/plugins/robotino/direct_com_thread.h
index bfb6527..a840a30 100644
--- a/src/plugins/robotino/direct_com_thread.h
+++ b/src/plugins/robotino/direct_com_thread.h
@@ -22,6 +22,7 @@
 #define __PLUGINS_ROBOTINO_DIRECT_COM_THREAD_H_
 
 #include "com_thread.h"
+#include "direct_com_message.h"
 #include <core/threading/thread.h>
 #include <aspect/logging.h>
 #include <aspect/clock.h>
@@ -32,6 +33,7 @@
 
 #include <memory>
 #include <boost/asio.hpp>
+#include <boost/thread/mutex.hpp>
 
 class DirectRobotinoComMessage;
 
@@ -77,12 +79,14 @@ class DirectRobotinoComThread
 	std::string find_device_udev();
 	void open_device();
 	void close_device();
+	void flush_device();
 	void check_deadline();
 
 	void read_packet();
 	void send_message(DirectRobotinoComMessage &msg);
 	std::shared_ptr<DirectRobotinoComMessage>
 		send_and_recv(DirectRobotinoComMessage &msg);
+	void process_message(DirectRobotinoComMessage::pointer m);
 
  private:
 	std::string     cfg_device_;
@@ -91,8 +95,9 @@ class DirectRobotinoComThread
 	unsigned int    cfg_sensor_update_cycle_time_;
 	bool            cfg_gripper_enabled_;
 
-	fawkes::Mutex    *data_mutex_;
-	bool              new_data_;
+	bool opened_;
+	unsigned int open_tries_;
+
 	fawkes::TimeWait *time_wait_;
 	unsigned int      last_seqnum_;
 
@@ -101,7 +106,8 @@ class DirectRobotinoComThread
 	boost::asio::io_service::work io_service_work_;
 	boost::asio::deadline_timer   deadline_;
 	boost::asio::streambuf        input_buffer_;
-
+	boost::mutex                  io_mutex_;
+	
 };
 
 
diff --git a/src/plugins/robotino/openrobotino_com_thread.cpp b/src/plugins/robotino/openrobotino_com_thread.cpp
index deac1ac..d9178e8 100644
--- a/src/plugins/robotino/openrobotino_com_thread.cpp
+++ b/src/plugins/robotino/openrobotino_com_thread.cpp
@@ -416,6 +416,8 @@ OpenRobotinoComThread::get_act_velocity(float &a1, float &a2, float &a3, unsigne
 	a3 = sensor_state.actualVelocity[2] / 1000.f;
 	seq = sensor_state.sequenceNumber;
 #else
+	t.stamp();
+
 	float mot_act_vel[motors_com_->numMotors()];
 	motors_com_->actualVelocities(mot_act_vel);
 




-- 
Fawkes Robotics Framework                 http://www.fawkesrobotics.org


More information about the fawkes-commits mailing list