Obstacle Avoidance

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_maze.launch

This will spawn the Erle-Rover inside a maze.

Figure 1 - General view of the simulation. The rover will be spawned inside a maze.

Run the node by running:

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

This will open the following window and start moving the rover trough the maze

Figure 2 - Window frame showing the result of the node

The blue lines represent the laser readings and the red line the direction that the rover needs to follow. The number is simply the angle that the line has with the Y axis.

Here is the code of the node

#!/usr/bin/env python
import rospy

import cv2
import numpy as np
import math
import mavros

from sensor_msgs.msg import LaserScan
from mavros.msg import OverrideRCIn

pub = rospy.Publisher('/mavros/rc/override', OverrideRCIn, queue_size=10)

def callback(data):
    frame = np.zeros((500, 500,3), np.uint8)
    angle = data.angle_max
    Vx = 250
    Vy = 250
    for r in data.ranges:
        if r == float ('Inf'):
            r = data.range_max
        x = math.trunc( (r * 10)*math.cos(angle + (-90*3.1416/180)) )
        y = math.trunc( (r * 10)*math.sin(angle + (-90*3.1416/180)) )
        cv2.line(frame,(250, 250),(x+250,y+250),(255,0,0),1)
        Vx+=x
        Vy+=y
        angle= angle - data.angle_increment

    cv2.line(frame,(250, 250),(250+Vx,250+Vy),(0,0,255),3)
    cv2.circle(frame, (250, 250), 2, (255, 255, 0))
    ang = -(math.atan2(Vx,Vy)-3.1416)*180/3.1416
    if ang > 180:
        ang -= 360
    cv2.putText(frame,str(ang)[:10], (50,400), cv2.FONT_HERSHEY_SIMPLEX, 2, (255,255,255))

    cv2.imshow('frame',frame)
    cv2.waitKey(1)

    yaw = 1500 + ang * 40 / 6
    throttle = 1900

    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
    pub.publish(msg)   

def laser_listener():
    rospy.init_node('laser_listener', anonymous=True)

    rospy.Subscriber("/scan", LaserScan,callback)
    rospy.spin()

if __name__ == '__main__':
    laser_listener()

Now, let's break the code down.

import rospy
import cv2
import numpy as np
import math
import mavros

Python modules that need to be included.

from sensor_msgs.msg import LaserScan
from mavros.msg import OverrideRCIn

ROS messages. LaserScan is for the laser readings and OverrideRCIn to override the RC of the Rover.

pub = rospy.Publisher('/mavros/rc/override', OverrideRCIn, queue_size=10)

The publisher that overrides the values of the RC.

def callback(data):

Definition of the callback method for the laser subscriber.

frame = np.zeros((500, 500,3), np.uint8)
angle = data.angle_max
Vx = 250
Vy = 250

Initialization of the frame where the information will be displayed, the steering angle and the vector that will represent the direction of the rover.

for r in data.ranges:
        if r == float ('Inf'):
            r = data.range_max
        x = math.trunc( (r * 10)*math.cos(angle + (-90*3.1416/180)) )
        y = math.trunc( (r * 10)*math.sin(angle + (-90*3.1416/180)) )
        cv2.line(frame,(250, 250),(x+250,y+250),(255,0,0),1)
        Vx+=x
        Vy+=y
        angle= angle - data.angle_increment

For laser reading calculate the mathematical vector between the rover and that poing using trigonometry. Then, draw the line in the frame and increment the direction vector (Vx,Vy). The sum of all the vector will give us the direction vector, which is the direction that the rover needs to follow. Finally, update the angle for the next iteration.

cv2.line(frame,(250, 250),(250+Vx,250+Vy),(0,0,255),3)
cv2.circle(frame, (250, 250), 2, (255, 255, 0))
ang = -(math.atan2(Vx,Vy)-3.1416)*180/3.1416
if ang > 180:
    ang -= 360
cv2.putText(frame,str(ang)[:10], (50,400), cv2.FONT_HERSHEY_SIMPLEX, 2, (255,255,255))

cv2.imshow('frame',frame)
cv2.waitKey(1)

Draw information in the frame:

  • The direction vector (red line)
  • The circle that represents the limit of the laser readings (blue lines)
  • The angle of the direction vector with the Y axis (white numbers in the bottom)
Figure 3 - Window frame showing the result of the node
yaw = 1500 + ang * 40 / 6
throttle = 1900

Calculate the yaw from the direction angle. 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
pub.publish(msg)   

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

def laser_listener():
    rospy.init_node('laser_listener', anonymous=True)

    rospy.Subscriber("/scan", LaserScan,callback)
    rospy.spin()

if __name__ == '__main__':
    laser_listener()

Main method of the script. It does the usual things that a subscriber node does: initialize ROS, subscribe to a topic (in this case the laser) and advertise the publisher.

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