Add support for Dex wrist joints to various nodes

Update README.md

Update start_rtab.launch

Launch file used wrong topics for sim.

Install instructions were not updated during merge. Still linked to original repo.

Fix yaml.load crash by specifying loader

This adds show_debug_images at the node level. When set to True,
debugging images are displayed via OpenCV, including RGB images
annotated with ArUco marker detections, marker masks, and depth
images used when estimating the pose of the ArUco markers.

This appears to fix an issue where the D435i's mode was
not being successfully changed on launch. The code now
sleeps for 3 seconds before trying to configure the D435i,
which appears to fix the problem for the typical launch
files in stretch_ros.

This also appears to resolve an issue that resulted in
warnings from the ArUco marker detection node and
calibration issues. Without a successful mode change,
depth images from the D435i often failed to provide
depth values for the pixels corresponding with the
ArUco markers on Stretch's body.

Addressing this issue will likely obviate the need
for Hello Robot's D435i patch that pins earlier
versions of the D435i software.

The "2D Nav Goal" arrow in RViz is magenta not green.

fix diagnostic_updater arguments

[noetic] ROS Diagnostics and RQT Dashboard

Feature/testing

Stretch RTAB-Map, OctoMap, and Navigation improvements