From 286b6fa419b314237f12546e2075982411b4ffb3 Mon Sep 17 00:00:00 2001 From: Liam Paull Date: Sun, 28 Apr 2019 22:40:42 -0400 Subject: [PATCH 1/3] runs on duckiebot --- duckiebot/Dockerfile | 26 ++++++++++++++++++++ duckiebot/Makefile | 0 duckiebot/duckiebot_bridge.py | 41 +++++++++++++++++++++++++++++++ duckiebot/requirements.txt | 2 ++ duckiebot/rosclient.py | 45 +++++++++++++++++++++++++++++++++++ 5 files changed, 114 insertions(+) create mode 100644 duckiebot/Dockerfile create mode 100644 duckiebot/Makefile create mode 100644 duckiebot/duckiebot_bridge.py create mode 100644 duckiebot/requirements.txt create mode 100644 duckiebot/rosclient.py diff --git a/duckiebot/Dockerfile b/duckiebot/Dockerfile new file mode 100644 index 0000000..40603fc --- /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", "duckiebot_bridge.py"] diff --git a/duckiebot/Makefile b/duckiebot/Makefile new file mode 100644 index 0000000..e69de29 diff --git a/duckiebot/duckiebot_bridge.py b/duckiebot/duckiebot_bridge.py new file mode 100644 index 0000000..397282c --- /dev/null +++ b/duckiebot/duckiebot_bridge.py @@ -0,0 +1,41 @@ +#!/usr/bin/env python2 + +from zuper_nodes_python2 import ComponentInterface +import cv2 +from rosclient import ROSClient +import numpy as np + +class DuckiebotBridge(object): + def __init__(self): + + 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 run(self): + + + while True: + if not self.client.initialized: + continue + np_arr = np.fromstring(self.client.image, np.uint8) + #image_np = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR) + 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..052d5b4 --- /dev/null +++ b/duckiebot/rosclient.py @@ -0,0 +1,45 @@ +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') + + self.r = rospy.Rate(15) + + + 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) \ No newline at end of file From 67a037c6d867e3a466a78f4ddcbdfbc2ab123e3f Mon Sep 17 00:00:00 2001 From: Liam Paull Date: Mon, 29 Apr 2019 22:04:29 -0400 Subject: [PATCH 2/3] LP --- duckiebot/Dockerfile | 2 +- duckiebot/Makefile | 12 ++++++++++++ duckiebot/duckiebot_bridge.py | 1 - 3 files changed, 13 insertions(+), 2 deletions(-) diff --git a/duckiebot/Dockerfile b/duckiebot/Dockerfile index 40603fc..c8f3b2e 100644 --- a/duckiebot/Dockerfile +++ b/duckiebot/Dockerfile @@ -23,4 +23,4 @@ RUN pip install pillow --user --upgrade RUN /bin/bash -c "export PYTHONPATH="/usr/local/lib/python2.7/dist-packages:$PYTHONPATH"" -CMD ["python", "duckiebot_bridge.py"] +CMD ["python", "-u", "duckiebot_bridge.py"] diff --git a/duckiebot/Makefile b/duckiebot/Makefile index e69de29..0393877 100644 --- a/duckiebot/Makefile +++ 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 index 397282c..572c42c 100644 --- a/duckiebot/duckiebot_bridge.py +++ b/duckiebot/duckiebot_bridge.py @@ -22,7 +22,6 @@ def run(self): if not self.client.initialized: continue np_arr = np.fromstring(self.client.image, np.uint8) - #image_np = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR) data = np_arr.tostring() obs = {u'camera': {u'jpg_data': data}} From ab1944313e68d11731ed1323e280de1c1aa48af8 Mon Sep 17 00:00:00 2001 From: Liam Paull Date: Mon, 29 Apr 2019 23:58:50 -0400 Subject: [PATCH 3/3] kill wheels on stop --- duckiebot/duckiebot_bridge.py | 15 ++++++++++++--- duckiebot/rosclient.py | 10 +++++++--- 2 files changed, 19 insertions(+), 6 deletions(-) diff --git a/duckiebot/duckiebot_bridge.py b/duckiebot/duckiebot_bridge.py index 572c42c..1945378 100644 --- a/duckiebot/duckiebot_bridge.py +++ b/duckiebot/duckiebot_bridge.py @@ -4,10 +4,15 @@ 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) @@ -15,23 +20,27 @@ def __init__(self): 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() diff --git a/duckiebot/rosclient.py b/duckiebot/rosclient.py index 052d5b4..1617ce0 100644 --- a/duckiebot/rosclient.py +++ b/duckiebot/rosclient.py @@ -20,10 +20,14 @@ def __init__(self): # 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 @@ -42,4 +46,4 @@ def send_commands(self, cmds): 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) \ No newline at end of file + self.cmd_pub.publish(cmd_msg)