add circle ride node

Dockerfile

@@ -1,15 +1,15 @@
 # parameters
-ARG REPO_NAME="<dt-automatic-charging>"
+ARG REPO_NAME="dt-core"
 ARG DESCRIPTION="Base image containing common libraries and environment setup for ROS applications."
 ARG MAINTAINER=" ()"
 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"

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 <this repo>; git clone <driver repo>; mkdir joystick; cd joystick; git clone <joystick repo>`
 
+## After cloning the charging driver repository, you need to build it with the command:
 
-## How to use it
+`dts devel build -f -H <autobot name>`
 
-### 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 <autobot name>.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 <autobot name>`
 
-### 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 <autobot name> && dts devel run -f -H <autobot name>`
 
+## 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.
+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

dependencies-py3.txt

@@ -1,3 +1,2 @@
 apriltag
 flask
-

packages/bot_camera/launch/default.launch

@@ -11,7 +11,10 @@
 
     <arg name="/camera/rect" default="true"/>
      <group if="$(arg /camera/rect)">
+         <remap from="bot_camera/connection_status" to="connection_status"/>     
+         <remap from="bot_camera/car_cmd" to="lane_controller_node/car_cmd"/>
          <remap from="bot_camera/image_in" to="camera_node/image/compressed"/>
+         <remap from="bot_camera/parking_start" to="parking_start"/>
          <remap from="bot_camera/image_out" to="camera_node/image/test"/>
         <include file="$(find bot_camera)/launch/test.launch">
             <arg name="veh" value="$(arg veh)"/>

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()