From 29054f72d4d3c9cb04a036b278b5d18e6914fe7d Mon Sep 17 00:00:00 2001
From: KonstantinKondratenko
<90711883+KonstantinKondratenko@users.noreply.github.com>
Date: Mon, 27 Feb 2023 22:40:39 +0300
Subject: [PATCH 01/24] change `default.launch`
---
packages/bot_camera/launch/default.launch | 1 +
1 file changed, 1 insertion(+)
diff --git a/packages/bot_camera/launch/default.launch b/packages/bot_camera/launch/default.launch
index cacd108..4ad2691 100644
--- a/packages/bot_camera/launch/default.launch
+++ b/packages/bot_camera/launch/default.launch
@@ -11,6 +11,7 @@
+
From 24dfd59c034266f2450d47e5a97653e6cb3e1860 Mon Sep 17 00:00:00 2001
From: KonstantinKondratenko
<90711883+KonstantinKondratenko@users.noreply.github.com>
Date: Mon, 27 Feb 2023 22:45:00 +0300
Subject: [PATCH 02/24] Changed bot_camera.py
---
packages/bot_camera/src/bot_camera.py | 82 ++++++++++-----------------
1 file changed, 29 insertions(+), 53 deletions(-)
diff --git a/packages/bot_camera/src/bot_camera.py b/packages/bot_camera/src/bot_camera.py
index 01347d8..f677a71 100755
--- a/packages/bot_camera/src/bot_camera.py
+++ b/packages/bot_camera/src/bot_camera.py
@@ -1,62 +1,38 @@
#!/usr/bin/env python3
-from typing import Optional, Any
-
-import cv2
-
+import sys
import rospy
+from duckietown.dtros import DTROS, NodeType
+from duckietown_msgs.msg import Twist2DStamped
-import numpy as np
-import os
-from sensor_msgs.msg import CompressedImage, Image
-from duckietown.dtros import DTROS, DTParam, NodeType, TopicType
-from dt_class_utils import DTReminder
-from turbojpeg import TurboJPEG
-from cv_bridge import CvBridge
-
-
-class BotCamera(DTROS):
- def __init__(self, node_name):
- super().__init__(node_name, node_type=NodeType.PERCEPTION)
-
- print(os.getcwd())
- # parameters
- self.publish_freq = DTParam("~publish_freq", -1)
+# std_msgs/Header header | не передано
+# float32 v | передано
+# float32 omega | передано
- # utility objects
- self.bridge = CvBridge()
- self.reminder = DTReminder(frequency=self.publish_freq.value)
- # subscribers
- self.sub_img = rospy.Subscriber(
- "~image_in", CompressedImage, self.cb_image, queue_size=1, buff_size="10MB"
- )
+class MyNode(DTROS):
- # publishers
- self.pub_img = rospy.Publisher(
- "~image_out",
- Image,
- queue_size=1,
- dt_topic_type=TopicType.PERCEPTION,
- dt_healthy_freq=self.publish_freq.value,
- dt_help="Raw image",
- )
-
- def cb_image(self, msg):
- # make sure this matters to somebody
-
- img = self.bridge.compressed_imgmsg_to_cv2(msg)
- img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
-
- # turn 'raw image' into 'raw image message'
- out_msg = self.bridge.cv2_to_imgmsg(img, "rgb8")
- # maintain original header
- out_msg.header = msg.header
- # publish image
- self.pub_img.publish(out_msg)
-
-
-if __name__ == "__main__":
- node = BotCamera("test_node")
+ 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)
+
+ def run(self):
+ rate = rospy.Rate(1) # 1 message per second
+ while (True):
+ msg = Twist2DStamped()
+ msg.v = 0.1
+ msg.omega = 1.0
+ rospy.loginfo("v;omega as 0.1;1.0")
+ self.pub.publish(msg)
+ rate.sleep()
+ sys.stdout.flush()
+
+
+if __name__ == '__main__':
+ # create the node
+ node = MyNode(node_name='circle_drive')
+ # run node
+ node.run()
+ # keep spinning
rospy.spin()
From 25af1a9a63d27e1be9b54c1c05bd9cc657f410c5 Mon Sep 17 00:00:00 2001
From: KonstantinKondratenko
<90711883+KonstantinKondratenko@users.noreply.github.com>
Date: Fri, 3 Mar 2023 16:38:24 +0300
Subject: [PATCH 03/24] First_version_parking
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
Бот паркуется к маркеру, если видет его, если не видит - стоит на месте и печатает сообщение, что не видит маркер.
Иногда (непонятно из-за чего) выдается ошибка :
================================================================================REQUIRED process [autobot08/bot_camera-1] has died!
process has died [pid 60, exit code -11, cmd /code/catkin_ws/src//packages/bot_camera/src/bot_camera.py bot_camera/car_cmd:=joy_mapper_node/car_cmd bot_camera/image_in:=camera_node/image/compressed bot_camera/image_out:=camera_node/image/test __name:=bot_camera __log:=/tmp/log/aa3682e6-b822-11ed-8e0d-dca63225c3f7/autobot08-bot_camera-1.log].
log file: /tmp/log/aa3682e6-b822-11ed-8e0d-dca63225c3f7/autobot08-bot_camera-1*.log
Initiating shutdown!
================================================================================
[autobot08/bot_camera-1] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done
<== App terminated!
__
Рисуется точка середина экрана (фиолетовая), однако она смещена относительно визуально наблюдаемого центра влево и вниз.
Бот подъезжает к маркеру, пока не перестает распознавать его, тут 2 случая могут быть:
1) бот подъехал ровно к маркеру, так близко, что весь маркер не помещается в камеру и бот останаваливается
2) бот не может повернуть на необходимый угол, подъезжает к маркеру под болшим углом, перестает распознавать маркер и останавливается.
---
packages/bot_camera/src/bot_camera.py | 213 ++++++++++++++++++++++----
1 file changed, 187 insertions(+), 26 deletions(-)
diff --git a/packages/bot_camera/src/bot_camera.py b/packages/bot_camera/src/bot_camera.py
index f677a71..126c18c 100755
--- a/packages/bot_camera/src/bot_camera.py
+++ b/packages/bot_camera/src/bot_camera.py
@@ -1,38 +1,199 @@
#!/usr/bin/env python3
+from typing import Optional, Any
+import cv2
import sys
import rospy
-from duckietown.dtros import DTROS, NodeType
+import numpy as np
+import os
+from sensor_msgs.msg import CompressedImage, Image
+from duckietown.dtros import DTROS, DTParam, NodeType, TopicType
+from dt_class_utils import DTReminder
+from turbojpeg import TurboJPEG
+from cv_bridge import CvBridge
+from dt_apriltags import Detector
from duckietown_msgs.msg import Twist2DStamped
-# std_msgs/Header header | не передано
-# float32 v | передано
-# float32 omega | передано
+class BotCamera(DTROS):
+ def __init__(self, node_name):
+ super().__init__(node_name, node_type=NodeType.PERCEPTION)
+ self.pub = rospy.Publisher("~car_cmd", Twist2DStamped, queue_size=1)
+ print(os.getcwd())
+ # parameters
+ self.publish_freq = DTParam("~publish_freq", -1)
+ # utility objects
+ self.bridge = CvBridge()
+ self.reminder = DTReminder(frequency=self.publish_freq.value)
+ # subscribers
+ self.sub_img = rospy.Subscriber(
+ "~image_in", CompressedImage, self.cb_image, queue_size=1, buff_size="10MB"
+ )
+ # publishers
+ self.pub_img = rospy.Publisher(
+ "~image_out",
+ Image,
+ queue_size=1,
+ dt_topic_type=TopicType.PERCEPTION,
+ dt_healthy_freq=self.publish_freq.value,
+ dt_help="Raw image",
+ )
+ def cb_image(self, msg):
+ # make sure this matters to somebody
-class MyNode(DTROS):
+ img = self.bridge.compressed_imgmsg_to_cv2(msg)
+ img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
+
+ # track the contour of detected markers
+ img = self.marker_detecting(img)
+
+ # turn 'raw image' into 'raw image message'
+ out_msg = self.bridge.cv2_to_imgmsg(img, "rgb8")
+ # maintain original header
+ out_msg.header = msg.header
+ # publish image
+ self.pub_img.publish(out_msg)
+
+ def marker_detecting(self, in_image):
+ # more info about the dt-apriltag package you can see here:
+ # https://github.com/duckietown/lib-dt-apriltags
+
+ # init AprilTag detector
+ tag_detector = Detector(families="tag36h11",
+ nthreads=1,
+ quad_decimate=2.0,
+ quad_sigma=0.0,
+ refine_edges=1,
+ decode_sharpening=0.25,
+ debug=0)
+
+ gray_img = cv2.cvtColor(in_image, cv2.COLOR_RGB2GRAY)
+ x_center_image = gray_img.shape[0]
+ x_center_image = x_center_image//2
+ #cv2.circle(in_image, tuple(gray_img.shape[0]//2, gray_img.shape[1]//2), 3, (0, 255, 255), -1)
+ #tuple(map(int, tag.center))
+ cv2.circle(in_image, tuple(map(int, [gray_img.shape[0]//2, gray_img.shape[1]//2])), 4, (222, 0, 255), -1)
+ tags = tag_detector.detect(gray_img)
+ if len(tags) == 0:
+ self.cant_find()
+
+ for tag in tags:
+ (topLeft, topRight, bottomRight, bottomLeft) = tag.corners
+
+ topRight = (int(topRight[0]), int(topRight[1]))
+ bottomRight = (int(bottomRight[0]), int(bottomRight[1]))
+ bottomLeft = (int(bottomLeft[0]), int(bottomLeft[1]))
+ topLeft = (int(topLeft[0]), int(topLeft[1]))
+
+ # draw the bounding box
+ line_width = 2
+ line_color = (0, 255, 0)
+ cv2.line(in_image, topLeft, topRight, line_color, line_width)
+ cv2.line(in_image, topRight, bottomRight, line_color, line_width)
+ cv2.line(in_image, bottomRight, bottomLeft, line_color, line_width)
+ cv2.line(in_image, bottomLeft, topLeft, line_color, line_width)
+
+ # draw the center of marker
+ cv2.circle(in_image, tuple(map(int, tag.center)), 2, (0, 0, 255), -1)
+ # parking
+ coordinates = tuple(map(int, tag.center))
+
+ x_center_marker = coordinates[0]
+ #x_center_marker = int(topLeft[0]) + (int(topRight[0]) - int(topLeft[0])) // 2
+ rospy.loginfo("find code")
+
+
+ if x_center_image > x_center_marker:
+ #rospy.loginfo("turn left: center-marker =", x_center_marker, "\t center-image =", x_center_image)
+ rospy.loginfo("\tgo left")
+ self.turn_left()
+
+
+ elif x_center_image <= x_center_marker:
+ #rospy.loginfo("turn right: center-marker =", x_center_marker, "\t center-image =", x_center_image)
+ rospy.loginfo("\tgo right")
+ self.turn_righ()
+
+ # else:
+ # rospy.loginfo("go straight")
+ # self.go_straight()
+
+ # draw the marker ID on the image
+ cv2.putText(in_image, str(tag.tag_id), org=(topLeft[0], topLeft[1] - 15),
+ fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=0.5, color=(255, 0, 0))
+
+ return in_image
+
+ def turn_righ(self):
+ rate = rospy.Rate(30)
+ msg = Twist2DStamped()
+ msg.v = 0.1
+ msg.omega = -1
+ rospy.loginfo("\t\tturning right befor publishing msg")
+ self.pub.publish(msg)
+ rospy.loginfo("\t\tturning right after publishing msg")
+ rate.sleep()
+ sys.stdout.flush()
+ pass
+
+ def turn_left(self):
+ rate = rospy.Rate(30)
+ msg = Twist2DStamped()
+ msg.v = 0.1
+ msg.omega = 1
+ rospy.loginfo("\t\tturning left befor publishing msg")
+ self.pub.publish(msg)
+ rospy.loginfo("\t\tturning left after publishing msg")
+ rate.sleep()
+ sys.stdout.flush()
+ pass
+
+ # def go_straight(self):
+ # rate = rospy.Rate(20)
+ # msg = Twist2DStamped()
+ # msg.v = 0.2
+ # msg.omega = 0
+ # rospy.loginfo("going straight")
+ # self.pub.publish(msg)
+ # rate.sleep()
+ # sys.stdout.flush()
+ # pass
+
+ def cant_find(self):
+ rate = rospy.Rate(5)
+ msg = Twist2DStamped()
+ msg.v = 0
+ msg.omega = 0
+ rospy.loginfo("\t\tcant find code befor publishing")
+ self.pub.publish(msg)
+ rospy.loginfo("\t\tcant find code after publishing")
+ rate.sleep()
+ sys.stdout.flush()
+ pass
+
+ # def run(self):
+ # rate = rospy.Rate(1) # 1 message per second
+ # a = "NOT_STOP"
+ # if a == "stop":
+ # msg = Twist2DStamped()
+ # msg.v = 0
+ # msg.omega = 0
+ # rospy.loginfo("STOPING")
+ # self.pub.publish(msg)
+ # rate.sleep()
+ # sys.stdout.flush()
+ # else:
+ # msg = Twist2DStamped()
+ # msg.v = 0
+ # msg.omega = 1.0
+ # rospy.loginfo("v;omega as 0;1.0")
+ # self.pub.publish(msg)
+ # rate.sleep()
+ # sys.stdout.flush()
- 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)
- def run(self):
- rate = rospy.Rate(1) # 1 message per second
- while (True):
- msg = Twist2DStamped()
- msg.v = 0.1
- msg.omega = 1.0
- rospy.loginfo("v;omega as 0.1;1.0")
- self.pub.publish(msg)
- rate.sleep()
- sys.stdout.flush()
-
-
-if __name__ == '__main__':
- # create the node
- node = MyNode(node_name='circle_drive')
- # run node
- node.run()
- # keep spinning
+if __name__ == "__main__":
+ node_cmd = BotCamera("test_node")
+ # node_cmd.run()
rospy.spin()
From e90a2c4ba624a25186a3790938ef86a79df8ae1e Mon Sep 17 00:00:00 2001
From: KonstantinKondratenko
<90711883+KonstantinKondratenko@users.noreply.github.com>
Date: Mon, 13 Mar 2023 22:50:17 +0300
Subject: [PATCH 04/24] update bot_camera node
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
Добавлена обработка конкретного маркера (строка 95 -- надо обозначать id маркера).
Добавлен в подписки топик, созданный Александром(там булевые значения -- служит для передачи информации с кнопоком джойстика); изображение обрабатывается только тогда, когда получено указание обработки из топика.
В 32 строке изменен buff_size: раньше принимал строку "10 MB", теперь передается число, эквивалентное 10 мегабайтам.
Реализованы методы поворотов налево, направо, езды назад, в некоторых из них используется sleep для приостановки работы ноды (иначе бот не успевает отъехать на достаточное расстояние или теряет маркер прежде стыковки с клемами зарядной станции).
Ошибка, при которой процесс умирает не исправлена. Есть предположение, что это связано либо с памятию, либо с типом с сообщением, передаваемым car_cmd (не передается std_msgs/Header header).
Реализован метод message_print, публикующий соощение в топик управления ботом, -- создан для того, чтобы код выглядел чище.
---
packages/bot_camera/src/bot_camera.py | 251 ++++++++++++--------------
1 file changed, 120 insertions(+), 131 deletions(-)
diff --git a/packages/bot_camera/src/bot_camera.py b/packages/bot_camera/src/bot_camera.py
index 126c18c..ca7a02a 100755
--- a/packages/bot_camera/src/bot_camera.py
+++ b/packages/bot_camera/src/bot_camera.py
@@ -1,5 +1,5 @@
#!/usr/bin/env python3
-
+
from typing import Optional, Any
import cv2
import sys
@@ -9,14 +9,17 @@
from sensor_msgs.msg import CompressedImage, Image
from duckietown.dtros import DTROS, DTParam, NodeType, TopicType
from dt_class_utils import DTReminder
-from turbojpeg import TurboJPEG
+#from turbojpeg import TurboJPEG
from cv_bridge import CvBridge
from dt_apriltags import Detector
-from duckietown_msgs.msg import Twist2DStamped
-
-
+from duckietown_msgs.msg import Twist2DStamped, BoolStamped
+
+
+# temp tag id == 20
+
class BotCamera(DTROS):
def __init__(self, node_name):
+
super().__init__(node_name, node_type=NodeType.PERCEPTION)
self.pub = rospy.Publisher("~car_cmd", Twist2DStamped, queue_size=1)
print(os.getcwd())
@@ -26,9 +29,8 @@ def __init__(self, node_name):
self.bridge = CvBridge()
self.reminder = DTReminder(frequency=self.publish_freq.value)
# subscribers
- self.sub_img = rospy.Subscriber(
- "~image_in", CompressedImage, self.cb_image, queue_size=1, buff_size="10MB"
- )
+ self.sub_img = rospy.Subscriber("~image_in", CompressedImage, self.cb_image, queue_size=1, buff_size=10485760)
+ self.sub_start_parking = rospy.Subscriber("~parking_start", BoolStamped, self.parking_start, queue_size=1)
# publishers
self.pub_img = rospy.Publisher(
"~image_out",
@@ -38,28 +40,41 @@ def __init__(self, node_name):
dt_healthy_freq=self.publish_freq.value,
dt_help="Raw image",
)
-
+ # logik helper
+ self.bool_start = False
+ self.last_command = "None"
+ self.back_riding_counter = 0
+ self.cant_find_counter = 0
+
+
+ def parking_start(self, msg):
+ if msg.data == True:
+ self.bool_start = True
+ elif msg.data == False:
+ self.bool_start = False
+ else:
+ rospy.loginfo("Error of Booltype in topik \'parking_start\'")
+
def cb_image(self, msg):
# make sure this matters to somebody
-
img = self.bridge.compressed_imgmsg_to_cv2(msg)
img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
-
# track the contour of detected markers
- img = self.marker_detecting(img)
-
+ if self.bool_start:
+ img = self.marker_detecting(img)
# turn 'raw image' into 'raw image message'
out_msg = self.bridge.cv2_to_imgmsg(img, "rgb8")
# maintain original header
out_msg.header = msg.header
# publish image
self.pub_img.publish(out_msg)
-
+
def marker_detecting(self, in_image):
# more info about the dt-apriltag package you can see here:
# https://github.com/duckietown/lib-dt-apriltags
-
+
# init AprilTag detector
+
tag_detector = Detector(families="tag36h11",
nthreads=1,
quad_decimate=2.0,
@@ -67,133 +82,107 @@ def marker_detecting(self, in_image):
refine_edges=1,
decode_sharpening=0.25,
debug=0)
-
gray_img = cv2.cvtColor(in_image, cv2.COLOR_RGB2GRAY)
- x_center_image = gray_img.shape[0]
- x_center_image = x_center_image//2
- #cv2.circle(in_image, tuple(gray_img.shape[0]//2, gray_img.shape[1]//2), 3, (0, 255, 255), -1)
- #tuple(map(int, tag.center))
- cv2.circle(in_image, tuple(map(int, [gray_img.shape[0]//2, gray_img.shape[1]//2])), 4, (222, 0, 255), -1)
+ x_center_image = in_image.shape[1]
+ x_center_image = x_center_image // 2
+ cv2.circle(in_image, tuple(map(int, [gray_img.shape[1] // 2, gray_img.shape[0] // 2])), 4, (255, 255, 0),
+ -1) # drow centre
tags = tag_detector.detect(gray_img)
if len(tags) == 0:
self.cant_find()
-
+ return in_image #тут бот упал 1 или 2 раза
for tag in tags:
- (topLeft, topRight, bottomRight, bottomLeft) = tag.corners
-
- topRight = (int(topRight[0]), int(topRight[1]))
- bottomRight = (int(bottomRight[0]), int(bottomRight[1]))
- bottomLeft = (int(bottomLeft[0]), int(bottomLeft[1]))
- topLeft = (int(topLeft[0]), int(topLeft[1]))
-
- # draw the bounding box
- line_width = 2
- line_color = (0, 255, 0)
- cv2.line(in_image, topLeft, topRight, line_color, line_width)
- cv2.line(in_image, topRight, bottomRight, line_color, line_width)
- cv2.line(in_image, bottomRight, bottomLeft, line_color, line_width)
- cv2.line(in_image, bottomLeft, topLeft, line_color, line_width)
-
- # draw the center of marker
- cv2.circle(in_image, tuple(map(int, tag.center)), 2, (0, 0, 255), -1)
- # parking
- coordinates = tuple(map(int, tag.center))
-
- x_center_marker = coordinates[0]
- #x_center_marker = int(topLeft[0]) + (int(topRight[0]) - int(topLeft[0])) // 2
- rospy.loginfo("find code")
-
-
- if x_center_image > x_center_marker:
- #rospy.loginfo("turn left: center-marker =", x_center_marker, "\t center-image =", x_center_image)
- rospy.loginfo("\tgo left")
- self.turn_left()
-
-
- elif x_center_image <= x_center_marker:
- #rospy.loginfo("turn right: center-marker =", x_center_marker, "\t center-image =", x_center_image)
- rospy.loginfo("\tgo right")
- self.turn_righ()
-
- # else:
- # rospy.loginfo("go straight")
- # self.go_straight()
-
- # draw the marker ID on the image
- cv2.putText(in_image, str(tag.tag_id), org=(topLeft[0], topLeft[1] - 15),
- fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=0.5, color=(255, 0, 0))
-
+ if tag.tag_id == 20: # there gotta be special value
+ self.cant_find_counter = 0
+ # self.back_riding_counter = 0
+ (topLeft, topRight, bottomRight, bottomLeft) = tag.corners
+ topRight = (int(topRight[0]), int(topRight[1]))
+ bottomRight = (int(bottomRight[0]), int(bottomRight[1]))
+ bottomLeft = (int(bottomLeft[0]), int(bottomLeft[1]))
+ topLeft = (int(topLeft[0]), int(topLeft[1]))
+ # draw the bounding box
+ line_width = 2
+ line_color = (0, 255, 0)
+ cv2.line(in_image, topLeft, topRight, line_color, line_width)
+ cv2.line(in_image, topRight, bottomRight, line_color, line_width)
+ cv2.line(in_image, bottomRight, bottomLeft, line_color, line_width)
+ cv2.line(in_image, bottomLeft, topLeft, line_color, line_width)
+ # draw the center of marker
+ cv2.circle(in_image, tuple(map(int, tag.center)), 2, (0, 0, 255), -1)
+ # parking
+ coordinates = tuple(map(int, tag.center))
+ x_center_marker = coordinates[0]
+ # x_center_marker = int(topLeft[0]) + (int(topRight[0]) - int(topLeft[0])) // 2
+ if x_center_image > x_center_marker:
+ rospy.loginfo("\tgo left")
+ self.turn_left()
+
+ elif x_center_image <= x_center_marker:
+ rospy.loginfo("\tgo right")
+ self.turn_right()
+
+ # draw the marker ID on the image
+ cv2.putText(in_image, str(tag.tag_id), org=(topLeft[0], topLeft[1] - 15),
+ fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=0.5, color=(255, 0, 0))
+
return in_image
-
- def turn_righ(self):
- rate = rospy.Rate(30)
- msg = Twist2DStamped()
- msg.v = 0.1
- msg.omega = -1
- rospy.loginfo("\t\tturning right befor publishing msg")
- self.pub.publish(msg)
- rospy.loginfo("\t\tturning right after publishing msg")
- rate.sleep()
- sys.stdout.flush()
- pass
-
+
+ def turn_right(self):
+ self.last_command = "Right"
+ self.message_print(0.1, -1, "\t\tturning right")
+
def turn_left(self):
- rate = rospy.Rate(30)
+ self.last_command = "Left"
+ self.message_print(0.1, 1, "\t\tturning left")
+
+ def message_print(self, v, omega, message):
msg = Twist2DStamped()
- msg.v = 0.1
- msg.omega = 1
- rospy.loginfo("\t\tturning left befor publishing msg")
+ msg.v = v
+ msg.omega = omega
+ rospy.loginfo(message)
self.pub.publish(msg)
- rospy.loginfo("\t\tturning left after publishing msg")
- rate.sleep()
sys.stdout.flush()
- pass
-
- # def go_straight(self):
- # rate = rospy.Rate(20)
- # msg = Twist2DStamped()
- # msg.v = 0.2
- # msg.omega = 0
- # rospy.loginfo("going straight")
- # self.pub.publish(msg)
- # rate.sleep()
- # sys.stdout.flush()
- # pass
-
+
+
def cant_find(self):
- rate = rospy.Rate(5)
- msg = Twist2DStamped()
- msg.v = 0
- msg.omega = 0
- rospy.loginfo("\t\tcant find code befor publishing")
- self.pub.publish(msg)
- rospy.loginfo("\t\tcant find code after publishing")
- rate.sleep()
- sys.stdout.flush()
- pass
-
- # def run(self):
- # rate = rospy.Rate(1) # 1 message per second
- # a = "NOT_STOP"
- # if a == "stop":
- # msg = Twist2DStamped()
- # msg.v = 0
- # msg.omega = 0
- # rospy.loginfo("STOPING")
- # self.pub.publish(msg)
- # rate.sleep()
- # sys.stdout.flush()
- # else:
- # msg = Twist2DStamped()
- # msg.v = 0
- # msg.omega = 1.0
- # rospy.loginfo("v;omega as 0;1.0")
- # self.pub.publish(msg)
- # rate.sleep()
- # sys.stdout.flush()
-
-
+ if self.back_riding_counter == 0:
+ self.back_riding_counter += 1
+ self.message_print(0, 0, "\tstopping bot before back riding")
+ rospy.sleep(0.1)
+ self.message_print(0.3, 0, "\ttry to connect")
+ rospy.sleep(0.2)
+
+ self.revers_riding()
+
+ elif self.back_riding_counter == 1:
+ self.back_riding_counter += 1
+ self.revers_riding()
+
+ else:
+ rospy.sleep(1)
+ self.back_riding_counter = 0
+ self.message_print(0, 0, "\tstop back riding")
+
+
+ def revers_riding(self):
+ if self.cant_find_counter < 5:
+ if self.last_command == "None":
+ self.message_print(0, 1, "\t\tThinks that's can be in start")
+ elif self.last_command == "Right":
+ self.message_print(-0.5, -1, "\t\tBack riding turning right")
+ rospy.sleep(0.3)
+ self.cant_find_counter += 1
+ elif self.last_command == "Left":
+ self.message_print(-0.5, 1, "\t\tBack riding turning left")
+ rospy.sleep(0.3)
+ self.cant_find_counter += 1
+ else:
+ self.message_print(0, 0, "\tError last_command value!")
+ else:
+ rospy.sleep(1)
+ self.message_print(0, 2, "\tStop plastic pollution!")
+
if __name__ == "__main__":
node_cmd = BotCamera("test_node")
- # node_cmd.run()
rospy.spin()
+
From e09cc2abf1145c5c4d3950496c3a7a5b6c2e997b Mon Sep 17 00:00:00 2001
From: KonstantinKondratenko
<90711883+KonstantinKondratenko@users.noreply.github.com>
Date: Fri, 24 Mar 2023 17:56:41 +0300
Subject: [PATCH 05/24] Update Dockerfile
---
Dockerfile | 21 +++++++++++++++------
1 file changed, 15 insertions(+), 6 deletions(-)
diff --git a/Dockerfile b/Dockerfile
index f23a09d..20817ae 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -1,5 +1,5 @@
# parameters
-ARG REPO_NAME=""
+ARG REPO_NAME="dt-core"
ARG DESCRIPTION="Base image containing common libraries and environment setup for ROS applications."
ARG MAINTAINER=" ()"
ARG ICON="cube"
@@ -7,9 +7,9 @@ ARG ICON="cube"
# ==================================================>
# ==> Do not change the code below this line
ARG ARCH=arm64v8
-ARG DISTRO=ente
+ARG DISTRO=daffy
ARG BASE_TAG=${DISTRO}-${ARCH}
-ARG BASE_IMAGE=dt-ros-commons
+ARG BASE_IMAGE=dt-core
ARG LAUNCHER=default
# define base image
@@ -27,6 +27,7 @@ ARG BASE_TAG
ARG BASE_IMAGE
ARG LAUNCHER
+
# check build arguments
RUN dt-build-env-check "${REPO_NAME}" "${MAINTAINER}" "${DESCRIPTION}"
@@ -46,13 +47,21 @@ ENV DT_REPO_PATH "${REPO_PATH}"
ENV DT_LAUNCH_PATH "${LAUNCH_PATH}"
ENV DT_LAUNCHER "${LAUNCHER}"
+
+
+# mount dirs
+#COPY /usr/lib/python2.7/dist-packages "${REPO_PATH}/packages"
+
# install apt dependencies
+RUN apt-key adv --keyserver keyserver.ubuntu.com --recv-keys F42ED6FBAB17C654
COPY ./dependencies-apt.txt "${REPO_PATH}/"
RUN dt-apt-install ${REPO_PATH}/dependencies-apt.txt
-# install python3 dependencies
-#COPY ./dependencies-py3.txt "${REPO_PATH}/"
-#RUN dt-pip3-install ${REPO_PATH}/dependencies-py3.txt
+RUN pip uninstall dt-apriltag
+COPY ./dependencies-py3.txt "${REPO_PATH}/"
+RUN dt-pip3-install ${REPO_PATH}/dependencies-py3.txt
+
+
# copy the source code
COPY ./packages "${REPO_PATH}/packages"
From e4b6e7e8e5aeea8b3f44c5ed762dc8f7278e5c72 Mon Sep 17 00:00:00 2001
From: KonstantinKondratenko
<90711883+KonstantinKondratenko@users.noreply.github.com>
Date: Fri, 24 Mar 2023 18:01:51 +0300
Subject: [PATCH 06/24] add apriltag
---
dependencies-py3.txt | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/dependencies-py3.txt b/dependencies-py3.txt
index e647c04..513878b 100644
--- a/dependencies-py3.txt
+++ b/dependencies-py3.txt
@@ -1,3 +1,3 @@
-
+apriltag
flask
From c0fea59d00a90e2ab50a73780bca692ed7e6bb37 Mon Sep 17 00:00:00 2001
From: KonstantinKondratenko
<90711883+KonstantinKondratenko@users.noreply.github.com>
Date: Fri, 24 Mar 2023 18:07:50 +0300
Subject: [PATCH 07/24] Fix bug of detection
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
Исправлена ошибка детекции маркера (процесс не умирает)
Убрана отрисовка изображения, убран Publisher.
Добавлен функционал просмотра детекции зарядки. закомментирован и помечен "#@uncomment"
Возникает ошибка: ModuleNotFoundError: No module named 'RPi'
Путь к библиотеке на боте: /usr/lib/python2.7/dist-packages
Попытался запустить через "dts devel run -f -H autobot04 -M /usr/lib/python2.7/dist-packages"
Не получилось, ошибка:
dts : The path '/usr/lib/python2.7/dist-packages' does not appear to be a Duckietown project.
: The metadata file '.dtproject' is missing.
WARNING:dts:Please update "pip" to have better debu
---
packages/bot_camera/src/bot_camera.py | 155 ++++++++++----------------
1 file changed, 57 insertions(+), 98 deletions(-)
diff --git a/packages/bot_camera/src/bot_camera.py b/packages/bot_camera/src/bot_camera.py
index ca7a02a..0dda46f 100755
--- a/packages/bot_camera/src/bot_camera.py
+++ b/packages/bot_camera/src/bot_camera.py
@@ -1,5 +1,5 @@
#!/usr/bin/env python3
-
+
from typing import Optional, Any
import cv2
import sys
@@ -8,142 +8,102 @@
import os
from sensor_msgs.msg import CompressedImage, Image
from duckietown.dtros import DTROS, DTParam, NodeType, TopicType
-from dt_class_utils import DTReminder
-#from turbojpeg import TurboJPEG
from cv_bridge import CvBridge
-from dt_apriltags import Detector
+import apriltag # from dt_apriltags import Detector
from duckietown_msgs.msg import Twist2DStamped, BoolStamped
-
-
-# temp tag id == 20
-
+#import RPi.GPIO as GPIO #@uncomment
+
class BotCamera(DTROS):
def __init__(self, node_name):
-
super().__init__(node_name, node_type=NodeType.PERCEPTION)
self.pub = rospy.Publisher("~car_cmd", Twist2DStamped, queue_size=1)
- print(os.getcwd())
- # parameters
- self.publish_freq = DTParam("~publish_freq", -1)
- # utility objects
self.bridge = CvBridge()
- self.reminder = DTReminder(frequency=self.publish_freq.value)
# subscribers
- self.sub_img = rospy.Subscriber("~image_in", CompressedImage, self.cb_image, queue_size=1, buff_size=10485760)
+ self.sub_img = rospy.Subscriber("~image_in", CompressedImage, self.cb_image, queue_size=1, buff_size="100MB")
self.sub_start_parking = rospy.Subscriber("~parking_start", BoolStamped, self.parking_start, queue_size=1)
# publishers
- self.pub_img = rospy.Publisher(
- "~image_out",
- Image,
- queue_size=1,
- dt_topic_type=TopicType.PERCEPTION,
- dt_healthy_freq=self.publish_freq.value,
- dt_help="Raw image",
- )
+
# logik helper
self.bool_start = False
self.last_command = "None"
self.back_riding_counter = 0
self.cant_find_counter = 0
-
-
+
+ self.is_conected = False
+
def parking_start(self, msg):
+ rospy.loginfo("INIT park")
+
if msg.data == True:
self.bool_start = True
elif msg.data == False:
self.bool_start = False
else:
rospy.loginfo("Error of Booltype in topik \'parking_start\'")
-
+
+ # @uncomment
+ # def get_connection_status(self):
+ # BUTTON_GPIO = 24
+ # GPIO.setmode(GPIO.BCM)
+ # GPIO.setup(BUTTON_GPIO, GPIO.IN)
+ # gpio_state = GPIO.input(BUTTON_GPIO)
+ # if gpio_state:
+ # self.is_conected = True
+ # self.message_print(0, 0, "Get connection!")
+ # else:
+ # self.is_conected = False
def cb_image(self, msg):
- # make sure this matters to somebody
img = self.bridge.compressed_imgmsg_to_cv2(msg)
img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
- # track the contour of detected markers
- if self.bool_start:
- img = self.marker_detecting(img)
- # turn 'raw image' into 'raw image message'
- out_msg = self.bridge.cv2_to_imgmsg(img, "rgb8")
- # maintain original header
- out_msg.header = msg.header
- # publish image
- self.pub_img.publish(out_msg)
-
+ # @uncomment
+ # self.get_conection_status()
+ if self.bool_start and not self.is_conected:
+ self.marker_detecting(img)
+
def marker_detecting(self, in_image):
- # more info about the dt-apriltag package you can see here:
- # https://github.com/duckietown/lib-dt-apriltags
-
- # init AprilTag detector
-
- tag_detector = Detector(families="tag36h11",
- nthreads=1,
- quad_decimate=2.0,
- quad_sigma=0.0,
- refine_edges=1,
- decode_sharpening=0.25,
- debug=0)
- gray_img = cv2.cvtColor(in_image, cv2.COLOR_RGB2GRAY)
- x_center_image = in_image.shape[1]
- x_center_image = x_center_image // 2
- cv2.circle(in_image, tuple(map(int, [gray_img.shape[1] // 2, gray_img.shape[0] // 2])), 4, (255, 255, 0),
- -1) # drow centre
- tags = tag_detector.detect(gray_img)
+
+ rospy.loginfo("start detection")
+
+ options = apriltag.DetectorOptions(families="tag36h11")
+ detector = apriltag.Detector(options)
+ gray_img = cv2.cvtColor(in_image, cv2.COLOR_RGB2GRAY) # при комменте не падаем
+ x_center_image = in_image.shape[1] // 2
+ tags = detector.detect(gray_img)
+ rospy.loginfo("after getting tags")
if len(tags) == 0:
self.cant_find()
- return in_image #тут бот упал 1 или 2 раза
+ return # тут бот упал
for tag in tags:
if tag.tag_id == 20: # there gotta be special value
+ rospy.loginfo("in tags")
self.cant_find_counter = 0
- # self.back_riding_counter = 0
- (topLeft, topRight, bottomRight, bottomLeft) = tag.corners
- topRight = (int(topRight[0]), int(topRight[1]))
- bottomRight = (int(bottomRight[0]), int(bottomRight[1]))
- bottomLeft = (int(bottomLeft[0]), int(bottomLeft[1]))
- topLeft = (int(topLeft[0]), int(topLeft[1]))
- # draw the bounding box
- line_width = 2
- line_color = (0, 255, 0)
- cv2.line(in_image, topLeft, topRight, line_color, line_width)
- cv2.line(in_image, topRight, bottomRight, line_color, line_width)
- cv2.line(in_image, bottomRight, bottomLeft, line_color, line_width)
- cv2.line(in_image, bottomLeft, topLeft, line_color, line_width)
- # draw the center of marker
- cv2.circle(in_image, tuple(map(int, tag.center)), 2, (0, 0, 255), -1)
- # parking
+ self.back_riding_counter = 0
coordinates = tuple(map(int, tag.center))
x_center_marker = coordinates[0]
- # x_center_marker = int(topLeft[0]) + (int(topRight[0]) - int(topLeft[0])) // 2
if x_center_image > x_center_marker:
rospy.loginfo("\tgo left")
self.turn_left()
-
elif x_center_image <= x_center_marker:
rospy.loginfo("\tgo right")
self.turn_right()
-
- # draw the marker ID on the image
- cv2.putText(in_image, str(tag.tag_id), org=(topLeft[0], topLeft[1] - 15),
- fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=0.5, color=(255, 0, 0))
-
- return in_image
-
+ rospy.loginfo("before return from detecting") # бот тут падает(
+ return
+
def turn_right(self):
self.last_command = "Right"
self.message_print(0.1, -1, "\t\tturning right")
-
+
def turn_left(self):
self.last_command = "Left"
self.message_print(0.1, 1, "\t\tturning left")
-
+
def message_print(self, v, omega, message):
msg = Twist2DStamped()
msg.v = v
msg.omega = omega
rospy.loginfo(message)
self.pub.publish(msg)
- sys.stdout.flush()
-
-
+
def cant_find(self):
if self.back_riding_counter == 0:
self.back_riding_counter += 1
@@ -151,38 +111,37 @@ def cant_find(self):
rospy.sleep(0.1)
self.message_print(0.3, 0, "\ttry to connect")
rospy.sleep(0.2)
-
+
self.revers_riding()
-
+
elif self.back_riding_counter == 1:
self.back_riding_counter += 1
self.revers_riding()
-
+
else:
rospy.sleep(1)
self.back_riding_counter = 0
self.message_print(0, 0, "\tstop back riding")
-
-
+
def revers_riding(self):
- if self.cant_find_counter < 5:
+ if self.cant_find_counter < 3:
if self.last_command == "None":
- self.message_print(0, 1, "\t\tThinks that's can be in start")
+ self.message_print(0, 1, "\t\tStart; try to find marker")
elif self.last_command == "Right":
- self.message_print(-0.5, -1, "\t\tBack riding turning right")
+ self.message_print(-0.3, -1, "\t\tBack riding turning right")
rospy.sleep(0.3)
self.cant_find_counter += 1
elif self.last_command == "Left":
- self.message_print(-0.5, 1, "\t\tBack riding turning left")
+ self.message_print(-0.3, 1, "\t\tBack riding turning left")
rospy.sleep(0.3)
self.cant_find_counter += 1
else:
self.message_print(0, 0, "\tError last_command value!")
else:
rospy.sleep(1)
- self.message_print(0, 2, "\tStop plastic pollution!")
-
+ self.message_print(0, 2, "\tStop")
+
+
if __name__ == "__main__":
node_cmd = BotCamera("test_node")
rospy.spin()
-
From 755268a64b9d500e98e9e06639ce1e736d345b63 Mon Sep 17 00:00:00 2001
From: Konstantin Evgenievich Kondratenko
<90711883+KonstantinKondratenko@users.noreply.github.com>
Date: Mon, 3 Apr 2023 14:30:37 +0300
Subject: [PATCH 08/24] merge { (parking state) && (parking logic) }
To start enter
"
dts devel build -f -H && dts devel run -f -H
"
---
packages/bot_camera/src/bot_camera.py | 38 ++++++++++++++++++++++-----
1 file changed, 31 insertions(+), 7 deletions(-)
diff --git a/packages/bot_camera/src/bot_camera.py b/packages/bot_camera/src/bot_camera.py
index 0dda46f..16e9edd 100755
--- a/packages/bot_camera/src/bot_camera.py
+++ b/packages/bot_camera/src/bot_camera.py
@@ -10,20 +10,25 @@
from duckietown.dtros import DTROS, DTParam, NodeType, TopicType
from cv_bridge import CvBridge
import apriltag # from dt_apriltags import Detector
-from duckietown_msgs.msg import Twist2DStamped, BoolStamped
+from duckietown_msgs.msg import Twist2DStamped, BoolStamped, FSMState
+
#import RPi.GPIO as GPIO #@uncomment
+
class BotCamera(DTROS):
def __init__(self, node_name):
super().__init__(node_name, node_type=NodeType.PERCEPTION)
self.pub = rospy.Publisher("~car_cmd", Twist2DStamped, queue_size=1)
+ self.pub_state = rospy.Publisher("fsm_node/mode", FSMState, queue_size=1, latch=True)
self.bridge = CvBridge()
# subscribers
- self.sub_img = rospy.Subscriber("~image_in", CompressedImage, self.cb_image, queue_size=1, buff_size="100MB")
+ self.sub_img = rospy.Subscriber("~image_in", CompressedImage, self.cb_image, queue_size=1, buff_size="10MB")
self.sub_start_parking = rospy.Subscriber("~parking_start", BoolStamped, self.parking_start, queue_size=1)
+ self.sub_fsm_mode = rospy.Subscriber("fsm_node/mode", FSMState, self.cbMode, queue_size=1)
# publishers
- # logik helper
+ # logic helper
+ self.mode = None
self.bool_start = False
self.last_command = "None"
self.back_riding_counter = 0
@@ -31,17 +36,32 @@ def __init__(self, node_name):
self.is_conected = False
- def parking_start(self, msg):
- rospy.loginfo("INIT park")
+ # def parking_start(self, msg):
+ # rospy.loginfo("INIT park")
+ #
+ # if msg.data == True:
+ # self.bool_start = True
+ # elif msg.data == False:
+ # self.bool_start = False
+ # else:
+ # rospy.loginfo("Error of Booltype in topik \'parking_start\'")
+ def parking_start(self, msg):
if msg.data == True:
self.bool_start = True
+ new_state = FSMState()
+ new_state.state = "PARKING"
+ self.pub_state.publish(new_state) # set a new state
+
elif msg.data == False:
self.bool_start = False
+ new_state = FSMState()
+ new_state.state = "NORMAL_JOYSTICK_CONTROL"
+ self.pub_state.publish(new_state) # set a default state
else:
- rospy.loginfo("Error of Booltype in topik \'parking_start\'")
+ rospy.loginfo("Error in topic \'parking_start\'")
- # @uncomment
+ # # @uncomment
# def get_connection_status(self):
# BUTTON_GPIO = 24
# GPIO.setmode(GPIO.BCM)
@@ -52,6 +72,10 @@ def parking_start(self, msg):
# self.message_print(0, 0, "Get connection!")
# else:
# self.is_conected = False
+
+ def cbMode(self, fsm_state_msg):
+ self.mode = fsm_state_msg.state # String of the current FSM state
+
def cb_image(self, msg):
img = self.bridge.compressed_imgmsg_to_cv2(msg)
img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
From 112d74d9c8f5212dacf2670f8fea9c6b8e25d0f8 Mon Sep 17 00:00:00 2001
From: Konstantin Evgenievich Kondratenko
<90711883+KonstantinKondratenko@users.noreply.github.com>
Date: Mon, 3 Apr 2023 14:33:02 +0300
Subject: [PATCH 09/24] Edit launcher
---
packages/bot_camera/launch/default.launch | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/packages/bot_camera/launch/default.launch b/packages/bot_camera/launch/default.launch
index 4ad2691..d72e68a 100644
--- a/packages/bot_camera/launch/default.launch
+++ b/packages/bot_camera/launch/default.launch
@@ -11,7 +11,7 @@
-
+
From a64895ddc6d9fb61cc3ee3e58731b730c9f074c9 Mon Sep 17 00:00:00 2001
From: Konstantin Evgenievich Kondratenko
<90711883+KonstantinKondratenko@users.noreply.github.com>
Date: Fri, 7 Apr 2023 13:37:16 +0300
Subject: [PATCH 10/24] Add except for rpi
---
packages/bot_camera/src/bot_camera.py | 47 ++++++++++++++-------------
1 file changed, 25 insertions(+), 22 deletions(-)
diff --git a/packages/bot_camera/src/bot_camera.py b/packages/bot_camera/src/bot_camera.py
index 16e9edd..a123949 100755
--- a/packages/bot_camera/src/bot_camera.py
+++ b/packages/bot_camera/src/bot_camera.py
@@ -12,7 +12,7 @@
import apriltag # from dt_apriltags import Detector
from duckietown_msgs.msg import Twist2DStamped, BoolStamped, FSMState
-#import RPi.GPIO as GPIO #@uncomment
+import RPi.GPIO as GPIO #@uncomment
class BotCamera(DTROS):
@@ -36,15 +36,15 @@ def __init__(self, node_name):
self.is_conected = False
- # def parking_start(self, msg):
- # rospy.loginfo("INIT park")
- #
- # if msg.data == True:
- # self.bool_start = True
- # elif msg.data == False:
- # self.bool_start = False
- # else:
- # rospy.loginfo("Error of Booltype in topik \'parking_start\'")
+ def parking_start(self, msg):
+ rospy.loginfo("INIT park")
+
+ if msg.data == True:
+ self.bool_start = True
+ elif msg.data == False:
+ self.bool_start = False
+ else:
+ rospy.loginfo("Error of Booltype in topik \'parking_start\'")
def parking_start(self, msg):
if msg.data == True:
@@ -61,17 +61,20 @@ def parking_start(self, msg):
else:
rospy.loginfo("Error in topic \'parking_start\'")
- # # @uncomment
- # def get_connection_status(self):
- # BUTTON_GPIO = 24
- # GPIO.setmode(GPIO.BCM)
- # GPIO.setup(BUTTON_GPIO, GPIO.IN)
- # gpio_state = GPIO.input(BUTTON_GPIO)
- # if gpio_state:
- # self.is_conected = True
- # self.message_print(0, 0, "Get connection!")
- # else:
- # self.is_conected = False
+ # @uncomment
+ def get_connection_status(self):
+ BUTTON_GPIO = 24
+ try:
+ GPIO.setup(BUTTON_GPIO, GPIO.IN)#swap from down
+ GPIO.setmode(GPIO.BCM) #swap from up
+ gpio_state = GPIO.input(BUTTON_GPIO)
+ if gpio_state:
+ self.is_conected = True
+ self.message_print(0, 0, "Get connection!")
+ else:
+ self.is_conected = False
+ except:
+ rospy.loginfo("Can't check connection")
def cbMode(self, fsm_state_msg):
self.mode = fsm_state_msg.state # String of the current FSM state
@@ -80,7 +83,7 @@ def cb_image(self, msg):
img = self.bridge.compressed_imgmsg_to_cv2(msg)
img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
# @uncomment
- # self.get_conection_status()
+ self.get_connection_status()
if self.bool_start and not self.is_conected:
self.marker_detecting(img)
From a84eaa68fecc3bac68415d0cb8c638473cd69319 Mon Sep 17 00:00:00 2001
From: Konstantin Evgenievich Kondratenko
<90711883+KonstantinKondratenko@users.noreply.github.com>
Date: Fri, 7 Apr 2023 13:41:21 +0300
Subject: [PATCH 11/24] Add rpi.gpio install
Error:
GPIO.setup(BUTTON_GPIO, GPIO.IN)
RuntimeError: No access to /dev/mem. Try running as root!
At previous commit added except for error lines -> printing "Can't check connection"
---
dependencies-py3.txt | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/dependencies-py3.txt b/dependencies-py3.txt
index 513878b..821a352 100644
--- a/dependencies-py3.txt
+++ b/dependencies-py3.txt
@@ -1,3 +1,3 @@
+rpi.gpio
apriltag
flask
-
From 7c55b453561fd78e59ea5ca6460cc12358da6388 Mon Sep 17 00:00:00 2001
From: Konstantin Evgenievich Kondratenko
<90711883+KonstantinKondratenko@users.noreply.github.com>
Date: Mon, 10 Apr 2023 18:44:24 +0300
Subject: [PATCH 12/24] Integrate charging status
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
Необходимо подобрать значение v omega для корректной парковки (не сложно)
При подключении к зарядке, бот останавливается и перестает обрабатывать изображение, также выводится из состояния экранирования джойстика
---
packages/bot_camera/src/bot_camera.py | 130 +++++++++++---------------
1 file changed, 56 insertions(+), 74 deletions(-)
diff --git a/packages/bot_camera/src/bot_camera.py b/packages/bot_camera/src/bot_camera.py
index a123949..b5223af 100755
--- a/packages/bot_camera/src/bot_camera.py
+++ b/packages/bot_camera/src/bot_camera.py
@@ -12,110 +12,89 @@
import apriltag # from dt_apriltags import Detector
from duckietown_msgs.msg import Twist2DStamped, BoolStamped, FSMState
-import RPi.GPIO as GPIO #@uncomment
-
+## Try to find best values v-omega for revers and riding
class BotCamera(DTROS):
def __init__(self, node_name):
super().__init__(node_name, node_type=NodeType.PERCEPTION)
- self.pub = rospy.Publisher("~car_cmd", Twist2DStamped, queue_size=1)
- self.pub_state = rospy.Publisher("fsm_node/mode", FSMState, queue_size=1, latch=True)
self.bridge = CvBridge()
+
# subscribers
self.sub_img = rospy.Subscriber("~image_in", CompressedImage, self.cb_image, queue_size=1, buff_size="10MB")
self.sub_start_parking = rospy.Subscriber("~parking_start", BoolStamped, self.parking_start, queue_size=1)
self.sub_fsm_mode = rospy.Subscriber("fsm_node/mode", FSMState, self.cbMode, queue_size=1)
+ self.sub_connection_status = rospy.Subscriber("~connection_status", BoolStamped, self.get_connection_status,
+ queue_size=1)
+
# publishers
+ self.pub = rospy.Publisher("~car_cmd", Twist2DStamped, queue_size=1)
+ self.pub_state = rospy.Publisher("fsm_node/mode", FSMState, queue_size=1, latch=True)
# logic helper
- self.mode = None
- self.bool_start = False
- self.last_command = "None"
- self.back_riding_counter = 0
- self.cant_find_counter = 0
-
- self.is_conected = False
-
- def parking_start(self, msg):
- rospy.loginfo("INIT park")
-
- if msg.data == True:
- self.bool_start = True
- elif msg.data == False:
- self.bool_start = False
- else:
- rospy.loginfo("Error of Booltype in topik \'parking_start\'")
-
- def parking_start(self, msg):
- if msg.data == True:
- self.bool_start = True
- new_state = FSMState()
+ self.mode = None # change state for bot
+ self.bool_start = False # false if press button "J" on joystick and true if press button "F" on joystick
+ self.last_command = "None" # save last command to revers riding
+ self.back_riding_counter = 0 # for count revers riding publishing
+ self.cant_find_counter = 0 # for count how many times we can's tag
+ self.is_connected = False # for checking connection status
+ self.delta_to_find = 5 # delta value to ride ahead
+
+ def parking_start(self, msg): # init joystick messages
+ self.bool_start = msg.data
+ new_state = FSMState()
+ if msg.data:
new_state.state = "PARKING"
- self.pub_state.publish(new_state) # set a new state
-
- elif msg.data == False:
- self.bool_start = False
- new_state = FSMState()
- new_state.state = "NORMAL_JOYSTICK_CONTROL"
- self.pub_state.publish(new_state) # set a default state
else:
- rospy.loginfo("Error in topic \'parking_start\'")
-
- # @uncomment
- def get_connection_status(self):
- BUTTON_GPIO = 24
- try:
- GPIO.setup(BUTTON_GPIO, GPIO.IN)#swap from down
- GPIO.setmode(GPIO.BCM) #swap from up
- gpio_state = GPIO.input(BUTTON_GPIO)
- if gpio_state:
- self.is_conected = True
- self.message_print(0, 0, "Get connection!")
- else:
- self.is_conected = False
- except:
- rospy.loginfo("Can't check connection")
+ new_state.state = "NORMAL_JOYSTICK_CONTROL"
+ self.pub_state.publish(new_state) # set a new state
- def cbMode(self, fsm_state_msg):
+ def cbMode(self, fsm_state_msg): # init joystick shielding
self.mode = fsm_state_msg.state # String of the current FSM state
- def cb_image(self, msg):
+ def cb_image(self, msg): # init image input
img = self.bridge.compressed_imgmsg_to_cv2(msg)
img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
- # @uncomment
- self.get_connection_status()
- if self.bool_start and not self.is_conected:
+ if self.bool_start and not self.is_connected:
self.marker_detecting(img)
- def marker_detecting(self, in_image):
-
- rospy.loginfo("start detection")
+ def get_connection_status(self, msg): # init connection status
+ self.is_connected = msg.data
+ if self.is_connected:
+ self.message_print(0, 0, "Connected ura! ura! ura!")
+ new_state = FSMState()
+ new_state.state = "NORMAL_JOYSTICK_CONTROL"
+ self.pub_state.publish(new_state)
+ else:
+ rospy.loginfo("NOT_connected")
+ def marker_detecting(self, in_image): # marker detecting method
options = apriltag.DetectorOptions(families="tag36h11")
detector = apriltag.Detector(options)
- gray_img = cv2.cvtColor(in_image, cv2.COLOR_RGB2GRAY) # при комменте не падаем
+ gray_img = cv2.cvtColor(in_image, cv2.COLOR_RGB2GRAY)
x_center_image = in_image.shape[1] // 2
tags = detector.detect(gray_img)
- rospy.loginfo("after getting tags")
if len(tags) == 0:
self.cant_find()
- return # тут бот упал
+ return
for tag in tags:
if tag.tag_id == 20: # there gotta be special value
- rospy.loginfo("in tags")
self.cant_find_counter = 0
self.back_riding_counter = 0
coordinates = tuple(map(int, tag.center))
x_center_marker = coordinates[0]
- if x_center_image > x_center_marker:
- rospy.loginfo("\tgo left")
+ rospy.loginfo(f"center image {x_center_image} \t center marker {x_center_marker} \n")
+ if x_center_image - self.delta_to_find < x_center_marker < x_center_image + self.delta_to_find:
+ self.go_ahead()
+ elif x_center_image > x_center_marker:
self.turn_left()
elif x_center_image <= x_center_marker:
- rospy.loginfo("\tgo right")
self.turn_right()
- rospy.loginfo("before return from detecting") # бот тут падает(
return
+ def go_ahead(self):
+ self.last_command = "Ahead"
+ self.message_print(0.2, 0, "\t\tgo ahead")
+
def turn_right(self):
self.last_command = "Right"
self.message_print(0.1, -1, "\t\tturning right")
@@ -134,17 +113,14 @@ def message_print(self, v, omega, message):
def cant_find(self):
if self.back_riding_counter == 0:
self.back_riding_counter += 1
- self.message_print(0, 0, "\tstopping bot before back riding")
rospy.sleep(0.1)
self.message_print(0.3, 0, "\ttry to connect")
rospy.sleep(0.2)
-
self.revers_riding()
elif self.back_riding_counter == 1:
self.back_riding_counter += 1
self.revers_riding()
-
else:
rospy.sleep(1)
self.back_riding_counter = 0
@@ -155,20 +131,26 @@ def revers_riding(self):
if self.last_command == "None":
self.message_print(0, 1, "\t\tStart; try to find marker")
elif self.last_command == "Right":
- self.message_print(-0.3, -1, "\t\tBack riding turning right")
- rospy.sleep(0.3)
+ self.message_print(-0.5, -0.6, "\t\tBack riding turning right")
+ rospy.sleep(0.5)
self.cant_find_counter += 1
elif self.last_command == "Left":
- self.message_print(-0.3, 1, "\t\tBack riding turning left")
- rospy.sleep(0.3)
+ self.message_print(-0.5, 0.6, "\t\tBack riding turning left")
+ rospy.sleep(0.5)
self.cant_find_counter += 1
else:
- self.message_print(0, 0, "\tError last_command value!")
+ self.message_print(-0.5, 0, "\tBack riding ahead")
+ self.cant_find_counter += 1
else:
rospy.sleep(1)
self.message_print(0, 2, "\tStop")
if __name__ == "__main__":
- node_cmd = BotCamera("test_node")
+ node_cmd = BotCamera("bot_camera")
rospy.spin()
+ # if rospy.is_shutdown():
+ # new_state = FSMState()
+ # new_state.state = "NORMAL_JOYSTICK_CONTROL"
+ # node_cmd.pub_state.publish(new_state)
+
From 9b5a5bd58aa1f93b8dc6978f5aa32781a99e2327 Mon Sep 17 00:00:00 2001
From: Konstantin Evgenievich Kondratenko
<90711883+KonstantinKondratenko@users.noreply.github.com>
Date: Mon, 10 Apr 2023 18:45:32 +0300
Subject: [PATCH 13/24] add remap for self-made topic
---
packages/bot_camera/launch/default.launch | 1 +
1 file changed, 1 insertion(+)
diff --git a/packages/bot_camera/launch/default.launch b/packages/bot_camera/launch/default.launch
index d72e68a..0594f89 100644
--- a/packages/bot_camera/launch/default.launch
+++ b/packages/bot_camera/launch/default.launch
@@ -11,6 +11,7 @@
+
From ae6302c546d19ef65c4babf987aac237795efdf7 Mon Sep 17 00:00:00 2001
From: Konstantin Evgenievich Kondratenko
<90711883+KonstantinKondratenko@users.noreply.github.com>
Date: Fri, 14 Apr 2023 14:44:33 +0300
Subject: [PATCH 14/24] Update dependencies-py3.txt
---
dependencies-py3.txt | 3 +--
1 file changed, 1 insertion(+), 2 deletions(-)
diff --git a/dependencies-py3.txt b/dependencies-py3.txt
index 7afd53a..8100ab7 100644
--- a/dependencies-py3.txt
+++ b/dependencies-py3.txt
@@ -1,3 +1,2 @@
-rpi.gpio
apriltag
-flask
\ No newline at end of file
+flask
From 9343da1509615ec4ad1fd33b708d77f1c74362b9 Mon Sep 17 00:00:00 2001
From: Konstantin Evgenievich Kondratenko
<90711883+KonstantinKondratenko@users.noreply.github.com>
Date: Fri, 14 Apr 2023 14:46:36 +0300
Subject: [PATCH 15/24] Update Dockerfile
---
Dockerfile | 3 +++
1 file changed, 3 insertions(+)
diff --git a/Dockerfile b/Dockerfile
index 20817ae..cd01a4a 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -61,6 +61,9 @@ RUN pip uninstall dt-apriltag
COPY ./dependencies-py3.txt "${REPO_PATH}/"
RUN dt-pip3-install ${REPO_PATH}/dependencies-py3.txt
+RUN pip install --no-cache-dir rpi.gpio
+# RUN sudo usermod -a -G gpio $USER
+
# copy the source code
From 5cf3bde0db7f865f07075d1bab0cd41a48d2cb91 Mon Sep 17 00:00:00 2001
From: Konstantin Evgenievich Kondratenko
<90711883+KonstantinKondratenko@users.noreply.github.com>
Date: Fri, 14 Apr 2023 14:51:54 +0300
Subject: [PATCH 16/24] Update default.launch
---
packages/bot_camera/launch/default.launch | 1 +
1 file changed, 1 insertion(+)
diff --git a/packages/bot_camera/launch/default.launch b/packages/bot_camera/launch/default.launch
index 0594f89..c6108f4 100644
--- a/packages/bot_camera/launch/default.launch
+++ b/packages/bot_camera/launch/default.launch
@@ -14,6 +14,7 @@
+
From 5f29e3ea0c80d6649a78336ade3a3d31ad37dd5b Mon Sep 17 00:00:00 2001
From: Konstantin Evgenievich Kondratenko
<90711883+KonstantinKondratenko@users.noreply.github.com>
Date: Fri, 14 Apr 2023 14:54:36 +0300
Subject: [PATCH 17/24] Update bot_camera.py
---
packages/bot_camera/src/bot_camera.py | 95 ++++++++-------------------
1 file changed, 27 insertions(+), 68 deletions(-)
diff --git a/packages/bot_camera/src/bot_camera.py b/packages/bot_camera/src/bot_camera.py
index b5223af..2875ad1 100755
--- a/packages/bot_camera/src/bot_camera.py
+++ b/packages/bot_camera/src/bot_camera.py
@@ -9,11 +9,10 @@
from sensor_msgs.msg import CompressedImage, Image
from duckietown.dtros import DTROS, DTParam, NodeType, TopicType
from cv_bridge import CvBridge
-import apriltag # from dt_apriltags import Detector
+import apriltag
from duckietown_msgs.msg import Twist2DStamped, BoolStamped, FSMState
-## Try to find best values v-omega for revers and riding
class BotCamera(DTROS):
def __init__(self, node_name):
super().__init__(node_name, node_type=NodeType.PERCEPTION)
@@ -33,13 +32,10 @@ def __init__(self, node_name):
# logic helper
self.mode = None # change state for bot
self.bool_start = False # false if press button "J" on joystick and true if press button "F" on joystick
- self.last_command = "None" # save last command to revers riding
- self.back_riding_counter = 0 # for count revers riding publishing
self.cant_find_counter = 0 # for count how many times we can's tag
self.is_connected = False # for checking connection status
- self.delta_to_find = 5 # delta value to ride ahead
- def parking_start(self, msg): # init joystick messages
+ def parking_start(self, msg):
self.bool_start = msg.data
new_state = FSMState()
if msg.data:
@@ -48,16 +44,16 @@ def parking_start(self, msg): # init joystick messages
new_state.state = "NORMAL_JOYSTICK_CONTROL"
self.pub_state.publish(new_state) # set a new state
- def cbMode(self, fsm_state_msg): # init joystick shielding
+ def cbMode(self, fsm_state_msg):
self.mode = fsm_state_msg.state # String of the current FSM state
- def cb_image(self, msg): # init image input
+ def cb_image(self, msg):
img = self.bridge.compressed_imgmsg_to_cv2(msg)
img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
if self.bool_start and not self.is_connected:
self.marker_detecting(img)
- def get_connection_status(self, msg): # init connection status
+ def get_connection_status(self, msg):
self.is_connected = msg.data
if self.is_connected:
self.message_print(0, 0, "Connected ura! ura! ura!")
@@ -67,10 +63,10 @@ def get_connection_status(self, msg): # init connection status
else:
rospy.loginfo("NOT_connected")
- def marker_detecting(self, in_image): # marker detecting method
+ def marker_detecting(self, in_image):
options = apriltag.DetectorOptions(families="tag36h11")
detector = apriltag.Detector(options)
- gray_img = cv2.cvtColor(in_image, cv2.COLOR_RGB2GRAY)
+ gray_img = cv2.cvtColor(in_image, cv2.COLOR_RGB2GRAY)
x_center_image = in_image.shape[1] // 2
tags = detector.detect(gray_img)
if len(tags) == 0:
@@ -78,30 +74,33 @@ def marker_detecting(self, in_image): # marker detecting method
return
for tag in tags:
if tag.tag_id == 20: # there gotta be special value
+ self.message_print(0, 0, "Find marker, stop")
self.cant_find_counter = 0
- self.back_riding_counter = 0
coordinates = tuple(map(int, tag.center))
x_center_marker = coordinates[0]
rospy.loginfo(f"center image {x_center_image} \t center marker {x_center_marker} \n")
- if x_center_image - self.delta_to_find < x_center_marker < x_center_image + self.delta_to_find:
- self.go_ahead()
- elif x_center_image > x_center_marker:
- self.turn_left()
+ if x_center_image > x_center_marker:
+ self.message_print(0.1, 1, "\t\tturning left")
elif x_center_image <= x_center_marker:
- self.turn_right()
+ self.message_print(0.1, -1, "\t\tturning right")
return
- def go_ahead(self):
- self.last_command = "Ahead"
- self.message_print(0.2, 0, "\t\tgo ahead")
-
- def turn_right(self):
- self.last_command = "Right"
- self.message_print(0.1, -1, "\t\tturning right")
-
- def turn_left(self):
- self.last_command = "Left"
- self.message_print(0.1, 1, "\t\tturning left")
+ def cant_find(self):
+ if self.cant_find_counter == 0:
+ self.cant_find_counter += 1
+ self.message_print(0.5, 0, "Try to connect")
+ rospy.sleep(0.2)
+ self.message_print(0, 0, "Checking charging sleeping time")
+ rospy.sleep(0.2)
+ elif self.cant_find_counter == 1:
+ self.cant_find_counter += 1
+ self.message_print(-0.5, 0, "\tBack riding ahead")
+ rospy.sleep(1)
+ self.message_print(0, 0, "Look around time")
+ rospy.sleep(0.2)
+ else:
+ self.message_print(0, 0.5, f"\tTurning №{self.cant_find_counter} and try to find marker")
+ self.cant_find_counter += 1
def message_print(self, v, omega, message):
msg = Twist2DStamped()
@@ -110,47 +109,7 @@ def message_print(self, v, omega, message):
rospy.loginfo(message)
self.pub.publish(msg)
- def cant_find(self):
- if self.back_riding_counter == 0:
- self.back_riding_counter += 1
- rospy.sleep(0.1)
- self.message_print(0.3, 0, "\ttry to connect")
- rospy.sleep(0.2)
- self.revers_riding()
-
- elif self.back_riding_counter == 1:
- self.back_riding_counter += 1
- self.revers_riding()
- else:
- rospy.sleep(1)
- self.back_riding_counter = 0
- self.message_print(0, 0, "\tstop back riding")
-
- def revers_riding(self):
- if self.cant_find_counter < 3:
- if self.last_command == "None":
- self.message_print(0, 1, "\t\tStart; try to find marker")
- elif self.last_command == "Right":
- self.message_print(-0.5, -0.6, "\t\tBack riding turning right")
- rospy.sleep(0.5)
- self.cant_find_counter += 1
- elif self.last_command == "Left":
- self.message_print(-0.5, 0.6, "\t\tBack riding turning left")
- rospy.sleep(0.5)
- self.cant_find_counter += 1
- else:
- self.message_print(-0.5, 0, "\tBack riding ahead")
- self.cant_find_counter += 1
- else:
- rospy.sleep(1)
- self.message_print(0, 2, "\tStop")
-
if __name__ == "__main__":
node_cmd = BotCamera("bot_camera")
rospy.spin()
- # if rospy.is_shutdown():
- # new_state = FSMState()
- # new_state.state = "NORMAL_JOYSTICK_CONTROL"
- # node_cmd.pub_state.publish(new_state)
-
From d18c370f6f000412a4dc63ce4dc3125c81ebe6c2 Mon Sep 17 00:00:00 2001
From: Konstantin Evgenievich Kondratenko
<90711883+KonstantinKondratenko@users.noreply.github.com>
Date: Fri, 14 Apr 2023 15:34:54 +0300
Subject: [PATCH 18/24] Update README.md
---
README.md | 58 +++++++++++++++----------------------------------------
1 file changed, 16 insertions(+), 42 deletions(-)
diff --git a/README.md b/README.md
index 9b37e8c..4c1a1ad 100644
--- a/README.md
+++ b/README.md
@@ -1,54 +1,28 @@
-# Template: template-core
+# Parking done
+## In order to run a node you need:
-This template provides a boilerplate repository
-for developing ROS-based software in Duckietown.
-Unlike the `template-ros` repository, this template
-builds on top of the module
-[`dt-core`](https://github.com/duckietown/dt-core).
-This is needed when your application requires access
-to tools and libraries defined in
-[`dt-core`](https://github.com/duckietown/dt-core).
+1. have a joystick version that supports charging buttons [`joystick`](https://github.com/AlexanderKamynin/dt-automatic-charging/tree/parking)
+2. have a charging driver version [`driver`](https://github.com/OSLL/charging-driver/tree/automatic-charging)
-**NOTE:** If you want to develop software that does not use
-ROS, check out [this template](https://github.com/duckietown/template-basic).
+## After cloning the repository, you need to build the project with the command:
+`dts devel build -f -H && dts devel run -f -H`
-## How to use it
+## After cloning the charge driver repository, you need to run the charge driver to create the charge state topic with the command:
-### 1. Fork this repository
+`docker -H .local run --name charging_driver -v /dev/mem --privileged --network=host -dit --restart unless-stopped -e ROBOT_TYPE=duckiebot docker.io/duckietown/charging-driver :automatic-charging-arm32v7`
-Use the fork button in the top-right corner of the github page to fork this template repository.
+## After cloning the joystick repository, you need to build it with the command:
+`dts devel build -f -H --tag daffy`
-### 2. Create a new repository
+## And run it with the command:
-Create a new repository on github.com while
-specifying the newly forked template repository as
-a template for your new repository.
+`dts duckiebot keyboard_control `
+## When all nodes (responsible for parking, charging status and joystick) are running, you can start the parking process
-### 3. Define dependencies
-
-List the dependencies in the files `dependencies-apt.txt` and
-`dependencies-py3.txt` (apt packages and pip packages respectively).
-
-
-### 4. Place your code
-
-Place your code in the directory `/packages/` of
-your new repository.
-
-
-### 5. Setup launchers
-
-The directory `/launchers` can contain as many launchers (launching scripts)
-as you want. A default launcher called `default.sh` must always be present.
-
-If you create an executable script (i.e., a file with a valid shebang statement)
-a launcher will be created for it. For example, the script file
-`/launchers/my-launcher.sh` will be available inside the Docker image as the binary
-`dt-launcher-my-launcher`.
-
-When launching a new container, you can simply provide `dt-launcher-my-launcher` as
-command.
\ No newline at end of file
+With the joystick active, you need to press the "F" button to start parking or "G" to end it.
+### Be careful!
+Due to the peculiarity of ROS, it is not enough to briefly press "F", in this case it is necessary to hold down "F" for 1-2 seconds
From e3ac1349f9038ffd3ccd866b2ed3cdf3603dfb6f Mon Sep 17 00:00:00 2001
From: Konstantin Evgenievich Kondratenko
<90711883+KonstantinKondratenko@users.noreply.github.com>
Date: Fri, 14 Apr 2023 15:42:00 +0300
Subject: [PATCH 19/24] Update README.md
---
README.md | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/README.md b/README.md
index 4c1a1ad..9c18755 100644
--- a/README.md
+++ b/README.md
@@ -7,9 +7,9 @@
## After cloning the repository, you need to build the project with the command:
-`dts devel build -f -H && dts devel run -f -H`
+`dts devel build -f -H && dts devel run -f -H `
-## After cloning the charge driver repository, you need to run the charge driver to create the charge state topic with the command:
+## After cloning the charging driver repository, you need to run the charging driver to create the charge state topic with the command:
`docker -H .local run --name charging_driver -v /dev/mem --privileged --network=host -dit --restart unless-stopped -e ROBOT_TYPE=duckiebot docker.io/duckietown/charging-driver :automatic-charging-arm32v7`
From 9b22b7e1a5c38c4693c96202d290a33f5e68d738 Mon Sep 17 00:00:00 2001
From: Konstantin Evgenievich Kondratenko
<90711883+KonstantinKondratenko@users.noreply.github.com>
Date: Fri, 14 Apr 2023 17:12:24 +0300
Subject: [PATCH 20/24] Update README.md
---
README.md | 6 +++++-
1 file changed, 5 insertions(+), 1 deletion(-)
diff --git a/README.md b/README.md
index 9c18755..f8dd982 100644
--- a/README.md
+++ b/README.md
@@ -9,7 +9,11 @@
`dts devel build -f -H && dts devel run -f -H `
-## After cloning the charging driver repository, you need to run the charging driver to create the charge state topic with the command:
+## After cloning the charging driver repository, you need to build the charging driver to create the charge state topic with the command:
+
+`dts devel build -f -H `
+
+## And run charging driver with the command:
`docker -H .local run --name charging_driver -v /dev/mem --privileged --network=host -dit --restart unless-stopped -e ROBOT_TYPE=duckiebot docker.io/duckietown/charging-driver :automatic-charging-arm32v7`
From e5bcb9d90ccb187ccb904668f52a19d009a5744a Mon Sep 17 00:00:00 2001
From: Konstantin Evgenievich Kondratenko
<90711883+KonstantinKondratenko@users.noreply.github.com>
Date: Fri, 21 Apr 2023 11:40:35 +0300
Subject: [PATCH 21/24] Update README.md
---
README.md | 8 ++++----
1 file changed, 4 insertions(+), 4 deletions(-)
diff --git a/README.md b/README.md
index f8dd982..954075b 100644
--- a/README.md
+++ b/README.md
@@ -5,10 +5,6 @@
2. have a charging driver version [`driver`](https://github.com/OSLL/charging-driver/tree/automatic-charging)
-## After cloning the repository, you need to build the project with the command:
-
-`dts devel build -f -H && dts devel run -f -H `
-
## After cloning the charging driver repository, you need to build the charging driver to create the charge state topic with the command:
`dts devel build -f -H `
@@ -25,6 +21,10 @@
`dts duckiebot keyboard_control `
+## After cloning this repository, you need to build the project with the command:
+
+`dts devel build -f -H && dts devel run -f -H `
+
## When all nodes (responsible for parking, charging status and joystick) are running, you can start the parking process
With the joystick active, you need to press the "F" button to start parking or "G" to end it.
From 0e7df5e612a223fceee5dbc5c3c62d506cd68583 Mon Sep 17 00:00:00 2001
From: Konstantin Evgenievich Kondratenko
<90711883+KonstantinKondratenko@users.noreply.github.com>
Date: Fri, 21 Apr 2023 12:33:44 +0300
Subject: [PATCH 22/24] Update README.md
---
README.md | 11 ++++++++---
1 file changed, 8 insertions(+), 3 deletions(-)
diff --git a/README.md b/README.md
index 954075b..70ca85d 100644
--- a/README.md
+++ b/README.md
@@ -5,13 +5,18 @@
2. have a charging driver version [`driver`](https://github.com/OSLL/charging-driver/tree/automatic-charging)
-## After cloning the charging driver repository, you need to build the charging driver to create the charge state topic with the command:
+### Be careful!
+Joystick package have name "automatic-charging" like package of this repository. You need to clone this in different directories.
+### Example:
+`mkdir allpr; cd allpr; git clone ; git clone ; mkdir joystick; cd joystick; git clone `
+
+## After cloning the charging driver repository, you need to it with the command:
`dts devel build -f -H `
## And run charging driver with the command:
-`docker -H .local run --name charging_driver -v /dev/mem --privileged --network=host -dit --restart unless-stopped -e ROBOT_TYPE=duckiebot docker.io/duckietown/charging-driver :automatic-charging-arm32v7`
+`docker -H .local run --name charging_driver -v /dev/mem --privileged --network=host -dit --restart unless-stopped -e ROBOT_TYPE=duckiebot docker.io/duckietown/charging-driver:automatic-charging-arm32v7`
## After cloning the joystick repository, you need to build it with the command:
@@ -27,6 +32,6 @@
## When all nodes (responsible for parking, charging status and joystick) are running, you can start the parking process
-With the joystick active, you need to press the "F" button to start parking or "G" to end it.
+With the active joystick, you need to press the "F" button to start parking or "G" to end it.
### Be careful!
Due to the peculiarity of ROS, it is not enough to briefly press "F", in this case it is necessary to hold down "F" for 1-2 seconds
From de48d13468c17f29e47609da76973cf36157774f Mon Sep 17 00:00:00 2001
From: Konstantin Evgenievich Kondratenko
<90711883+KonstantinKondratenko@users.noreply.github.com>
Date: Fri, 21 Apr 2023 17:56:40 +0300
Subject: [PATCH 23/24] Update README.md
---
README.md | 8 ++++----
1 file changed, 4 insertions(+), 4 deletions(-)
diff --git a/README.md b/README.md
index 70ca85d..99cb7c2 100644
--- a/README.md
+++ b/README.md
@@ -10,7 +10,7 @@ Joystick package have name "automatic-charging" like package of this repository.
### Example:
`mkdir allpr; cd allpr; git clone ; git clone ; mkdir joystick; cd joystick; git clone `
-## After cloning the charging driver repository, you need to it with the command:
+## After cloning the charging driver repository, you need to build it with the command:
`dts devel build -f -H `
@@ -20,13 +20,13 @@ Joystick package have name "automatic-charging" like package of this repository.
## After cloning the joystick repository, you need to build it with the command:
-`dts devel build -f -H --tag daffy`
+`dts devel build -f`
## And run it with the command:
-`dts duckiebot keyboard_control `
+`dts duckiebot keyboard_control --gui_image duckietown/dt-automatic-charging `
-## After cloning this repository, you need to build the project with the command:
+## After cloning this repository, you need to build and run the project with the command:
`dts devel build -f -H && dts devel run -f -H `
From f6593eeebee2e84bc5d44fd331133cdfa79011b8 Mon Sep 17 00:00:00 2001
From: Konstantin Evgenievich Kondratenko
<90711883+KonstantinKondratenko@users.noreply.github.com>
Date: Fri, 21 Apr 2023 18:01:31 +0300
Subject: [PATCH 24/24] Add new logic
---
packages/bot_camera/src/bot_camera.py | 25 ++++++++++++++++++++++---
1 file changed, 22 insertions(+), 3 deletions(-)
diff --git a/packages/bot_camera/src/bot_camera.py b/packages/bot_camera/src/bot_camera.py
index 2875ad1..62ee4a0 100755
--- a/packages/bot_camera/src/bot_camera.py
+++ b/packages/bot_camera/src/bot_camera.py
@@ -5,11 +5,12 @@
import sys
import rospy
import numpy as np
+import math
import os
from sensor_msgs.msg import CompressedImage, Image
from duckietown.dtros import DTROS, DTParam, NodeType, TopicType
from cv_bridge import CvBridge
-import apriltag
+import apriltag
from duckietown_msgs.msg import Twist2DStamped, BoolStamped, FSMState
@@ -34,6 +35,7 @@ def __init__(self, node_name):
self.bool_start = False # false if press button "J" on joystick and true if press button "F" on joystick
self.cant_find_counter = 0 # for count how many times we can's tag
self.is_connected = False # for checking connection status
+ self.deltaLR = 0
def parking_start(self, msg):
self.bool_start = msg.data
@@ -66,7 +68,7 @@ def get_connection_status(self, msg):
def marker_detecting(self, in_image):
options = apriltag.DetectorOptions(families="tag36h11")
detector = apriltag.Detector(options)
- gray_img = cv2.cvtColor(in_image, cv2.COLOR_RGB2GRAY)
+ gray_img = cv2.cvtColor(in_image, cv2.COLOR_RGB2GRAY)
x_center_image = in_image.shape[1] // 2
tags = detector.detect(gray_img)
if len(tags) == 0:
@@ -75,6 +77,19 @@ def marker_detecting(self, in_image):
for tag in tags:
if tag.tag_id == 20: # there gotta be special value
self.message_print(0, 0, "Find marker, stop")
+
+ (topLeft, topRight, bottomRight, bottomLeft) = tag.corners
+
+ topRight = (int(topRight[0]), int(topRight[1]))
+ bottomRight = (int(bottomRight[0]), int(bottomRight[1]))
+ bottomLeft = (int(bottomLeft[0]), int(bottomLeft[1]))
+ topLeft = (int(topLeft[0]), int(topLeft[1]))
+
+ length_left = math.sqrt((bottomLeft[1] - topLeft[1]) ** 2 + (bottomLeft[0] - topLeft[0]) ** 2)
+ length_right = math.sqrt((bottomRight[1] - topRight[1]) ** 2 + (bottomRight[0] - topRight[0]) ** 2)
+ rospy.loginfo(f"length left and right side marker: left {length_left} right {length_right}\n")
+ self.deltaLR = length_left - length_right
+
self.cant_find_counter = 0
coordinates = tuple(map(int, tag.center))
x_center_marker = coordinates[0]
@@ -94,7 +109,11 @@ def cant_find(self):
rospy.sleep(0.2)
elif self.cant_find_counter == 1:
self.cant_find_counter += 1
- self.message_print(-0.5, 0, "\tBack riding ahead")
+ # self.message_print(-0.5, 0, "\tBack riding ahead")
+ if self.deltaLR < 0:
+ self.message_print(-1, -0.5, "ahead and left")
+ else:
+ self.message_print(-1, 0.5, "ahead and right")
rospy.sleep(1)
self.message_print(0, 0, "Look around time")
rospy.sleep(0.2)