[Fawkes Git] branch/timn/robotino-direct: 7 revs updated. (0.5.0-3100-gaa26d1e)

Tim Niemueller niemueller at kbsg.rwth-aachen.de
Wed Apr 13 10:50:30 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 updated
  discards  4734a2007ae5673f1c3543e45cc2967e3c681e50 (commit)
  discards  65f0da04d5666dec75b0c1931147de84bbb25ca9 (commit)
        to  aa26d1ea265fb1fea6f2c722289c59a21ce5506d (commit)
       via  d290644ddb63e89611eb1c0a18d137d10462ae32 (commit)
       via  cdbc1a08699bebaf2d841cb9d2107fd755cc1846 (commit)
       via  a54ce07c37f27024b1f8b8b2386e62dac1171a78 (commit)
       via  852d27ad5ea40d106c296f7b1565cdc79fe503f1 (commit)
       via  cceb762465160de2af9eee56d351663e88852e82 (commit)
       via  8d9312fa6a0da7d7b50b4783fac6fa2a8a79f2db (commit)

This update added new revisions after undoing existing revisions.  That is
to say, the old revision is not a strict subset of the new revision.  This
situation occurs when you --force push a change and generate a repository
containing something like this:

 * -- * -- B -- O -- O -- O (4734a2007ae5673f1c3543e45cc2967e3c681e50)
            \
             N -- N -- N (aa26d1ea265fb1fea6f2c722289c59a21ce5506d)

When this happens we assume that you've already had alert emails for all
of the O revisions, and so we here report only the revisions in the N
branch from the common base, B.

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

Those revisions listed above that are new to this repository have
not appeared on any other notification email; so we list those
revisions in full, below.

- *Log* ---------------------------------------------------------------
commit 8d9312fa6a0da7d7b50b4783fac6fa2a8a79f2db
Author:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
AuthorDate: Tue Apr 12 22:48:08 2016 +0200
Commit:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
CommitDate: Wed Apr 13 10:50:24 2016 +0200

    robotino: add velocity controller and safety checks
    
    Restructure the module to move the drive execution into the (faster
    timed) com thread. Add a simple linear controller for the velocities to
    smooth the driving behavior. Add safety checks and bounds on velocities
    of the omni drive model and the motor RPMs.
    
    The overall behavior in driving is much smoother and improved.
    
    I have seen more transmission errors. Some improvements to error
    handling have been made. Investigating further why the connection errors
    increased seems worthwhile.

http://git.fawkesrobotics.org/fawkes.git/commit/8d9312f
http://trac.fawkesrobotics.org/changeset/8d9312f

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit cceb762465160de2af9eee56d351663e88852e82
Author:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
AuthorDate: Tue Apr 12 23:41:07 2016 +0200
Commit:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
CommitDate: Wed Apr 13 10:50:24 2016 +0200

    robotino: determine if controld3 is running in direct mode
    
    Throw an exception if controld3 is still running. We need it to be
    stopped to take over communication with the base.

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

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 852d27ad5ea40d106c296f7b1565cdc79fe503f1
Author:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
AuthorDate: Wed Apr 13 09:17:42 2016 +0200
Commit:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
CommitDate: Wed Apr 13 10:50:24 2016 +0200

    joystick: change acquisition thread variable style
    
    Translate __var to var_.

http://git.fawkesrobotics.org/fawkes.git/commit/852d27a
http://trac.fawkesrobotics.org/changeset/852d27a

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit a54ce07c37f27024b1f8b8b2386e62dac1171a78
Author:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
AuthorDate: Wed Apr 13 10:25:20 2016 +0200
Commit:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
CommitDate: Wed Apr 13 10:50:24 2016 +0200

    joystick-teleop: apply threshold to individual axes
    
    The threshold when to accept an axis was applied to each axis but then
    globally enabled all axes to be transmitted.
    
    The new version applies the threshold to the individual axes and only
    send the axes which exceed the threshold. This gets rid of most of the
    noise in joystick commands.

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

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit cdbc1a08699bebaf2d841cb9d2107fd755cc1846
Author:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
AuthorDate: Wed Apr 13 10:29:23 2016 +0200
Commit:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
CommitDate: Wed Apr 13 10:50:25 2016 +0200

    joystick: add re-enabling of safety lockout, change default config
    
    The safety lockout is now re-enabled after a certain time of inactivity.
    
    Adapt the default configuration to fit the used controller even better.

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

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit d290644ddb63e89611eb1c0a18d137d10462ae32
Author:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
AuthorDate: Wed Apr 13 10:30:21 2016 +0200
Commit:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
CommitDate: Wed Apr 13 10:50:25 2016 +0200

    robotino: better default configuration

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

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit aa26d1ea265fb1fea6f2c722289c59a21ce5506d
Author:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
AuthorDate: Wed Apr 13 10:47:45 2016 +0200
Commit:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
CommitDate: Wed Apr 13 10:50:25 2016 +0200

    robotino: add informative output when connection is lost
    
    This helped to find that the USB device altogether disappeared for a
    short time :-/

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

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


- *Summary* -----------------------------------------------------------
 cfg/conf.d/joystick.yaml                        |   41 +++--
 cfg/conf.d/robotino.yaml                        |   31 +--
 src/plugins/joystick/acquisition_thread.cpp     |  277 +++++++++++++----------
 src/plugins/joystick/acquisition_thread.h       |   44 ++--
 src/plugins/joystick/joystick_teleop_thread.cpp |   24 ++-
 src/plugins/robotino/com_thread.cpp             |   23 ++-
 src/plugins/robotino/direct_com_thread.cpp      |    1 +
 7 files changed, 258 insertions(+), 183 deletions(-)


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

- *commit* 8d9312fa6a0da7d7b50b4783fac6fa2a8a79f2db - - - - - - - - - -
Author:  Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
Date:    Tue Apr 12 22:48:08 2016 +0200
Subject: robotino: add velocity controller and safety checks

 cfg/conf.d/robotino.yaml                         |   33 +++-
 src/plugins/robotino/act_thread.cpp              |  132 +++---------
 src/plugins/robotino/act_thread.h                |   23 +-
 src/plugins/robotino/com_thread.cpp              |  258 ++++++++++++++++++++
 src/plugins/robotino/com_thread.h                |   62 ++++-
 src/plugins/robotino/direct_com_thread.cpp       |  272 +++++++++++++++-------
 src/plugins/robotino/direct_com_thread.h         |   20 +-
 src/plugins/robotino/openrobotino_com_thread.cpp |    7 +
 src/plugins/robotino/openrobotino_com_thread.h   |   12 +-
 9 files changed, 586 insertions(+), 233 deletions(-)

_Diff for modified files_:
diff --git a/cfg/conf.d/robotino.yaml b/cfg/conf.d/robotino.yaml
index ab4cff5..43c182b 100644
--- a/cfg/conf.d/robotino.yaml
+++ b/cfg/conf.d/robotino.yaml
@@ -48,7 +48,20 @@ hardware/robotino:
     # the group dialout: usermod -a -G dialout robotino (and reboot)
     # device: /dev/ttyACM1
 
-  motor-layout:
+
+  # The motor and drive settings and limits are enforced for direct
+  # communication, for the openrobotino driver, these values are
+  # checked by controld3, cf. /etc/robotino/controld3.conf on the
+  # robotino base
+
+  motor:
+    # Minimum and maximum acceleration of motors
+    acceleration-limits: [-11500, 11500]
+
+    rpm-max: 3000
+  
+  drive:
+    layout:
     # Distance from Robotino center to wheel center in meters
     rb: 0.175
 
@@ -58,6 +71,14 @@ hardware/robotino:
     # Gear ratio between motors and wheels
     gear: 32.0
 
+    # Maximum acceleration and deceleration for translation, m/s^2
+    trans-acceleration: 0.4
+    trans-deceleration: 0.4
+
+    # Maximum acceleration and deceleration for rotation, deg/s^2
+    rot-acceleration: 0.4
+    rot-deceleration: 0.4
+
   bumper:
     # Set to true to enable the emergency stop on bumper contact (provided
     # by OpenRobotino, or false to keep going (warning, you should handle
@@ -120,3 +141,13 @@ hardware/robotino:
 
     # ID of the Gripper Interface
     gripper_id: Robotino
+
+    # Maximum current in A for the motor output.
+    # If the current is larger than maxcurrent for more than
+    # max-time milliseconds the set-point is set to 0. To recover
+    # from the over current situation decrease the set-point in your
+    # application below the set-point when the over current flag had
+    # been set
+    # Currently not implemented
+    #current-max: 1
+    #current-max-time: 400
diff --git a/src/plugins/robotino/act_thread.cpp b/src/plugins/robotino/act_thread.cpp
index e33e535..18642b3 100644
--- a/src/plugins/robotino/act_thread.cpp
+++ b/src/plugins/robotino/act_thread.cpp
@@ -63,7 +63,7 @@ RobotinoActThread::init()
 	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_odom_frame_           = config->get_string("/frames/odom");
 	cfg_base_frame_           = config->get_string("/frames/base");
 	std::string odom_mode     = config->get_string("/hardware/robotino/odometry/mode");
 	cfg_odom_corr_phi_        =
@@ -71,9 +71,18 @@ 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");
+	cfg_rb_   = config->get_float("/hardware/robotino/drive/layout/rb");
+	cfg_rw_   = config->get_float("/hardware/robotino/drive/layout/rw");
+	cfg_gear_ = config->get_float("/hardware/robotino/drive/layout/gear");
+
+	cfg_trans_accel_ = config->get_float("/hardware/robotino/drive/trans-acceleration");
+	cfg_trans_decel_ = config->get_float("/hardware/robotino/drive/trans-deceleration");
+	cfg_rot_accel_   = config->get_float("/hardware/robotino/drive/rot-acceleration");
+	cfg_rot_decel_   = config->get_float("/hardware/robotino/drive/rot-deceleration");
+
+	com_->set_drive_layout(cfg_rb_, cfg_rw_, cfg_gear_);
+	com_->set_drive_limits(cfg_trans_accel_, cfg_trans_decel_,
+	                       cfg_rot_accel_, cfg_rot_decel_);
 
 	std::string imu_if_id;
 
@@ -95,10 +104,6 @@ RobotinoActThread::init()
 	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);
 
@@ -155,10 +160,8 @@ RobotinoActThread::loop()
 		return;
 	}
 
-	bool set_speed_points = false;
-	float s1 = 0., s2 = 0., s3 = 0.;
 	bool reset_odometry = false;
-
+	bool set_des_vel = false;
 	while (! motor_if_->msgq_empty()) {
 		if (MotorInterface::SetMotorStateMessage *msg = motor_if_->msgq_first_safe(msg))
 		{
@@ -171,18 +174,15 @@ RobotinoActThread::loop()
 
 		else if (MotorInterface::TransRotMessage *msg = motor_if_->msgq_first_safe(msg))
 		{
-			project(&s1, &s2, &s3, msg->vx(), msg->vy(), msg->omega());
-
 			des_vx_    = msg->vx();
 			des_vy_    = msg->vy();
 			des_omega_ = msg->omega();
 
-			set_speed_points = true;
-        
 			last_msg_time_ = clock->now();
-			msg_received_ = true;
-	
-			msg_zero_vel_ = (s1 == 0.0 && s2 == 0.0 && s3 == 0.0);
+			msg_received_ = true;	
+
+			set_des_vel = true;
+			msg_zero_vel_ = (des_vx_ == 0.0 && des_vy_ == 0.0 && des_omega_ == 0.0);
 		}
 
 		else if (motor_if_->msgq_first_is<MotorInterface::ResetOdometryMessage>())
@@ -226,21 +226,22 @@ RobotinoActThread::loop()
 	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;
+		des_vx_ = des_vy_ = des_omega_ = 0.;
+		msg_zero_vel_ = true;
+		set_des_vel = 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))) {
+		if (set_des_vel && ((des_vx_ != 0.0) || (des_vy_ != 0.0) || (des_omega_ != 0.0))) {
 			logger->log_warn(name(), "Motor command received while disabled, ignoring");
 		}
-		s1 = s2 = s3 = 0.;
-		set_speed_points = true;
+		des_vx_ = des_vy_ = des_omega_ = 0.;
+		set_des_vel = true;
 	}
 
-	if (set_speed_points)  com_->set_speed_points(s1, s2, s3);
-	if (reset_odometry)    com_->reset_odometry();
+	if (reset_odometry)  com_->reset_odometry();
+	if (set_des_vel)     com_->set_desired_vel(des_vx_, des_vy_, des_omega_);
 
 	publish_odometry();
 
@@ -263,7 +264,7 @@ RobotinoActThread::publish_odometry()
 		last_seqnum_ = seq;
 
 		float vx = 0., vy = 0., omega = 0.;
-		unproject(&vx, &vy, &omega, a1, a2, a3);
+		com_->unproject(&vx, &vy, &omega, a1, a2, a3);
 
 		motor_if_->set_vx(vx);
 		motor_if_->set_vy(vy);
@@ -410,82 +411,3 @@ RobotinoActThread::publish_gripper()
 	}
 }
 
-/** 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 2e099e4..64dd26e 100644
--- a/src/plugins/robotino/act_thread.h
+++ b/src/plugins/robotino/act_thread.h
@@ -75,9 +75,6 @@ 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_;
 
@@ -93,23 +90,23 @@ class RobotinoActThread
 	bool        			  msg_zero_vel_;
 	fawkes::Time 			  last_msg_time_;
 
-	float        			  cfg_deadman_threshold_;
-	float        			  cfg_odom_time_offset_;
-	bool 				  cfg_gripper_enabled_;
+	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_;
+	bool                            cfg_bumper_estop_enabled_;	
 	float                           cfg_rb_;
 	float                           cfg_rw_;
 	float                           cfg_gear_;
-	
-	float                           des_vx_;
-	float                           des_vy_;
-	float                           des_omega_;
+	float                           cfg_trans_accel_;
+	float                           cfg_trans_decel_;
+	float                           cfg_rot_accel_;
+	float                           cfg_rot_decel_;
 
 	bool                            gripper_close_;
 
@@ -118,6 +115,10 @@ class RobotinoActThread
 	float                           odom_phi_;
 	float                           odom_gyro_origin_;
 	fawkes::Time                   *odom_time_;
+
+	float                           des_vx_;
+	float                           des_vy_;
+	float                           des_omega_;
 };
 
 
diff --git a/src/plugins/robotino/com_thread.cpp b/src/plugins/robotino/com_thread.cpp
index 8ac72bf..33afa0b 100644
--- a/src/plugins/robotino/com_thread.cpp
+++ b/src/plugins/robotino/com_thread.cpp
@@ -71,6 +71,11 @@ using namespace fawkes;
  * @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
+ *
+ * @fn void RobotinoComThread::set_motor_accel_limits(float min_accel, float max_accel) = 0
+ * Set acceleration limits of motors.
+ * @param min_accel minimum acceleration
+ * @param max_accel maximum acceleration
  */
 
 /** @class RobotinoComThread::SensorData "com_thread.h"
@@ -96,6 +101,31 @@ RobotinoComThread::RobotinoComThread(const char *thread_name)
 {
 	data_mutex_  = new Mutex();
 	new_data_    = false;
+
+	vel_mutex_ = new Mutex();
+	vel_last_update_ = new Time();
+	vel_last_zero_ = false;
+	des_vx_    = 0.;
+	des_vy_    = 0.;
+	des_omega_ = 0.;
+
+	set_vx_    = 0.;
+	set_vy_    = 0.;
+	set_omega_ = 0.;
+
+	cfg_rb_   = 0.;
+	cfg_rw_   = 0.;
+	cfg_gear_ = 0.;	
+
+	cfg_trans_accel_ = 0.;
+	cfg_trans_decel_ = 0.;
+	cfg_rot_accel_   = 0.;
+	cfg_rot_decel_   = 0.;
+
+#ifdef USE_VELOCITY_RECORDING
+	f_ = fopen("comdata.csv", "w");
+	start_ = new Time();
+#endif
 }
 
 
@@ -103,6 +133,9 @@ RobotinoComThread::RobotinoComThread(const char *thread_name)
 RobotinoComThread::~RobotinoComThread()
 {
 	delete data_mutex_;
+#ifdef USE_VELOCITY_RECORDING
+	fclose(f_);
+#endif
 }
 
 
@@ -124,3 +157,228 @@ RobotinoComThread::get_data(SensorData &sensor_data)
 		return false;
 	}
 }
+
+
+/** Set omni drive layout parameters.
+ * @param rb Distance from Robotino center to wheel center in meters
+ * @param rw Wheel radius in meters
+ * @param gear Gear ratio between motors and wheels
+ */
+void
+RobotinoComThread::set_drive_layout(float rb, float rw, float gear)
+{
+	cfg_rb_   = rb;
+	cfg_rw_   = rw;
+	cfg_gear_ = gear;	
+}
+
+
+/** Set the omni drive limits.
+ * @param trans_accel maximum acceleration in translation
+ * @param trans_decel maximum deceleration in translation
+ * @param rot_accel maximum acceleration in rotation
+ * @param rot_decel maximum deceleration in rotation
+ */
+void
+RobotinoComThread::set_drive_limits(float trans_accel, float trans_decel, float rot_accel, float rot_decel)
+{
+	cfg_trans_accel_ = trans_accel;
+	cfg_trans_decel_ = trans_decel;
+	cfg_rot_accel_   = rot_accel;
+	cfg_rot_decel_   = rot_decel;
+}
+
+
+/** Set desired velocities.
+ * @param vx desired velocity in base_link frame X direction ("forward")
+ * @param vy desired velocity in base_link frame Y direction ("sideward")
+ * @param omega desired rotational velocity
+ */
+void
+RobotinoComThread::set_desired_vel(float vx, float vy, float omega)
+{
+	des_vx_    = vx;
+	des_vy_    = vy;
+	des_omega_ = omega;
+}
+
+
+/** Update velocity values.
+ * This method must be called periodically while driving to update the controller.
+ * @return true if the method must be called again, false otherwise
+ */
+bool
+RobotinoComThread::update_velocities()
+{
+	bool set_speed = false;
+
+	Time now(clock);
+	float diff_sec = now - vel_last_update_;
+	*vel_last_update_ = now;
+
+	set_vx_    = update_speed(des_vx_, set_vx_, cfg_trans_accel_, cfg_trans_decel_, diff_sec);
+	set_vy_    = update_speed(des_vy_, set_vy_, cfg_trans_accel_, cfg_trans_decel_, diff_sec);
+	set_omega_ = update_speed(des_omega_, set_omega_, cfg_rot_accel_, cfg_rot_decel_, diff_sec);
+
+	/*
+	logger->log_info(name(), "VX: %.2f -> %.2f (%.2f)   VY: %.2f -> %.2f (%.2f)   Omg: %.2f -> %.2f (%.2f)",
+	                 old_set_vx, set_vx_, des_vx_,
+	                 old_set_vy, set_vy_, des_vy_,
+	                 old_set_omega, set_omega_);
+	*/
+
+	if (set_vx_ == 0.0 && set_vy_ == 0.0 && set_omega_ == 0.0) {
+		if (! vel_last_zero_) {
+			set_speed = true;
+			vel_last_zero_ = true;
+		}
+	} else {
+		set_speed = true;
+		vel_last_zero_ = false;
+	}
+
+	if (set_speed) {
+		float s1 = 0., s2 = 0., s3 = 0.;
+		project(&s1, &s2, &s3, set_vx_, set_vy_, set_omega_);
+		set_speed_points(s1, s2, s3);
+
+#ifdef USE_VELOCITY_RECORDING
+		{
+			Time now(clock);
+			float time_diff = now - start_;
+	
+			fprintf(f_, "%f\t%f\t%f\t%f\t%f\t%f\t%f\n", time_diff,
+			        des_vx_, set_vx_, des_vy_, set_vy_, des_omega_, set_omega_);
+		}
+#endif
+
+	}
+
+	return ! vel_last_zero_;
+}
+
+float
+RobotinoComThread::update_speed(float des, float set, float accel, float decel, float diff_sec)
+{
+	if (des >= 0 && set < 0) {
+		const float decrement = std::copysign(decel, set) * diff_sec;
+		if (des > set - decrement) {
+			//logger->log_debug(name(), "    Case 1a  %f  %f  %f", decrement, decel, diff_sec);
+			set -= decrement;
+		} else {
+			//logger->log_debug(name(), "    Case 1b");
+			set = des;
+		}
+
+	} else if (des <= 0 && set > 0) {
+		const float decrement = std::copysign(decel, set) * diff_sec;
+		if (des < set - decrement ) {
+			//logger->log_debug(name(), "    Case 1c  %f  %f  %f", decrement, decel, diff_sec);
+			set -= decrement;
+		} else {
+			//logger->log_debug(name(), "    Case 1d");
+			set = des;
+		}
+
+	} else if (fabs(des) > fabs(set)) {
+		const float increment = std::copysign(accel, des) * diff_sec;
+		if (fabs(des) > fabs(set + increment)) {
+			//logger->log_debug(name(), "    Case 2a  %f  %f", increment, accel, diff_sec);
+			set += increment;
+		} else {
+			//logger->log_debug(name(), "    Case 2b");
+			set  = des;
+		}
+	} else if (fabs(des) < fabs(set)) {
+		const float decrement = std::copysign(decel, des) * diff_sec;
+		if (fabs(des) < fabs(set - decrement)) {
+			//logger->log_debug(name(), "    Case 3a  %f  %f  %f", decrement, decel, diff_sec);
+			set -= decrement;
+		} else {
+			//logger->log_debug(name(), "    Case 3b");
+			set  = des;
+		}
+	}
+
+	return set;
+}
+
+/** 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
+RobotinoComThread::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
+RobotinoComThread::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/com_thread.h b/src/plugins/robotino/com_thread.h
index 168d39a..55b530f 100644
--- a/src/plugins/robotino/com_thread.h
+++ b/src/plugins/robotino/com_thread.h
@@ -24,25 +24,22 @@
 #include <core/threading/thread.h>
 #include <aspect/logging.h>
 #include <aspect/clock.h>
-#include <aspect/configurable.h>
-#include <aspect/blackboard.h>
 
 #include <utils/time/time.h>
 
+#include <cstdio>
+
 namespace fawkes {
 	class Mutex;
 	class Clock;
-	class TimeWait;
-
-	class BatteryInterface;
-	class RobotinoSensorInterface;
-	class IMUInterface;
 }
 
 #define NUM_IR_SENSORS 9
 
 class RobotinoComThread
-: public fawkes::Thread
+: public fawkes::Thread,
+	public fawkes::ClockAspect,
+	public fawkes::LoggingAspect
 {
  public:
 	struct SensorData {
@@ -88,20 +85,59 @@ class RobotinoComThread
 	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_bumper_estop_enabled(bool enabled) = 0;
+	virtual void set_motor_accel_limits(float min_accel, float max_accel) = 0;
 	
 	virtual bool get_data(SensorData &sensor_data);
 
+	        void set_drive_layout(float rb, float rw, float gear);
+	        void set_drive_limits(float trans_accel, float trans_decel, float rot_accel, float rot_decel);
+	virtual void set_desired_vel(float vx, float vy, float omega);
+
+	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;
+
+ protected:
+	bool update_velocities();
+	
+ private:
+
+	float update_speed(float des, float set, float accel, float decel, float diff_sec);
+
  protected:
 	/** Mutex to protect data_. Lock whenever accessing it. */
-	fawkes::Mutex    *data_mutex_;
+	fawkes::Mutex  *data_mutex_;
 	/** Data struct that must be updated whenever new data is available. */
-	SensorData        data_;
+	SensorData      data_;
 	/** Flag to indicate new data, set to true if data_ is modified. */
-	bool              new_data_;
-
-
+	bool            new_data_;
+
+ private:
+	float           cfg_rb_;
+	float           cfg_rw_;
+	float           cfg_gear_;
+	float           cfg_trans_accel_;
+	float           cfg_trans_decel_;
+	float           cfg_rot_accel_;
+	float           cfg_rot_decel_;
+	
+	fawkes::Mutex  *vel_mutex_;
+	fawkes::Time   *vel_last_update_;
+	bool            vel_last_zero_;
+	float           des_vx_;
+	float           des_vy_;
+	float           des_omega_;
+
+	float           set_vx_;
+	float           set_vy_;
+	float           set_omega_;
+
+#ifdef USE_VELOCITY_RECORDING
+	FILE *f_;
+	fawkes::Time     *start_;
+#endif
 };
 
 
diff --git a/src/plugins/robotino/direct_com_thread.cpp b/src/plugins/robotino/direct_com_thread.cpp
index 7425ad0..1a77809 100644
--- a/src/plugins/robotino/direct_com_thread.cpp
+++ b/src/plugins/robotino/direct_com_thread.cpp
@@ -50,7 +50,7 @@ using namespace fawkes;
 DirectRobotinoComThread::DirectRobotinoComThread()
 	: RobotinoComThread("DirectRobotinoComThread"),
 	  serial_(io_service_), io_service_work_(io_service_), deadline_(io_service_),
-	  request_timer_(io_service_), nodata_timer_(io_service_)
+	  request_timer_(io_service_), nodata_timer_(io_service_), drive_timer_(io_service_)
 {
 	set_prepfin_conc_loop(true);
 }
@@ -69,6 +69,7 @@ DirectRobotinoComThread::init()
 	cfg_sensor_update_cycle_time_ =
 		config->get_uint("/hardware/robotino/cycle-time");
 	cfg_gripper_enabled_ = config->get_bool("/hardware/robotino/gripper/enable_gripper");
+	cfg_rpm_max_ = config->get_float("/hardware/robotino/motor/rpm-max");
 
 	// -------------------------------------------------------------------------- //
 
@@ -85,8 +86,10 @@ DirectRobotinoComThread::init()
 	deadline_.expires_at(boost::posix_time::pos_infin);
 	check_deadline();
 
-	open_device();
-	opened_ = true;
+	request_timer_.expires_from_now(boost::posix_time::milliseconds(-1));
+	drive_timer_.expires_at(boost::posix_time::pos_infin);
+
+	open_device(/* wait for replies */ true);
 	open_tries_ = 0;
 }
 
@@ -97,6 +100,8 @@ DirectRobotinoComThread::prepare_finalize_user()
 	//logger->log_info(name(), "Prepare Finalize");
 	request_timer_.cancel();
 	nodata_timer_.cancel();
+	drive_timer_.cancel();
+	drive_timer_.expires_at(boost::posix_time::pos_infin);
 	request_timer_.expires_at(boost::posix_time::pos_infin);
 	nodata_timer_.expires_at(boost::posix_time::pos_infin);
 	deadline_.expires_at(boost::posix_time::pos_infin);
@@ -135,22 +140,27 @@ DirectRobotinoComThread::loop()
 			update_nodata_timer();
 		} catch (Exception &e) {
 			if (! finalize_prepared) {
-				logger->log_warn(name(), "Transmission error, sending ping");
-				logger->log_warn(name(), e);
-				input_buffer_.consume(input_buffer_.size());
-				DirectRobotinoComMessage req(DirectRobotinoComMessage::CMDID_GET_HW_VERSION);
-				send_message(req);
-				//} else {
-				//logger->log_warn(name(), "Transmission error, but finalize prepared");
-				//logger->log_warn(name(), e);
+				if (opened_) {
+					logger->log_warn(name(), "Transmission error, sending ping");
+					logger->log_warn(name(), e);
+					input_buffer_.consume(input_buffer_.size());
+					try {
+						DirectRobotinoComMessage req(DirectRobotinoComMessage::CMDID_GET_HW_VERSION);
+						send_message(req);
+						request_data();
+					} catch (Exception &e) {}
+				} else {
+					logger->log_warn(name(), "Transmission error, connection closed");
+				}
 			}
 		}
 	} else {
 		try {
 			logger->log_info(name(), "Re-opening device");
-			open_device();
-			opened_ = true;
+			open_device(/* wait for replies */ false);
 			logger->log_info(name(), "Connection re-established after %u tries", open_tries_ + 1);
+			open_tries_ = 0;
+			request_data();
 		} catch (Exception &e) {
 			open_tries_ += 1;
 			if (open_tries_ >= (1000 / cfg_sensor_update_cycle_time_)) {
@@ -241,11 +251,35 @@ DirectRobotinoComThread::process_message(DirectRobotinoComMessage::pointer m)
 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);
+	try {
+		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);
+	} catch (Exception &e) {
+		logger->log_error(name(), "Resetting odometry failed, exception follows");
+		logger->log_error(name(), e);
+	}
+}
+
+
+void
+DirectRobotinoComThread::set_motor_accel_limits(float min_accel, float max_accel)
+{
+	try {
+		DirectRobotinoComMessage req;
+		for (int i = 0; i < 2; ++i) {
+			req.add_command(DirectRobotinoComMessage::CMDID_SET_MOTOR_ACCEL_LIMITS);
+			req.add_uint8(i);
+			req.add_float(min_accel);
+			req.add_float(max_accel);
+		}
+		send_message(req);
+	} catch (Exception &e) {
+		logger->log_error(name(), "Setting motor accel limits failed, exception follows");
+		logger->log_error(name(), e);
+	}
 }
 
 
@@ -288,17 +322,26 @@ 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);
+	float bounded_s1 = std::max(-cfg_rpm_max_, std::min(cfg_rpm_max_, s1));
+	float bounded_s2 = std::max(-cfg_rpm_max_, std::min(cfg_rpm_max_, s2));
+	float bounded_s3 = std::max(-cfg_rpm_max_, std::min(cfg_rpm_max_, s3));
+	
+	try {
+		DirectRobotinoComMessage m;
+		m.add_command(DirectRobotinoComMessage::CMDID_SET_MOTOR_SPEED);
+		m.add_uint8(0);
+		m.add_uint16((uint16_t)roundf(bounded_s1));
+		m.add_command(DirectRobotinoComMessage::CMDID_SET_MOTOR_SPEED);
+		m.add_uint8(1);
+		m.add_uint16((uint16_t)roundf(bounded_s2));
+		m.add_command(DirectRobotinoComMessage::CMDID_SET_MOTOR_SPEED);
+		m.add_uint8(2);
+		m.add_uint16((uint16_t)roundf(bounded_s3));
+		send_message(m);
+	} catch (Exception &e) {
+		logger->log_error(name(), "Setting speed points failed, exception follows");
+		logger->log_error(name(), e);
+	}
 }
 
 void
@@ -309,12 +352,17 @@ 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);
+	try {
+		DirectRobotinoComMessage m(DirectRobotinoComMessage::CMDID_SET_EMERGENCY_BUMPER);
+		m.add_uint8(enabled ? 1 : 0);
+		send_message(m);
 
-	MutexLocker lock(data_mutex_);
-	data_.bumper_estop_enabled = enabled;
+		MutexLocker lock(data_mutex_);
+		data_.bumper_estop_enabled = enabled;
+	} catch (Exception &e) {
+		logger->log_error(name(), "Setting bumper estop state failed, exception follows");
+		logger->log_error(name(), e);
+	}
 }
 
 
@@ -392,7 +440,7 @@ DirectRobotinoComThread::find_device_udev()
 }
 
 void
-DirectRobotinoComThread::open_device()
+DirectRobotinoComThread::open_device(bool wait_replies)
 {
 	if (finalize_prepared)  return;
 
@@ -405,10 +453,8 @@ DirectRobotinoComThread::open_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));
 		serial_.set_option(boost::asio::serial_port::baud_rate(115200));
-    
-		//send_init_packet(/* enable transfer */ true);
-
-		//resync();
+		
+		opened_ = true;
 	} catch (boost::system::system_error &e) {
 		throw Exception("RobotinoDirect failed I/O: %s", e.what());
 	}
@@ -417,34 +463,44 @@ DirectRobotinoComThread::open_device()
 		DirectRobotinoComMessage req;
 		req.add_command(DirectRobotinoComMessage::CMDID_GET_HW_VERSION);
 		req.add_command(DirectRobotinoComMessage::CMDID_GET_SW_VERSION);
-		DirectRobotinoComMessage::pointer m = send_and_recv(req);
-
-		//logger->log_info(name(), "Escaped:\n%s", m->to_string(true).c_str());
-		//logger->log_info(name(), "Un-Escaped:\n%s", m->to_string(false).c_str());
-		std::string hw_version, sw_version;
-		DirectRobotinoComMessage::command_id_t msgid;
-		while ((msgid = m->next_command()) != DirectRobotinoComMessage::CMDID_NONE) {
-			if (msgid == DirectRobotinoComMessage::CMDID_HW_VERSION) {
-				hw_version = m->get_string();
-			} else if (msgid == DirectRobotinoComMessage::CMDID_SW_VERSION) {
-				sw_version = m->get_string();
+
+		if (wait_replies) {
+			DirectRobotinoComMessage::pointer m = send_and_recv(req);
+
+			//logger->log_info(name(), "Escaped:\n%s", m->to_string(true).c_str());
+			//logger->log_info(name(), "Un-Escaped:\n%s", m->to_string(false).c_str());
+			std::string hw_version, sw_version;
+			DirectRobotinoComMessage::command_id_t msgid;
+			while ((msgid = m->next_command()) != DirectRobotinoComMessage::CMDID_NONE) {
+				if (msgid == DirectRobotinoComMessage::CMDID_HW_VERSION) {
+					hw_version = m->get_string();
+				} else if (msgid == DirectRobotinoComMessage::CMDID_SW_VERSION) {
+					sw_version = m->get_string();
 				
-			} 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());
-				//} else {
-				//logger->log_debug(name(), "  - %u\n", msgid);
+				} 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());
+					//} else {
+					//logger->log_debug(name(), "  - %u\n", msgid);
+				}
+			}
+			if (hw_version.empty() || sw_version.empty()) {
+				close_device();
+				throw Exception("RobotinoDirect: no reply to version inquiry from robot");
+			}
+			logger->log_debug(name(), "Connected, HW Version: %s  SW Version: %s",
+			                  hw_version.c_str(), sw_version.c_str());
+		} else {
+			try {
+				send_message(req);
+			} catch (Exception &e) {
+				logger->log_error(name(), "Requesting version information failed, exception follows");
+				logger->log_error(name(), e);
 			}
 		}
-		if (hw_version.empty() || sw_version.empty()) {
-			close_device();
-			throw Exception("RobotinoDirect: no reply to version inquiry from robot");
-		}
-		logger->log_debug(name(), "Connected, HW Version: %s  SW Version: %s",
-		                  hw_version.c_str(), sw_version.c_str());
 	}
 }
 
@@ -452,9 +508,10 @@ DirectRobotinoComThread::open_device()
 void
 DirectRobotinoComThread::close_device()
 {
-	boost::mutex::scoped_lock lock(io_mutex_);
 	serial_.cancel();
 	serial_.close();
+	opened_ = false;
+	open_tries_ = 0;
 }
 
 
@@ -496,25 +553,36 @@ DirectRobotinoComThread::send_message(DirectRobotinoComMessage &msg)
 	boost::mutex::scoped_lock lock(io_mutex_);
 	if (opened_) {
 		//logger->log_warn(name(), "Sending");
-		boost::asio::write(serial_, boost::asio::const_buffers_1(msg.buffer()));
+		boost::system::error_code ec;
+		boost::asio::write(serial_, boost::asio::const_buffers_1(msg.buffer()), ec);
+
+		if (ec) {
+			close_device();
+			throw Exception("Error while writing message (%s), closing connection",
+			                ec.message().c_str());
+		}
 	}
 }
 
-/*
-  std::shared_ptr<DirectRobotinoComMessage>
-  DirectRobotinoComMessage::send_and_recv(std::shared_ptr<DirectRobotinoComMessage> msg)
-  {
-	
-  }
-*/
-
 std::shared_ptr<DirectRobotinoComMessage>
 DirectRobotinoComThread::send_and_recv(DirectRobotinoComMessage &msg)
 {
 	boost::mutex::scoped_lock lock(io_mutex_);
-	boost::asio::write(serial_, boost::asio::const_buffers_1(msg.buffer()));
-	std::shared_ptr<DirectRobotinoComMessage> m = read_packet();
-	return m;
+	if (opened_) {
+		boost::system::error_code ec;
+		boost::asio::write(serial_, boost::asio::const_buffers_1(msg.buffer()), ec);
+		if (ec) {
+			logger->log_error(name(), "Error while writing message (%s), closing connection",
+			                  ec.message().c_str());
+
+			close_device();
+			throw Exception("RobotinoDirect: write failed (%s)", ec.message().c_str());
+		}
+		std::shared_ptr<DirectRobotinoComMessage> m = read_packet();
+		return m;
+	} else {
+		throw Exception("RobotinoDirect: serial device not opened");
+	}
 }
 
 /// @cond INTERNAL
@@ -571,9 +639,9 @@ DirectRobotinoComThread::read_packet()
 	}
 
 	// Read all potential junk before the start header
-	if (bytes_read > 1) {
-		logger->log_warn(name(), "Read junk off line");
-	}
+	// if (bytes_read > 1) {
+	// 	logger->log_warn(name(), "Read junk off line");
+	// }
 	input_buffer_.consume(bytes_read - 1);
 	
 	// start timeout for remaining packet
@@ -660,9 +728,11 @@ DirectRobotinoComThread::request_data()
 {
 	if (finalize_prepared)  return;
 
-	request_timer_.expires_from_now(boost::posix_time::milliseconds(cfg_sensor_update_cycle_time_));
-	request_timer_.async_wait(boost::bind(&DirectRobotinoComThread::handle_request_data, this,
-	                                      boost::asio::placeholders::error));
+	if (request_timer_.expires_from_now() < boost::posix_time::milliseconds(0)) {
+		request_timer_.expires_from_now(boost::posix_time::milliseconds(cfg_sensor_update_cycle_time_));
+		request_timer_.async_wait(boost::bind(&DirectRobotinoComThread::handle_request_data, this,
+		                                      boost::asio::placeholders::error));
+	}
 }
 
 void
@@ -693,16 +763,48 @@ DirectRobotinoComThread::handle_request_data(const boost::system::error_code &ec
 			logger->log_warn(name(), e);
 		}
 
+	} else {		
+		logger->log_warn(name(), "Request timer failed: %s", ec.message().c_str());
+	}
+
+	if (! finalize_prepared && opened_) {
 		request_data();
 	}
 }
 
+void
+DirectRobotinoComThread::set_desired_vel(float vx, float vy, float omega)
+{
+	RobotinoComThread::set_desired_vel(vx, vy, omega);
+	drive();
+}
+
+
+void
+DirectRobotinoComThread::drive()
+{
+	if (finalize_prepared)  return;
+
+	drive_timer_.expires_from_now(boost::posix_time::milliseconds(10));
+	drive_timer_.async_wait(boost::bind(&DirectRobotinoComThread::handle_drive, this,
+	                                    boost::asio::placeholders::error));	
+}
+
+
+void
+DirectRobotinoComThread::handle_drive(const boost::system::error_code &ec)
+{
+	if (! ec) {
+		if (update_velocities())  drive();
+	}
+}
+
 
 void
 DirectRobotinoComThread::update_nodata_timer()
 {
 	nodata_timer_.cancel();
-	nodata_timer_.expires_from_now(boost::posix_time::milliseconds(1000));
+	nodata_timer_.expires_from_now(boost::posix_time::milliseconds(2000));
 	nodata_timer_.async_wait(boost::bind(&DirectRobotinoComThread::handle_nodata, this,
 	                                     boost::asio::placeholders::error));
 }
@@ -714,7 +816,5 @@ DirectRobotinoComThread::handle_nodata(const boost::system::error_code &ec)
 	if (! ec) {
 		logger->log_error(name(), "No data received for too long, re-establishing connection");
 		close_device();
-		opened_ = false;
-		open_tries_ = 0;
 	}
 }
diff --git a/src/plugins/robotino/direct_com_thread.h b/src/plugins/robotino/direct_com_thread.h
index a20684d..f419604 100644
--- a/src/plugins/robotino/direct_com_thread.h
+++ b/src/plugins/robotino/direct_com_thread.h
@@ -24,8 +24,6 @@
 #include "com_thread.h"
 #include "direct_com_message.h"
 #include <core/threading/thread.h>
-#include <aspect/logging.h>
-#include <aspect/clock.h>
 #include <aspect/configurable.h>
 #include <aspect/blackboard.h>
 
@@ -39,7 +37,6 @@ class DirectRobotinoComMessage;
 
 namespace fawkes {
 	class Mutex;
-	class Clock;
 	class TimeWait;
 
 	class BatteryInterface;
@@ -49,9 +46,7 @@ namespace fawkes {
 
 class DirectRobotinoComThread
 : public RobotinoComThread,
-	public fawkes::LoggingAspect,
-	public fawkes::ConfigurableAspect,
-	public fawkes::ClockAspect
+	public fawkes::ConfigurableAspect
 {
  public:
 	DirectRobotinoComThread();
@@ -71,15 +66,19 @@ class DirectRobotinoComThread
 	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);
+	virtual void set_motor_accel_limits(float min_accel, float max_accel);
+
+	virtual void set_desired_vel(float vx, float vy, float omega);
 
 	/** Stub to see name in backtrace for easier debugging. @see Thread::run() */
  protected: virtual void run() { Thread::run(); }
 
  private:
 	std::string find_device_udev();
-	void open_device();
+	void open_device(bool wait_replies);
 	void close_device();
 	void flush_device();
 	void check_deadline();
@@ -87,7 +86,10 @@ class DirectRobotinoComThread
 	void handle_request_data(const boost::system::error_code &ec);
 	void handle_nodata(const boost::system::error_code &ec);
 	void update_nodata_timer();
-	
+
+	void drive();
+	void handle_drive(const boost::system::error_code &ec);
+
 	DirectRobotinoComMessage::pointer read_packet();
 	void send_message(DirectRobotinoComMessage &msg);
 	DirectRobotinoComMessage::pointer
@@ -99,6 +101,7 @@ class DirectRobotinoComThread
 	bool            cfg_enable_gyro_;
 	unsigned int    cfg_sensor_update_cycle_time_;
 	bool            cfg_gripper_enabled_;
+	float           cfg_rpm_max_;
 
 	bool opened_;
 	unsigned int open_tries_;
@@ -112,6 +115,7 @@ class DirectRobotinoComThread
 
 	boost::asio::deadline_timer   request_timer_;
 	boost::asio::deadline_timer   nodata_timer_;
+	boost::asio::deadline_timer   drive_timer_;
 	
 };
 
diff --git a/src/plugins/robotino/openrobotino_com_thread.cpp b/src/plugins/robotino/openrobotino_com_thread.cpp
index 4b2492b..c12e542 100644
--- a/src/plugins/robotino/openrobotino_com_thread.cpp
+++ b/src/plugins/robotino/openrobotino_com_thread.cpp
@@ -382,6 +382,13 @@ OpenRobotinoComThread::reset_odometry()
 }
 
 
+void
+OpenRobotinoComThread::set_motor_accel_limits(float min_accel, float max_accel)
+{
+	throw Exception("Setting motor accel limits for OpenRobotino driver not supported, configure controld3");
+}
+
+
 /** Check if we are connected to OpenRobotino.
  * @return true if the connection has been established, false otherwise
  */
diff --git a/src/plugins/robotino/openrobotino_com_thread.h b/src/plugins/robotino/openrobotino_com_thread.h
index bfc177c..b9b7869 100644
--- a/src/plugins/robotino/openrobotino_com_thread.h
+++ b/src/plugins/robotino/openrobotino_com_thread.h
@@ -23,8 +23,6 @@
 
 #include "com_thread.h"
 #include <core/threading/thread.h>
-#include <aspect/logging.h>
-#include <aspect/clock.h>
 #include <aspect/configurable.h>
 #include <aspect/blackboard.h>
 
@@ -68,10 +66,6 @@ namespace fawkes {
 	class Mutex;
 	class Clock;
 	class TimeWait;
-
-	class BatteryInterface;
-	class RobotinoSensorInterface;
-	class IMUInterface;
 }
 
 class OpenRobotinoComThread
@@ -79,9 +73,7 @@ class OpenRobotinoComThread
 #ifdef HAVE_OPENROBOTINO_API_1
 	public rec::robotino::com::Com,
 #endif
-	public fawkes::LoggingAspect,
-	public fawkes::ConfigurableAspect,
-	public fawkes::ClockAspect
+	public fawkes::ConfigurableAspect
 {
  public:
 	OpenRobotinoComThread();
@@ -99,8 +91,10 @@ class OpenRobotinoComThread
 	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);
+	virtual void set_motor_accel_limits(float min_accel, float max_accel);
 
 	/** Stub to see name in backtrace for easier debugging. @see Thread::run() */
  protected: virtual void run() { Thread::run(); }

- *commit* cceb762465160de2af9eee56d351663e88852e82 - - - - - - - - - -
Author:  Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
Date:    Tue Apr 12 23:41:07 2016 +0200
Subject: robotino: determine if controld3 is running in direct mode

 src/plugins/robotino/direct_com_thread.cpp |   62 ++++++++++++++++++++++++++++
 src/plugins/robotino/direct_com_thread.h   |    1 +
 src/plugins/robotino/robotino.mk           |    2 +-
 3 files changed, 64 insertions(+), 1 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/robotino/direct_com_thread.cpp b/src/plugins/robotino/direct_com_thread.cpp
index 1a77809..0b04679 100644
--- a/src/plugins/robotino/direct_com_thread.cpp
+++ b/src/plugins/robotino/direct_com_thread.cpp
@@ -38,6 +38,7 @@
 #include <boost/bind.hpp>
 #include <boost/lambda/bind.hpp>
 #include <boost/lambda/lambda.hpp>
+#include <boost/filesystem.hpp>
 
 using namespace fawkes;
 
@@ -73,6 +74,10 @@ DirectRobotinoComThread::init()
 
 	// -------------------------------------------------------------------------- //
 
+	if (find_controld3()) {
+		throw Exception("Found running controld3, stop using 'sudo initctl stop controld3'");
+	}
+
 	try {
 		cfg_device_ = config->get_string(("/hardware/robotino/direct/device"));
 	} catch (Exception &e) {
@@ -439,6 +444,63 @@ DirectRobotinoComThread::find_device_udev()
 	return cfg_device;
 }
 
+
+bool
+DirectRobotinoComThread::find_controld3()
+{
+	bool rv = false;
+	boost::filesystem::path p("/proc");
+
+	using namespace boost::filesystem;
+ 
+  try {
+	  if (boost::filesystem::exists(p) && boost::filesystem::is_directory(p)) {
+
+		  directory_iterator di;
+		  for (di = directory_iterator(p); di != directory_iterator(); ++di) {
+			  directory_entry &d = *di;
+			  //for (directory_entry &d : directory_iterator(p))
+			  std::string f = d.path().filename().native();
+			  bool is_process = true;
+			  for (std::string::size_type i = 0; i < f.length(); ++i) {
+				  if (! isdigit(f[i])) {
+					  is_process = false;
+					  break;
+				  }
+			  }
+			  if (is_process) {
+				  path pproc(d.path());
+				  pproc /= "stat";
+
+				  FILE *f = fopen(pproc.c_str(), "r");
+				  if (f) {
+					  int pid;
+					  char *procname;
+					  if (fscanf(f, "%d (%m[a-z0-9])", &pid, &procname) == 2) {
+						  
+						  if (strcmp("controld3", procname) == 0) {
+							  rv = true;
+						  }
+						  ::free(procname);
+					  }
+					  fclose(f);
+				  }
+			  }
+		  }
+
+	  } else {
+		  logger->log_warn(name(), "Cannot open /proc, cannot determine if controld3 is running");
+		  return false;
+	  }
+
+  } catch (const boost::filesystem::filesystem_error &ex) {
+	  logger->log_warn(name(), "Failure to determine if controld3 is running: %s", ex.what());
+	  return false;
+  }
+
+  return rv;
+}
+
 void
 DirectRobotinoComThread::open_device(bool wait_replies)
 {
diff --git a/src/plugins/robotino/direct_com_thread.h b/src/plugins/robotino/direct_com_thread.h
index f419604..1a53532 100644
--- a/src/plugins/robotino/direct_com_thread.h
+++ b/src/plugins/robotino/direct_com_thread.h
@@ -78,6 +78,7 @@ class DirectRobotinoComThread
 
  private:
 	std::string find_device_udev();
+	bool find_controld3();
 	void open_device(bool wait_replies);
 	void close_device();
 	void flush_device();
diff --git a/src/plugins/robotino/robotino.mk b/src/plugins/robotino/robotino.mk
index 5401f71..c8c0f62 100644
--- a/src/plugins/robotino/robotino.mk
+++ b/src/plugins/robotino/robotino.mk
@@ -50,7 +50,7 @@ ifeq ($(HAVE_LIBUSB),1)
   LDFLAGS_LIBUSB = $(shell $(PKGCONFIG) --libs 'libusb-1.0')
 endif
 
-ROBOTINO_DIRECT_REQ_BOOST_LIBS = thread asio system
+ROBOTINO_DIRECT_REQ_BOOST_LIBS = thread asio system filesystem
 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))

- *commit* 852d27ad5ea40d106c296f7b1565cdc79fe503f1 - - - - - - - - - -
Author:  Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
Date:    Wed Apr 13 09:17:42 2016 +0200
Subject: joystick: change acquisition thread variable style

 src/plugins/joystick/acquisition_thread.cpp |  210 +++++++++++++-------------
 src/plugins/joystick/acquisition_thread.h   |   32 ++--
 2 files changed, 121 insertions(+), 121 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/joystick/acquisition_thread.cpp b/src/plugins/joystick/acquisition_thread.cpp
index ba56d75..c3b45f7 100644
--- a/src/plugins/joystick/acquisition_thread.cpp
+++ b/src/plugins/joystick/acquisition_thread.cpp
@@ -56,10 +56,10 @@ JoystickAcquisitionThread::JoystickAcquisitionThread()
   : Thread("JoystickAcquisitionThread", Thread::OPMODE_CONTINUOUS)
 {
   set_prepfin_conc_loop(true);
-  __data_mutex = NULL;
-  __axis_values = NULL;
-  __bbhandler = NULL;
-  __ff = NULL;
+  data_mutex_ = NULL;
+  axis_values_ = NULL;
+  bbhandler_ = NULL;
+  ff_ = NULL;
   logger = NULL;
 }
 
@@ -77,10 +77,10 @@ JoystickAcquisitionThread::JoystickAcquisitionThread(const char *device_file,
   : Thread("JoystickAcquisitionThread", Thread::OPMODE_CONTINUOUS)
 {
   set_prepfin_conc_loop(true);
-  __data_mutex = NULL;
-  __axis_values = NULL;
-  __ff = NULL;
-  __bbhandler = handler;
+  data_mutex_ = NULL;
+  axis_values_ = NULL;
+  ff_ = NULL;
+  bbhandler_ = handler;
   this->logger = logger;
   init(device_file);
 }
@@ -90,21 +90,21 @@ void
 JoystickAcquisitionThread::init()
 {
   try {
-    __cfg_device_file    = config->get_string("/hardware/joystick/device_file");
+    cfg_device_file_    = config->get_string("/hardware/joystick/device_file");
   } catch (Exception &e) {
     e.append("Could not read all required config values for %s", name());
     throw;
   }
 
-  __safety_lockout = true;
+  safety_lockout_ = true;
   try {
-	  __safety_lockout = config->get_bool("/hardware/joystick/safety_lockout");
+	  safety_lockout_ = config->get_bool("/hardware/joystick/safety_lockout");
   } catch (Exception &e) {} // ignore, use default
-  for (int i = 0; i < 5; ++i) __safety_combo[i] = false;
+  for (int i = 0; i < 5; ++i) safety_combo_[i] = false;
 
-  init(__cfg_device_file);
+  init(cfg_device_file_);
 
-  if (__safety_lockout) {
+  if (safety_lockout_) {
 	  logger->log_info(name(), "To enable joystick, move primary cross all the way in all "
 	                   "directions while holding first button. Then let go of button.");
   }
@@ -114,75 +114,75 @@ JoystickAcquisitionThread::init()
 void
 JoystickAcquisitionThread::open_joystick()
 {
-  __fd = open(__cfg_device_file.c_str(), O_RDONLY);
-  if ( __fd == -1 ) {
-    throw CouldNotOpenFileException(__cfg_device_file.c_str(), errno,
+  fd_ = open(cfg_device_file_.c_str(), O_RDONLY);
+  if ( fd_ == -1 ) {
+    throw CouldNotOpenFileException(cfg_device_file_.c_str(), errno,
 				    "Opening the joystick device file failed");
   }
 
-  if ( ioctl(__fd, JSIOCGNAME(sizeof(__joystick_name)), __joystick_name) < 0) {
+  if ( ioctl(fd_, JSIOCGNAME(sizeof(joystick_name_)), joystick_name_) < 0) {
     throw Exception(errno, "Failed to get name of joystick");
   }
-  if ( ioctl(__fd, JSIOCGAXES, &__num_axes) < 0 ) {
+  if ( ioctl(fd_, JSIOCGAXES, &num_axes_) < 0 ) {
     throw Exception(errno, "Failed to get number of axes for joystick");
   }
-  if ( ioctl(__fd, JSIOCGBUTTONS, &__num_buttons) < 0 ) {
+  if ( ioctl(fd_, JSIOCGBUTTONS, &num_buttons_) < 0 ) {
     throw Exception(errno, "Failed to get number of buttons for joystick");
   }
 
-  if (__axis_values == NULL) {
+  if (axis_values_ == NULL) {
     // memory had not been allocated
     // minimum of 8 because there are 8 axes in the interface
-    __axis_array_size = std::max((int)__num_axes, 8);
-    __axis_values   = (float *)malloc(sizeof(float) * __axis_array_size);
-  } else if ( __num_axes > std::max((int)__axis_array_size, 8) ) {
+    axis_array_size_ = std::max((int)num_axes_, 8);
+    axis_values_   = (float *)malloc(sizeof(float) * axis_array_size_);
+  } else if ( num_axes_ > std::max((int)axis_array_size_, 8) ) {
     // We loose axes as we cannot increase BB interface on-the-fly
-    __num_axes = __axis_array_size;
+    num_axes_ = axis_array_size_;
   }
 
-  logger->log_debug(name(), "Joystick device:   %s", __cfg_device_file.c_str());
-  logger->log_debug(name(), "Joystick name:     %s", __joystick_name);
-  logger->log_debug(name(), "Number of Axes:    %i", __num_axes);
-  logger->log_debug(name(), "Number of Buttons: %i", __num_buttons);
-  logger->log_debug(name(), "Axis Array Size:   %u", __axis_array_size);
+  logger->log_debug(name(), "Joystick device:   %s", cfg_device_file_.c_str());
+  logger->log_debug(name(), "Joystick name:     %s", joystick_name_);
+  logger->log_debug(name(), "Number of Axes:    %i", num_axes_);
+  logger->log_debug(name(), "Number of Buttons: %i", num_buttons_);
+  logger->log_debug(name(), "Axis Array Size:   %u", axis_array_size_);
 
-  memset(__axis_values, 0, sizeof(float) * __axis_array_size);
-  __pressed_buttons = 0;
+  memset(axis_values_, 0, sizeof(float) * axis_array_size_);
+  pressed_buttons_ = 0;
 
-  if ( __bbhandler ) {
-    __bbhandler->joystick_plugged(__num_axes, __num_buttons);
+  if ( bbhandler_ ) {
+    bbhandler_->joystick_plugged(num_axes_, num_buttons_);
   }
-  __connected = true;
+  connected_ = true;
 }
 
 void
 JoystickAcquisitionThread::open_forcefeedback()
 {
-  __ff = new JoystickForceFeedback(__joystick_name);
-  logger->log_debug(name(), "Force Feedback:    %s", (__ff) ? "Yes" : "No");
+  ff_ = new JoystickForceFeedback(joystick_name_);
+  logger->log_debug(name(), "Force Feedback:    %s", (ff_) ? "Yes" : "No");
   logger->log_debug(name(), "Supported effects:");
 
-  if (__ff->can_rumble())   logger->log_debug(name(), "  rumble");
-  if (__ff->can_periodic()) logger->log_debug(name(), "  periodic");
-  if (__ff->can_constant()) logger->log_debug(name(), "  constant");
-  if (__ff->can_spring())   logger->log_debug(name(), "  spring");
-  if (__ff->can_friction()) logger->log_debug(name(), "  friction");
-  if (__ff->can_damper())   logger->log_debug(name(), "  damper");
-  if (__ff->can_inertia())  logger->log_debug(name(), "  inertia");
-  if (__ff->can_ramp())     logger->log_debug(name(), "  ramp");
-  if (__ff->can_square())   logger->log_debug(name(), "  square");
-  if (__ff->can_triangle()) logger->log_debug(name(), "  triangle");
-  if (__ff->can_sine())     logger->log_debug(name(), "  sine");
-  if (__ff->can_saw_up())   logger->log_debug(name(), "  saw up");
-  if (__ff->can_saw_down()) logger->log_debug(name(), "  saw down");
-  if (__ff->can_custom())   logger->log_debug(name(), "  custom");
+  if (ff_->can_rumble())   logger->log_debug(name(), "  rumble");
+  if (ff_->can_periodic()) logger->log_debug(name(), "  periodic");
+  if (ff_->can_constant()) logger->log_debug(name(), "  constant");
+  if (ff_->can_spring())   logger->log_debug(name(), "  spring");
+  if (ff_->can_friction()) logger->log_debug(name(), "  friction");
+  if (ff_->can_damper())   logger->log_debug(name(), "  damper");
+  if (ff_->can_inertia())  logger->log_debug(name(), "  inertia");
+  if (ff_->can_ramp())     logger->log_debug(name(), "  ramp");
+  if (ff_->can_square())   logger->log_debug(name(), "  square");
+  if (ff_->can_triangle()) logger->log_debug(name(), "  triangle");
+  if (ff_->can_sine())     logger->log_debug(name(), "  sine");
+  if (ff_->can_saw_up())   logger->log_debug(name(), "  saw up");
+  if (ff_->can_saw_down()) logger->log_debug(name(), "  saw down");
+  if (ff_->can_custom())   logger->log_debug(name(), "  custom");
 }
 
 void
 JoystickAcquisitionThread::init(std::string device_file)
 {
-  __new_data = false;
-  __cfg_device_file = device_file;
+  new_data_ = false;
+  cfg_device_file_ = device_file;
   open_joystick();
   try {
     open_forcefeedback();
@@ -190,7 +190,7 @@ JoystickAcquisitionThread::init(std::string device_file)
     logger->log_warn(name(), "Initializing force feedback failed, disabling");
     logger->log_warn(name(), e);
   }
-  __data_mutex = new Mutex();
+  data_mutex_ = new Mutex();
 
 }
 
@@ -198,95 +198,95 @@ JoystickAcquisitionThread::init(std::string device_file)
 void
 JoystickAcquisitionThread::finalize()
 {
-  if ( __fd >= 0 )  close(__fd);
-  free(__axis_values);
-  delete __data_mutex;
+  if ( fd_ >= 0 )  close(fd_);
+  free(axis_values_);
+  delete data_mutex_;
 }
 
 
 void
 JoystickAcquisitionThread::loop()
 {
-  if ( __connected ) {
+  if ( connected_ ) {
     struct js_event e;
 
-    if ( read(__fd, &e, sizeof(struct js_event)) < (int)sizeof(struct js_event) ) {
+    if ( read(fd_, &e, sizeof(struct js_event)) < (int)sizeof(struct js_event) ) {
       logger->log_warn(name(), "Joystick removed, will try to reconnect.");
-      close(__fd);
-      __fd = -1;
-      __connected = false;
-      if ( __bbhandler ) {
-	__bbhandler->joystick_unplugged();
+      close(fd_);
+      fd_ = -1;
+      connected_ = false;
+      if ( bbhandler_ ) {
+	bbhandler_->joystick_unplugged();
       }
       return;
     }
 
-    __data_mutex->lock();
-    __new_data = ! __safety_lockout;
+    data_mutex_->lock();
+    new_data_ = ! safety_lockout_;
 
     if ((e.type & ~JS_EVENT_INIT) == JS_EVENT_BUTTON) {
       //logger->log_debug(name(), "Button %u button event: %f", e.number, e.value);
       if (e.number <= 32) {
 	if (e.value) {
-	  __pressed_buttons |=  (1 << e.number);
+	  pressed_buttons_ |=  (1 << e.number);
 	} else {
-	  __pressed_buttons &= ~(1 << e.number);
+	  pressed_buttons_ &= ~(1 << e.number);
 	}
       } else {
 	logger->log_warn(name(), "Button value for button > 32, ignoring");
       }
     } else if ((e.type & ~JS_EVENT_INIT) == JS_EVENT_AXIS) {
-      if ( e.number >= __axis_array_size ) {
+      if ( e.number >= axis_array_size_ ) {
 	logger->log_warn(name(),
 			 "Got value for axis %u, but only %u axes registered. "
 			 "Plugged in a different joystick? Ignoring.",
-			 e.number + 1 /* natural numbering */, __axis_array_size);
+			 e.number + 1 /* natural numbering */, axis_array_size_);
       } else {
 	// Joystick axes usually go positive right, down, twist right, min speed,
 	// hat right, and hat down. In the Fawkes coordinate system we actually
 	// want opposite directions, hence multiply each value by -1
-	__axis_values[e.number] = (e.value == 0) ? 0. : (e.value / -32767.f);
+	axis_values_[e.number] = (e.value == 0) ? 0. : (e.value / -32767.f);
 	
 	//logger->log_debug(name(), "Axis %u new X: %f",
-	//                  axis_index, __axis_values[e.number]);
+	//                  axis_index, axis_values_[e.number]);
       }
     }
 
-    __data_mutex->unlock();
+    data_mutex_->unlock();
 
-    if (__safety_lockout) {
+    if (safety_lockout_) {
 	    // the actual axis directions don't matter, we are just interested
 	    // that they take both extremes once.
-	    if (__num_axes < 2 || __num_buttons == 0) {
-		    __safety_combo[COMBO_IDX_UP]      = true;
-		    __safety_combo[COMBO_IDX_DOWN]    = true;
-		    __safety_combo[COMBO_IDX_RIGHT]   = true;
-		    __safety_combo[COMBO_IDX_LEFT]    = true;
-		    __safety_combo[COMBO_IDX_RELEASE] = true;
+	    if (num_axes_ < 2 || num_buttons_ == 0) {
+		    safety_combo_[COMBO_IDX_UP]      = true;
+		    safety_combo_[COMBO_IDX_DOWN]    = true;
+		    safety_combo_[COMBO_IDX_RIGHT]   = true;
+		    safety_combo_[COMBO_IDX_LEFT]    = true;
+		    safety_combo_[COMBO_IDX_RELEASE] = true;
 	    } else {
-		    if (__pressed_buttons > 0) {
-			    if (__axis_values[0] >  0.9)  __safety_combo[COMBO_IDX_UP]    = true;
-			    if (__axis_values[0] < -0.9)  __safety_combo[COMBO_IDX_DOWN]  = true;
-			    if (__axis_values[1] >  0.9)  __safety_combo[COMBO_IDX_RIGHT] = true;
-			    if (__axis_values[1] < -0.9)  __safety_combo[COMBO_IDX_LEFT]  = true;
+		    if (pressed_buttons_ > 0) {
+			    if (axis_values_[0] >  0.9)  safety_combo_[COMBO_IDX_UP]    = true;
+			    if (axis_values_[0] < -0.9)  safety_combo_[COMBO_IDX_DOWN]  = true;
+			    if (axis_values_[1] >  0.9)  safety_combo_[COMBO_IDX_RIGHT] = true;
+			    if (axis_values_[1] < -0.9)  safety_combo_[COMBO_IDX_LEFT]  = true;
 		    }
-		    if (__safety_combo[COMBO_IDX_UP] && __safety_combo[COMBO_IDX_DOWN] &&
-		        __safety_combo[COMBO_IDX_LEFT] && __safety_combo[COMBO_IDX_RIGHT] &&
-		        __pressed_buttons == 0) {
-			    __safety_combo[COMBO_IDX_RELEASE] = true;
+		    if (safety_combo_[COMBO_IDX_UP] && safety_combo_[COMBO_IDX_DOWN] &&
+		        safety_combo_[COMBO_IDX_LEFT] && safety_combo_[COMBO_IDX_RIGHT] &&
+		        pressed_buttons_ == 0) {
+			    safety_combo_[COMBO_IDX_RELEASE] = true;
 		    }
 	    }
 
-	    if (__safety_combo[COMBO_IDX_UP] && __safety_combo[COMBO_IDX_DOWN] &&
-	        __safety_combo[COMBO_IDX_LEFT] && __safety_combo[COMBO_IDX_RIGHT] &&
-	        __safety_combo[COMBO_IDX_RELEASE])
+	    if (safety_combo_[COMBO_IDX_UP] && safety_combo_[COMBO_IDX_DOWN] &&
+	        safety_combo_[COMBO_IDX_LEFT] && safety_combo_[COMBO_IDX_RIGHT] &&
+	        safety_combo_[COMBO_IDX_RELEASE])
 	    {
 		    logger->log_warn(name(), "Joystick safety lockout DISABLED (combo received)");
-		    __safety_lockout = false;
+		    safety_lockout_ = false;
 	    }
     } else {
-	    if ( __bbhandler ) {
-		    __bbhandler->joystick_changed(__pressed_buttons, __axis_values);
+	    if ( bbhandler_ ) {
+		    bbhandler_->joystick_changed(pressed_buttons_, axis_values_);
 	    }
     }
   } else {
@@ -315,11 +315,11 @@ JoystickAcquisitionThread::loop()
 bool
 JoystickAcquisitionThread::lock_if_new_data()
 {
-  __data_mutex->lock();
-  if (__new_data) {
+  data_mutex_->lock();
+  if (new_data_) {
     return true;
   } else {
-    __data_mutex->unlock();
+    data_mutex_->unlock();
     return false;
   }
 }
@@ -329,8 +329,8 @@ JoystickAcquisitionThread::lock_if_new_data()
 void
 JoystickAcquisitionThread::unlock()
 {
-  __new_data = false;
-  __data_mutex->unlock();
+  new_data_ = false;
+  data_mutex_->unlock();
 }
 
 
@@ -340,7 +340,7 @@ JoystickAcquisitionThread::unlock()
 char
 JoystickAcquisitionThread::num_axes() const
 {
-  return __num_axes;
+  return num_axes_;
 }
 
 
@@ -350,7 +350,7 @@ JoystickAcquisitionThread::num_axes() const
 char
 JoystickAcquisitionThread::num_buttons() const
 {
-  return __num_buttons;
+  return num_buttons_;
 }
 
 
@@ -360,7 +360,7 @@ JoystickAcquisitionThread::num_buttons() const
 const char *
 JoystickAcquisitionThread::joystick_name() const
 {
-  return __joystick_name;
+  return joystick_name_;
 }
 
 
@@ -370,7 +370,7 @@ JoystickAcquisitionThread::joystick_name() const
 unsigned int
 JoystickAcquisitionThread::pressed_buttons() const
 {
-  return __pressed_buttons;
+  return pressed_buttons_;
 }
 
 
@@ -380,5 +380,5 @@ JoystickAcquisitionThread::pressed_buttons() const
 float *
 JoystickAcquisitionThread::axis_values()
 {
-  return __axis_values;
+  return axis_values_;
 }
diff --git a/src/plugins/joystick/acquisition_thread.h b/src/plugins/joystick/acquisition_thread.h
index 228188a..00a0ca4 100644
--- a/src/plugins/joystick/acquisition_thread.h
+++ b/src/plugins/joystick/acquisition_thread.h
@@ -66,7 +66,7 @@ class JoystickAcquisitionThread
 
   /** Access force feedback of joystick.
    * @return instance of JoystickForceFeedback class for current joystick. */
-  JoystickForceFeedback *  ff() const { return __ff; }
+  JoystickForceFeedback *  ff() const { return ff_; }
 
  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
  protected: virtual void run() { Thread::run(); }
@@ -77,26 +77,26 @@ class JoystickAcquisitionThread
   void open_forcefeedback();
 
  private:
-  std::string __cfg_device_file;
+  std::string cfg_device_file_;
 
-  bool        __safety_combo[5];
-  bool        __safety_lockout;
+  bool        safety_combo_[5];
+  bool        safety_lockout_;
 
-  int  __fd;
-  bool __connected;
-  unsigned int __axis_array_size;
-  char __num_axes;
-  char __num_buttons;
-  char __joystick_name[128];
+  int  fd_;
+  bool connected_;
+  unsigned int axis_array_size_;
+  char num_axes_;
+  char num_buttons_;
+  char joystick_name_[128];
 
-  bool            __new_data;
-  fawkes::Mutex  *__data_mutex;
+  bool            new_data_;
+  fawkes::Mutex  *data_mutex_;
 
-  unsigned int    __pressed_buttons;
-  float          *__axis_values;
+  unsigned int    pressed_buttons_;
+  float          *axis_values_;
 
-  JoystickBlackBoardHandler *__bbhandler;
-  JoystickForceFeedback *__ff;
+  JoystickBlackBoardHandler *bbhandler_;
+  JoystickForceFeedback *ff_;
 };
 
 

- *commit* a54ce07c37f27024b1f8b8b2386e62dac1171a78 - - - - - - - - - -
Author:  Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
Date:    Wed Apr 13 10:25:20 2016 +0200
Subject: joystick-teleop: apply threshold to individual axes

 src/plugins/joystick/joystick_teleop_thread.cpp |   24 +++++++++++++++++-----
 1 files changed, 18 insertions(+), 6 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/joystick/joystick_teleop_thread.cpp b/src/plugins/joystick/joystick_teleop_thread.cpp
index 9226561..8ceb1ad 100644
--- a/src/plugins/joystick/joystick_teleop_thread.cpp
+++ b/src/plugins/joystick/joystick_teleop_thread.cpp
@@ -218,13 +218,25 @@ JoystickTeleOpThread::loop()
           (cfg_drive_mode_ax_thresh_ <  0 &&
           joystick_if_->axis(cfg_drive_mode_axis_) < cfg_drive_mode_ax_thresh_))))
       {
-        vx    = joystick_if_->axis(cfg_axis_forward_) * cfg_special_max_vx_;
-        vy    = joystick_if_->axis(cfg_axis_sideward_) * cfg_special_max_vy_;
-        omega = joystick_if_->axis(cfg_axis_rotation_) * cfg_special_max_omega_;
+	      if (fabsf(joystick_if_->axis(cfg_axis_forward_)) > cfg_axis_threshold_) {
+		      vx    = joystick_if_->axis(cfg_axis_forward_) * cfg_special_max_vx_;
+	      }
+	      if (fabsf(joystick_if_->axis(cfg_axis_sideward_)) > cfg_axis_threshold_) {
+		      vy    = joystick_if_->axis(cfg_axis_sideward_) * cfg_special_max_vy_;
+	      }
+	      if (fabsf(joystick_if_->axis(cfg_axis_rotation_)) > cfg_axis_threshold_) {
+		      omega = joystick_if_->axis(cfg_axis_rotation_) * cfg_special_max_omega_;
+	      }
       } else {
-        vx    = joystick_if_->axis(cfg_axis_forward_) * cfg_normal_max_vx_;
-        vy    = joystick_if_->axis(cfg_axis_sideward_) * cfg_normal_max_vy_;
-        omega = joystick_if_->axis(cfg_axis_rotation_) * cfg_normal_max_omega_;
+	      if (fabsf(joystick_if_->axis(cfg_axis_forward_)) > cfg_axis_threshold_) {
+		      vx    = joystick_if_->axis(cfg_axis_forward_) * cfg_normal_max_vx_;
+	      }
+	      if (fabsf(joystick_if_->axis(cfg_axis_sideward_)) > cfg_axis_threshold_) {
+		      vy    = joystick_if_->axis(cfg_axis_sideward_) * cfg_normal_max_vy_;
+	      }
+	      if (fabsf(joystick_if_->axis(cfg_axis_rotation_)) > cfg_axis_threshold_) {
+		      omega = joystick_if_->axis(cfg_axis_rotation_) * cfg_normal_max_omega_;
+	      }
       }
 
       float theta, distance;

- *commit* cdbc1a08699bebaf2d841cb9d2107fd755cc1846 - - - - - - - - - -
Author:  Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
Date:    Wed Apr 13 10:29:23 2016 +0200
Subject: joystick: add re-enabling of safety lockout, change default config

 cfg/conf.d/joystick.yaml                    |   41 +++++++----
 src/plugins/joystick/acquisition_thread.cpp |   99 +++++++++++++++++---------
 src/plugins/joystick/acquisition_thread.h   |   14 ++--
 3 files changed, 100 insertions(+), 54 deletions(-)

_Diff for modified files_:
diff --git a/cfg/conf.d/joystick.yaml b/cfg/conf.d/joystick.yaml
index 57f690b..91cccd6 100644
--- a/cfg/conf.d/joystick.yaml
+++ b/cfg/conf.d/joystick.yaml
@@ -7,11 +7,19 @@ doc-url: !url http://trac.fawkesrobotics.org/wiki/Plugins/joystick
   # Joystick device file
   device_file: /dev/input/js0
 
-  # True to enable safety lockout feature.
-  # This feature requires that axes 0 and 1 (usually primary cross) must be
-  # moved all the way once while any button is pressed. After the button is
-  # then released the data is passed through to the blackboard
-  safety_lockout: true
+  safety_lockout:
+    # True to enable safety lockout feature.
+    # This feature requires that axes 0 and 1 (usually primary cross) must be
+    # moved all the way once while any button is pressed. After the button is
+    # then released the data is passed through to the blackboard
+    enable: true
+
+    # 128: accept only start button on XBox controller
+    button-mask: 128
+
+    # Time in seconds after which to re-enable the safety lockout if no
+    # data has been received.
+    timeout: 30.0
 
   # Configuration settings for the joystick-teleop plugin
   # The example configuration is suitable for an Xbox 360 controller
@@ -40,7 +48,7 @@ doc-url: !url http://trac.fawkesrobotics.org/wiki/Plugins/joystick
     # than the configured value. To disable using an axis as deadman
     # switch comment out the following line.
     deadman_axis: 5
-    deadman_axis_threshold: -0.1
+    deadman_axis_threshold: -0.5
 
     # Deadman button
     # The button mask is AND'ed to the JoystickInterface::pressed_buttons
@@ -48,7 +56,12 @@ doc-url: !url http://trac.fawkesrobotics.org/wiki/Plugins/joystick
     # movement commands. To get the appropriate value sum up values of
     # the appropriate BUTTON_* constants of the JoystickInterface.
     # To react to all buttons, set value to 4294967295.
-    deadman_button_mask: 4294967295
+    # The value "4294965759" accepts all buttons except the axis push-down
+    # buttons on the XBox-Controller. They have rather surprising effects
+    # making the robot drive unexpectedly.
+    # The value "4294965631" accepts all buttons except the axis push-down
+    # buttons and the start button
+    deadman_button_mask: 4294965631
 
     # Drive mode switching
     # Certain button or an axis can be configured that will allow to
@@ -63,21 +76,21 @@ doc-url: !url http://trac.fawkesrobotics.org/wiki/Plugins/joystick
     # The conditions work the same way as the deadman switch documented
     # above.
     drive_mode_button_mask: 0
-    drive_mode_axis: 6
-    drive_mode_axis_threshold: -0.2
+    drive_mode_axis: 2
+    drive_mode_axis_threshold: -0.5
 
     # Maximum velocity forward and sideward; m/s
     # Maximum rotation; rad/s
     drive_modes:
       normal:
-        max_vx: 0.6
-        max_vy: 0.6
+        max_vx: 0.5
+        max_vy: 0.5
         max_omega: 1.57
 
       special:
-        max_vx: 0.3
-        max_vy: 0.3
-        max_omega: 1.0
+        max_vx: 1.0
+        max_vy: 1.0
+        max_omega: 2.36
 
   # This feature stops the robot from moving forward if an obstacle is
   # detected up front.
diff --git a/src/plugins/joystick/acquisition_thread.cpp b/src/plugins/joystick/acquisition_thread.cpp
index c3b45f7..9bad2b7 100644
--- a/src/plugins/joystick/acquisition_thread.cpp
+++ b/src/plugins/joystick/acquisition_thread.cpp
@@ -26,6 +26,8 @@
 #include <core/threading/mutex.h>
 #include <core/exceptions/system.h>
 
+#include <utils/time/time.h>
+
 #include <algorithm>
 #include <linux/joystick.h>
 #include <cstdlib>
@@ -98,8 +100,12 @@ JoystickAcquisitionThread::init()
 
   safety_lockout_ = true;
   try {
-	  safety_lockout_ = config->get_bool("/hardware/joystick/safety_lockout");
+	  safety_lockout_ = config->get_bool("/hardware/joystick/safety_lockout/enable");
   } catch (Exception &e) {} // ignore, use default
+  if (safety_lockout_) {
+	  cfg_safety_lockout_timeout_ = config->get_float("/hardware/joystick/safety_lockout/timeout");
+	  cfg_safety_button_mask_ = config->get_uint("/hardware/joystick/safety_lockout/button-mask");
+  }
   for (int i = 0; i < 5; ++i) safety_combo_[i] = false;
 
   init(cfg_device_file_);
@@ -133,8 +139,8 @@ JoystickAcquisitionThread::open_joystick()
   if (axis_values_ == NULL) {
     // memory had not been allocated
     // minimum of 8 because there are 8 axes in the interface
-    axis_array_size_ = std::max((int)num_axes_, 8);
-    axis_values_   = (float *)malloc(sizeof(float) * axis_array_size_);
+    axis_array_size_  = std::max((int)num_axes_, 8);
+    axis_values_      = (float *)malloc(sizeof(float) * axis_array_size_);
   } else if ( num_axes_ > std::max((int)axis_array_size_, 8) ) {
     // We loose axes as we cannot increase BB interface on-the-fly
     num_axes_ = axis_array_size_;
@@ -148,7 +154,7 @@ JoystickAcquisitionThread::open_joystick()
 
   memset(axis_values_, 0, sizeof(float) * axis_array_size_);
   pressed_buttons_ = 0;
-
+  
   if ( bbhandler_ ) {
     bbhandler_->joystick_plugged(num_axes_, num_buttons_);
   }
@@ -199,7 +205,7 @@ void
 JoystickAcquisitionThread::finalize()
 {
   if ( fd_ >= 0 )  close(fd_);
-  free(axis_values_);
+  if (axis_values_)  free(axis_values_);
   delete data_mutex_;
 }
 
@@ -210,46 +216,71 @@ JoystickAcquisitionThread::loop()
   if ( connected_ ) {
     struct js_event e;
 
-    if ( read(fd_, &e, sizeof(struct js_event)) < (int)sizeof(struct js_event) ) {
-      logger->log_warn(name(), "Joystick removed, will try to reconnect.");
-      close(fd_);
-      fd_ = -1;
-      connected_ = false;
-      if ( bbhandler_ ) {
-	bbhandler_->joystick_unplugged();
-      }
-      return;
+    long int timeout_sec  = (long int)truncf(cfg_safety_lockout_timeout_);
+    long int timeout_usec = (cfg_safety_lockout_timeout_ - timeout_sec) * 10000000;
+    timeval timeout = {timeout_sec, timeout_usec};
+
+    fd_set read_fds;
+    FD_ZERO(&read_fds);
+    FD_SET(fd_, &read_fds);
+
+    int rv = 0;
+    rv = select(fd_ + 1, &read_fds, NULL, NULL, &timeout);
+
+    if (rv == 0) {
+	    if (! safety_lockout_) {
+		    logger->log_warn(name(), "No action for %.2f seconds, re-enabling safety lockout",
+		                     cfg_safety_lockout_timeout_);
+		    safety_lockout_ = true;
+		    for (int i = 0; i < 5; ++i) safety_combo_[i] = false;
+	    }
+	    new_data_ = false;
+	    return;
+    }
+    
+    if (rv == -1 || read(fd_, &e, sizeof(struct js_event)) < (int)sizeof(struct js_event)) {
+	    logger->log_warn(name(), "Joystick removed, will try to reconnect.");
+	    close(fd_);
+	    fd_ = -1;
+	    connected_ = false;
+	    safety_lockout_ = true;
+	    new_data_ = false;
+	    if ( bbhandler_ ) {
+		    bbhandler_->joystick_unplugged();
+	    }
+	    return;
     }
 
     data_mutex_->lock();
+
     new_data_ = ! safety_lockout_;
 
     if ((e.type & ~JS_EVENT_INIT) == JS_EVENT_BUTTON) {
       //logger->log_debug(name(), "Button %u button event: %f", e.number, e.value);
       if (e.number <= 32) {
-	if (e.value) {
-	  pressed_buttons_ |=  (1 << e.number);
-	} else {
-	  pressed_buttons_ &= ~(1 << e.number);
-	}
+	      if (e.value) {
+		      pressed_buttons_ |=  (1 << e.number);
+	      } else {
+		      pressed_buttons_ &= ~(1 << e.number);
+	      }
       } else {
-	logger->log_warn(name(), "Button value for button > 32, ignoring");
+	      logger->log_warn(name(), "Button value for button > 32, ignoring");
       }
     } else if ((e.type & ~JS_EVENT_INIT) == JS_EVENT_AXIS) {
-      if ( e.number >= axis_array_size_ ) {
-	logger->log_warn(name(),
-			 "Got value for axis %u, but only %u axes registered. "
-			 "Plugged in a different joystick? Ignoring.",
-			 e.number + 1 /* natural numbering */, axis_array_size_);
-      } else {
-	// Joystick axes usually go positive right, down, twist right, min speed,
-	// hat right, and hat down. In the Fawkes coordinate system we actually
-	// want opposite directions, hence multiply each value by -1
-	axis_values_[e.number] = (e.value == 0) ? 0. : (e.value / -32767.f);
+	    if ( e.number >= axis_array_size_ ) {
+		    logger->log_warn(name(),
+		                     "Got value for axis %u, but only %u axes registered. "
+		                     "Plugged in a different joystick? Ignoring.",
+		                     e.number + 1 /* natural numbering */, axis_array_size_);
+	    } else {
+		    // Joystick axes usually go positive right, down, twist right, min speed,
+		    // hat right, and hat down. In the Fawkes coordinate system we actually
+		    // want opposite directions, hence multiply each value by -1
+		    axis_values_[e.number] = (e.value == 0) ? 0. : (e.value / -32767.f);
 	
-	//logger->log_debug(name(), "Axis %u new X: %f",
-	//                  axis_index, axis_values_[e.number]);
-      }
+		    //logger->log_debug(name(), "Axis %u new X: %f",
+		    //                  axis_index, axis_values_[e.number]);
+	    }
     }
 
     data_mutex_->unlock();
@@ -264,7 +295,7 @@ JoystickAcquisitionThread::loop()
 		    safety_combo_[COMBO_IDX_LEFT]    = true;
 		    safety_combo_[COMBO_IDX_RELEASE] = true;
 	    } else {
-		    if (pressed_buttons_ > 0) {
+		    if (pressed_buttons_ & cfg_safety_button_mask_) {
 			    if (axis_values_[0] >  0.9)  safety_combo_[COMBO_IDX_UP]    = true;
 			    if (axis_values_[0] < -0.9)  safety_combo_[COMBO_IDX_DOWN]  = true;
 			    if (axis_values_[1] >  0.9)  safety_combo_[COMBO_IDX_RIGHT] = true;
diff --git a/src/plugins/joystick/acquisition_thread.h b/src/plugins/joystick/acquisition_thread.h
index 00a0ca4..01fff8b 100644
--- a/src/plugins/joystick/acquisition_thread.h
+++ b/src/plugins/joystick/acquisition_thread.h
@@ -46,10 +46,10 @@ class JoystickAcquisitionThread
   public fawkes::ConfigurableAspect
 {
  public:
-  JoystickAcquisitionThread();
-  JoystickAcquisitionThread(const char *device_file,
-			    JoystickBlackBoardHandler *handler,
-			    fawkes::Logger *logger);
+	JoystickAcquisitionThread();
+	JoystickAcquisitionThread(const char *device_file,
+                            JoystickBlackBoardHandler *handler,
+                            fawkes::Logger *logger);
 
   virtual void init();
   virtual void finalize();
@@ -75,9 +75,11 @@ class JoystickAcquisitionThread
   void init(std::string device_file);
   void open_joystick();
   void open_forcefeedback();
-
+  
  private:
-  std::string cfg_device_file_;
+  std::string  cfg_device_file_;
+  float        cfg_safety_lockout_timeout_;
+  unsigned int cfg_safety_button_mask_;
 
   bool        safety_combo_[5];
   bool        safety_lockout_;

- *commit* d290644ddb63e89611eb1c0a18d137d10462ae32 - - - - - - - - - -
Author:  Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
Date:    Wed Apr 13 10:30:21 2016 +0200
Subject: robotino: better default configuration

 cfg/conf.d/robotino.yaml |   31 +++++++++++++------------------
 1 files changed, 13 insertions(+), 18 deletions(-)

_Diff for modified files_:
diff --git a/cfg/conf.d/robotino.yaml b/cfg/conf.d/robotino.yaml
index 43c182b..7eb041e 100644
--- a/cfg/conf.d/robotino.yaml
+++ b/cfg/conf.d/robotino.yaml
@@ -21,9 +21,8 @@ hardware/robotino:
   # without waiting between receive calls
   cycle-time: 5
 
-  act:
-    # Time threshold after all motor velocities are set to zero if no new messages arrived inbetween
-    deadman_time_threshold: 0.5
+  # Time threshold after all motor velocities are set to zero if no new messages arrived inbetween
+  deadman_time_threshold: 0.5
 
   openrobotino:
     # Configuration settings specific to the OpenRobotino driver
@@ -62,22 +61,22 @@ hardware/robotino:
   
   drive:
     layout:
-    # Distance from Robotino center to wheel center in meters
-    rb: 0.175
+      # Distance from Robotino center to wheel center in meters
+      rb: 0.175
 
-    # Wheel radius in meters
-    rw: 0.060
+      # Wheel radius in meters
+      rw: 0.060
 
-    # Gear ratio between motors and wheels
-    gear: 32.0
+      # Gear ratio between motors and wheels
+      gear: 32.0
 
     # Maximum acceleration and deceleration for translation, m/s^2
-    trans-acceleration: 0.4
-    trans-deceleration: 0.4
+    trans-acceleration: 0.8
+    trans-deceleration: 1.5
 
-    # Maximum acceleration and deceleration for rotation, deg/s^2
-    rot-acceleration: 0.4
-    rot-deceleration: 0.4
+    # Maximum acceleration and deceleration for rotation, rad/s^2
+    rot-acceleration: 1.6
+    rot-deceleration: 3.14
 
   bumper:
     # Set to true to enable the emergency stop on bumper contact (provided
@@ -100,7 +99,6 @@ hardware/robotino:
     interface_id: IMU Robotino
 
   odometry:
-
     # Odometry mode, one of "calc" or "copy"
     # calc: calculate on our own based on IMUInterface and velocity data
     # copy: copy values as-is from OpenRobotino
@@ -110,9 +108,6 @@ hardware/robotino:
     mode: calc
     #mode: copy
 
-    # Odometry coordinate frame
-    frame: !frame odom
-
     # Odom TF offset. This time is added to the current time when the
     # odometry <- base_link TF is published
     time_offset: 0.0

- *commit* aa26d1ea265fb1fea6f2c722289c59a21ce5506d - - - - - - - - - -
Author:  Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
Date:    Wed Apr 13 10:47:45 2016 +0200
Subject: robotino: add informative output when connection is lost

 src/plugins/robotino/direct_com_thread.cpp |    1 +
 1 files changed, 1 insertions(+), 0 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/robotino/direct_com_thread.cpp b/src/plugins/robotino/direct_com_thread.cpp
index 0b04679..cc83823 100644
--- a/src/plugins/robotino/direct_com_thread.cpp
+++ b/src/plugins/robotino/direct_com_thread.cpp
@@ -167,6 +167,7 @@ DirectRobotinoComThread::loop()
 			open_tries_ = 0;
 			request_data();
 		} catch (Exception &e) {
+			logger->log_warn(name(), "Re-open failed: %s", e.what_no_backtrace());
 			open_tries_ += 1;
 			if (open_tries_ >= (1000 / cfg_sensor_update_cycle_time_)) {
 				logger->log_error(name(), "Connection problem to base persists");




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


More information about the fawkes-commits mailing list