Fix lift cabin graphics on rotated floor plans

[TanJunKiat]

Bug

This PR aims to solve the lift alignment issue presented in #505.

There are two issues to resolve.

Firstly is the graphics of the lift cabin itself, and secondly is the vertex within the lift.

Fix

For the lift cabin, the methodology is straight forward.

We defined a function to calculate a mean rotation based on the fiducials available and store it as a new variable in the transform class. Then when a compute_transform function is called, the rotation will be used to calculate the correct translation between levels. Then the transform will flow down to the draw function of the lift, which will use the same rotation to calculate the position of the cabin in the level.

Outstanding

We still need to shift the lift_cabin vertex. Right now, all vertices are transformed using the same translation and scale method across all levels. But for lifts, there is a reference level for each level.

The transformation of the lift_cabin vertex should bypass the typical vertex transform, get the transform between the current level and the reference level, and apply accordingly.

We can either do a filter and skip transform for vertices with lift_cabin properties, and implement the drawing of lift_cabin vertex within the draw function of the lifts, or pass the transform function of all the lifts from the respective reference levels to the current viewport level down to the vertex.draw function.

[mxgrey]

We still need to shift the lift_cabin vertex.

Looks like this was finished by cfbb8cb right? When I test with this branch it looks like everything is working as intended.

[TanJunKiat]

We still need to shift the lift_cabin vertex.

Looks like this was finished by cfbb8cb right? When I test with this branch it looks like everything is working as intended.

The rmf_traffic_editor package seems to be working.

I have yet to check but I feel we will need to make the corresponding changes in the rmf_building_map_tools package.

Need to make sure the nav_graphs and the world files are generated correctly.

Will get on that soon.

[mxgrey]

Need to make sure the nav_graphs and the world files are generated correctly.

Right, I'm testing that out right now and it's looking good. There are just two issues that I've seen:

• It turns out we've been using the wrong rotation for floorplans that are sent to rviz (fixed here)
• The alignment optimizer is not quite aligning the levels precisely enough for rmf_fleet_adapter's graph parser. The vertices in the lift are a little outside the 10cm error threshold. I'm tracking down how to tighten the optimization right now.

[mxgrey]

After spending a long time being baffled because I couldn't find an error in the code anywhere, I eventually realized that the lift vertices weren't aligning correctly because the new code to set the vertex position wasn't able to run on them until the lift vertex itself gets moved.

After wiggling the vertices of both lifts and regenerating the nav graph, the vertices are well within the threshold.

[mxgrey]

Thanks for this contribution, @TanJunKiat ! The alignment seems to be working exactly the way it should.