moving on GREEN button

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 
+

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,15 +187,15 @@ 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():
                     m = soc.readMsg()
                     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 ):