TUTORIAL 6: Erle-Spider obstacle avoidance

Blockly is a ROS package that provides web-based visualization and block programming tools for robots and drones.

This workspace creates a simple obstacle avoidance behaviour using 'Find path' block. This block returns the center angle of the widest path found. Then, the spider is moved using 'Walk' and 'Turn degrees' blocks.

The aim is to move always to the center of the widest path. To do that, the spider is turned towards the path center the amount of degrees deflected. Hokuyo has a reading of 270º, so the center is 135º in this case.

Since we are using the IMU to turn and the Hokuyo laser to scan the field, we need to use their corresponding initialization blocks.

This is a brief explanation of how the 'Find path' block works.

Avoiding Obstacles part has two loops. The first one removes invalid values from the reading and the second one calculates the center of the widest path.

Laser parameters are read from the 'scan' topic and calculations are done using them. This allows the block to be usable with any kind of sensor able to publish a 'scan' topic.

The simplicity of the block allows the user to build its own obstacle avoidance behaviour. It also allows the block to be used with any robot using a laser.

imports ...
from sensor_msgs.msg import Joy
from sensor_msgs.msg import LaserScan

################
## INITIALIZE ##
################
pub = rospy.Publisher('/joy', Joy, queue_size=10)
msg = Joy()
msg.header.stamp = rospy.Time.now()
rate = rospy.Rate(10)
valueAxe = 0.0
valueButton = 0
for i in range (0, 20):
 msg.axes.append(valueAxe)
for e in range (0, 17):
 msg.buttons.append(valueButton)

rad_to_deg = 57.2958
###################
## LASER PARAMS. ##
###################
laser = rospy.wait_for_message('/scan', LaserScan, timeout=3)

range_min = laser.range_min #hokuyo 0.019, sick 0.05
range_max = laser.range_max #hoyuko 60, sick 25
angle_increment = laser.angle_increment #hokuyo 0.0043, sick 0.005817
mid_angle = laser.angle_max*rad_to_deg #hokuyo 135, sick 135

################
## AVOID OBS. ##
################
#while not rospy.is_shutdown():

laser = rospy.wait_for_message('/scan', LaserScan, timeout=3)

path_distance = 1.2
obstacle_distance = 0.2

path_size = 0
path_beg = 0
path_end = 0
MAX_path_size = 0
MAX_path_beg = 0
MAX_path_end = 0

#create a list of tuples with valid values
valid_ranges = []
stop = 0
for (i,r) in enumerate(laser.ranges):
    if (r >= range_min) and (r <= range_max):
        tup = [i,r]
        valid_ranges.append(tup)

for (j,w) in enumerate(valid_ranges): #w is a tuple of [i,r]
    if w[1] > path_distance:
        if valid_ranges[j-1][1] > path_distance:
            path_size += w[0] - valid_ranges[j-1][0]
            path_end = j
            if path_size > MAX_path_size:
                MAX_path_size = path_size
                MAX_path_beg = path_beg
                MAX_path_end = path_end
        else:
            path_beg = j
            path_size = 0
    '''elif w[1] <= obstacle_distance:
        stop = 1
        print("STOP - "+str(j)+" "+str(w[1]))'''

path_center = (MAX_path_end+MAX_path_beg)/2
path_center_degrees = path_center*angle_increment*rad_to_deg

blockly comes preinstalled in Erle-Brain 2.0. Acquire this artificial brain for making robots and drones here