diff --git a/Dockerfile b/Dockerfile index f23a09d..cd01a4a 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,24 @@ 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 + +RUN pip install --no-cache-dir rpi.gpio +# RUN sudo usermod -a -G gpio $USER + + # copy the source code COPY ./packages "${REPO_PATH}/packages" diff --git a/README.md b/README.md index 9b37e8c..99cb7c2 100644 --- a/README.md +++ b/README.md @@ -1,54 +1,37 @@ -# 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). +### 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 build it with the command: -## How to use it +`dts devel build -f -H ` -### 1. Fork this repository +## And run charging driver with the command: -Use the fork button in the top-right corner of the github page to fork this template 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` +## After cloning the joystick repository, you need to build it with the command: -### 2. Create a new repository +`dts devel build -f` -Create a new repository on github.com while -specifying the newly forked template repository as -a template for your new repository. +## And run it with the command: +`dts duckiebot keyboard_control --gui_image duckietown/dt-automatic-charging ` -### 3. Define dependencies +## After cloning this repository, you need to build and run the project with the command: -List the dependencies in the files `dependencies-apt.txt` and -`dependencies-py3.txt` (apt packages and pip packages respectively). +`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 -### 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 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 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/packages/bot_camera/launch/default.launch b/packages/bot_camera/launch/default.launch index cacd108..c6108f4 100644 --- a/packages/bot_camera/launch/default.launch +++ b/packages/bot_camera/launch/default.launch @@ -11,7 +11,10 @@ + + + diff --git a/packages/bot_camera/src/bot_camera.py b/packages/bot_camera/src/bot_camera.py index 01347d8..62ee4a0 100755 --- a/packages/bot_camera/src/bot_camera.py +++ b/packages/bot_camera/src/bot_camera.py @@ -1,62 +1,134 @@ #!/usr/bin/env python3 from typing import Optional, Any - import cv2 - +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 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 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.sub_connection_status = rospy.Subscriber("~connection_status", BoolStamped, self.get_connection_status, + 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", - ) + 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 # change state for bot + 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 + new_state = FSMState() + if msg.data: + new_state.state = "PARKING" + else: + new_state.state = "NORMAL_JOYSTICK_CONTROL" + self.pub_state.publish(new_state) # set a new state + + def cbMode(self, fsm_state_msg): + self.mode = fsm_state_msg.state # String of the current FSM state 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 self.bool_start and not self.is_connected: + self.marker_detecting(img) + + def get_connection_status(self, msg): + 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): + 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: # 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] + rospy.loginfo(f"center image {x_center_image} \t center marker {x_center_marker} \n") + if x_center_image > x_center_marker: + self.message_print(0.1, 1, "\t\tturning left") + elif x_center_image <= x_center_marker: + self.message_print(0.1, -1, "\t\tturning right") + return + + 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") + 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) + 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() + msg.v = v + msg.omega = omega + rospy.loginfo(message) + self.pub.publish(msg) if __name__ == "__main__": - node = BotCamera("test_node") + node_cmd = BotCamera("bot_camera") rospy.spin()