From e6a6e942bddfd1e8f27f6c51a030b96db6abc131 Mon Sep 17 00:00:00 2001
From: martind <martind@mapfactor.com>
Date: Wed, 3 Sep 2014 21:01:28 +0200
Subject: [PATCH] moving on GREEN button

---
 ros/huskyros.py | 73 +++++++++++++++++++++++++++++++++++++++++++++++++
 ros/node.py     | 15 ++++++----
 2 files changed, 83 insertions(+), 5 deletions(-)
 create mode 100644 ros/huskyros.py

diff --git a/ros/huskyros.py b/ros/huskyros.py
new file mode 100644
index 0000000..5560a5a
--- /dev/null
+++ b/ros/huskyros.py
@@ -0,0 +1,73 @@
+#!/usr/bin/python
+"""
+  ROS wrapper for Husky
+  usage:
+     ./husky.py <task> [<metalog file>]
+"""
+
+from node import NodeROS, setIPs
+import sys
+
+class HuskyROS:
+    def __init__( self, metalog=None ):
+        self.node = NodeROS( 
+            subscribe=['/imu/data', 
+                '/husky/data/encoders', 
+                '/husky/data/power_status',
+                '/husky/data/safety_status', 
+                '/joy', 
+                '/husky/data/system_status'], 
+            publish=['/husky/cmd_vel'],
+            heartbeat='/husky/data/encoders',                
+            metalog=metalog )
+        self.speed = 0.0
+        self.angularSpeed = 0.0
+        self.greenPressed = None
+        self.redPressed = None
+
+    def update( self ):
+        self.node.cmdList.append( ('/husky/cmd_vel', (self.speed,self.angularSpeed)) )
+        status = self.node.update()
+        for topic,data in status:
+            if topic == '/joy':
+                print data
+                axes, buttons = data
+                self.greenPressed = (buttons[1]==1)
+                self.redPressed = (buttons[2]==1)
+
+
+def test( metalog ):
+    robot = HuskyROS( metalog=metalog )
+    robot.speed = 0.0
+    while True:
+        robot.update()
+        if robot.greenPressed:
+            print "GREEN"
+            robot.speed = 0.1
+        else:
+            print "PAUSE"
+            robot.speed = 0.0
+        if robot.redPressed:
+            print "RED"
+            robot.speed = 0.0
+            break
+
+    robot.speed = 0.0
+    robot.update()
+
+
+if __name__ == "__main__":
+    if len(sys.argv) < 2 or (len(sys.argv)==2 and "meta" not in sys.argv[1]):
+        print __doc__
+        sys.exit(1)
+
+    metalog = None
+    if "meta" in sys.argv[1]:
+        metalog = sys.argv[1]
+    else:
+        setIPs( sys.argv[1], 'http://'+sys.argv[2]+':11311' )
+    test( metalog )
+
+#-------------------------------------------------------------------
+# vim: expandtab sw=4 ts=4 
+
diff --git a/ros/node.py b/ros/node.py
index 64ea140..0a1401d 100644
--- a/ros/node.py
+++ b/ros/node.py
@@ -21,6 +21,12 @@
 NODE_HOST, NODE_PORT = (None, 8000)
 PUBLISH_PORT = 8123
 
+def setIPs( nodeIP, masterIP ):
+    global ROS_MASTER_URI
+    global NODE_HOST
+    ROS_MASTER_URI = masterIP
+    NODE_HOST = nodeIP
+
 
 from xmlrpclib import ServerProxy
 from SimpleXMLRPCServer import SimpleXMLRPCServer
@@ -181,6 +187,7 @@ def update( self ):
             self.publishSockets[topic].writeMsg( self.lookupTopicType(topic)[3]( *cmd ) ) # 4th param is packing function
         self.cmdList = []
         atLeastOne = False
+        ret = []
         while not atLeastOne and len(self.sockets) > 0:
             if self.callerApi != None:
                 for topic,soc in self.sockets.items():
@@ -188,8 +195,7 @@ def update( self ):
                     if m != None:
                         self.metalog.write( topic + '\n' )
                         self.metalog.flush()
-                        print topic
-                        print self.lookupTopicType(topic)[2]( m )
+                        ret.append( (topic,self.lookupTopicType(topic)[2]( m )) )
                         if self.heartbeat == None or topic == self.heartbeat:
                             atLeastOne = True
             else:
@@ -199,12 +205,11 @@ def update( self ):
                     break
                 soc = self.sockets[ topic ]
                 m = soc.readMsg()
-                print topic
-                print self.lookupTopicType(topic)[2]( m )
+                ret.append( (topic,self.lookupTopicType(topic)[2]( m )) )
         if self.callerApi != None:
             self.metalog.write( '---\n' )
             self.metalog.flush()
-
+        return ret
 
 
 def testNode1( metalog ):