include outputs in input message

[ros_roboint.git] / scripts / robo_explorer.py

diff --git a/scripts/robo_explorer.py b/scripts/robo_explorer.py
index 24584e78a3443e82a900b530e0925f1d717a8095..514fe6dd04cdb9b1f81e26911d4b1e95dbf46955 100755 (executable)
--- a/scripts/robo_explorer.py
+++ b/scripts/robo_explorer.py
@@ -16,7 +16,6 @@ class RoboExplorer:
        def __init__(self):
                rospy.init_node('robo_explorer')
 
-               self.speed = (0, 0)
                self.x = 0
                self.y = 0
                self.alpha = 0
@@ -62,14 +61,15 @@ class RoboExplorer:
                in_now = msg.input[:2]
                if self.last_in is not None:
                        in_diff = [abs(a - b) for a, b in zip(in_now, self.last_in)] # get changed inputs
+                       print "update", in_diff, msg.output
                        # fix in_diff from actual motor direction
-                       if self.speed[0] < 0:
+                       if msg.output[1] > 0: # left reverse
                                in_diff[0] = -in_diff[0]
-                       elif self.speed[0] == 0:
+                       elif msg.output[0] == 0 and msg.output[1] == 0: # left stop
                                in_diff[0] = 0
-                       if self.speed[1] < 0:
+                       if msg.output[3] > 0: # right reverse
                                in_diff[1] = -in_diff[1]
-                       elif self.speed[1] == 0:
+                       elif msg.output[2] == 0 and msg.output[3] == 0: # right stop
                                in_diff[1] = 0
 
                        dist_dir = (in_diff[1] - in_diff[0])*self.wheel_size*pi/8 # steps_changed in different direction => m
@@ -180,19 +180,17 @@ class RoboExplorer:
                if speed_r < -7: speed_r = -7
                elif speed_r > 7: speed_r = 7
 
-               #print "Speed wanted: %.2f m/s %.2f rad/s, set: %d %d" % (trans, rot, round(speed_l), round(speed_r))
+               print "Speed wanted: %.2f m/s %.2f rad/s, set: %d %d" % (trans, rot, round(speed_l), round(speed_r))
 
                outmsg = Motor()
-               outmsg.num = 1
+               outmsg.num = 0
                outmsg.speed = round(speed_l)
                self.pub_motor.publish(outmsg)
                
                outmsg = Motor()
-               outmsg.num = 2
+               outmsg.num = 1
                outmsg.speed = round(speed_r)
                self.pub_motor.publish(outmsg)
 
-               self.speed = (speed_l, speed_r)
-
 if __name__ == '__main__':
        RoboExplorer()