From dc386480359de1f5067e635ab8c37afef33f4ed2 Mon Sep 17 00:00:00 2001 From: light5551 Date: Thu, 25 Nov 2021 19:56:27 +0300 Subject: [PATCH 01/15] prepare template --- packages/circle_drive/scripts/circle_drive.py | 33 +++++++++++++++---- packages/circle_drive/scripts/solution.py | 3 ++ 2 files changed, 29 insertions(+), 7 deletions(-) create mode 100644 packages/circle_drive/scripts/solution.py diff --git a/packages/circle_drive/scripts/circle_drive.py b/packages/circle_drive/scripts/circle_drive.py index 3db98995..654ac248 100755 --- a/packages/circle_drive/scripts/circle_drive.py +++ b/packages/circle_drive/scripts/circle_drive.py @@ -3,29 +3,48 @@ import rospy from duckietown.dtros import DTROS, NodeType from duckietown_msgs.msg import Twist2DStamped +from sensor_msgs.msg import CompressedImage +from cv_bridge import CvBridge +from solution import solution + class MyNode(DTROS): def __init__(self, node_name): super(MyNode, self).__init__(node_name=node_name, node_type=NodeType.DEBUG) self.pub = rospy.Publisher("~car_cmd", Twist2DStamped, queue_size=1) + self.bridge = CvBridge() + self.cur_img = None + self.sub_image = rospy.Subscriber( + "~image/compressed", + CompressedImage, + self.image_cb, + buff_size=10000000, + queue_size=1 + ) def run(self): # publish message every 1 second + rate = rospy.Rate(1) # 1Hz while not rospy.is_shutdown(): + vel, omega = solution(self.cur_img) msg = Twist2DStamped() - msg.v = 0.0 - msg.omega = 5.0 - rospy.loginfo("Publishing message 0/0.5") - self.pub.publish(msg) - rate.sleep() - msg.omega = 0.0 - rospy.loginfo("Publishing message 0/0.0") + msg.v = vel + msg.omega = omega self.pub.publish(msg) rate.sleep() sys.stdout.flush() + def image_cb(self, image_msg): + try: + image = self.bridge.compressed_imgmsg_to_cv2(image_msg) + self.cur_img = image + except ValueError as e: + self.logerr('Could not decode image: %s' % e) + return + print(image.shape) + if __name__ == '__main__': # create the node node = MyNode(node_name='circle_drive_node') diff --git a/packages/circle_drive/scripts/solution.py b/packages/circle_drive/scripts/solution.py new file mode 100644 index 00000000..ca5d57cb --- /dev/null +++ b/packages/circle_drive/scripts/solution.py @@ -0,0 +1,3 @@ + +def solution(obs): + return [0.2, 0.2] \ No newline at end of file From f2c84a40bfc958b02a7dde1aca9884ccf1f36e2d Mon Sep 17 00:00:00 2001 From: light5551 Date: Tue, 7 Dec 2021 10:09:26 +0300 Subject: [PATCH 02/15] add arm64 --- Dockerfile | 9 +-------- packages/circle_drive/scripts/solution.py | 2 +- 2 files changed, 2 insertions(+), 9 deletions(-) diff --git a/Dockerfile b/Dockerfile index c5e59c27..567d5fc1 100644 --- a/Dockerfile +++ b/Dockerfile @@ -3,14 +3,7 @@ ARG REPO_NAME="template-ros-core" # ==================================================> # ==> Do not change this code -ARG ARCH=arm32v7 -ARG MAJOR=daffy -ARG BASE_TAG=${MAJOR}-${ARCH} -ARG BASE_IMAGE=dt-core - -# define base image -FROM duckietown/${BASE_IMAGE}:${BASE_TAG} - +FROM light5551/ado_torch_template:arm64 # define repository path ARG REPO_NAME ARG REPO_PATH="${CATKIN_WS_DIR}/src/${REPO_NAME}" diff --git a/packages/circle_drive/scripts/solution.py b/packages/circle_drive/scripts/solution.py index ca5d57cb..8c849c1d 100644 --- a/packages/circle_drive/scripts/solution.py +++ b/packages/circle_drive/scripts/solution.py @@ -1,3 +1,3 @@ def solution(obs): - return [0.2, 0.2] \ No newline at end of file + return [0.0, 1] \ No newline at end of file From 0b6e7eb7bc84a6f116a1e4e5b1029518d29c24a9 Mon Sep 17 00:00:00 2001 From: light5551 Date: Tue, 7 Dec 2021 10:36:16 +0300 Subject: [PATCH 03/15] change to official image from OSLL --- Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Dockerfile b/Dockerfile index 567d5fc1..641de08c 100644 --- a/Dockerfile +++ b/Dockerfile @@ -3,7 +3,7 @@ ARG REPO_NAME="template-ros-core" # ==================================================> # ==> Do not change this code -FROM light5551/ado_torch_template:arm64 +FROM osll/ado_torch_template:arm64 # define repository path ARG REPO_NAME ARG REPO_PATH="${CATKIN_WS_DIR}/src/${REPO_NAME}" From 83f86e1a421adbdb1a0b11877ac17d1c5fa56b47 Mon Sep 17 00:00:00 2001 From: light5551 Date: Tue, 7 Dec 2021 10:57:49 +0300 Subject: [PATCH 04/15] fix --- Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Dockerfile b/Dockerfile index 641de08c..ef9203ad 100644 --- a/Dockerfile +++ b/Dockerfile @@ -3,7 +3,7 @@ ARG REPO_NAME="template-ros-core" # ==================================================> # ==> Do not change this code -FROM osll/ado_torch_template:arm64 +FROM osll/ado_torch_template # define repository path ARG REPO_NAME ARG REPO_PATH="${CATKIN_WS_DIR}/src/${REPO_NAME}" From 71f275b357e7b27258d28b65eac610404018ad72 Mon Sep 17 00:00:00 2001 From: light5551 Date: Tue, 7 Dec 2021 11:13:21 +0300 Subject: [PATCH 05/15] fix --- Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Dockerfile b/Dockerfile index ef9203ad..567d5fc1 100644 --- a/Dockerfile +++ b/Dockerfile @@ -3,7 +3,7 @@ ARG REPO_NAME="template-ros-core" # ==================================================> # ==> Do not change this code -FROM osll/ado_torch_template +FROM light5551/ado_torch_template:arm64 # define repository path ARG REPO_NAME ARG REPO_PATH="${CATKIN_WS_DIR}/src/${REPO_NAME}" From 56c2a344120bc562eb15a5c509275db7d6113723 Mon Sep 17 00:00:00 2001 From: Sergey Glazunov <31547459+light5551@users.noreply.github.com> Date: Tue, 7 Dec 2021 11:21:27 +0300 Subject: [PATCH 06/15] Update solution.py --- packages/circle_drive/scripts/solution.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/packages/circle_drive/scripts/solution.py b/packages/circle_drive/scripts/solution.py index 8c849c1d..b10179bd 100644 --- a/packages/circle_drive/scripts/solution.py +++ b/packages/circle_drive/scripts/solution.py @@ -1,3 +1,3 @@ def solution(obs): - return [0.0, 1] \ No newline at end of file + return [0.1, 1] From db2aa62cad7498a5607ae0ab238274573500f92e Mon Sep 17 00:00:00 2001 From: light5551 Date: Tue, 7 Dec 2021 11:43:17 +0300 Subject: [PATCH 07/15] from obs to action --- packages/circle_drive/scripts/circle_drive.py | 27 +++++++------------ 1 file changed, 10 insertions(+), 17 deletions(-) diff --git a/packages/circle_drive/scripts/circle_drive.py b/packages/circle_drive/scripts/circle_drive.py index 654ac248..dd1f7b5d 100755 --- a/packages/circle_drive/scripts/circle_drive.py +++ b/packages/circle_drive/scripts/circle_drive.py @@ -16,34 +16,27 @@ def __init__(self, node_name): self.bridge = CvBridge() self.cur_img = None self.sub_image = rospy.Subscriber( - "~image/compressed", + "~camera_node/image/compressed", + #"~image/compressed", CompressedImage, - self.image_cb, + self.action, buff_size=10000000, queue_size=1 ) - def run(self): - # publish message every 1 second - - rate = rospy.Rate(1) # 1Hz - while not rospy.is_shutdown(): - vel, omega = solution(self.cur_img) - msg = Twist2DStamped() - msg.v = vel - msg.omega = omega - self.pub.publish(msg) - rate.sleep() - sys.stdout.flush() - - def image_cb(self, image_msg): + def action(self, image_msg): try: image = self.bridge.compressed_imgmsg_to_cv2(image_msg) self.cur_img = image except ValueError as e: self.logerr('Could not decode image: %s' % e) return - print(image.shape) + vel, omega = solution(self.cur_img) + msg = Twist2DStamped() + msg.v = vel + msg.omega = omega + self.pub.publish(msg) + sys.stdout.flush() if __name__ == '__main__': # create the node From 6df3219144aba79a58e693bdbb237726fc947d0e Mon Sep 17 00:00:00 2001 From: light5551 Date: Tue, 7 Dec 2021 11:44:53 +0300 Subject: [PATCH 08/15] from obs to action --- packages/circle_drive/scripts/circle_drive.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/packages/circle_drive/scripts/circle_drive.py b/packages/circle_drive/scripts/circle_drive.py index dd1f7b5d..225b1deb 100755 --- a/packages/circle_drive/scripts/circle_drive.py +++ b/packages/circle_drive/scripts/circle_drive.py @@ -24,6 +24,9 @@ def __init__(self, node_name): queue_size=1 ) + def run(self): + pass + def action(self, image_msg): try: image = self.bridge.compressed_imgmsg_to_cv2(image_msg) From 19078e9fd4805fbf3c125f6f59ab2dbed581170b Mon Sep 17 00:00:00 2001 From: light5551 Date: Tue, 7 Dec 2021 11:47:20 +0300 Subject: [PATCH 09/15] from obs to action --- packages/circle_drive/scripts/circle_drive.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/packages/circle_drive/scripts/circle_drive.py b/packages/circle_drive/scripts/circle_drive.py index 225b1deb..91243a0d 100755 --- a/packages/circle_drive/scripts/circle_drive.py +++ b/packages/circle_drive/scripts/circle_drive.py @@ -16,7 +16,7 @@ def __init__(self, node_name): self.bridge = CvBridge() self.cur_img = None self.sub_image = rospy.Subscriber( - "~camera_node/image/compressed", + "autobot20/camera_node/image/compressed", #"~image/compressed", CompressedImage, self.action, @@ -26,11 +26,12 @@ def __init__(self, node_name): def run(self): pass - + def action(self, image_msg): try: image = self.bridge.compressed_imgmsg_to_cv2(image_msg) self.cur_img = image + print(self.cur_img.shape) except ValueError as e: self.logerr('Could not decode image: %s' % e) return From 90c6a83605d0c7f508547275760c290c5be29436 Mon Sep 17 00:00:00 2001 From: light5551 Date: Tue, 7 Dec 2021 11:48:08 +0300 Subject: [PATCH 10/15] from obs to action --- packages/circle_drive/scripts/circle_drive.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/packages/circle_drive/scripts/circle_drive.py b/packages/circle_drive/scripts/circle_drive.py index 91243a0d..1e194317 100755 --- a/packages/circle_drive/scripts/circle_drive.py +++ b/packages/circle_drive/scripts/circle_drive.py @@ -16,7 +16,7 @@ def __init__(self, node_name): self.bridge = CvBridge() self.cur_img = None self.sub_image = rospy.Subscriber( - "autobot20/camera_node/image/compressed", + "/autobot20/camera_node/image/compressed", #"~image/compressed", CompressedImage, self.action, From d98fdc6b423a134177a01c8e2db323759383f78f Mon Sep 17 00:00:00 2001 From: light5551 Date: Tue, 7 Dec 2021 11:53:34 +0300 Subject: [PATCH 11/15] fix --- packages/circle_drive/launch/circle_drive.launch | 1 + packages/circle_drive/scripts/circle_drive.py | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/packages/circle_drive/launch/circle_drive.launch b/packages/circle_drive/launch/circle_drive.launch index 4cde3d2d..b1d5e2f6 100644 --- a/packages/circle_drive/launch/circle_drive.launch +++ b/packages/circle_drive/launch/circle_drive.launch @@ -2,6 +2,7 @@ + diff --git a/packages/circle_drive/scripts/circle_drive.py b/packages/circle_drive/scripts/circle_drive.py index 1e194317..ae1b39d0 100755 --- a/packages/circle_drive/scripts/circle_drive.py +++ b/packages/circle_drive/scripts/circle_drive.py @@ -16,8 +16,8 @@ def __init__(self, node_name): self.bridge = CvBridge() self.cur_img = None self.sub_image = rospy.Subscriber( - "/autobot20/camera_node/image/compressed", - #"~image/compressed", + #"/autobot20/camera_node/image/compressed", + "~image/compressed", CompressedImage, self.action, buff_size=10000000, From 8887f4c14f9ad3c89848cdfc11cb78c0132e3ee3 Mon Sep 17 00:00:00 2001 From: light5551 Date: Tue, 7 Dec 2021 11:59:39 +0300 Subject: [PATCH 12/15] fix --- packages/circle_drive/launch/circle_drive.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/packages/circle_drive/launch/circle_drive.launch b/packages/circle_drive/launch/circle_drive.launch index b1d5e2f6..607b24e3 100644 --- a/packages/circle_drive/launch/circle_drive.launch +++ b/packages/circle_drive/launch/circle_drive.launch @@ -2,7 +2,7 @@ - + From 9869b0ff545994679f3808beb1315066323a4b99 Mon Sep 17 00:00:00 2001 From: light5551 Date: Tue, 7 Dec 2021 12:12:54 +0300 Subject: [PATCH 13/15] fix --- packages/circle_drive/launch/circle_drive.launch | 2 +- packages/circle_drive/scripts/circle_drive.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/packages/circle_drive/launch/circle_drive.launch b/packages/circle_drive/launch/circle_drive.launch index 607b24e3..1fb70de9 100644 --- a/packages/circle_drive/launch/circle_drive.launch +++ b/packages/circle_drive/launch/circle_drive.launch @@ -2,7 +2,7 @@ - + diff --git a/packages/circle_drive/scripts/circle_drive.py b/packages/circle_drive/scripts/circle_drive.py index ae1b39d0..0fb497e2 100755 --- a/packages/circle_drive/scripts/circle_drive.py +++ b/packages/circle_drive/scripts/circle_drive.py @@ -17,7 +17,7 @@ def __init__(self, node_name): self.cur_img = None self.sub_image = rospy.Subscriber( #"/autobot20/camera_node/image/compressed", - "~image/compressed", + "~image", CompressedImage, self.action, buff_size=10000000, From 5c723ebff8a31a3b912b862cb36c17fe28063492 Mon Sep 17 00:00:00 2001 From: light5551 Date: Tue, 7 Dec 2021 12:26:26 +0300 Subject: [PATCH 14/15] fix --- packages/circle_drive/scripts/circle_drive.py | 3 +-- packages/circle_drive/scripts/solution.py | 1 + 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/packages/circle_drive/scripts/circle_drive.py b/packages/circle_drive/scripts/circle_drive.py index 0fb497e2..76fadce0 100755 --- a/packages/circle_drive/scripts/circle_drive.py +++ b/packages/circle_drive/scripts/circle_drive.py @@ -20,7 +20,6 @@ def __init__(self, node_name): "~image", CompressedImage, self.action, - buff_size=10000000, queue_size=1 ) @@ -31,7 +30,7 @@ def action(self, image_msg): try: image = self.bridge.compressed_imgmsg_to_cv2(image_msg) self.cur_img = image - print(self.cur_img.shape) + except ValueError as e: self.logerr('Could not decode image: %s' % e) return diff --git a/packages/circle_drive/scripts/solution.py b/packages/circle_drive/scripts/solution.py index b10179bd..297e0ab0 100644 --- a/packages/circle_drive/scripts/solution.py +++ b/packages/circle_drive/scripts/solution.py @@ -1,3 +1,4 @@ def solution(obs): + print(obs.shape) return [0.1, 1] From 3f80bcede5cbc3e47a6f883ca8bfb4df55694d50 Mon Sep 17 00:00:00 2001 From: light5551 Date: Tue, 7 Dec 2021 12:35:52 +0300 Subject: [PATCH 15/15] fix --- packages/circle_drive/launch/circle_drive.launch | 2 +- packages/circle_drive/scripts/circle_drive.py | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/packages/circle_drive/launch/circle_drive.launch b/packages/circle_drive/launch/circle_drive.launch index 1fb70de9..90cfe775 100644 --- a/packages/circle_drive/launch/circle_drive.launch +++ b/packages/circle_drive/launch/circle_drive.launch @@ -2,7 +2,7 @@ - + diff --git a/packages/circle_drive/scripts/circle_drive.py b/packages/circle_drive/scripts/circle_drive.py index 76fadce0..e811159b 100755 --- a/packages/circle_drive/scripts/circle_drive.py +++ b/packages/circle_drive/scripts/circle_drive.py @@ -16,8 +16,8 @@ def __init__(self, node_name): self.bridge = CvBridge() self.cur_img = None self.sub_image = rospy.Subscriber( - #"/autobot20/camera_node/image/compressed", - "~image", + "/autobot20/camera_node/image/compressed", + #"~image", CompressedImage, self.action, queue_size=1