From deca550032852edd1199d74b12fd203a14a15c4f Mon Sep 17 00:00:00 2001 From: AlexanderKamynin Date: Fri, 24 Feb 2023 15:55:04 +0300 Subject: [PATCH 1/3] marker tracking --- Dockerfile | 2 ++ packages/bot_camera/src/bot_camera.py | 47 ++++++++++++++++++++++++++- 2 files changed, 48 insertions(+), 1 deletion(-) diff --git a/Dockerfile b/Dockerfile index f23a09d..f34f661 100644 --- a/Dockerfile +++ b/Dockerfile @@ -47,12 +47,14 @@ ENV DT_LAUNCH_PATH "${LAUNCH_PATH}" ENV DT_LAUNCHER "${LAUNCHER}" # 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 install dt-apriltags # copy the source code COPY ./packages "${REPO_PATH}/packages" diff --git a/packages/bot_camera/src/bot_camera.py b/packages/bot_camera/src/bot_camera.py index 01347d8..00c8e0a 100755 --- a/packages/bot_camera/src/bot_camera.py +++ b/packages/bot_camera/src/bot_camera.py @@ -13,6 +13,7 @@ from dt_class_utils import DTReminder from turbojpeg import TurboJPEG from cv_bridge import CvBridge +from dt_apriltags import Detector class BotCamera(DTROS): @@ -48,13 +49,57 @@ def cb_image(self, msg): 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) + tags = tag_detector.detect(gray_img) + + 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) + + #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 if __name__ == "__main__": From 5a29d3f3c74e5b1df1f9cf93774e1831395eb4d7 Mon Sep 17 00:00:00 2001 From: AlexanderKamynin Date: Mon, 3 Apr 2023 11:13:34 +0300 Subject: [PATCH 2/3] add new state for parking --- Dockerfile | 18 ++- dependencies-apt.txt | 3 +- dependencies-py3.txt | 2 +- launchers/default.sh | 2 +- packages/bot_camera/launch/default.launch | 4 +- packages/bot_camera/launch/test.launch | 2 +- packages/bot_camera/src/bot_camera.py | 182 +++++++++++++--------- run_file.py | 6 +- 8 files changed, 128 insertions(+), 91 deletions(-) diff --git a/Dockerfile b/Dockerfile index f34f661..f56b380 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}" @@ -51,10 +52,11 @@ 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 install dt-apriltags +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" @@ -83,4 +85,4 @@ LABEL org.duckietown.label.module.type="${REPO_NAME}" \ org.duckietown.label.base.tag="${BASE_TAG}" \ org.duckietown.label.maintainer="${MAINTAINER}" # <== Do not change the code above this line -# <================================================== +# <================================================== \ No newline at end of file diff --git a/dependencies-apt.txt b/dependencies-apt.txt index 91731d3..37cdaeb 100644 --- a/dependencies-apt.txt +++ b/dependencies-apt.txt @@ -5,9 +5,10 @@ # TODO: to `ros-noetic-joy`, as some deps are missing in the APT sources list. ros-noetic-joy + # commons gfortran # these are pretty small, they can stay in the ros-commons image ros-noetic-image-transport -ros-noetic-image-transport-plugins +ros-noetic-image-transport-plugins \ No newline at end of file diff --git a/dependencies-py3.txt b/dependencies-py3.txt index e647c04..cafc7d1 100644 --- a/dependencies-py3.txt +++ b/dependencies-py3.txt @@ -1,3 +1,3 @@ +apriltag flask - diff --git a/launchers/default.sh b/launchers/default.sh index 050a425..1cde29e 100755 --- a/launchers/default.sh +++ b/launchers/default.sh @@ -13,7 +13,7 @@ dt-launchfile-init # NOTE: Use `dt-exec COMMAND` to run the main process (blocking process) # set module's health -dt-set-module-healthy +# dt-set-module-healthy # launching app dt-exec roslaunch bot_camera default.launch veh:="$VEHICLE_NAME" diff --git a/packages/bot_camera/launch/default.launch b/packages/bot_camera/launch/default.launch index cacd108..2a91c69 100644 --- a/packages/bot_camera/launch/default.launch +++ b/packages/bot_camera/launch/default.launch @@ -11,11 +11,13 @@ + + - + \ No newline at end of file diff --git a/packages/bot_camera/launch/test.launch b/packages/bot_camera/launch/test.launch index acc7adb..87615bc 100644 --- a/packages/bot_camera/launch/test.launch +++ b/packages/bot_camera/launch/test.launch @@ -7,4 +7,4 @@ - + \ No newline at end of file diff --git a/packages/bot_camera/src/bot_camera.py b/packages/bot_camera/src/bot_camera.py index 00c8e0a..0c6779c 100755 --- a/packages/bot_camera/src/bot_camera.py +++ b/packages/bot_camera/src/bot_camera.py @@ -1,107 +1,137 @@ #!/usr/bin/env python3 from typing import Optional, Any - import cv2 - +import sys import rospy - 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 +import apriltag +from duckietown_msgs.msg import Twist2DStamped, BoolStamped, FSMState 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) - - # utility objects + # 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) 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="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.mode = None + self.bool_start = False + self.last_command = "None" + self.back_riding_counter = 0 + self.cant_find_counter = 0 - # 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 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 in topic \'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) - - # 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 self.mode == "PARKING": + # self.turn_left() + 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) - + options = apriltag.DetectorOptions(families="tag36h11") + detector = apriltag.Detector(options) gray_img = cv2.cvtColor(in_image, cv2.COLOR_RGB2GRAY) - tags = tag_detector.detect(gray_img) - + x_center_image = in_image.shape[1] // 2 + tags = detector.detect(gray_img) + if len(tags) == 0: + self.cant_find() + return 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) - - #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 + if tag.tag_id == 20: + 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: + self.turn_left() + elif x_center_image <= x_center_marker: + self.turn_right() + 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=None): + msg = Twist2DStamped() + msg.v = v + msg.omega = omega + self.pub.publish(msg) + + 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 + 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") + + def cbMode(self, fsm_state_msg): + self.mode = fsm_state_msg.state # String of the current FSM state if __name__ == "__main__": - node = BotCamera("test_node") + node_cmd = BotCamera("test_node") rospy.spin() diff --git a/run_file.py b/run_file.py index 71d3211..9b23ec4 100644 --- a/run_file.py +++ b/run_file.py @@ -8,6 +8,9 @@ import sys from shutil import which +# from utils.misc_utils import sanitize_hostname +# from utils.networking_utils import get_duckiebot_ip + __all__ = ["get_clean_env", "start_command_in_subprocess", "ask_confirmation", "check_program_dependency"] @@ -85,6 +88,7 @@ def check_program_dependency(exe): parser.add_argument('--branch', type=str, default='daffy') parser.add_argument('--bot_name', type=str, default='autobot03') parser.add_argument('--container_name', type=str, default='dts-automatic-charging') + parser.add_argument('--package-name', default='bot_camera') args = parser.parse_args() client = docker.from_env() @@ -109,11 +113,9 @@ def check_program_dependency(exe): 'mode': 'rw'}}, 'network_mode': 'host', 'ports': {}} - client.containers.run(**arguments) attach_cmd = "docker attach %s" % args.container_name try: start_command_in_subprocess(attach_cmd) except Exception as e: pass - From 6a40c5b2467f00c2ccceebd4d370bd281f674bfc Mon Sep 17 00:00:00 2001 From: AlexanderKamynin Date: Mon, 3 Apr 2023 11:22:19 +0300 Subject: [PATCH 3/3] change the topic for publish car cmd message --- 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 2a91c69..8d56d32 100644 --- a/packages/bot_camera/launch/default.launch +++ b/packages/bot_camera/launch/default.launch @@ -11,7 +11,7 @@ - +