April tag tag search system on the openmv cam h7 plus

For the doll museum, we are developing an assistant robot that, when approaching the exhibit, will give out information about the exhibit by voice and on the screen. The robot uses the operating system for ROS robots (NVIDIA jetson nano + Ubuntu 18.04 + ROS melodic (https://www.hackster.io/belochka/controlling-the-jetbot-robot-from-ros-014620)).

To search for an exhibit, we decided to use the April tag tag system, each exhibit is equipped with an April tag tag. To determine the label, we will use OPEN MV H7+. There is also an OpenMV LCD Shield available

Apriltag tags can be printed in the Openmv IDE

And print it out

We are writing a program for recognition. Search for labels and send data to the serial port and output to the OpenMV LCD Shield (with scaling)

# Search AprilTags
#
import sensor, image, time, lcd, pyb
from pyb import UART
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.VGA)
sensor.skip_frames(time = 2000)
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)
lcd.init()
clock = time.clock()
uart = UART(3, 9600, timeout_char=1000)
uart.init(9600, bits=8, parity=None, stop=1, timeout_char=1000)
uartget=False
tag_families = 0
tag_families |= image.TAG16H5 # comment out to disable this family
tag_families |= image.TAG25H7 # comment out to disable this family
tag_families |= image.TAG25H9 # comment out to disable this family
tag_families |= image.TAG36H10 # comment out to disable this family
tag_families |= image.TAG36H11 # comment out to disable this family (default family)
tag_families |= image.ARTOOLKIT # comment out to disable this family
start = pyb.millis()
while(True):
   #clock.tick()
   img = sensor.snapshot()
   if pyb.elapsed_millis(start) >= 200:
      arrobj=img.find_apriltags(families=tag_families)
      #print(len(arrobj))
      #if len(img.find_apriltags(families=tag_families))>0:
      if len(arrobj)>0 :
         strsend="#"+str(len(arrobj))+";"
         for tag in arrobj: # defaults to TAG36H11 without "families".
            img.draw_rectangle(tag.rect(), color = (255, 0, 0))
            img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0))
            strsend=strsend+str(tag.id())+";"+str(tag.cx())+";"+str(tag.cy())+";"+str(tag.h())+";"+str(tag.w())+";"
         strsend=strsend+"$"
         print(strsend)
         uart.write(strsend)
         start = pyb.millis()
         pyb.delay(200)
   # get uart data
   if (uart.any()>0):
      s=uart.read()
      str2=str2+s
      if s=='$':
         uartget=True
         if uartget == True:
            #
            print("get data from uart = %s",str2)
            uartget=False
            str2=""
   #
   lcd.display(img.scale(0.25,0.25))
   #print(clock.fps())

Sending data to the port

N;id1;x1;y1;h1;w1;……;idn;xn;yn;hn;wn

Копируем скрипт на openmv cam и переименовываем его в main.py, чтобы он запускался при запуске openmv cam h7+.

Подсоединение к компьютеру по последовательному порту, питание по USB от NVIDIA jetson nano.

The robot is equipped with the rosbridge package and robot_ros recycled (see the material https://www.hackster.io/belochka/controlling-the-jetbot-robot-from-ros-014620 ). The original is here - https://github.com/dusty-nv/jetbot_ros. Modified package - https://github.com/victoruni/jetbot_ros/tree/master/

The results of determining the label will be sent via the serial port to NVIDIA jetson nano with the Ubuntu 18.04 operating system and the ROS Melodic version to the script jetbot_search_apriltag.py. The content of the script

#!/usr/bin/env python
import rospy
import time
import serial

from std_msgs.msg import String, Int16
from geometry_msgs.msg import Twist

rospy.set_param("/jetbot_apriltag/goal",0)
rospy.set_param("/jetbot_apriltag/sub_goal",0)
serial_openmv=serial.Serial("/dev/ttyUSB0", 9600, timeout=1)
pub=rospy.Publisher("/jetbot_motors/cmd_raw",Twist)

def parsestring(data):
   go=0
   rospy.loginfo("data_serial = %s",data)
   # get tags
   tags=[[0]*5 for i in range(6)]
      dd=data.replace("#","").split(";")
      #rospy.loginfo(dd)
      for i in range(int(dd[0])):
         for j in range(5):
           tags[i][j%5]=int(dd[i*5+j+1])
      #rospy.loginfo(tags)
      #
      goal= rospy.get_param("/jetbot_apriltag/goal")
      for i in range(int(dd[0])):
         rospy.loginfo('goal=%s',goal)
         if goal==0:
            go=0
         elif tags[i][0]==goal:
            go=2
            r=tags[i][1]+tags[i][3]/2
            rospy.loginfo('r=%s',r)
            if r<280 or r>380 :
               go=4
            elif tags[i][3]>200 :
               go=0
               rospy.set_param("/jetbot_apriltag/goal",0)
         i=int(dd[0])
         sub_goal= rospy.get_param("/jetbot_apriltag/sub_goal")
         for i in range(int(dd[0])):
            if goal==0:
               go=0
            elif tags[i][1]==goal:
               go=2
            elif tags[i][j%5]==sub_goal:
               go=3
            #elif tags[i][j%5]==sub_goal:
            # go=3
            else: #
               go=4
         rospy.loginfo('go=%s',go)
         if go==4:
            msg=Twist()
            msg.linear.x=0.0;
            msg.linear.y=0.0;
            msg.linear.z=0.0;
            msg.angular.x=0.0;
            msg.angular.y=0.0;
            msg.angular.z=0.5;
            pub.publish(msg)
         elif go==2:
            msg=Twist()
            msg.linear.x=0.5;
            msg.linear.y=0.0;
            msg.linear.z=0.0;
            msg.angular.x=0.0;
            msg.angular.y=0.0;
            msg.angular.z=0.0;
            pub.publish(msg)
         elif go==0:
            msg=Twist()
            msg.linear.x=0.0;
            msg.linear.y=0.0;
            msg.linear.z=0.0;
            msg.angular.x=0.0;
            msg.angular.y=0.0;
            msg.angular.z=0.0;
            pub.publish(msg)

def set_apriltag_purpose(msg):
   rospy.set_param("/jetbot_apriltag/goal",msg.data)
   rospy.loginfo(' apriltag_purpose=%d', msg.data)
   # initialization

if __name__ == '__main__':
   data_serial=""
   data_complete=False
   # setup ros node
   rospy.init_node('jetbot_search_apriltag')
   rospy.Subscriber('set_apriltag_goal', Int16, set_apriltag_purpose)
   # start running
   while not rospy.core.is_shutdown():
      while serial_openmv.inWaiting()>0:
         c=serial_openmv.read()
         #rospy.loginfo(c)
         if c=='$':
            data_complete=True
            break
         else:
            data_serial=data_serial+c
         if data_complete == True:
            parsestring(data_serial)
            data_serial=""
            data_complete=False
         # Update ROS
         rospy.rostime.wallsleep(0.1)

Selecting a label to search from a web page

When you select a tag and press the Search tag key from a web page, the ROS parameter is set

goal_april_tag = new ROSLIB.Param({
   ros : ros,
   name : "/jetbot_apriltag/goal",
});

function start_search() {
   if(startsearchtag==0) {
      document.getElementById("button_search_apriltag").value="STOP_Search_tag";
      var n=document.getElementById("choice_apriltag").options.selectedIndex;
      var val=document.getElementById("choice_apriltag").options[n].value;
      goal_april_tag.set(parseInt(val));
   }
   else {
      document.getElementById("button_search_apriltag").value="Search_tag";
      goal_april_tag.set(0);
   }
startsearchtag=1-startsearchtag;
}

The script jetbot_search_apriltag.py analyzes the value of the parameter and gives a command to rotate the platform until the necessary label falls into the center on the x axis.

msg=Twist()
msg.linear.x=0.0;
msg.linear.y=0.0;
msg.linear.z=0.0;
msg.angular.x=0.0;
msg.angular.y=0.0;
msg.angular.z=0.5;
pub.publish(msg)

If the placemark is close to the center on the x-axis, commands are sent to move the platform straight,

msg=Twist()
msg.linear.x=0.5;
msg.linear.y=0.0;
msg.linear.z=0.0;
msg.angular.x=0.0;
msg.angular.y=0.0;
msg.angular.z=0.0;
pub.publish(msg)

When approaching the placemark at a close distance

msg=Twist()
msg.linear.x=0.5;
msg.linear.y=0.0;
msg.linear.z=0.0;
msg.angular.x=0.0;
msg.angular.y=0.0;
msg.angular.z=0.0;
pub.publish(msg)

And set

rospy.set_param("/jetbot_apriltag/goal",0)

The search is finished !!!

Launch

Terminal 1

roscore 

Terminal 2

roslaunch jetbot_ros launch05. launch

Terminal 3 rosrun

jetbot_ros jetbot_search_apriltag.py

All materials on github https://github.com/victoruni/jetbot_ros/tree/master/