[Fawkes Git] branch/timn/robotino-direct: 3 revs updated. (0.5.0-3111-g7d9de92)

Tim Niemueller niemueller at kbsg.rwth-aachen.de
Fri Apr 15 16:21:59 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
        to  7d9de92414f9268b847db72a06a8493ef47ab148 (commit)
       via  e413a9902aa836811103134e6a6f1c159465f33b (commit)
       via  e4e6cce12c35a39477095701036e1e1982d92a17 (commit)
      from  ce7348c9d08c158f39bf42d988e9c4024739e1fb (commit)

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 e4e6cce12c35a39477095701036e1e1982d92a17
Author:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
AuthorDate: Fri Apr 15 15:40:53 2016 +0200
Commit:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
CommitDate: Fri Apr 15 15:43:49 2016 +0200

    joystick: add bypass button mask
    
    Buttons matched by the bypass mask are written to the interface even
    during the safety lockout.

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

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit e413a9902aa836811103134e6a6f1c159465f33b
Author:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
AuthorDate: Fri Apr 15 16:15:41 2016 +0200
Commit:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
CommitDate: Fri Apr 15 16:15:41 2016 +0200

    joystick-teleop: add runstop support
    
    Allow configuring buttons for enabling and disabling the runstop, i.e.,
    setting the motor state to disabled. The buttons are accepted even
    during safety lockout.

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

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 7d9de92414f9268b847db72a06a8493ef47ab148
Author:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
AuthorDate: Fri Apr 15 16:17:14 2016 +0200
Commit:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
CommitDate: Fri Apr 15 16:17:14 2016 +0200

    robotino: add support for setting digital outputs
    
    Add appropriate fields and messages to the interface and the code to
    handle the setting in the direct driver.

http://git.fawkesrobotics.org/fawkes.git/commit/7d9de92
http://trac.fawkesrobotics.org/changeset/7d9de92

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


- *Summary* -----------------------------------------------------------
 cfg/conf.d/joystick.yaml                           |   16 ++
 src/plugins/joystick/acquisition_thread.cpp        |   25 +++-
 src/plugins/joystick/acquisition_thread.h          |    1 +
 src/plugins/joystick/joystick_teleop_thread.cpp    |   32 +++
 src/plugins/joystick/joystick_teleop_thread.h      |    6 +-
 src/plugins/robotino/com_thread.cpp                |    6 +
 src/plugins/robotino/com_thread.h                  |    3 +
 src/plugins/robotino/direct_com_thread.cpp         |   38 ++++
 src/plugins/robotino/direct_com_thread.h           |    4 +
 .../interfaces/RobotinoSensorInterface.cpp         |  204 +++++++++++++++++++-
 .../interfaces/RobotinoSensorInterface.h_ext       |   43 ++++-
 .../interfaces/RobotinoSensorInterface.tolua       |   17 ++
 .../interfaces/RobotinoSensorInterface.xml         |   13 ++-
 src/plugins/robotino/openrobotino_com_thread.cpp   |    6 +
 src/plugins/robotino/openrobotino_com_thread.h     |    1 +
 src/plugins/robotino/sensor_thread.cpp             |    9 +
 16 files changed, 417 insertions(+), 7 deletions(-)


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

- *commit* e4e6cce12c35a39477095701036e1e1982d92a17 - - - - - - - - - -
Author:  Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
Date:    Fri Apr 15 15:40:53 2016 +0200
Subject: joystick: add bypass button mask

 cfg/conf.d/joystick.yaml                    |    4 ++++
 src/plugins/joystick/acquisition_thread.cpp |   25 +++++++++++++++++++++++--
 src/plugins/joystick/acquisition_thread.h   |    1 +
 3 files changed, 28 insertions(+), 2 deletions(-)

_Diff for modified files_:
diff --git a/cfg/conf.d/joystick.yaml b/cfg/conf.d/joystick.yaml
index 91cccd6..0ecdc0f 100644
--- a/cfg/conf.d/joystick.yaml
+++ b/cfg/conf.d/joystick.yaml
@@ -21,6 +21,10 @@ doc-url: !url http://trac.fawkesrobotics.org/wiki/Plugins/joystick
     # data has been received.
     timeout: 30.0
 
+    # Accept exactly buttons matching this mask even during safety lockout
+    # On xbox controller 44 are: RB, X, and Y
+    bypass-button-mask: 44
+
   # Configuration settings for the joystick-teleop plugin
   # The example configuration is suitable for an Xbox 360 controller
   teleop:
diff --git a/src/plugins/joystick/acquisition_thread.cpp b/src/plugins/joystick/acquisition_thread.cpp
index 5f69135..8c0876e 100644
--- a/src/plugins/joystick/acquisition_thread.cpp
+++ b/src/plugins/joystick/acquisition_thread.cpp
@@ -105,6 +105,10 @@ JoystickAcquisitionThread::init()
   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");
+	  cfg_safety_bypass_button_mask_ = 0;
+	  try {
+		  cfg_safety_bypass_button_mask_ = config->get_uint("/hardware/joystick/safety_lockout/bypass-button-mask");
+	  } catch (Exception &e) {} // ignore, use default
   }
   for (int i = 0; i < 5; ++i) safety_combo_[i] = false;
 
@@ -256,7 +260,8 @@ JoystickAcquisitionThread::loop()
     data_mutex_->lock();
 
     new_data_ = ! safety_lockout_;
-
+    unsigned int last_pressed_buttons = pressed_buttons_;
+    
     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) {
@@ -285,6 +290,16 @@ JoystickAcquisitionThread::loop()
 	    }
     }
 
+    // As a special case, allow a specific button combination to be
+    // written even during safety lockout. Can be used to implement
+    // an emergency stop, for example.
+    if (safety_lockout_ &&
+        ((cfg_safety_bypass_button_mask_ & pressed_buttons_) ||
+         ((cfg_safety_bypass_button_mask_ & last_pressed_buttons) && pressed_buttons_ == 0)))
+    {
+	    new_data_ = true;
+    }
+    
     data_mutex_->unlock();
 
     if (safety_lockout_) {
@@ -404,7 +419,13 @@ JoystickAcquisitionThread::joystick_name() const
 unsigned int
 JoystickAcquisitionThread::pressed_buttons() const
 {
-	return safety_lockout_ ? 0 : pressed_buttons_;
+	if (! safety_lockout_) {
+		return pressed_buttons_;
+	} else if (pressed_buttons_ & cfg_safety_bypass_button_mask_) {
+		return pressed_buttons_ & cfg_safety_bypass_button_mask_;
+	} else {
+		return 0;
+	}
 }
 
 
diff --git a/src/plugins/joystick/acquisition_thread.h b/src/plugins/joystick/acquisition_thread.h
index 00671f5..537360e 100644
--- a/src/plugins/joystick/acquisition_thread.h
+++ b/src/plugins/joystick/acquisition_thread.h
@@ -80,6 +80,7 @@ class JoystickAcquisitionThread
   std::string  cfg_device_file_;
   float        cfg_safety_lockout_timeout_;
   unsigned int cfg_safety_button_mask_;
+  unsigned int cfg_safety_bypass_button_mask_;
 
   bool        safety_combo_[5];
   bool        safety_lockout_;

- *commit* e413a9902aa836811103134e6a6f1c159465f33b - - - - - - - - - -
Author:  Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
Date:    Fri Apr 15 16:15:41 2016 +0200
Subject: joystick-teleop: add runstop support

 cfg/conf.d/joystick.yaml                        |   12 ++++++++
 src/plugins/joystick/joystick_teleop_thread.cpp |   32 +++++++++++++++++++++++
 src/plugins/joystick/joystick_teleop_thread.h   |    6 +++-
 3 files changed, 49 insertions(+), 1 deletions(-)

_Diff for modified files_:
diff --git a/cfg/conf.d/joystick.yaml b/cfg/conf.d/joystick.yaml
index 0ecdc0f..b54a6ab 100644
--- a/cfg/conf.d/joystick.yaml
+++ b/cfg/conf.d/joystick.yaml
@@ -67,6 +67,18 @@ doc-url: !url http://trac.fawkesrobotics.org/wiki/Plugins/joystick
     # buttons and the start button
     deadman_button_mask: 4294965631
 
+    # Run-stop buttons
+    # Whenever this button combination is pressed, enable/disable the
+    # motor enabled state. For this to work always, even during safety
+    # lockout, set the very same button combination(s) as safety bypass
+    # buttons above.
+    # These button combos may be the same for both, in which case toggling
+    # happens only after the button has been released.
+    # Enable on xboxctrl: RB and Y
+    runstop-enable-buttons: 40
+    # Disable on xboxctrl: RB and X
+    runstop-disable-buttons: 36
+
     # Drive mode switching
     # Certain button or an axis can be configured that will allow to
     # use a different drive mode as long as the button is pushed.
diff --git a/src/plugins/joystick/joystick_teleop_thread.cpp b/src/plugins/joystick/joystick_teleop_thread.cpp
index f63b888..0e63a7d 100644
--- a/src/plugins/joystick/joystick_teleop_thread.cpp
+++ b/src/plugins/joystick/joystick_teleop_thread.cpp
@@ -90,6 +90,9 @@ JoystickTeleOpThread::init()
   cfg_collision_safety_distance_ = config->get_float(CFG_PREFIX"collision_safety/distance");
   cfg_collision_safety_angle_    = config->get_uint(CFG_PREFIX"collision_safety/angle");
 
+  cfg_runstop_enable_buttons_  = config->get_uint(CFG_PREFIX"runstop-enable-buttons");
+  cfg_runstop_disable_buttons_ = config->get_uint(CFG_PREFIX"runstop-disable-buttons");
+
   cfg_ifid_motor_        = config->get_string(CFG_PREFIX"motor_interface_id");
   motor_if_ = blackboard->open_for_reading<MotorInterface>(cfg_ifid_motor_.c_str());
 
@@ -131,6 +134,7 @@ JoystickTeleOpThread::init()
     logger->log_warn(name(), "Collision safety for joystick is disabled.");
   }
 
+  runstop_pressed_ = false;
   stopped_ = false;
 }
 
@@ -206,6 +210,7 @@ JoystickTeleOpThread::loop()
 {
   joystick_if_->read();
   if (laser_if_)  laser_if_->read();
+  motor_if_->read();
 
   if ((! joystick_if_->has_writer() || joystick_if_->num_axes() == 0) && ! stopped_) {
     logger->log_warn(name(), "Joystick disconnected, stopping");
@@ -218,11 +223,32 @@ JoystickTeleOpThread::loop()
   {
     logger->log_warn(name(), "Axis number out of range, stopping");
     stop();
+  } else if (joystick_if_->pressed_buttons() == cfg_runstop_enable_buttons_ &&
+             ! runstop_pressed_ &&
+             motor_if_->motor_state() != MotorInterface::MOTOR_DISABLED)
+  {
+	  stop();
+	  MotorInterface::SetMotorStateMessage *msg =
+		  new MotorInterface::SetMotorStateMessage(MotorInterface::MOTOR_DISABLED);
+	  motor_if_->msgq_enqueue(msg);
+	  logger->log_warn(name(), "Runstop ENABLED");
+	  runstop_pressed_ = true;
+  } else if (joystick_if_->pressed_buttons() == cfg_runstop_disable_buttons_ &&
+             ! runstop_pressed_ &&
+             motor_if_->motor_state() == MotorInterface::MOTOR_DISABLED)
+  {
+	  stop();
+	  MotorInterface::SetMotorStateMessage *msg =
+		  new MotorInterface::SetMotorStateMessage(MotorInterface::MOTOR_ENABLED);
+	  motor_if_->msgq_enqueue(msg);
+	  logger->log_warn(name(), "Runstop DISABLED");
+	  runstop_pressed_ = true;
   } else if ((joystick_if_->pressed_buttons() & cfg_deadman_butmask_) ||
 	     (cfg_deadman_use_axis_ &&
 	      ((cfg_deadman_ax_thresh_ >= 0 && joystick_if_->axis(cfg_deadman_axis_) > cfg_deadman_ax_thresh_) ||
 	       (cfg_deadman_ax_thresh_ <  0 && joystick_if_->axis(cfg_deadman_axis_) < cfg_deadman_ax_thresh_))))
   {
+	  runstop_pressed_ = false;
     if (fabsf(joystick_if_->axis(cfg_axis_forward_)) < cfg_axis_threshold_ &&
          fabsf(joystick_if_->axis(cfg_axis_sideward_)) < cfg_axis_threshold_ &&
          fabsf(joystick_if_->axis(cfg_axis_rotation_)) < cfg_axis_threshold_) {
@@ -289,6 +315,7 @@ JoystickTeleOpThread::loop()
 	        ff_strong_ = false;
         }
         send_transrot(vx, vy, omega);
+        runstop_pressed_ = false;
       }
       else if (cfg_use_laser_ && ! area_free)
       {
@@ -310,6 +337,11 @@ JoystickTeleOpThread::loop()
       }
     }
   } else if (! stopped_) {
+    runstop_pressed_ = false;
     stop();
+  } else if (joystick_if_->pressed_buttons() != cfg_runstop_enable_buttons_ &&
+             joystick_if_->pressed_buttons() != cfg_runstop_enable_buttons_)
+  {
+	  runstop_pressed_ = false;
   }
 }
diff --git a/src/plugins/joystick/joystick_teleop_thread.h b/src/plugins/joystick/joystick_teleop_thread.h
index a8a177d..b0a181b 100644
--- a/src/plugins/joystick/joystick_teleop_thread.h
+++ b/src/plugins/joystick/joystick_teleop_thread.h
@@ -91,12 +91,16 @@ class JoystickTeleOpThread
   bool         cfg_use_laser_;
   std::string  cfg_ifid_laser_;
   bool         cfg_use_ff_;
-
+  unsigned int cfg_runstop_enable_buttons_;
+  unsigned int cfg_runstop_disable_buttons_;
+  
   bool         stopped_;
   float        min_distance_;
 
   bool         ff_weak_;
   bool         ff_strong_;
+
+  bool         runstop_pressed_;
 };
 
 

- *commit* 7d9de92414f9268b847db72a06a8493ef47ab148 - - - - - - - - - -
Author:  Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
Date:    Fri Apr 15 16:17:14 2016 +0200
Subject: robotino: add support for setting digital outputs

 src/plugins/robotino/com_thread.cpp                |    6 +
 src/plugins/robotino/com_thread.h                  |    3 +
 src/plugins/robotino/direct_com_thread.cpp         |   38 ++++
 src/plugins/robotino/direct_com_thread.h           |    4 +
 .../interfaces/RobotinoSensorInterface.cpp         |  204 +++++++++++++++++++-
 .../interfaces/RobotinoSensorInterface.h_ext       |   43 ++++-
 .../interfaces/RobotinoSensorInterface.tolua       |   17 ++
 .../interfaces/RobotinoSensorInterface.xml         |   13 ++-
 src/plugins/robotino/openrobotino_com_thread.cpp   |    6 +
 src/plugins/robotino/openrobotino_com_thread.h     |    1 +
 src/plugins/robotino/sensor_thread.cpp             |    9 +
 11 files changed, 340 insertions(+), 4 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/robotino/com_thread.cpp b/src/plugins/robotino/com_thread.cpp
index 33afa0b..c25301d 100644
--- a/src/plugins/robotino/com_thread.cpp
+++ b/src/plugins/robotino/com_thread.cpp
@@ -76,6 +76,11 @@ using namespace fawkes;
  * Set acceleration limits of motors.
  * @param min_accel minimum acceleration
  * @param max_accel maximum acceleration
+ *
+ * @fn void RobotinoComThread::set_digital_output(unsigned int digital_out, bool enable) = 0
+ * Set digital output state.
+ * @param digital_out digital output as written on the robot, i.e., 1 to 8
+ * @param enable true to enable output, false to disable
  */
 
 /** @class RobotinoComThread::SensorData "com_thread.h"
@@ -86,6 +91,7 @@ using namespace fawkes;
 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},
+	  digital_out{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.},
diff --git a/src/plugins/robotino/com_thread.h b/src/plugins/robotino/com_thread.h
index 55b530f..42c29b4 100644
--- a/src/plugins/robotino/com_thread.h
+++ b/src/plugins/robotino/com_thread.h
@@ -54,6 +54,7 @@ class RobotinoComThread
 		bool         bumper;
 		bool         bumper_estop_enabled;
 		bool         digital_in[8];
+		bool         digital_out[8];
 		float        analog_in[8];
 
 		float        bat_voltage;
@@ -89,6 +90,7 @@ class RobotinoComThread
 	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 void set_digital_output(unsigned int digital_out, bool enable) = 0;
 	
 	virtual bool get_data(SensorData &sensor_data);
 
@@ -96,6 +98,7 @@ class RobotinoComThread
 	        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;
 
diff --git a/src/plugins/robotino/direct_com_thread.cpp b/src/plugins/robotino/direct_com_thread.cpp
index ebd6b8b..9fb3a63 100644
--- a/src/plugins/robotino/direct_com_thread.cpp
+++ b/src/plugins/robotino/direct_com_thread.cpp
@@ -97,8 +97,16 @@ DirectRobotinoComThread::init()
 	request_timer_.expires_from_now(boost::posix_time::milliseconds(-1));
 	drive_timer_.expires_at(boost::posix_time::pos_infin);
 
+	digital_outputs_ = 0;
+
 	open_device(/* wait for replies */ true);
 	open_tries_ = 0;
+
+	{ // Disable all digital outputs initially
+		DirectRobotinoComMessage req(DirectRobotinoComMessage::CMDID_SET_ALL_DIGITAL_OUTPUTS);
+		req.add_uint8(digital_outputs_);
+		send_message(req);
+	}
 }
 
 
@@ -291,6 +299,36 @@ DirectRobotinoComThread::set_motor_accel_limits(float min_accel, float max_accel
 	}
 }
 
+void
+DirectRobotinoComThread::set_digital_output(unsigned int digital_out, bool enable)
+{
+	if (digital_out < 1 || digital_out > 8) {
+		throw Exception("Invalid digital output, must be in range [1..8], got %u",
+		                digital_out);
+	}
+
+	unsigned int digital_out_idx = digital_out - 1;
+
+	if (enable) {
+		digital_outputs_ |= (1 << digital_out_idx);
+	} else {
+		digital_outputs_ &= ~(1 << digital_out_idx);
+	}
+	
+	try {
+		DirectRobotinoComMessage req(DirectRobotinoComMessage::CMDID_SET_ALL_DIGITAL_OUTPUTS);
+		req.add_uint8(digital_outputs_);
+		send_message(req);
+	} catch (Exception &e) {
+		logger->log_error(name(), "Setting digital outputs failed, exception follows");
+		logger->log_error(name(), e);
+	}
+
+	MutexLocker lock(data_mutex_);
+	for (int i = 0; i < 8; ++i)  data_.digital_out[i] = (digital_outputs_ & (1 << i)) ? true : false;
+	new_data_ = true;
+}
+
 
 bool
 DirectRobotinoComThread::is_connected()
diff --git a/src/plugins/robotino/direct_com_thread.h b/src/plugins/robotino/direct_com_thread.h
index d58d1d0..6ee3660 100644
--- a/src/plugins/robotino/direct_com_thread.h
+++ b/src/plugins/robotino/direct_com_thread.h
@@ -70,6 +70,7 @@ class DirectRobotinoComThread
 	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_digital_output(unsigned int digital_out, bool enable);
 
 	virtual void set_desired_vel(float vx, float vy, float omega);
 
@@ -106,10 +107,13 @@ class DirectRobotinoComThread
 	unsigned int    cfg_nodata_timeout_;
 	unsigned int    cfg_drive_update_interval_;
 	unsigned int    cfg_read_timeout_;
+
 	
 	bool opened_;
 	unsigned int open_tries_;
 
+	uint8_t         digital_outputs_;
+
 	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/interfaces/RobotinoSensorInterface.cpp b/src/plugins/robotino/interfaces/RobotinoSensorInterface.cpp
index 09e1058..93171d4 100644
--- a/src/plugins/robotino/interfaces/RobotinoSensorInterface.cpp
+++ b/src/plugins/robotino/interfaces/RobotinoSensorInterface.cpp
@@ -3,7 +3,7 @@
  *  RobotinoSensorInterface.cpp - Fawkes BlackBoard Interface - RobotinoSensorInterface
  *
  *  Templated created:   Thu Oct 12 10:49:19 2006
- *  Copyright  2012  Tim Niemueller
+ *  Copyright  2012-2016  Tim Niemueller
  *
  ****************************************************************************/
 
@@ -54,10 +54,12 @@ RobotinoSensorInterface::RobotinoSensorInterface() : Interface()
   add_fieldinfo(IFT_BOOL, "bumper", 1, &data->bumper);
   add_fieldinfo(IFT_FLOAT, "distance", 9, &data->distance);
   add_fieldinfo(IFT_BOOL, "digital_in", 8, &data->digital_in);
+  add_fieldinfo(IFT_BOOL, "digital_out", 8, &data->digital_out);
   add_fieldinfo(IFT_FLOAT, "analog_in", 8, &data->analog_in);
   add_fieldinfo(IFT_BOOL, "bumper_estop_enabled", 1, &data->bumper_estop_enabled);
   add_messageinfo("SetBumperEStopEnabledMessage");
-  unsigned char tmp_hash[] = {0x3f, 0x22, 0xb4, 0xc5, 0xe8, 0x9c, 0xd9, 0xa8, 0x90, 0x80, 0x23, 0xb2, 0x55, 0xed, 0xfd, 0x52};
+  add_messageinfo("SetDigitalOutputMessage");
+  unsigned char tmp_hash[] = {0xa5, 0xb, 0xa1, 0x94, 0xea, 0x39, 0x14, 0x7, 0x98, 0x77, 0x10, 0xc, 0x25, 0x72, 0x57, 0xa0};
   set_hash(tmp_hash);
 }
 
@@ -398,6 +400,66 @@ RobotinoSensorInterface::set_digital_in(unsigned int index, const bool new_digit
   data->digital_in[index] = new_digital_in;
   data_changed = true;
 }
+/** Get digital_out value.
+ * Digital output values.
+ * @return digital_out value
+ */
+bool *
+RobotinoSensorInterface::is_digital_out() const
+{
+  return data->digital_out;
+}
+
+/** Get digital_out value at given index.
+ * Digital output values.
+ * @param index index of value
+ * @return digital_out value
+ * @exception Exception thrown if index is out of bounds
+ */
+bool
+RobotinoSensorInterface::is_digital_out(unsigned int index) const
+{
+  if (index > 8) {
+    throw Exception("Index value %u out of bounds (0..8)", index);
+  }
+  return data->digital_out[index];
+}
+
+/** Get maximum length of digital_out value.
+ * @return length of digital_out value, can be length of the array or number of 
+ * maximum number of characters for a string
+ */
+size_t
+RobotinoSensorInterface::maxlenof_digital_out() const
+{
+  return 8;
+}
+
+/** Set digital_out value.
+ * Digital output values.
+ * @param new_digital_out new digital_out value
+ */
+void
+RobotinoSensorInterface::set_digital_out(const bool * new_digital_out)
+{
+  memcpy(data->digital_out, new_digital_out, sizeof(bool) * 8);
+  data_changed = true;
+}
+
+/** Set digital_out value at given index.
+ * Digital output values.
+ * @param new_digital_out new digital_out value
+ * @param index index for of the value
+ */
+void
+RobotinoSensorInterface::set_digital_out(unsigned int index, const bool new_digital_out)
+{
+  if (index > 8) {
+    throw Exception("Index value %u out of bounds (0..8)", index);
+  }
+  data->digital_out[index] = new_digital_out;
+  data_changed = true;
+}
 /** Get analog_in value.
  * Analog input values.
  * @return analog_in value
@@ -499,6 +561,8 @@ RobotinoSensorInterface::create_message(const char *type) const
 {
   if ( strncmp("SetBumperEStopEnabledMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
     return new SetBumperEStopEnabledMessage();
+  } else if ( strncmp("SetDigitalOutputMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
+    return new SetDigitalOutputMessage();
   } else {
     throw UnknownTypeException("The given type '%s' does not match any known "
                                "message type for this interface type.", type);
@@ -623,6 +687,138 @@ RobotinoSensorInterface::SetBumperEStopEnabledMessage::clone() const
 {
   return new RobotinoSensorInterface::SetBumperEStopEnabledMessage(this);
 }
+/** @class RobotinoSensorInterface::SetDigitalOutputMessage <interfaces/RobotinoSensorInterface.h>
+ * SetDigitalOutputMessage Fawkes BlackBoard Interface Message.
+ * 
+    
+ */
+
+
+/** Constructor with initial values.
+ * @param ini_digital_out initial value for digital_out
+ * @param ini_enabled initial value for enabled
+ */
+RobotinoSensorInterface::SetDigitalOutputMessage::SetDigitalOutputMessage(const uint8_t ini_digital_out, const bool ini_enabled) : Message("SetDigitalOutputMessage")
+{
+  data_size = sizeof(SetDigitalOutputMessage_data_t);
+  data_ptr  = malloc(data_size);
+  memset(data_ptr, 0, data_size);
+  data      = (SetDigitalOutputMessage_data_t *)data_ptr;
+  data_ts   = (message_data_ts_t *)data_ptr;
+  data->digital_out = ini_digital_out;
+  data->enabled = ini_enabled;
+  add_fieldinfo(IFT_UINT8, "digital_out", 1, &data->digital_out);
+  add_fieldinfo(IFT_BOOL, "enabled", 1, &data->enabled);
+}
+/** Constructor */
+RobotinoSensorInterface::SetDigitalOutputMessage::SetDigitalOutputMessage() : Message("SetDigitalOutputMessage")
+{
+  data_size = sizeof(SetDigitalOutputMessage_data_t);
+  data_ptr  = malloc(data_size);
+  memset(data_ptr, 0, data_size);
+  data      = (SetDigitalOutputMessage_data_t *)data_ptr;
+  data_ts   = (message_data_ts_t *)data_ptr;
+  add_fieldinfo(IFT_UINT8, "digital_out", 1, &data->digital_out);
+  add_fieldinfo(IFT_BOOL, "enabled", 1, &data->enabled);
+}
+
+/** Destructor */
+RobotinoSensorInterface::SetDigitalOutputMessage::~SetDigitalOutputMessage()
+{
+  free(data_ptr);
+}
+
+/** Copy constructor.
+ * @param m message to copy from
+ */
+RobotinoSensorInterface::SetDigitalOutputMessage::SetDigitalOutputMessage(const SetDigitalOutputMessage *m) : Message("SetDigitalOutputMessage")
+{
+  data_size = m->data_size;
+  data_ptr  = malloc(data_size);
+  memcpy(data_ptr, m->data_ptr, data_size);
+  data      = (SetDigitalOutputMessage_data_t *)data_ptr;
+  data_ts   = (message_data_ts_t *)data_ptr;
+}
+
+/* Methods */
+/** Get digital_out value.
+ * 
+	    The number of the digital output to set.
+    
+ * @return digital_out value
+ */
+uint8_t
+RobotinoSensorInterface::SetDigitalOutputMessage::digital_out() const
+{
+  return data->digital_out;
+}
+
+/** Get maximum length of digital_out value.
+ * @return length of digital_out value, can be length of the array or number of 
+ * maximum number of characters for a string
+ */
+size_t
+RobotinoSensorInterface::SetDigitalOutputMessage::maxlenof_digital_out() const
+{
+  return 1;
+}
+
+/** Set digital_out value.
+ * 
+	    The number of the digital output to set.
+    
+ * @param new_digital_out new digital_out value
+ */
+void
+RobotinoSensorInterface::SetDigitalOutputMessage::set_digital_out(const uint8_t new_digital_out)
+{
+  data->digital_out = new_digital_out;
+}
+
+/** Get enabled value.
+ * 
+      True to enable digital out, false to disable.
+    
+ * @return enabled value
+ */
+bool
+RobotinoSensorInterface::SetDigitalOutputMessage::is_enabled() const
+{
+  return data->enabled;
+}
+
+/** Get maximum length of enabled value.
+ * @return length of enabled value, can be length of the array or number of 
+ * maximum number of characters for a string
+ */
+size_t
+RobotinoSensorInterface::SetDigitalOutputMessage::maxlenof_enabled() const
+{
+  return 1;
+}
+
+/** Set enabled value.
+ * 
+      True to enable digital out, false to disable.
+    
+ * @param new_enabled new enabled value
+ */
+void
+RobotinoSensorInterface::SetDigitalOutputMessage::set_enabled(const bool new_enabled)
+{
+  data->enabled = new_enabled;
+}
+
+/** Clone this message.
+ * Produces a message of the same type as this message and copies the
+ * data to the new message.
+ * @return clone of this message
+ */
+Message *
+RobotinoSensorInterface::SetDigitalOutputMessage::clone() const
+{
+  return new RobotinoSensorInterface::SetDigitalOutputMessage(this);
+}
 /** Check if message is valid and can be enqueued.
  * @param message Message to check
  * @return true if the message is valid, false otherwise.
@@ -634,6 +830,10 @@ RobotinoSensorInterface::message_valid(const Message *message) const
   if ( m0 != NULL ) {
     return true;
   }
+  const SetDigitalOutputMessage *m1 = dynamic_cast<const SetDigitalOutputMessage *>(message);
+  if ( m1 != NULL ) {
+    return true;
+  }
   return false;
 }
 
diff --git a/src/plugins/robotino/interfaces/RobotinoSensorInterface.h_ext b/src/plugins/robotino/interfaces/RobotinoSensorInterface.h_ext
index 258b5c5..f7da921 100644
--- a/src/plugins/robotino/interfaces/RobotinoSensorInterface.h_ext
+++ b/src/plugins/robotino/interfaces/RobotinoSensorInterface.h_ext
@@ -3,7 +3,7 @@
  *  RobotinoSensorInterface.h - Fawkes BlackBoard Interface - RobotinoSensorInterface
  *
  *  Templated created:   Thu Oct 12 10:49:19 2006
- *  Copyright  2012  Tim Niemueller
+ *  Copyright  2012-2016  Tim Niemueller
  *
  ****************************************************************************/
 
@@ -50,6 +50,7 @@ class RobotinoSensorInterface : public Interface
     bool bumper; /**< Bumper pressed indicator. */
     float distance[9]; /**< Distance sensor values. */
     bool digital_in[8]; /**< Digital input values. */
+    bool digital_out[8]; /**< Digital output values. */
     float analog_in[8]; /**< Analog input values. */
     bool bumper_estop_enabled; /**< 
       True if emergency stop on bumper contact is enabled, false otherwise.
@@ -91,6 +92,41 @@ class RobotinoSensorInterface : public Interface
     virtual Message * clone() const;
   };
 
+  class SetDigitalOutputMessage : public Message
+  {
+   private:
+#pragma pack(push,4)
+    /** Internal data storage, do NOT modify! */
+    typedef struct {
+      int64_t timestamp_sec;  /**< Interface Unix timestamp, seconds */
+      int64_t timestamp_usec; /**< Interface Unix timestamp, micro-seconds */
+      uint8_t digital_out; /**< 
+	    The number of the digital output to set.
+     */
+      bool enabled; /**< 
+      True to enable digital out, false to disable.
+     */
+    } SetDigitalOutputMessage_data_t;
+#pragma pack(pop)
+
+    SetDigitalOutputMessage_data_t *data;
+
+   public:
+    SetDigitalOutputMessage(const uint8_t ini_digital_out, const bool ini_enabled);
+    SetDigitalOutputMessage();
+    ~SetDigitalOutputMessage();
+
+    SetDigitalOutputMessage(const SetDigitalOutputMessage *m);
+    /* Methods */
+    uint8_t digital_out() const;
+    void set_digital_out(const uint8_t new_digital_out);
+    size_t maxlenof_digital_out() const;
+    bool is_enabled() const;
+    void set_enabled(const bool new_enabled);
+    size_t maxlenof_enabled() const;
+    virtual Message * clone() const;
+  };
+
   virtual bool message_valid(const Message *message) const;
  private:
   RobotinoSensorInterface();
@@ -126,6 +162,11 @@ class RobotinoSensorInterface : public Interface
   void set_digital_in(unsigned int index, const bool new_digital_in);
   void set_digital_in(const bool * new_digital_in);
   size_t maxlenof_digital_in() const;
+  bool * is_digital_out() const;
+  bool is_digital_out(unsigned int index) const;
+  void set_digital_out(unsigned int index, const bool new_digital_out);
+  void set_digital_out(const bool * new_digital_out);
+  size_t maxlenof_digital_out() const;
   float * analog_in() const;
   float analog_in(unsigned int index) const;
   void set_analog_in(unsigned int index, const float new_analog_in);
diff --git a/src/plugins/robotino/interfaces/RobotinoSensorInterface.tolua b/src/plugins/robotino/interfaces/RobotinoSensorInterface.tolua
index 639527d..0b3190a 100644
--- a/src/plugins/robotino/interfaces/RobotinoSensorInterface.tolua
+++ b/src/plugins/robotino/interfaces/RobotinoSensorInterface.tolua
@@ -17,6 +17,20 @@ class RobotinoSensorInterface : public Interface
     int maxlenof_enabled() const;
   };
 
+  class SetDigitalOutputMessage : public Message
+  {
+    SetDigitalOutputMessage(unsigned char ini_digital_out, bool ini_enabled);
+    SetDigitalOutputMessage();
+    ~SetDigitalOutputMessage();
+
+    unsigned char digital_out();
+    void set_digital_out(const unsigned char new_digital_out);
+    int maxlenof_digital_out() const;
+    bool is_enabled();
+    void set_enabled(const bool new_enabled);
+    int maxlenof_enabled() const;
+  };
+
   float mot_velocity(int index);
   void set_mot_velocity(unsigned int index, const float new_mot_velocity);
   int maxlenof_mot_velocity() const;
@@ -35,6 +49,9 @@ class RobotinoSensorInterface : public Interface
   bool is_digital_in(int index);
   void set_digital_in(unsigned int index, const bool new_digital_in);
   int maxlenof_digital_in() const;
+  bool is_digital_out(int index);
+  void set_digital_out(unsigned int index, const bool new_digital_out);
+  int maxlenof_digital_out() const;
   float analog_in(int index);
   void set_analog_in(unsigned int index, const float new_analog_in);
   int maxlenof_analog_in() const;
diff --git a/src/plugins/robotino/interfaces/RobotinoSensorInterface.xml b/src/plugins/robotino/interfaces/RobotinoSensorInterface.xml
index 7e02b73..a7ba5a9 100644
--- a/src/plugins/robotino/interfaces/RobotinoSensorInterface.xml
+++ b/src/plugins/robotino/interfaces/RobotinoSensorInterface.xml
@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <!DOCTYPE interface SYSTEM "interface.dtd">
-<interface name="RobotinoSensorInterface" author="Tim Niemueller" year="2012">
+<interface name="RobotinoSensorInterface" author="Tim Niemueller" year="2012-2016">
   <data>
     <comment>Sensor information of a Robotino robot</comment>
     <field type="float" name="mot_velocity" length="3">Velocities of the wheels.</field>
@@ -9,6 +9,7 @@
     <field type="bool" name="bumper">Bumper pressed indicator.</field>
     <field type="float" name="distance" length="9">Distance sensor values.</field>
     <field type="bool" name="digital_in" length="8">Digital input values.</field>
+    <field type="bool" name="digital_out" length="8">Digital output values.</field>
     <field type="float" name="analog_in" length="8">Analog input values.</field>
     <field type="bool" name="bumper_estop_enabled">
       True if emergency stop on bumper contact is enabled, false otherwise.
@@ -23,4 +24,14 @@
     </field>
   </message>
 
+  <message name="SetDigitalOutput">
+    <comment>Set a digital output.</comment>
+    <field type="uint8" name="digital_out">
+	    The number of the digital output to set.
+    </field>
+    <field type="bool" name="enabled">
+      True to enable digital out, false to disable.
+    </field>
+  </message>
+
 </interface>
diff --git a/src/plugins/robotino/openrobotino_com_thread.cpp b/src/plugins/robotino/openrobotino_com_thread.cpp
index c12e542..7e73934 100644
--- a/src/plugins/robotino/openrobotino_com_thread.cpp
+++ b/src/plugins/robotino/openrobotino_com_thread.cpp
@@ -388,6 +388,12 @@ OpenRobotinoComThread::set_motor_accel_limits(float min_accel, float max_accel)
 	throw Exception("Setting motor accel limits for OpenRobotino driver not supported, configure controld3");
 }
 
+void
+OpenRobotinoComThread::set_digital_output(unsigned int digital_out, bool enable)
+{
+	logger->log_error(name(), "Setting digital outputs not supported with openrobotino driver");
+}
+
 
 /** 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 b9b7869..f2d8c02 100644
--- a/src/plugins/robotino/openrobotino_com_thread.h
+++ b/src/plugins/robotino/openrobotino_com_thread.h
@@ -95,6 +95,7 @@ class OpenRobotinoComThread
 	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_digital_output(unsigned int digital_out, bool enable);
 
 	/** Stub to see name in backtrace for easier debugging. @see Thread::run() */
  protected: virtual void run() { Thread::run(); }
diff --git a/src/plugins/robotino/sensor_thread.cpp b/src/plugins/robotino/sensor_thread.cpp
index f136bbe..eaa671d 100644
--- a/src/plugins/robotino/sensor_thread.cpp
+++ b/src/plugins/robotino/sensor_thread.cpp
@@ -95,6 +95,7 @@ RobotinoSensorThread::loop()
 		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_digital_out(data.digital_out);
 		sens_if_->set_analog_in(data.analog_in);
 		update_distances(data.ir_voltages);
 		sens_if_->write();
@@ -133,6 +134,14 @@ RobotinoSensorThread::process_sensor_msgs()
 		    sens_if_->msgq_first_safe(msg))
 		{
 			com_->set_bumper_estop_enabled(msg->is_enabled());
+		} else if (RobotinoSensorInterface::SetDigitalOutputMessage *msg =
+		    sens_if_->msgq_first_safe(msg))
+		{
+			try {
+				com_->set_digital_output(msg->digital_out(), msg->is_enabled());
+			} catch (Exception &e) {
+				logger->log_warn(name(), e);
+			}
 		}
 		sens_if_->msgq_pop();
 	} // while sensor msgq




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


More information about the fawkes-commits mailing list