We split up our project into 3 main design components:
We wanted to use 3D cameras in order to gain better information about objects’ 3D shape and location, which we could use to identify objects and better interact with them. We were presented with two 3D camera options which 106A had on hand: Intel RealSense D435 or Microsoft Kinect. From our research, we found that the RealSense camera was more effective at shorter distances (0.2-1.2m), while the Kinect was optimized for longer ranges (0.5m-4.4m). Since the scope of our project was limited to working on a smaller scale (small boxes at closer ranges), we chose to use the RealSense D435. However, future work involving full-scale chairs or tables in a full-sized room may find more success in using the Microsoft Kinect, or some fusion of the two camera sensors.
We originally planned to connect the RealSense camera directly to the turtlebot via USB; however, attempts to do this were unsuccessful, as the depth module of the ROS realsense2_camera package (used to publish data from the camera) had some incompatibilities with the Turtlebot’s hardware. This required us to instead connect the camera directly to the lab computer via a long wire, which limited the range and movement of the robot.
We chose to implement our 3D occupancy grid using a 3D matrix of log odds, similar to the 2D implementation we used in Lab 8. While we wanted our occupancy grid to be as high resolution as possible, each iteration of processing camera input and updating the occupancy grid ran incredibly slowly; for instance, running our occupancy grid on a resolution of 1 mm took almost two minutes to run one update iteration, which was far too slow to use effectively for live updates. So, we settled on an occupancy grid resolution of 1 cm; through various other optimizations (discussed further in the Implementation section), we were able to bring the update time down to around 2-3 seconds.
Once we generated our occupancy grid, we want to be able to process and segment the occupancy grid to determine the location of the objects. We chose to use agglomerative clustering on the occupied voxels to segment individual objects by color.
In order to better classify objects that were primarily of solid colors, we additionally added a color parameter to our occupancy grid such that every voxel has an assigned color, corresponding to the detected color at that location. This assisted in clustering voxels and identifying objects.
When the occupancy grid updates, it automatically triggers a path planning loop. Upon updating, the path planning loop would find the optimal path through D* Lite, a searching algorithm that is capable of handling changes in the world efficiently. The path planning loop then publishes a TF frame detailing the coordinates of the next waypoint which is used by the actuation code.
A good alternative would be LPA*, which is essentially D* Lite but the search goes from the current node to the goal state rather than from the goal state back to the robot state. However, D* Lite is shown to be better in the scenario that the state exploration occurs at the boundaries of the known state. The exact consequence of the tradeoff could be explored experimentally, but given our limited time frame, we choose to implement D* Lite rather than do a comparison.
There are also interesting design choices around information passing from the mapping and detection code to the path planning part. While we could have set up a publisher and subscriber architecture where the mapping and detection code publishes all the information in the occupancy grid and path planning subscribes to it, we decided to merge those two processes in a giant node. This proves to cause problems later on, as the frequency of publishing waypoints becomes limited by object detection and path planning algorithms combined, rather than by a set frequency, causing sync issues with TF frames. Furthermore, the controller reacts to robot state updates too slowly, leaving the robot to be unable to correct on an erroneous path.