From a7324d75ab44575c70812d337df67d3ea0dfabb7 Mon Sep 17 00:00:00 2001
From: Aitor Martinez <aitor.martinez.fernandez@gmail.com>
Date: Fri, 13 Apr 2018 10:44:07 +0200
Subject: [PATCH] [#1130] Comm with better support for bumper

---
 .../jderobot/comm/ros/listenerBumper.hpp      |  2 ++
 src/libs/comm_cpp/src/ros/listenerBumper.cpp  | 19 +++++++++++++++----
 src/libs/comm_py/comm/ros/listenerBumper.py   | 15 ++++++++++++---
 3 files changed, 29 insertions(+), 7 deletions(-)

diff --git a/src/libs/comm_cpp/include/jderobot/comm/ros/listenerBumper.hpp b/src/libs/comm_cpp/include/jderobot/comm/ros/listenerBumper.hpp
index 4c0cb32d8..37885c886 100644
--- a/src/libs/comm_cpp/include/jderobot/comm/ros/listenerBumper.hpp
+++ b/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;
 		
diff --git a/src/libs/comm_cpp/src/ros/listenerBumper.cpp b/src/libs/comm_cpp/src/ros/listenerBumper.cpp
index bdf21b001..b03918952 100644
--- a/src/libs/comm_cpp/src/ros/listenerBumper.cpp
+++ b/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;
 	}
diff --git a/src/libs/comm_py/comm/ros/listenerBumper.py b/src/libs/comm_py/comm/ros/listenerBumper.py
index b8bdc90ac..4d5f140f7 100644
--- a/src/libs/comm_py/comm/ros/listenerBumper.py
+++ b/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()