-
Notifications
You must be signed in to change notification settings - Fork 2
Code Architecture
A visualisation of a slam map is very important for mobile robots as they use it for navigation. The interface provides a point of view from the Robot looking out on the SLAM map. Because of this perspective, I took the 2D map and turned it into a 3D mesh to work with the 3D VR environment and reduce cognitive load of mentally converting 2d to 3d.
Using SLAM_Toolbox has smoother motion than using Cartographer, but either works with my system as long as they publish the /map topic and provide a /tf transform
The 3D mesh is generated by taking the 2D map and creating a vertex at every corner of each pixel, scaled based on the scale presented in the message. Initially, this was all it was while taking any vertices bordering a pixel over a threshold of 50, and increasing the height. However, this made the walls sloped which blocked off areas which were actually possible to navigate through. To fix this, the grid maintained the same height and a second grid of vertices was added above the first one, then triangles were added to make each pixel under 50 a face on the first grid, and pixels above 50 a face on the second grid. Faces were then added on the boundaries between pixels on different sides of the threshold, as vertical walls. Since this is quite an intensive operation, I initially throttled the generation to be once a second which helped, but produced spikes in frame times every time the mesh was generated. I then refactored it to use the unity Job system and burst compiler so that the mesh vertices (vertex buffer) and triangles (index buffer) were generated in parallel on a separate thread, and only update the mesh on the main thread once the calculations were complete.
There is also a minimap version of this map which allows for navigation by providing a destination. This was on the floor, but because of VR UX it was moved to a panel in front of the user as having things at head level is more comfortable. It has also been made to have an adjustable height and rotation.
Having a 3D slam map allowed me to project the camera image onto the SLAM world. This is beneficial because it allows users to understand what specific blockages are. Originally different visualisations and controls were place all around the users, but that is a lot of movement, and VR UX should keep things within a specific range of angles in front of the user.
Each of these two views were generating the mesh separately, so to help stop the double workload, I created a separate component (OccupancyMeshGenerator) which subscribes to the topic and generates the mesh, and then a separate component (OccupancyMesh) is attached to any gameobject that should show that mesh, readjusts the position of the gameobject and renders the mesh. The minimap version also has an OccupancyMaterialController which works in conjunction with a custom shader to only show the part of the map that fits on the minimap region. The controller takes the centre of the minimap which passes it to the shader which takes the fragment (pixel) location in world space and finds the distance from the centre in both the local x and z coordinates. That distance is then used to determine the opacity of the map. The edge opacity is a slow gradient combined with a slight noise function.
Even as the map is technically stationary and the robot is moving, the focus of the interface is on the robot. I therefore made both the minimap and the global map move relative to the stationary robot. This is done by the script InverseMapTracking It has been implemented so that either the robot or the map can move or rotate. The robot can move using the TurtlebotTracking script. The current chosen method is that the minimap has the map move position, and has the robot rotate. The world map rotates and moves while the cockpit which represents the robot is stationary.
LiDAR is visualised using the /scan topic. This topic returns an array of distances telling how far each laser reached, as well as the start and change in rotation for each one. The component LidarParticleSystem uses a unity particle system to display where the lidar would have hit. It displays only the distances that lie within the minimum and maximum bounds, and spawns them in world space from the current gameobject’s position at the angle and distance. There is a slight delay in updating the scan based on the ros update rate, so if the virtual gameobject moves, the particles move separately, and only move once there is a new message providing the location. The alternative is using local space, but this causes the issue if it is attached to the robot, then when the robot turns according to odometry, then so do the particles, where they should stay still relative to the map.
The camera has 2 different visualisations. The first one is being attached as a texture to the virtual screen (a plane with the texture set on the material) This component is called CameraPreview. It also allows the user to click the screen which enlarges it to be directly in front of the user rather than above. When it is enlarged, a mini SLAM map is shown where the small screen used to be. This can be toggled back and forth. The second visualisation is projecting the texture from the virtual location of the camera onto the world 3D SLAM map. Originally, I intended to use the Projector component, but that requires a shader which is no longer available, so I switched to URP (Universal Render Pipeline) which has Decals. However these decals are an orthographic projection, which means that the projection does not line up with the obstacles. Finally I set the texture to be the light cookie on a spotlight. This texture has to be put on a RenderTexture which is what is actually set as the light cookie. This is because constantly reassigning the light cookie causes the light cookie atlas to fill up, and Unity resizes the textures to fit. However, by using a RenderTexture makes Unity treat it as the same texture and not reshuffle the cookie atlas. There is an issue with this approach. The light has an inverse square falloff which means that when I increase the intensity to allow it to reach the obstacles, the floor is blown out and over exposed. It can also be overwhelmed by the main directional light. This works for now, but an ideal solution would be to create a shader to just project the colour without any lighting information. The light cookie is controlled by the LightProjection script.
The main issue these scripts used to have to deal with is that the incoming image has 0, 0 in the upper left corner while unity uses the lower left corner, causing the image to be flipped vertically. What I do to counteract this in the LightProjection script is to use the Unity Job system along with the Burst compiler to manually flip the image vertically before assigning the values to the image. The CameraPreview does not need to do this because the plane is scaled by -1 on the z axis which flips the UVs.
However, to reduce network bandwidth usage, I switched to using JPEG compression on the image. This is then decoded once in the interface by CameraSubscriber on a different thread. Both CameraPreview and LightProjection were refactored to read the decoded image from there.
The navigation path is rendered using a line renderer controlled by the RobotPlan component. The destination at the end of the navigation path is shown using a sphere controlled by the GoalPosePosition script.
There are two forms of teleoperation, direct controls and setting a destination. Direct controls are an alternative to the minimap. There are several possible ways of doing this. The current implementation is not very intuitive, but it consists of 2 joysticks. One for forward movement and the other for turning movement. An alternative that could be implemented is a steering wheel that only turns when there is forward acceleration. Another is using the controller buttons as directional control. All of these would work by sending messages to the /cmd_vel topic via the TeleoperationController script.
The destination based controls use the minimap, and is determined by the RobotNavigationInteractable script which sends the destination to the TeleoperationController to publish on the /goal_pose topic. There are 3 different controls. The first is setting a destination based on where the user is pointing on the minimap, (This should also be enabled for the global SLAM map.) Publishing to /goal_pose can and should be refactored to use the action server to allow cancelling the navigation, and having multiple waypoints. The main reason this was not the initial implementation is because the ROS Tcp Connector does not have an action server/client implementation. There is an implementation made by a third party for a different project called Fetch VR. The 2nd 2 controls are for moving and rotating the minimap to look at areas that are known, but are not visible when centred on the robot. However these are not completely intuitive, so it should be reimplemented where any one controller is purely moving it, while the other can rotate around the first, similar to google maps. (Scaling could a possibility with that)
The /rosout topic provides all of the logs from every ros node. This was a challenge because text in VR can be hard to read.To do this, there is an initial panel which just has the sources of logs which you can select. This opens a panel to allow you to switch between the severities of logs. This shows on the right a list of the most recent logs of that severity, truncated to prevent overflowing. Clicking any of these messages displays the fuller message along with the time, source node, source file, and line number.
-
LoggerSource- data structure to contain all logs for a single node/log source -
LoggersUIController- subscribes to topic, adds logs to correct source, and triggers UI updates -
LoggerSourceToggle- a toggle for every source which opens the secondary panel with the corresponding LoggerSource when selected -
LoggerSourceUIController- filters by severity, and allows switching of selected log message -
LogMsgListItem- contains an individual log message and talks to LoggerSourceUIController when pressed