Line Follower

Get the real vehicle at our store!

Source code



REMINDER: configure your environment before starting this tutorial.


source ~/simulation/ros_catkin_ws/devel/setup.bash
cd ~/simulation/ardupilot/APMrover2
../Tools/autotest/sim_vehicle.sh -j 4 -f Gazebo
# once MAVProxy has launched completely, load the parameters
param load /[path_to_your_home_directory]/simulation/ardupilot/Tools/Frame_params/3DR_Rover.param
# NOTE: replace [path_to_your_home_directory] with the actual path to your home directory.
# Example: param load /home/john/simulation/ardupilot/Tools/Frame_params/3DR_Rover.param
param set SYSID_MYGCS 1

In another terminal

source ~/simulation/ros_catkin_ws/devel/setup.bash
roslaunch ardupilot_sitl_gazebo_plugin rover_circuit.launch

This will spawn the Erle-Rover inside a circuit.

Figure 1 - General view of the simulation. The rover will be spawned inside a circuit.
Figure 2 - Closer look of the rover inside the circuit.

Run the node by running:

cd ~/simulation/ros_catkin_ws
source devel/setup.bash
rosrun erle_rover_followline erle_rover_followline.py

This will open the following window and start moving the rover trough the circuit following the red line

Figure 3 - Window frame showing the result of the node

You will notice that the window shows two different images. The first one, on the left, shows the raw image from the camera of the rover. The second one, though, shows the processed image after extracting the red color from the first image. The green dot in the second image shows the centroid of the red area, which represents the direction that the rover needs to follow.

Here is the code of the node

#!/usr/bin/env python

import roslib
import sys
import rospy
import cv2
import numpy as np
import mavros
from cv_bridge import CvBridge, CvBridgeError

from sensor_msgs.msg import Image
from mavros.msg import OverrideRCIn

class image_converter:

  def __init__(self):

    self.bridge = CvBridge()
    self.pub = rospy.Publisher('/mavros/rc/override', OverrideRCIn, queue_size=10)
    self.image_sub = rospy.Subscriber("/rover/front/image_front_raw",Image,self.callback)

  def callback(self,data):
    try:
      cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
    except CvBridgeError as e:
      print(e)

    height, width, channels = cv_image.shape
    crop_img = cv_image[200:(height)/2+150][1:width]

    lower = np.array([0, 0, 79], dtype = "uint8")
    upper = np.array([40, 40, 191], dtype = "uint8")

    mask = cv2.inRange(crop_img, lower, upper)
    extraction = cv2.bitwise_and(crop_img, crop_img, mask = mask)
    m = cv2.moments(mask, False)
    try:
      x, y = m['m10']/m['m00'], m['m01']/m['m00']
    except ZeroDivisionError:
      x, y = height/2, width/2
    cv2.circle(extraction,(int(x), int(y)), 2,(0,255,0),3)

    cv2.imshow("Image window", np.hstack([crop_img,extraction]))
    cv2.waitKey(1)

    yaw = 1500 + (x - width/2) * 1.5
    print "center=" + str(width/2) + "point=" + str(x) + "yaw=" +  str(yaw)
    throttle = 1900

    if (yaw > 1900):
      yaw = 1900
    elif (yaw < 1100):
      yaw = 1100

    msg = OverrideRCIn()
    msg.channels[0] = yaw
    msg.channels[1] = 0
    msg.channels[2] = throttle
    msg.channels[3] = 0
    msg.channels[4] = 0
    msg.channels[5] = 0
    msg.channels[6] = 0
    msg.channels[7] = 0
    self.pub.publish(msg)

def main(args):
  ic = image_converter()
  rospy.init_node('erle_rover_followline', anonymous=True)
  try:
    rospy.spin()
  except KeyboardInterrupt:
    print("Shutting down")
  cv2.destroyAllWindows()

if __name__ == '__main__':
    main(sys.argv)

Now, let's break the code down.

import roslib
import sys
import rospy
import cv2
import numpy as np
import mavros
from cv_bridge import CvBridge, CvBridgeError

Python modules that need to be included.

from sensor_msgs.msg import Image
from mavros.msg import OverrideRCIn

ROS messages. Image is for the messages from the camera topic and OverrideRCIn to override the RC of the Rover.

class image_converter:

Class definition.

def __init__(self):

    self.bridge = CvBridge()
    self.pub = rospy.Publisher('/mavros/rc/override', OverrideRCIn, queue_size=10)
    self.image_sub = rospy.Subscriber("/rover/front/image_front_raw",Image,self.callback)

Definition of the class initialization method. Initializes the OpenCV bridge, the publisher of the RC and the subscriber to the front facing camera of the rover.

def callback(self,data):

Definition of the callback method for the laser subscriber.

try:
      cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
    except CvBridgeError as e:
      print(e)

Read the image from the message and convert it to CV2.

height, width, channels = cv_image.shape
crop_img = cv_image[200:(height)/2+150][1:width]

Crop the image to ignore the lower part of the image where we can see the wheels of the rover.

lower = np.array([0, 0, 79], dtype = "uint8")
upper = np.array([40, 40, 191], dtype = "uint8")

Set the lower and the upper limits for the red color. All the BGR values between (0,0,79) and (40,40,191) will be considered red.

mask = cv2.inRange(crop_img, lower, upper)
extraction = cv2.bitwise_and(crop_img, crop_img, mask = mask)

Extract the red color from the image.

m = cv2.moments(mask, False)
try:
  x, y = m['m10']/m['m00'], m['m01']/m['m00']
except ZeroDivisionError:
  x, y = height/2, width/2
cv2.circle(extraction,(int(x), int(y)), 2,(0,255,0),3)

Find the center of the red area we extracted.

cv2.imshow("Image window", np.hstack([crop_img,extraction]))
cv2.waitKey(1)

Show both images: the original raw image and the extracted red area.

yaw = 1500 + (x - width/2) * 1.5
print "center=" + str(width/2) + "point=" + str(x) + "yaw=" +  str(yaw)
throttle = 1900

if (yaw > 1900):
  yaw = 1900
elif (yaw < 1100):
  yaw = 1100

Calculate the yaw from the direction angle and limit it. The throttle remains constant.

msg = OverrideRCIn()
msg.channels[0] = yaw
msg.channels[1] = 0
msg.channels[2] = throttle
msg.channels[3] = 0
msg.channels[4] = 0
msg.channels[5] = 0
msg.channels[6] = 0
msg.channels[7] = 0
self.pub.publish(msg)   

Create and publish the ROS message that will override the RC that controls the rover.

def main(args):
  ic = image_converter()
  rospy.init_node('erle_rover_followline', anonymous=True)
  try:
    rospy.spin()
  except KeyboardInterrupt:
    print("Shutting down")
  cv2.destroyAllWindows()

if __name__ == '__main__':
    main(sys.argv)

Main method of the script. Initializes ROS and closes cv2 windows on exit.

If besides simulation, you're interested in translating these behaviors to real scenarios you can purchase the real vehicle at our store