From 5416fccb43c49e7e7c9003edfa6515007ba959b2 Mon Sep 17 00:00:00 2001
From: Patrick Goebel <patrick@pirobot.org>
Date: Sat, 15 Dec 2012 18:37:17 -0800
Subject: [PATCH] Fixed bug in GP2D12 distance function

---
 ros_arduino_python/src/arduino_sensors.py  |  38 +++++++++++++--------
 ros_arduino_python/src/arduino_sensors.pyc | Bin 12063 -> 12162 bytes
 ros_arduino_python/src/base_controller.py  |   1 +
 ros_arduino_python/src/base_controller.pyc | Bin 7155 -> 7160 bytes
 stack.xml                                  |   2 --
 5 files changed, 24 insertions(+), 17 deletions(-)

diff --git a/ros_arduino_python/src/arduino_sensors.py b/ros_arduino_python/src/arduino_sensors.py
index 403148f..ced0fa0 100755
--- a/ros_arduino_python/src/arduino_sensors.py
+++ b/ros_arduino_python/src/arduino_sensors.py
@@ -66,16 +66,11 @@ class Sensor(object):
                 except:
                     return          
     
-            # The Arduino returns ranges in cm so convert to meters
+            # For range sensors, assign the value to the range message field
             if self.message_type == MessageType.RANGE:
-                try:
-                    self.value /= 100.0
-                    self.msg.range = self.value
-                except:
-                    return
+                self.msg.range = self.value
             else:
                 self.msg.value = self.value
-                
 
             # At a timestamp and publish the message
             self.msg.header.stamp = rospy.Time.now()
@@ -170,15 +165,19 @@ class IRSensor(RangeSensor):
 class Ping(SonarSensor):
     def __init__(self,*args, **kwargs):
         super(Ping, self).__init__(*args, **kwargs)
-        
-        #self.controller.pin_mode(self.pin, INPUT)
-        
+                
         self.msg.field_of_view = 0.785398163
         self.msg.min_range = 0.02
         self.msg.max_range = 3.0
         
     def read_value(self):
-        return self.controller.ping(self.pin)
+        # The Arduino Ping code returns the distance in centimeters
+        cm = self.controller.ping(self.pin)
+        
+        # Convert it to meters for ROS
+        distance = cm / 100.0
+        
+        return distance
     
         
 class GP2D12(IRSensor):
@@ -191,12 +190,21 @@ class GP2D12(IRSensor):
         
     def read_value(self):
         value = self.controller.analog_read(self.pin)
+        
+        if value <= 3.0:
+            return self.msg.max_range
+        
         try:
-            distance = (6787 / (value - 3)) - 4
+            distance = (6787.0 / (float(value) - 3.0)) - 4.0
         except:
-            distance = 80
-        if distance > 80: distance = 80
-        if distance < 10: distance = 10
+            return self.msg.max_range
+            
+        # Convert to meters
+        distance /= 100.0
+        
+        # If we get a spurious reading, set it to the max_range
+        if distance > self.msg.max_range: distance = self.msg.max_range
+        if distance < self.msg.min_range: distance = self.msg.max_range
         
         return distance
     
diff --git a/ros_arduino_python/src/arduino_sensors.pyc b/ros_arduino_python/src/arduino_sensors.pyc
index 899ea6d54c5d25adac93fcb382530de46a7bb1a1..5cdbfe2b3b78422fec35fd68623f1a334c596712 100644
GIT binary patch
delta 802
zcmYL{OK1~e5XWbBvw6RovfCt03?`{vsgK&D_=vWmrL@{DMJvT%8x;G5Bo`r4ZBB|x
zV0us#S|vWP_7Ij<(Ssg5>ZyW=7W5!Ji>E4r^Cf9r*dPCI=FfaHvpauoHf@uCd7Mq{
zA1byrJAS7h_86{_;z&1?pa3vtt<nu?4-h#9oe}`_86Kwj$I*!xr^BwOJs|nyAT~;L
zB(9p*TzLr%S`Q`Zo~qMPZyASa-TM>M^x0=(zj@359&wb;1}ZpWx*JC%oTh1Y1FxEw
zGzqat6HUuFWyXW=5N~juzc@!jyPhcr_}1&@T=N*>UBcKC<sb*{n2XUt#03s4;=H*O
zdw}?uCX=CRtDWq~C8(4PMhgfT1oesp1qlZ9>L3?6&CgJZZf_6yWtK*Skr&C)NjlU*
zz9p79D+44|c8Uqh!ABxyVOY^&%ZudkQypFVI_PbTsFY~IZdysK;7e{Jj!(tyGqu~t
z5wdO7jmj`)fq}~r$ViY^fYZfle?c`PLy((1J>>;N6yKocJ;QBhEU>>ez*c<R0@>pK
zlAN6aF=5?cg>isvd1mu$fi1^?u5jml^&Qw#HBo$37vSF%Ln+<HPQxp(SHLfj5IDx*
z{sAsbOrOsiU99x2JsPU8oM2EgI!oWemc$!A<iZR<RdS<Kk}-lZx@4yu03Y%zR=l8e
zv*QKPj{Dd{?X3^-qq*8@L;OTv^yf+sCs)mzJH6=dWoc{^=o2_euM-vifrr~ht@S{E
vm~g%N{|N6_vhUt-iRy-^V)T-G&+m|~HE3?FS@UT&&8h{oi006IZgk*3{b{Kb

delta 723
zcmX|8O-NKx82!$hH*e<Y{HW7hXL4-PDH$`fTm%V)WFTS5Fk)H@r=JlU4J*eMf)*q}
ztHeH91X&1%q!Gxom>{V|y9O$-rL<}tAp{mt-yOoo`wr)vd+&EY&b^&^GT?D-`pb$B
ziYuNCFaGj3ZydkbZD(3u&_q2=5}CsmWesXGGM1L2H2Nsxr@rfJ1RzV))6X~T-jIuk
zy3>BaAu0LJwma)~2e6ML<+VW<f(XF{uMspO2yv=GIqC9o1ASbG?%^xpB!>bExWI+L
z4`g{M=-`YqP*DUfb5qp{hMbM6iw4HIJ-mfGPMb1-yL`0zCGyT@^?P7O`uxE|ezSjx
z&)1l^=M2^i1CRMrY#DRTWNk0-RK%jYU4~n5ZpP<PxhypA6(B@SAAI0*;Wn=+`dV&u
zh!@o?AFMaAs=LOHi@hY3*XtH+d)yMznQA?8sFi2q%=Z&}(aD9xE4=32b<tZok`8?!
zC!mv)A0=YYG-wJmp(ra3bjp!%)<uyigZf7OrD#nOF$(IRG$?D*h)H89O|&ch#B|dL
z(b%9S+h$KvPt~29-g$&5r$@-?d2=ZZWdcd<r2`sTG*oPRF)hOaJYUDLcS7<z1PEby
zI3-mEAKAL(`R3i7vQ*%o4Uepo!qU6=b7Brf=Wx;keB@(|&)wSp;CyckprWg^MZ+l#
zr+F>0f-n5G>5}!|c&27bVSm+OyY>Ec9%<ex*Vm*=H+1pU1HYops#5t>jS4D{vQ$8Y
Kl&yk3*!Txy#h?2C

diff --git a/ros_arduino_python/src/base_controller.py b/ros_arduino_python/src/base_controller.py
index 45fe25f..c35c287 100755
--- a/ros_arduino_python/src/base_controller.py
+++ b/ros_arduino_python/src/base_controller.py
@@ -2,6 +2,7 @@
 
 """
     A base controller class for the Arduino microcontroller
+    
     Borrowed heavily from Mike Feguson's ArbotiX base_controller.py code.
     
     Created for the Pi Robot Project: http://www.pirobot.org
diff --git a/ros_arduino_python/src/base_controller.pyc b/ros_arduino_python/src/base_controller.pyc
index f687ccbb946df2f17dc6b25340cbfff7e6719e5d..dcef1662620fc65f8433b0bd79d15816a52a5610 100644
GIT binary patch
delta 94
zcmext{==M|`7<xqGLf?r*$*=sPdx3inTxTCn^9?VE6*_|#@Nk1{A<}5OE&)!&0%Cb
yx;b6kf{F3u<OPyX85K9TNd+-Vi!m@T6bk_fHbx!>UPc~99wr$^8Abs{0cHToT^OAJ

delta 72
zcmV-O0Js16H}f|K1M>|EXJ^Wh2g3m_k;_E06ajS$0U@)B48{ThT(eORtp)*ev;P%r
e0RhCbY!^5J0mzf38RY>Wvx^#10Ra`0-WwXfQ5iA-

diff --git a/stack.xml b/stack.xml
index 14965f5..e31df54 100644
--- a/stack.xml
+++ b/stack.xml
@@ -6,9 +6,7 @@
   <url>http://ros.org/wiki/ros_arduino_bridge</url>
   <depend stack="ros" />
   <depend stack="ros_comm" /> <!-- std_srvs, rospy, roscpp -->
-  <depend stack="common" /> <!-- actionlib -->
   <depend stack="common_msgs" /> <!-- nav_msgs, geometry_msgs, sensor_msgs, diagnostic_msgs -->
-  <depend stack="control" /> <!-- control_msgs -->
   <depend stack="geometry" /> <!-- tf -->
 
 </stack>