Week 4 (4th Period)

This week, testing with the real robot in a real environment commenced to assess the replication of simulation results in the physical world. Two distinct tests were conducted, one for each developed node, mirroring the simulation setup.

The first test involved navigating a narrow corridor, a task manageable with the robot’s arm in a contained position but impossible with the arm fully extended. Unexpectedly, the robot failed to recognize the collision risk in the latter scenario and continued to move forward. It became evident that the local path planner utilized by the SocRob@Home team was not configured to handle dynamic footprints, as it assumed a static circular shape. This limitation likely influenced the robot’s behavior.

For the height adjustment node, a bag was suspended from the ceiling at a height that posed an obstacle depending on the robot’s elevation. While the robot performed as expected at longer distances, it struggled to detect the bag at closer ranges due to limitations in the cameras and sensors. Although adjustments were made to correct this issue, occasional detection errors persisted, leading to unintended objects being identified as obstacles and impacting the projection of the bag.

While these challenges are not directly attributable to the code’s quality or reliability, they hindered the accurate assessment of the height adjustment node’s performance. Nevertheless, efforts to resolve these issues are ongoing, ensuring the continued refinement of the system.

In Week 7 (3^rd Period), the tests carried out on the TIAGo robot did not proceed as expected. The possible causes were varied and could be related to the performance of the algorithms in a real environment or to the map itself. However, with the new approach adopted for mapping, FAST-LIO , the maps undoubtedly acquired much more detail and points, which proved to be a great advantage for localization. Thus, during this week, the final tests for this task were finally completed.

The HDL algorithm was executed without any issues, demonstrating outstanding performance. As shown in the figure below, the accuracy of the localization is evident.

To verify it, as explained in the results obtained in the simulation, it can be noted that the pointcloud captured by the sensor in real-time overlaps with the map, denoting the correct pose of the robot. Initially, it is necessary to manually indicate the pose estimate in rviz; however, this is a common problem in robotics and does not represent a significant drawback. Once the pose has been estimated, the robot localizes itself and remains localized throughout its journey.

In simulation, this algorithm didn’t perform well, and it maintained this behaviour with the real robot. As shown in the video below, the robot delocalizes itself very easily, especially during rotations. This can be seen due to the mismatch between the map and the point cloud data.

The next steps in this task involve computing the errors associated with the Optitrack cameras, as mentioned in the proposal documentation and project presentation. Therefore, later this week, the cameras were calibrated and rosbags were generated — i.e., data was recorded as the robot moved with the different combinations of parameters for both algorithms (as can be seen in the code block below).

This approach is used since the ground truth of the OptiTrack cameras is the one that will be considered. These cameras, which are strategically positioned around the testbed, use a triangulation system to provide a more accurate estimate of the robot’s pose.

Within the .launch files of the algorithms, various parameters are adjustable:

//HDL Localization
<arg name="enable_robot_odometry_prediction" value="false" /> // The use of Odometry is optional, it's value can be "true" or "false"
<arg name="use_imu" default="false" /> // The use of IMU is optional, it's value can be "true" or "false"

//MCL3D
<arg name="use_odom" default="false" /> // The use of Odometry is optional, it's value can be "true" or "false"
<arg name="use_imu" default="false" /> // The use of IMU is optional, it's value can be "true" or "false"

Multiple combinations were tested to calculate the errors and determine the optimal configurations.

However, the robot’s data acquisition rate is higher than the OptiTrack PRIME13 cameras, which means that it will be necessary to synchronize the data, in order to find the moments when both sources were capturing information, to calculate the errors accurately and generate the necessary plots.