diff --git a/duckiebot/Dockerfile b/duckiebot/Dockerfile new file mode 100644 index 0000000..c8f3b2e --- /dev/null +++ b/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"] diff --git a/duckiebot/Makefile b/duckiebot/Makefile new file mode 100644 index 0000000..0393877 --- /dev/null +++ b/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) diff --git a/duckiebot/duckiebot_bridge.py b/duckiebot/duckiebot_bridge.py new file mode 100644 index 0000000..1945378 --- /dev/null +++ b/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() diff --git a/duckiebot/requirements.txt b/duckiebot/requirements.txt new file mode 100644 index 0000000..28cb418 --- /dev/null +++ b/duckiebot/requirements.txt @@ -0,0 +1,2 @@ +-e git://github.com/AndreaCensi/zuper-nodes-python2.git@z2#egg=zuper-nodes-python2 +cbor2 diff --git a/duckiebot/rosclient.py b/duckiebot/rosclient.py new file mode 100644 index 0000000..1617ce0 --- /dev/null +++ b/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)