Proposed: Better wall finder algorithm!

[peterborkuti]

Hi,

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

1. 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()

[bayodesegun]

@peterborkuti
Thank you very much for sharing this with us. We will review it as soon as possible and let you know.

[peterborkuti]

About how this works

It needs only some easy maths:

Imagine, that the robot is in point A. Let’s assume, that the room is a rectangle and that the robot is in parallel with the room’s wall. Which means, that the room’s walls are parallel with the coordinate system’s axis. So the upper and lower wall is parallel with the x-axis and the right and left walls are parallel with the y-axis.
The B point is a measured laser distance (the red line).
The DEF lines are the part of an outer rectangle. I named the DE segment as the right width and the EF segment as the upper high of the outer rectangle. This we do not know yet.

For the B point, the smallest outer rectangle’s “upper high” and “right width” are the lengths of AC and BC.
Because we know the alfa angle (this is the laser angle for the B point) we can have the “upper high” and “right width” as height = c*cos(alfa) and width = c*sin(alfa).
Lets imagine, that we counted all the height and width for the laser distances in the upper-right quadrant of the coordinate system. The maximum of the heights will give us FE and the maximum of the widths gives us DE.
We have to do it for all the quadrants. The maximum of the positive values will give us the upper right point of the outer rectangle, the minimum of the negative values will give us the lower-left point of the outer rectangle.

[peterborkuti]

The next step is solving the general case, when the robot is not parallel with the walls.
The algorithm above gives us good results in 4 cases. When the robot is faced up, down, left, or right, because in these cases, the robot is parallel with two walls (and perpendicular with the other two).

Imagine, that the robot is a sligthly turned to the right.
Now the outer rectangle is not optimal. It is a bit bigger then the optimal one.
So what we need is “rotating” the coordinate system one by one (with the degree of the minimal angle of the laser sensor) and computing the outer rectangles.
When the computed outer rectangle area is the smallest, that is the good outer rectangle.
Because of the symmetry, we do not have to rotate between 0 and 360 degrees, only between 0 and 90.

The last step is identifying the wall points in the laser distances.
A laser distance is a wall, if it is “on the outer rectangle”. In other words, where the point’s “height” or “width” is equal to the outer rectangle appropriate “width” and “height”:

        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]