Hi,

I did not like the wall finder method described in the project:

- Because of the 360º laser, the robot can detect the closest wall (shortest laser ray)

I think, this is obviously bad, because if the robot appears near an obstacle, then the robot will go to the obstacle not to the wall.

So I constructed another “wall finder” algorithm. Because it seems to me work and it was fun to make, I would like to share it with you.

**I assumed, that the robot is in a rectangle room.**

So the algorithm finds the smallest outer rectangle based on laser scanned distances.

It was easy if I assumed, that the robot is parallel with the walls:

```
ranges: List[float] = np.copy(msg.ranges)
angles: List[float] = [i * msg.angle_increment for i,r in enumerate(ranges)]
heights: List[float] = [r * math.cos(angles[i]) for i,r in enumerate(ranges)]
widths: List[float] = [r * math.sin(angles[i]) for i,r in enumerate(ranges)]
# rectangle height upward from the robot
h_plus = max(heights)
# rectangle height downward from the robot, this is negative
h_minus = min(heights)
# rectangle width on the right side of the robot
w_plus = max(widths)
# rectangle width on the left side of the robot, this is negative
w_minus = min(widths)
rectangle_area = (h_plus - h_minus) * (w_plus - w_minus)
```

In the next version, I tried to rotate the coordinate of the rectangle in the range of 0-90 degrees.

If the rectangle has the minimum area, then this is the rotation of the robot compared to the room:

```
ranges: List[float] = np.copy(msg.ranges)
for beta in range(90):
angles: List[float] = [(i + beta) * msg.angle_increment for i,r in enumerate(ranges)]
heights: List[float] = [r * math.cos(angles[i]) for i,r in enumerate(ranges)]
widths: List[float] = [r * math.sin(angles[i]) for i,r in enumerate(ranges)]
# rectangle height upward from the robot
h_plus = max(heights)
# rectangle height downward from the robot, this is negative
h_minus = min(heights)
# rectangle width on the right side of the robot
w_plus = max(widths)
# rectangle width on the left side of the robot, this is negative
w_minus = min(widths)
rectangle_area = (h_plus - h_minus) * (w_plus - w_minus)
# if rectangle_area is less than for the rectangles for previous betas, store it as
# the minimum
```

I put the coordinates of the wall into a topic:

```
def sendWall(self, msg: LaserScan, h_plus, h_minus, w_plus, w_minus, heights, widths):
wall = copy.deepcopy(msg)
for i,v in enumerate(wall.ranges):
wall.ranges[i]=0.0
for i,r in enumerate(msg.ranges):
if self.eq(heights[i], h_plus, h_minus) or self.eq(widths[i], w_plus, w_minus):
wall.ranges[i] = msg.ranges[i]
self.publisher.publish(wall)
def eq(self, v, v_plus, v_minus):
return abs(v - v_plus) < 0.03 or abs(v - v_minus) < 0.03
```

Here the red dots are the laser scan ranges and the white dots are the laser scanned ranges considered to be walls:

It is a bit slow (360 * 90 computation), so do not hesitate to share me if you find optimization or other faster methods. But this is just for fun, probably no real-world application as we have the navigation/mapping packages.

Here is the whole class:

```
from typing import List
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan
from rclpy.qos import ReliabilityPolicy, QoSProfile
import numpy as np
import math
import copy
class OuterRectangleFinder(Node):
def __init__(self):
super().__init__('outer_rectangle_finder')
self.subscriber = self.create_subscription(
LaserScan,
'/scan',
self.listener_callback,
QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE))
self.publisher = self.create_publisher(LaserScan, 'wall', 10)
def listener_callback(self, msg: LaserScan):
if msg.ranges == None:
return
m_heights = []
m_widths = []
m_h_plus = 0
m_h_minus = 0
m_w_plus = 0
m_w_minus = 0
m_beta = 0
m_area = float("inf")
ranges: List[float] = np.copy(msg.ranges)
offset_pi_half = round(math.pi / 2.0 / msg.angle_increment)
for beta in range(offset_pi_half):
angles: List[float] = [(i + beta) * msg.angle_increment for i,r in enumerate(ranges)]
heights: List[float] = [r * math.cos(angles[i]) for i,r in enumerate(ranges)]
widths: List[float] = [r * math.sin(angles[i]) for i,r in enumerate(ranges)]
# rectangle height upward from the robot
h_plus = max(heights)
# rectangle height downward from the robot, this is negative
h_minus = min(heights)
# rectangle width on the right side of the robot
w_plus = max(widths)
# rectangle width on the left side of the robot, this is negative
w_minus = min(widths)
rectangle_area = (h_plus - h_minus) * (w_plus - w_minus)
if m_area > rectangle_area:
m_beta = beta
m_area = rectangle_area
m_heights = heights
m_widths = widths
m_h_plus = h_plus
m_h_minus = h_minus
m_w_plus = w_plus
m_w_minus = w_minus
if m_area == float("inf"):
self.get_logger().info('Rectangle are is inf. Quitting')
return
self.get_logger().info('Beta:' + str(m_beta) + ' Rectangle area:' + str(m_area))
self.sendWall(msg, m_h_plus, m_h_minus, m_w_plus, m_w_minus, m_heights, m_widths)
def sendWall(self, msg: LaserScan, h_plus, h_minus, w_plus, w_minus, heights, widths):
wall = copy.deepcopy(msg)
for i,v in enumerate(wall.ranges):
wall.ranges[i]=0.0
for i,r in enumerate(msg.ranges):
if self.eq(heights[i], h_plus, h_minus) or self.eq(widths[i], w_plus, w_minus):
wall.ranges[i] = msg.ranges[i]
self.publisher.publish(wall)
def eq(self, v, v_plus, v_minus):
return abs(v - v_plus) < 0.03 or abs(v - v_minus) < 0.03
def main(args=None):
rclpy.init(args=args)
simple_subscriber = OuterRectangleFinder()
rclpy.spin(simple_subscriber)
simple_subscriber.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
```