Merge pull request #1132 from aitormf/libs/comm/bumper

[fix #1130] Comm with better support for bumper

src/libs/comm_cpp/include/jderobot/comm/ros/listenerBumper.hpp

@@ -27,6 +27,7 @@
 #include <jderobot/types/bumperData.h>
 #include <jderobot/comm/interfaces/bumperClient.hpp>
 #include <jderobot/comm/ros/translators.hpp>
+#include <sys/time.h>
 
 namespace Comm {
 	class ListenerBumper: public Comm::BumperClient {
@@ -45,6 +46,7 @@ namespace Comm {
 		ros::Subscriber sub;
 		std::string topic;
 		std::string nodeName;
+		int current_time;
 
 		ros::AsyncSpinner* spinner;
 

src/libs/comm_cpp/src/ros/listenerBumper.cpp

@@ -45,17 +45,28 @@ namespace Comm {
 
 	void 
 	ListenerBumper::bumpercallback(const kobuki_msgs::BumperEventConstPtr& bumper_msg){
-		pthread_mutex_lock(&mutex);
-		this->bumperData = Comm::translate_bumper_messages(bumper_msg);
-		pthread_mutex_unlock(&mutex);
+		timeval curTime;
+		if (this->bumperData.state > 0){
+			pthread_mutex_lock(&mutex);
+				gettimeofday(&curTime, NULL);
+				this->current_time = curTime.tv_usec / 1000;
+				this->bumperData = Comm::translate_bumper_messages(bumper_msg);
+			pthread_mutex_unlock(&mutex);
+		}
 
 	}
 
 	JdeRobotTypes::BumperData  
 	ListenerBumper::getBumperData(){
+		timeval curTime;
 		JdeRobotTypes::BumperData ld;
 		pthread_mutex_lock(&mutex);
-		ld = this->bumperData;
+			gettimeofday(&curTime, NULL);
+			int t = curTime.tv_usec / 1000;
+			if ((t - this->current_time) > 500){
+				this->bumperData.state = 0;
+			}
+			ld = this->bumperData;
 		pthread_mutex_unlock(&mutex);
 		return ld;
 	}

src/libs/comm_py/comm/ros/listenerBumper.py

@@ -2,6 +2,9 @@
 from kobuki_msgs.msg import BumperEvent
 import threading
 from jderobotTypes import BumperData
+import time
+
+current_milli_time = lambda: int(round(time.time() * 1000))
 
 
 
@@ -47,6 +50,7 @@ def __init__(self, topic):
         '''
         self.topic = topic
         self.data = BumperData()
+        self.time = current_milli_time()
         self.sub = None
         self.lock = threading.Lock()
         self.start()
@@ -62,9 +66,11 @@ def __callback (self, event):
         '''
         bump = bumperEvent2BumperData(event)
 
-        self.lock.acquire()
-        self.data = bump
-        self.lock.release()
+        if bump.state == 1:
+            self.lock.acquire()
+            self.time = current_milli_time()
+            self.data = bump
+            self.lock.release()
 
     def stop(self):
         '''
@@ -88,6 +94,9 @@ def getBumperData(self):
 
         '''
         self.lock.acquire()
+        t = current_milli_time()
+        if (t - self.time) > 500:
+            self.data.state = 0
         bump = self.data
         self.lock.release()