I want to follow a wall and avoid any obstacles on the way. My robot has a 360 degree sensor (lidar). I want to calculate a line (vector) to follow.

I tried a lot of different solutions but I didn't achieve the goal. I also checked this article, Calculate robot heading to follow wall and avoid obstacles, but it didn't really help or explain things.

My robot sometimes stuck between an obstacle (turning in both sides). I think PID can't help here.

What can I do to avoid obstacles on the way?

Here is the code.

#! /usr/bin/python
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan
from std_msgs.msg import Float64
from geometry_msgs.msg import Twist
import numpy as np
import time

class Wall(Node):
    def __init__(self):
        self.forward_speed = 0.25 
        self.turning_speed = 0.5 # rad/s

        self.publisher = self.create_publisher(Twist, "/cmd_vel", 10)
        self.subscriber = self.create_subscription(LaserScan, "/scan", self.scan_callback, 10)

    def scan_callback(self, scan: LaserScan):
        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

        let = 4.0 #range of an obstacle, wall ..
        left = Average(scan.ranges[225:315]) # 90 degrees from left (get more precise values from range)
        front = Average(scan.ranges[160:200]) # 40 degrees from front (get more precise values from range)

        if (left < let and front < let): #left and front are locked
            msg.angular.z = -self.turning_speed #move right
        elif (left > let and front > let): #left and front are unlocked
            msg.angular.z = self.turning_speed / 2 #move left
        elif (left > let and front < let): #left is unlocked, front is locked
            msg.angular.z = self.turning_speed / 2 #move left
            msg.linear.x = self.forward_speed


def Average(lst):
    # return sum(lst) / len(lst)
    res = np.sum(np.ma.masked_invalid(lst)) / len(lst) #ignore Inf
    res =  res.__float__() # '--' to nan
    if (np.isnan(res)):
        res = 10.0 #assume there is a lot space
    return res

def check_wall(args=None):
    node = Wall()

Floor plan of obstacles


1 Answer 1


From this code you might only get to solve the wall following behavior but to add another behavior of obstacle avoiding you need to define in states. So you can easily shift between them . PID is just for smoothing things .

Look into Bug 2 Algorithm -> heads straight for the target until it hits an obstacle, then follows the obstacle's contour until the straight-line path it was initially following is optimal (i.e., it won't get any shorter by continuing to follow the obstacle's contour). This kind of algorithm could be applied to your problem.


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.