diff --git a/Dockerfile b/Dockerfile index f23a09d..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}" @@ -47,12 +48,15 @@ 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 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" @@ -81,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 513878b..8100ab7 100644 --- a/dependencies-py3.txt +++ b/dependencies-py3.txt @@ -1,3 +1,2 @@ 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..8d56d32 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 01347d8..0c6779c 100755 --- a/packages/bot_camera/src/bot_camera.py +++ b/packages/bot_camera/src/bot_camera.py @@ -1,62 +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 +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" - ) - - # 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", - ) + 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 + + 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) - - # 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) + # track the contour of detected markers + if self.mode == "PARKING": + # self.turn_left() + self.marker_detecting(img) + + def marker_detecting(self, in_image): + 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) + if len(tags) == 0: + self.cant_find() + return + for tag in tags: + 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 -