Integration and testing of the SLAM system

[emcauliffe]

@haashimrehan

Goal:
get the intel camera working in conjunction with GPS, LoRa, and encoder data to give the robot the ability to map and navigate its environment.

[haashimrehan]

• My laptop with the D435 and ROS was publishing consistantly at 30hz

• the Jetson Tx2 with the d435 and ROS was publishing at 1-3hz on multiple camera topics (camera placed on table facing open space)

• The realsense-viewer program (from Intel) running on the TX2 through X11 reports 29-30fps just fine on both RGB and Depth.

• looking into it online, it's said the auto-exposure negatively effects the fps. I tried disabling it with rqt_reconfigure but haven't seen any noticable changes to the fps yet.

• At some point I put the camera back in the drawer and surprisingly the rate increased to 17 Hz! After that I realized when there are really close objects to the camera the rate increases. (odd since the realsense-viewer consistantly updates the frames at 30fps on the jetson)

[haashimrehan]

Uninstalled and reinstalled every package related to the D435 (librealsense2, ros-melodic-librealsense2, ros-melodic-realsense2-camera, ros-melodic-realsense2-description). The camera now publishes at a consistant 30hz on rs-camera.launch and a consistant ~25hz when setting align_depth:=true (needed for rtabmap)

I suspect it could have been a dependency version compatibility issue.

RTAB-Map was tested with these faster camera topics and performed better/faster.

Next steps:
Take visual odometry, IMU, and GPS and send it to pose_ekf node to generate a single fused odometry source. Provide this /odom topic to RTAB-Map and test out performance.

[haashimrehan]

Localization Notes:

In my test-base robot I only needed one EKF (Extended Kalman Filter) to fuse my odometry sources since the whole system was confined indoors with the robot using only relative-based sensor data. This changes once we receive absolute data from sensors like the GPS and Visual Odometry Output from RTAB-Map

More info on why we need to implement a dual EKF system here:
https://answers.ros.org/question/318328/robot_localization-dual-ekfhow-the-two-ekf-nodes-work-together/

Essentially, relative sources give you data much faster, but is highly prone to drift over time. Absolute sensor sources are slower, but less prone to drift and useful to 'fix' the localisation here and there. This is especially useful for the rover needing to navigate to specific GPS Waypoints.

Notes:
IMU publish to /imu/data
GPS publish to /gps/fix

EKF Nodes:
Local EKF:

• Uses IMU, and Visual Odometry [and eventually Wheel Odometry]
• Generates a odom->base_link Transform

Global EKF:

• Uses IMU, Visual Odometry, and GPS [and eventually Wheel Odometry]
• Used for map->odom Transform
• Utilizes the odom->base_link transform generated by odom EKF in two areas:
  • Calculate the map->odom transform
  • Converts sensor measurements of odom frame (like those from wheel odometry) to map frame in order to have them fused.