adds stop command if no velocity received for 0.5 seconds

uml-robotics · Nov 17, 2012 · e3850e9 · e3850e9
1 parent 3d9eaab
commit e3850e9
Show file tree

Hide file tree

Showing 3 changed files with 31 additions and 2 deletions.
diff --git a/atrvjr_drivers/rflex/include/rflex/atrvjr_driver.h b/atrvjr_drivers/rflex/include/rflex/atrvjr_driver.h
@@ -3,6 +3,7 @@
 
 #include <rflex/rflex_driver.h>
 #include <sensor_msgs/PointCloud.h>
+#include <boost/thread.hpp>
 
 /**
  * \brief ATRVJR Driver class
@@ -52,6 +53,12 @@ class ATRVJR : public RFLEX {
          * \return number of active bump sensors */
 //        int getBaseBumps(sensor_msgs::PointCloud* cloud) const;
 
+        /**
+         * Overriding rflex setVelocity to be able to watch when no
+         * velocity command arrives within interval.
+         */
+        void setVelocity( const double tvel, const double rvel);
+
         /** Processes the DIO packets - called from RFflex Driver
          * \param address origin
          * \param data values */
@@ -83,6 +90,9 @@ class ATRVJR : public RFLEX {
 
 //        int** bumps;
 
+        ros::Time last_velocity_time;
+        void watchdogThread();///< Thread that stops robot if no vel command received
+
         // Not allowed to use these
         ATRVJR(const ATRVJR &atrvjr); 				///< Private constructor - Don't use
         ATRVJR &operator=(const ATRVJR &atrvjr); 	///< Private constructor - Don't use

diff --git a/atrvjr_drivers/rflex/src/atrvjr_driver.cc b/atrvjr_drivers/rflex/src/atrvjr_driver.cc
@@ -28,9 +28,13 @@
 #include <rflex/atrvjr_config.h>
 #include <math.h>
 #include <cstdio>
+#include <boost/thread.hpp>
 
-ATRVJR::ATRVJR()
+ATRVJR::ATRVJR():
+   last_velocity_time(ros::Time::now())
 {
+    // Start watchdog thread
+    boost::thread(boost::bind(&ATRVJR::watchdogThread, this));
 //    bumps = new int*[2];
 //
 //    for (int index=0;index<2;index++) {
@@ -167,3 +171,17 @@ void ATRVJR::processDioEvent(unsigned char address, unsigned short data) {
         printf("ATRVJR DIO: address 0x%02x (%d) value 0x%02x (%d)\n", address, address, data, data);
     }
 }
+
+void ATRVJR::setVelocity(const double tvel, const double rvel) {
+    last_velocity_time = ros::Time::now();
+    RFLEX::setVelocity(tvel, rvel);
+}
+
+void ATRVJR::watchdogThread() {
+    while (true) {
+        if (ros::Time::now() - last_velocity_time > ros::Duration(0.5)) {
+            RFLEX::setVelocity(0, 0);
+        }
+        usleep(100000);
+    }
+}
diff --git a/atrvjr_drivers/rflex/src/atrvjr_node.cc b/atrvjr_drivers/rflex/src/atrvjr_node.cc
@@ -37,6 +37,8 @@
  *
  */
 class ATRVJRNode {
+    public:
+        ros::NodeHandle n;
     private:
         ATRVJR driver;
 
@@ -72,7 +74,6 @@ class ATRVJRNode {
         void reconfigureCb(rflex::AtrvjrParamsConfig& config, uint32_t level);
 
     public:
-        ros::NodeHandle n;
         ATRVJRNode();
         ~ATRVJRNode();
         int initialize(const char* port);