Home » BLOG » MRPiZ

Category: MRPiZ

MRPiZ – suivie de ligne avec openCV & python

Ce tutorial présente l’implémentation d’un suivi de ligne pour le robot mobile MRPiZ.

Matériel nécessaire

  • Un robot MRPiZ
  • Une caméra compatible, idéalement grand angle.
 

Logiciels nécessaire

  • Python 2.7 (déja installé par défaut),
  • La bibliothèque Python MRPiZ (déja installée par défaut),
  • OpenCV pour python

 

Activation de video4linux

Deux méthodes sont possibles pour accéder à la caméra:

  1. PiCamera: la méthode la plus répandue, mais lente car il est nécessaire de transformer l’image pour la mettre au bon format,
  2. v4l: qui s’interface directement avec OpenCV, c’est la méthode choisie pour ce tutorial.

Il nous faut donc activer v4l:

$ sudo modprobe bcm2835-v4l2

Warning

Cette commande est a effectuer à chaque redémarrage.

 

Le suivi de ligne

Le fichier complet se trouve dans Software/Python/tutorials/line_follower/line.py.

Warning

Utilisez CTRL+C pour arrêter le robot.

Importation des modules

import numpy as np
import cv2
import sys
from mrpiZ_lib import *

Paramétrés globaux

# image size
WIDTH = 640
HEIGHT = 480

# turn coeff
COEFF = 0.05
# base robot speed in straight line
SPEED = 30

Activation de la caméra

Pour améliorer les performances, la résolution est réduite à 640 pixels en largeur et 480 en hauteur.

video_capture = cv2.VideoCapture(0)
video_capture.set(3, WIDTH)
video_capture.set(4, HEIGHT)

Boucle principale

La boucle principale va fonctionner à l’infini, pour l’arrêter il faudra appuyer sur CTRL+C.

try:
    while(True):

Capture de l’image

Première étape, on commence par capturer une image.

# Capture the frames
ret, frame = video_capture.read()

Voici un exemple d’image capturée:

0_init.jpg

Suppression de la partie haute

Pour améliorer les performances, on ne va garder que la partie basse de l’image:

# Crop the image
# Keep the 100 lower pixels
crop_img = frame[379:480, 0:640]

1_crop.jpg

Niveaux de gris

Ensuite on passe l’image en niveaux de gris:

# Convert to grayscale
gray = cv2.cvtColor(crop_img, cv2.COLOR_BGR2GRAY)

2_gray.jpg

Flou

Un filtre afin de rendre flou les lignes de l’image est appliqué, il permet de rendre plus efficace les étapes suivantes:

# Gaussian blur
blur = cv2.GaussianBlur(gray,(5,5),0)

3_blur.jpg

Seuillage

Ensuite on va filtrer les parties claires de l’image pour ne garder les parties noires, pour cela, un filtre de seuillage est appliqué:

# Color thresholding
ret,thresh = cv2.threshold(blur,60,255,cv2.THRESH_BINARY_INV)

4_thresh.jpg

Détection de contours

Ensuite, on va utiliser openCV pour détecter les contours:

# Find the contours of the frame
contours,hierarchy = cv2.findContours(thresh.copy(), 1, cv2.CHAIN_APPROX_NONE)

5_contour.jpg

Extraction du plus gros contour

Il nous faut ensuite extraire la ligne la plus large trouvée afin d’éliminer les fausses détections:

# Find the biggest contour (if detected)
if len(contours) > 0:
    c = max(contours, key=cv2.contourArea)
    M = cv2.moments(c)

    # Skip to avoid div by zero
    if int(M['m00']) == 0:
        continue

Calcul du milieu de la ligne

Une fois les contours de la ligne détectée, on calcul le centre de la ligne, c’est la que l’on veut que le robot aille:

# Get the line center
cx = int(M['m10']/M['m00'])
cy = int(M['m01']/M['m00'])

Contrôle des moteurs

Une correction proportionnelle à la différence entre la position de la ligne et le milieu de l’image est calculée. Les moteurs sont ensuite commandés pour ralentir un des moteurs et accélérer l’autre, ceci afin de faire tourner le robot en direction du centre de la ligne.

delta = COEFF * (cx - 320)
motorRight(0, SPEED - delta)
motorLeft(0, SPEED + delta)

Clavier

Enfin, deux lignes de code permettent d’arrêter le robot quand on appuie sur CTRL+C.

except KeyboardInterrupt:
    stop()

Création d’une carte avec le robot MRPiZ

Voici un exemple de création d’une carte avec le robot MRPiZ en utilisant le capteur de distance laser VL53L0X :

Le programme python de la création d’une carte :

  • Le robot tourne à 360° et lit le capteur de distance,
  • Enregistrement de l’orientation du robot et de la distance du capteur dans un fichier csv.
from mrpiZ_lib import *
import time, csv

NameFile = 'carte.csv'

def read_laser():
  p3_1 = proxSensor(3)
  p3_2 = proxSensor(3)
  p3_3 = proxSensor(3)
  p3_4 = proxSensor(3)
  p3_5 = proxSensor(3)
  p3 = (p3_1 + p3_2 + p3_3 + p3_4 + p3_5)/5
  return p3


############################################# MAIN

# wait init
time.sleep(2)

Fichier = open(NameFile,'w')
Robot_tourne = 0

# enable asv control
controlEnable()

while 1:
  # read laser distance sensor
  distance = read_laser()

  # read robot orientation
  orientation = robotPositionO()

  # write in file
  Fichier.write(str(distance))
  Fichier.write(";")
  Fichier.write(str(orientation))
  Fichier.write("\r\n")# nouvelle ligne

  if(Robot_tourne == 0):
    turnLeftDegree(8,360)
    Robot_tourne = 1

Fichier.close()
exit()

Le programme de lecture et d’affichage de la carte  :

import numpy as np
import math
import matplotlib.pyplot as plt
import numpy as np
import csv

liste_distance = []
liste_orientation = []

fichier = open("carte.csv", "r")

c = 0

# lecture fichier
while True:
  ligne = fichier.readline()
  if ligne =='':
    break # fin fichier
  # lecture de la distance (capteur distance)
  distance = ligne.split(';')[0]
  # lecture orientation
  orientation = ligne.split(';')[1]
  # correction de la distance par rapport au centre du robot et la position du capteur
  distance = float(distance) + 50

  # conversion en radian
  if float(orientation) > 0:
    orientation = (float(orientation)*3.14)/180

  liste_distance.append(float(distance))
  liste_orientation.append(float(orientation))
  c = c + 1

ax = plt.subplot(111, projection='polar')
ax.plot(liste_orientation, liste_distance)
ax.grid(True)
plt.show()

 

Exemple d’affichage : 

Imitation learning with MRPiZ robot

Imitation learning exemple with the MRPiZ robot :

  • record the speed of the two motors
  • repead the speed of the motors

 

The python code :

from mrpiZ_lib import *
import time


# acquisition frequency
TIME_ACQ = 0.1

# acquisition time
TEMPS_ACQ = 200

liste_mr = []
liste_ml = []

# main program

# record loop
c = 0

motorsDisable()

time.sleep(1)

while c < TEMPS_ACQ:
  ml = motorLeftSpeed() # read left motor speed
  mr = motorRightSpeed() # read right motor speed
  liste_mr.append(mr)
  liste_ml.append(ml)
  time.sleep(TIME_ACQ)
  print ml, mr, c
  c = c + 1


print "END RECORD -----------"
time.sleep(2)

c=0
cmd_right=0
cmd_left=0
dir_right = 0
dir_left = 0

# H-bridge enable
motorsEnable()

##########################################################

# imitation
while c < TEMPS_ACQ:
  if liste_mr[c] >= 0:
    cmd_right = 2.8*liste_mr[c]
    dir_right = 0
  elif liste_mr[c] < 0:
    cmd_right = 2.8*liste_mr[c]
    dir_right = 1
  else:
    dir_right = 0
    cmd_right = 0

  if liste_ml[c] >= 0:
    cmd_left = 2.8*liste_ml[c]
    dir_left = 0
  elif liste_ml[c] < 0:
    cmd_left = 2.8*liste_ml[c]
    dir_left = 1
  else:
    cmd_left = 0
    dir_left = 0
  print cmd_left, cmd_right, c
  c= c + 1
  # motors commands
  motorRight(dir_right,abs(cmd_right))
  motorLeft(dir_left,abs(cmd_left))
  time.sleep(TIME_ACQ)

stop()
# end

Avancer vers un but avec gestion des obstacles

Un programme python pour faire avancer le robot MRPiZ vers un but avec la prise en compte des obstacles.

#!/usr/bin/env python
# Mace Robotics

from mrpiZ_lib import *
import time

# erreur odometrie
error_odo = 20

# sleep 2 secondes
time.sleep(2)

# but en millimetre
goal_robot = 200

# activer controle
controlEnable()

# lecture du capteur avant
sensor_p3 = proxSensor(3)

# lecture position du robot (axe X)
position_robot = robotPositionX()

distance_robot = goal_robot

while (position_robot < goal_robot-error_odo):
  forwardmm(10,distance_robot)# avancer vers le but
  sensor_p3 = proxSensor(3)# lecture capteur
  time.sleep(0.2) # pause 200 ms
  position_robot = robotPositionX()# lecture position robot
 
  # si obstacle 
  if (sensor_p3 < 100):
    stop()# arret du robot
    distance_robot = goal_robot - position_robot # erreur sur la distance


#end

MRPiZ – odometry

 

 

 

import sys
from mrpiZ_lib import *
import time

time.sleep(5)

controlEnable()
forward_mm(10,220)
turnLeft_degree(10,90)
forward_mm(10,170)
turnLeft_degree(10,90)
forward_mm(10,170)
turnRight_degree(10,90)

forward_mm(10,290)
turnRight_degree(10,90)

forward_mm(10,375)

turnRight_degree(10,90)
forward_mm(10,450)

Dance de robots MRPiZ

 

La gestion des robots en parallèle est gérer par fabric (www.fabfile.org). Un outil SSH permettant d’envoyer plusieurs commandes SSH en parallèle. Chaques robots MRPiZ est identifié par une adresse IP unique:

  • IP du robot n°1: 192.168.1.41
  • IP du robot n°2: 192.168.1.37
  • IP du robot n°3: 192.168.1.47

Le script de contrôle des robots :

#!/usr/bin/env python
import fabric
from fabric.api import run, env, task
from fabric.api import *
import time


env.hosts = ['192.168.1.41', '192.168.1.37', '192.168.1.47']

env.user = 'pi'
env.password = 'raspberry'

@parallel
def hello():
 run('python /home/pi/MRPiZ/Dance.py')

Le script python pour les robots :

#!/usr/bin/env python

import sys
from mrpiZ_lib import *
import time

controlEnable()
while 1:
 forward_mm(13,120)
 back_mm(11,100)
 forward_mm(11,100)
 turnRight_degree(25,360)
 time.sleep(1)
 turnRight_degree(15,90)
 turnLeft_degree(15,180)
 turnRight_degree(15,90)
 back_mm(12,90)

Partie 1 : OpenCV et le robot MRPiZ

Ce tutoriel permet de faire du traitement d’images avec le robot MRPiZ en langage Python.

OpenCV_Logo

Le matériel nécessaire pour le tutoriel :

  • Un robot MRPiZ
  • Un support caméra pour le robot MRPiZ
  • Une caméra Raspberry Pi 8MP

Installation

La première étape est d’installer la camera sur la carte Raspberry Pi :

  • Installation de la camera Raspberry Pi 8MP : ici
  • Mise à jour du système:
sudo apt-get update
sudo apt-get upgrade
  • Installation de la bibliothèque de traitement d’image OpenCv :
sudo apt-get install python-opencv
  • Installation de imutils:
sudo easy_install pip
sudo pip install imutil
sudo apt-get install python-picamera python3-picamera

Une simple photo

Prendre une simple photo avec la camera et l’enregistrer dans une image, voic

#!/usr/bin/python
import picamera

camera = picamera.PiCamera()

# initialisation de la resolution
camera.resolution = (1920, 1080)

# capture d'une image
camera.capture('image.jpg')

Simple photo

Lecture d’une image

Un script pour la lecture d’une image avec OpenCv :

#!/usr/bin/python
import picamera
import cv2

camera = picamera.PiCamera()

# initialisation de la resolution
camera.resolution = (100, 100)

# capture d'une image
camera.capture('image.jpg')

image = cv2.read('image.jpg')

print image

Conversion en HSV

Conversion colorimétrique.

HSV : Hue Saturation Value

#!/usr/bin/python
import picamera
import cv2

camera = picamera.PiCamera()

# initialisation de la resolution
camera.resolution = (1920, 1080)

# capture d'une image
camera.capture('image.jpg')

# lecture de l'image
frame = cv2.imread('image.jpg')


# conversion RGB en HSV de l'image
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)

# enregistrer image hsv
cv2.imwrite('image_hsv.png', hsv)

image HSV

 

 

Fin de la première partie.

MRPiZ en Java

Dans ce tutorial vous allez apprendre à programmer le robot MRPiZ en langage Java.

Remarque : le robot MRPiZ doit etre connecté à internet.

Installation

  • Mise à jour et installation de java :
sudo apt-get update && sudo apt-get install oracle-java7-jdk
  • Installation de la librairie Pi4J pour l’accès hardware de la Raspberry Pi:
curl -s get.pi4j.com | sudo bash

Librairie

La librairie java est disponible sur le Github : ici

Exemple

Voici un exemple de programme (Exemple1.java):

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
import java.io.IOException;
import java.util.Date;
import java.util.concurrent.TimeUnit;

public class Exemple1 {

/****************/
public static void main(String args[]) throws InterruptedException, IOException
{
float tension_batterie;
float distance_sensor1;
float distance_sensor2;

MRPiZ robot = new MRPiZ();

// Lecture de la tension de la batterie
tension_batterie = robot.proxSensor(1);
System.out.println("Batterie = " + tension_batterie);

// avancer le robot pendant 1 seconde
robot.forward(30);

TimeUnit.SECONDS.sleep(2);

// reculer le robot pendant 1 seconde
robot.back(30);

TimeUnit.SECONDS.sleep(2);

// tourner à droite
robot.turnRight(30);

TimeUnit.SECONDS.sleep(1);

// arret du robot
robot.stop();

// Lecture du capteur de proximitè n°1 et n°2
distance_sensor1 = robot.proxSensor(1);
distance_sensor2 = robot.proxSensor(2);

// afficher les valeurs
System.out.println("Sensor 1 = " + distance_sensor1);
System.out.println("Sensor 2 = " + distance_sensor2);

}
}
  • Compilation du programme :
javac -classpath .:classes:/opt/pi4j/lib/'*' -d . Exemple1.java
  • Exécution du programme :
sudo java -classpath .:classes:/opt/pi4j/lib/'*' Exemple1