# Follow wall and avoid obstacles using lidar

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):
super().__init__("PID")
self.forward_speed = 0.25

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
else:
msg.linear.x = self.forward_speed

self.publisher.publish(msg)

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):
rclpy.init(args=args)
node = Wall()
rclpy.spin(node)
rclpy.shutdown()