Parking state

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}"
 
@@ -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
-# <==================================================
+# <==================================================

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

dependencies-py3.txt

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

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"

packages/bot_camera/launch/default.launch

@@ -11,11 +11,13 @@
 
     <arg name="/camera/rect" default="true"/>
      <group if="$(arg /camera/rect)">
+         <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)"/>
         </include>
     </group>
 
-</launch>
+</launch>

packages/bot_camera/launch/test.launch

@@ -7,4 +7,4 @@
     <group ns="$(arg veh)">
         <node pkg="bot_camera" type="$(arg node_name).py" name="$(arg node_name)" output="screen" required="true" />
     </group>
-</launch>
+</launch>

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

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
-