Navigation towards a brick
We apply some image processing using OpenCV on each segmented frame: lego bricks are bounded by blue rectangles, obstacles by red rectangles and edges by yellow contours. Since the obstacles and edges are segmented as the same class, we identify an edge as an obstacle which covers the entire width of the frame (with the assumption that other obstacles are smaller in width). On the edge contour, we also mark the middle lowest point, to have an estimation of how close the robot is to the edge.
When searching for a brick in the workspace, we divide the frame of the lower camera into 3 sections: left, middle and right. The middle section is a bit narrower in the lower part than the upper part of the frame, for a smoother navigation of the robot. To direct the robot when a brick is detected, we follow the lowest blue rectangle: if it's on the right - turn right; if it's on the left - turn left; if it's in the middle - move forward, and when it's at the bottom of the frame - pick it up.
In fact, there are 2 types of frame division, one for the bricks and the other for obstacles. For the latter, the middle section is wider at the bottom than the top. The logic for avoiding obstacles will be explained later with more details.
Navigation towards a box
Once a brick is picked up, the navigation is done according to the upper camera. The same image processing (described above) is applied on each segmented frame. The only difference is that on the way to a box, every object in the frame is considered as an obstacle.
Having 3 frame sections, and different marker for each box, we explore the workspace until the target marker ID is found. The directions of the robot to a marker is very similar to the one towards a brick: we follow the rectangle around the marker: if it's on the right - turn right; if it's on the left - turn left; if it's in the middle - move forward, and when it's close enough, i.e. the length of both vertical edges are above a certain threshold - drop the brick.
In order to avoid obstacles in the workspace, we implemented 2 operation modes: put away and dodge.
This opperation mode is applied only on the way to a brick, assuming the obstacles in the workspace are small and light. Since the middle section of the obstacles frame sections is wide enough, the navigation is as follows: we follow the lowest red rectangle. if it's on the right or on the left - move forward; if it's in the middle and there exists a brick in the frame - moveforward, and when it's at the bottom of the frame - pick it up, move it aside, drop it and return to your previous position before picking it up. The following video demonstrates the logic of moving an obstacle aside and placing a brick in a box.
This opperation mode can be applied on the way to a brick, for any size of obstacles. Since the middle section of the obstacles frame sections is wide enough, the navigation is as follows: we follow the lowest red rectangle. if it's on the right or on the left - move forward; if it's in the right side of the middle - turn left, otherwise - turn right. The following video demonstrates the logic of dodging an obstacle.