Merge branch 'aido2' of github.com:duckietown/challenge-aido2_LF into…

… aido2

duckiebot/Dockerfile

@@ -0,0 +1,26 @@
+# We start from a base ROS image
+FROM duckietown/rpi-duckiebot-base:master19-no-arm
+
+RUN apt-get update -y && apt-get install -y --no-install-recommends \
+         gcc \
+         libc-dev\
+         git \
+         bzip2 \
+         python-tk \
+         python-wheel \
+         python-pip && \
+     rm -rf /var/lib/apt/lists/*
+
+
+WORKDIR /project
+COPY . .
+RUN pip install -r requirements.txt
+RUN pip list
+
+# For ROS Agent - Need to upgrade Pillow for Old ROS stack
+RUN pip install pillow --user --upgrade
+
+RUN /bin/bash -c "export PYTHONPATH="/usr/local/lib/python2.7/dist-packages:$PYTHONPATH""
+
+
+CMD ["python", "-u", "duckiebot_bridge.py"]

duckiebot/Makefile

@@ -0,0 +1,12 @@
+repo=challenge-aido_lf-duckiebot
+branch=$(shell git rev-parse --abbrev-ref HEAD)
+tag=duckietown/$(repo):$(branch)
+
+build:
+	docker build -t $(tag) .
+
+build-no-cache:
+	docker build -t $(tag) --no-cache .
+
+push: build
+	docker push $(tag)

duckiebot/duckiebot_bridge.py

@@ -0,0 +1,49 @@
+#!/usr/bin/env python2
+
+from zuper_nodes_python2 import ComponentInterface
+import cv2
+from rosclient import ROSClient
+import numpy as np
+import signal
+import sys
+
+class DuckiebotBridge(object):
+    def __init__(self):
+
+        signal.signal(signal.SIGINT, self.exit_gracefully)
+        signal.signal(signal.SIGTERM, self.exit_gracefully)
+
+        AIDONODE_DATA_IN = '/fifos/agent-in'
+        AIDONODE_DATA_OUT = '/fifos/agent-out'
+        self.ci = ComponentInterface(AIDONODE_DATA_IN, AIDONODE_DATA_OUT, 'agent', timeout=5)
+        self.ci.write_topic_and_expect_zero(u'seed', 32)
+        self.ci.write_topic_and_expect_zero(u'episode_start', {u'episode_name': u'episode'})
+        self.client=ROSClient()
+
+    def exit_gracefully(self, signum, frame):
+        sys.exit(0)
+
+    def run(self):
+
+        while True:
+            if not self.client.initialized:
+                continue
+
+            np_arr = np.fromstring(self.client.image, np.uint8)
+            data = np_arr.tostring()
+
+            obs = {u'camera': {u'jpg_data': data}}
+            self.ci.write_topic_and_expect_zero(u'observations', obs)
+            commands = self.ci.write_topic_and_expect(u'get_commands', expect=u'commands')
+            commands = commands.data[u'wheels']
+
+            self.client.send_commands(commands)
+
+
+
+def main():
+    node = DuckiebotBridge()
+    node.run()
+
+if __name__ == '__main__':
+    main()

duckiebot/requirements.txt

@@ -0,0 +1,2 @@
+-e git://github.com/AndreaCensi/zuper-nodes-python2.git@z2#egg=zuper-nodes-python2
+cbor2

duckiebot/rosclient.py

@@ -0,0 +1,49 @@
+import rospy
+from sensor_msgs.msg import CompressedImage, CameraInfo
+from duckietown_msgs.msg import Twist2DStamped, WheelsCmdStamped
+import os
+
+
+class ROSClient(object):
+    def __init__(self):
+        # Get the vehicle name, which comes in as HOSTNAME
+        # TODO not sure about this
+        self.vehicle = os.getenv('HOSTNAME')
+
+        self.cam_sub = rospy.Subscriber('/{}/camera_node/image/compressed'.format(
+            self.vehicle),CompressedImage, self._cam_cb)
+
+        self.cmd_pub = rospy.Publisher('/{}/wheels_driver_node/wheels_cmd'.format(
+            self.vehicle), WheelsCmdStamped, queue_size=10)
+
+        self.initialized=False
+
+        # Initializes the node
+        rospy.init_node('ROSClient')
+        rospy.on_shutdown(self.on_shutdown)
+
+        self.r = rospy.Rate(15)
+
+    def on_shutdown(self):
+        commands = {u'motor_right': 0.0, u'motor_left': 0.0}
+        self.send_commands(commands)
+
+    def _cam_cb(self, msg):
+        """
+        Callback to listen to last outputted camera image and store it
+        """
+        self.image = msg.data
+        self.initialized=True
+
+
+    def send_commands(self, cmds):
+        """
+        Publishes the wheel commands to ROS
+        """
+        time = rospy.get_rostime()
+        cmd_msg = WheelsCmdStamped()
+        cmd_msg.header.stamp.secs = time.secs
+        cmd_msg.header.stamp.nsecs = time.nsecs
+        cmd_msg.vel_right = cmds[u'motor_right']
+        cmd_msg.vel_left  = cmds[u'motor_left']
+        self.cmd_pub.publish(cmd_msg)