[Fawkes Git] branch/timn/robotino-direct: 5 revs updated. (0.5.0-3108-gce7348c)

Tim Niemueller niemueller at kbsg.rwth-aachen.de
Wed Apr 13 23:27:38 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  ce7348c9d08c158f39bf42d988e9c4024739e1fb (commit)
       via  df4248deb5ec8edd349b1b642c03cbcd28e45c82 (commit)
       via  0aabc3cbcc30f5f6bbde1a92feb28fb3e8473d7c (commit)
       via  accf06d19be740868434eda2b1f74c5183d728be (commit)
       via  96798a48ac8376ceb2c4840325c0d8ed76515197 (commit)
      from  599b51a644df68d4347316df55bdbd4270f41e98 (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 96798a48ac8376ceb2c4840325c0d8ed76515197
Author:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
AuthorDate: Wed Apr 13 19:31:07 2016 +0200
Commit:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
CommitDate: Wed Apr 13 19:31:07 2016 +0200

    robotino: set zero velocity on disabling motor
    
    The desired velocity was not reset when disabling motor, meaning the
    robot would not stop and even worse, that the instructor could not tell
    it to stop anymore...

http://git.fawkesrobotics.org/fawkes.git/commit/96798a4
http://trac.fawkesrobotics.org/changeset/96798a4

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit accf06d19be740868434eda2b1f74c5183d728be
Author:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
AuthorDate: Wed Apr 13 23:21:59 2016 +0200
Commit:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
CommitDate: Wed Apr 13 23:21:59 2016 +0200

    joystick: give axis and button data once after connection
    
    This information is given bypassing the safety lockout once.

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

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 0aabc3cbcc30f5f6bbde1a92feb28fb3e8473d7c
Author:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
AuthorDate: Wed Apr 13 23:23:56 2016 +0200
Commit:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
CommitDate: Wed Apr 13 23:23:56 2016 +0200

    joystick: avoid busy waiting if joystick disconnected
    
    Just try to reconnect every 100ms.

http://git.fawkesrobotics.org/fawkes.git/commit/0aabc3c
http://trac.fawkesrobotics.org/changeset/0aabc3c

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit df4248deb5ec8edd349b1b642c03cbcd28e45c82
Author:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
AuthorDate: Wed Apr 13 23:26:17 2016 +0200
Commit:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
CommitDate: Wed Apr 13 23:26:17 2016 +0200

    joystick-teleop: fix condition on safety checks

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

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit ce7348c9d08c158f39bf42d988e9c4024739e1fb
Author:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
AuthorDate: Wed Apr 13 23:26:40 2016 +0200
Commit:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
CommitDate: Wed Apr 13 23:26:40 2016 +0200

    joystick-teleop: add support for force feedback
    
    Unfortuantely the XBox driver seems to be broken and only rarely really
    activate the force feedback.

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

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


- *Summary* -----------------------------------------------------------
 src/plugins/joystick/acquisition_thread.cpp     |   14 ++++--
 src/plugins/joystick/acquisition_thread.h       |    1 +
 src/plugins/joystick/joystick_teleop_thread.cpp |   56 ++++++++++++++++++++++-
 src/plugins/joystick/joystick_teleop_thread.h   |    4 ++
 src/plugins/robotino/act_thread.cpp             |    3 +
 5 files changed, 72 insertions(+), 6 deletions(-)


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

- *commit* 96798a48ac8376ceb2c4840325c0d8ed76515197 - - - - - - - - - -
Author:  Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
Date:    Wed Apr 13 19:31:07 2016 +0200
Subject: robotino: set zero velocity on disabling motor

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

_Diff for modified files_:
diff --git a/src/plugins/robotino/act_thread.cpp b/src/plugins/robotino/act_thread.cpp
index 18642b3..05228a9 100644
--- a/src/plugins/robotino/act_thread.cpp
+++ b/src/plugins/robotino/act_thread.cpp
@@ -170,6 +170,9 @@ RobotinoActThread::loop()
 			                 ? "En" : "Dis");
 			motor_if_->set_motor_state(msg->motor_state());
 			motor_if_->write();
+
+			des_vx_ = des_vy_ = des_omega_ = 0.;
+			set_des_vel = true;
 		}
 
 		else if (MotorInterface::TransRotMessage *msg = motor_if_->msgq_first_safe(msg))

- *commit* accf06d19be740868434eda2b1f74c5183d728be - - - - - - - - - -
Author:  Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
Date:    Wed Apr 13 23:21:59 2016 +0200
Subject: joystick: give axis and button data once after connection

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

_Diff for modified files_:
diff --git a/src/plugins/joystick/acquisition_thread.cpp b/src/plugins/joystick/acquisition_thread.cpp
index 9bad2b7..8e515c0 100644
--- a/src/plugins/joystick/acquisition_thread.cpp
+++ b/src/plugins/joystick/acquisition_thread.cpp
@@ -159,6 +159,7 @@ JoystickAcquisitionThread::open_joystick()
     bbhandler_->joystick_plugged(num_axes_, num_buttons_);
   }
   connected_ = true;
+  just_connected_ = true;
 }
 
 void
@@ -243,6 +244,7 @@ JoystickAcquisitionThread::loop()
 	    close(fd_);
 	    fd_ = -1;
 	    connected_ = false;
+	    just_connected_ = false;
 	    safety_lockout_ = true;
 	    new_data_ = false;
 	    if ( bbhandler_ ) {
@@ -347,7 +349,8 @@ bool
 JoystickAcquisitionThread::lock_if_new_data()
 {
   data_mutex_->lock();
-  if (new_data_) {
+  if (new_data_ || just_connected_) {
+	  just_connected_ = false;
     return true;
   } else {
     data_mutex_->unlock();
@@ -401,7 +404,7 @@ JoystickAcquisitionThread::joystick_name() const
 unsigned int
 JoystickAcquisitionThread::pressed_buttons() const
 {
-  return pressed_buttons_;
+	return safety_lockout_ ? 0 : pressed_buttons_;
 }
 
 
@@ -411,5 +414,8 @@ JoystickAcquisitionThread::pressed_buttons() const
 float *
 JoystickAcquisitionThread::axis_values()
 {
+	if (safety_lockout_) {
+		memset(axis_values_, 0, axis_array_size_ * sizeof(float));
+	}
   return axis_values_;
 }
diff --git a/src/plugins/joystick/acquisition_thread.h b/src/plugins/joystick/acquisition_thread.h
index 01fff8b..00671f5 100644
--- a/src/plugins/joystick/acquisition_thread.h
+++ b/src/plugins/joystick/acquisition_thread.h
@@ -86,6 +86,7 @@ class JoystickAcquisitionThread
 
   int  fd_;
   bool connected_;
+  bool just_connected_;
   unsigned int axis_array_size_;
   char num_axes_;
   char num_buttons_;

- *commit* 0aabc3cbcc30f5f6bbde1a92feb28fb3e8473d7c - - - - - - - - - -
Author:  Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
Date:    Wed Apr 13 23:23:56 2016 +0200
Subject: joystick: avoid busy waiting if joystick disconnected

 src/plugins/joystick/acquisition_thread.cpp |    4 ++--
 1 files changed, 2 insertions(+), 2 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/joystick/acquisition_thread.cpp b/src/plugins/joystick/acquisition_thread.cpp
index 8e515c0..5f69135 100644
--- a/src/plugins/joystick/acquisition_thread.cpp
+++ b/src/plugins/joystick/acquisition_thread.cpp
@@ -332,8 +332,8 @@ JoystickAcquisitionThread::loop()
       } catch (Exception &e) {
 	logger->log_warn(name(), "Initializing force feedback failed, disabling");
       }
-    } catch (...) {
-      // ignored
+    } catch (Exception &e) {
+	    usleep(100000);
     }
   }
 }

- *commit* df4248deb5ec8edd349b1b642c03cbcd28e45c82 - - - - - - - - - -
Author:  Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
Date:    Wed Apr 13 23:26:17 2016 +0200
Subject: joystick-teleop: fix condition on safety checks

 src/plugins/joystick/joystick_teleop_thread.cpp |    5 +++--
 1 files changed, 3 insertions(+), 2 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/joystick/joystick_teleop_thread.cpp b/src/plugins/joystick/joystick_teleop_thread.cpp
index 8ceb1ad..f0677ca 100644
--- a/src/plugins/joystick/joystick_teleop_thread.cpp
+++ b/src/plugins/joystick/joystick_teleop_thread.cpp
@@ -241,7 +241,8 @@ JoystickTeleOpThread::loop()
 
       float theta, distance;
       cart2polar2d(vx, vy, &theta, &distance);
-      if (!cfg_use_laser_ || is_area_free(rad2deg(theta))) // if we have no laser or area is free, move
+      bool area_free = is_area_free(rad2deg(theta));
+      if (!cfg_use_laser_ || area_free) // if we have no laser or area is free, move
       {  
         if (laser_if_ && laser_if_->has_writer() && min_distance_ < 2*cfg_collision_safety_distance_)
         {
@@ -251,7 +252,7 @@ JoystickTeleOpThread::loop()
         }
         send_transrot(vx, vy, omega);
       }
-      else // If area is not free, only allow rotation
+      else if (cfg_use_laser_ && ! area_free)
       {
         logger->log_warn(name(),"obstacle reached");
         send_transrot(0.,0.,omega);

- *commit* ce7348c9d08c158f39bf42d988e9c4024739e1fb - - - - - - - - - -
Author:  Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
Date:    Wed Apr 13 23:26:40 2016 +0200
Subject: joystick-teleop: add support for force feedback

 src/plugins/joystick/joystick_teleop_thread.cpp |   51 +++++++++++++++++++++++
 src/plugins/joystick/joystick_teleop_thread.h   |    4 ++
 2 files changed, 55 insertions(+), 0 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/joystick/joystick_teleop_thread.cpp b/src/plugins/joystick/joystick_teleop_thread.cpp
index f0677ca..f63b888 100644
--- a/src/plugins/joystick/joystick_teleop_thread.cpp
+++ b/src/plugins/joystick/joystick_teleop_thread.cpp
@@ -108,6 +108,25 @@ JoystickTeleOpThread::init()
     } catch (Exception &e) {
       logger->log_warn(name(), "No laser_interface_id configured, ignoring");
     }
+
+    cfg_use_ff_ = false;
+    ff_weak_ = false;
+    ff_strong_ = false;
+    try {
+	    cfg_use_ff_ = config->get_bool(CFG_PREFIX"collision_safety/use-force-feedback");
+    } catch (Exception &e) {} // ignore, use default
+    logger->log_debug(name(), "Collision safety force feedback %sabled", cfg_use_ff_ ? "En" : "Dis");
+    if (cfg_use_ff_) {
+	    JoystickInterface::StartRumbleMessage *msg =
+		    new JoystickInterface::StartRumbleMessage();
+
+	    msg->set_strong_magnitude(0xFFFF);
+	    msg->set_weak_magnitude(0x8000);
+	    msg->set_length(1000);
+
+	    joystick_if_->msgq_enqueue(msg);
+
+    }
   } else {
     logger->log_warn(name(), "Collision safety for joystick is disabled.");
   }
@@ -249,6 +268,25 @@ JoystickTeleOpThread::loop()
           logger->log_warn(name(),"slow down");
           vx = vx * min_distance_ / 2 / cfg_collision_safety_distance_;
           vy = vy * min_distance_ / 2 / cfg_collision_safety_distance_;
+
+          if (cfg_use_ff_ && ! ff_weak_ && joystick_if_->supported_ff_effects() != 0) {
+	          JoystickInterface::StartRumbleMessage *msg =
+		          new JoystickInterface::StartRumbleMessage();
+
+	          msg->set_strong_magnitude(0xFFFF);
+	          msg->set_weak_magnitude(0x8000);
+
+	          joystick_if_->msgq_enqueue(msg);
+	          ff_weak_ = true;
+	          ff_strong_ = false;
+          }
+        } else if (ff_weak_ || ff_strong_) {
+	        JoystickInterface::StopRumbleMessage *msg =
+		        new JoystickInterface::StopRumbleMessage();
+
+	        joystick_if_->msgq_enqueue(msg);
+	        ff_weak_ = false;
+	        ff_strong_ = false;
         }
         send_transrot(vx, vy, omega);
       }
@@ -256,6 +294,19 @@ JoystickTeleOpThread::loop()
       {
         logger->log_warn(name(),"obstacle reached");
         send_transrot(0.,0.,omega);
+
+        if (cfg_use_ff_ && ! ff_weak_ && joystick_if_->supported_ff_effects() != 0) {
+	        JoystickInterface::StartRumbleMessage *msg =
+		        new JoystickInterface::StartRumbleMessage();
+
+	        msg->set_strong_magnitude(0x8000);
+	        msg->set_weak_magnitude(0xFFFF);
+      
+	        logger->log_debug(name(), "Enabling strong rumble");
+	        joystick_if_->msgq_enqueue(msg);
+	        ff_strong_ = true;
+	        ff_weak_ = false;
+        }
       }
     }
   } else if (! stopped_) {
diff --git a/src/plugins/joystick/joystick_teleop_thread.h b/src/plugins/joystick/joystick_teleop_thread.h
index 7e9863b..a8a177d 100644
--- a/src/plugins/joystick/joystick_teleop_thread.h
+++ b/src/plugins/joystick/joystick_teleop_thread.h
@@ -90,9 +90,13 @@ class JoystickTeleOpThread
   std::string  cfg_ifid_joystick_;
   bool         cfg_use_laser_;
   std::string  cfg_ifid_laser_;
+  bool         cfg_use_ff_;
 
   bool         stopped_;
   float        min_distance_;
+
+  bool         ff_weak_;
+  bool         ff_strong_;
 };
 
 




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


More information about the fawkes-commits mailing list