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.

Removes two IMU launch files. They are not used by other
stretch_ros code, and they appear to not function properly.
For example, they do not appear to use a proper tf frame for
the IMU, can produce problematic tf trees, and can result
in tf query timing problems.

I primarily removed commented code.

I changed the rviz configuration file for
wheel_odometry_test.launch to avoid a
point cloud visualization issue.

The ROS Noetic Respeaker package did not function as expected.
For now, I'm removing this launch file that depends on it.

I tried and failed to use the following version:

$ apt show ros-noetic-respeaker-ros
Package: ros-noetic-respeaker-ros
Version: 2.1.21-2focal.20210424.002454
Priority: optional
Section: misc
Maintainer: Yuki Furuta <furushchev@jsk.imi.i.u-tokyo.ac.jp>
Installed-Size: 153 kB
Depends: flac, ros-noetic-angles, ros-noetic-audio-common-msgs, ros-noetic-dynamic-reconfigure, ros-noetic-geometry-msgs, ros-noetic-std-msgs, ros-noetic-tf
Download-Size: 20.8 kB
APT-Manual-Installed: yes
APT-Sources: http://packages.ros.org/ros/ubuntu focal/main amd64 Packages
Description: The respeaker_ros package

Node that outputs d435i frustums now works with ROS Noetic, etc.
I've also added a new launch file to more easily test and
visualize the frustums. It has an associated rviz configuration
file.

The tilt backlash model is supposed to compare the current tilt
angle of the head with the head_tilt_backlash_transition_angle
to determine if head_tilt_looking_up element of the backlash state
is true or false. This is due to the head resting against
different sides of the tilt joint based on whether it's looking up
or looking down.

At some point, an error was introduced in which
head_tilt_looking_up was set based on the recent relative motion
of the head tilt joint. This is the way backlash state is
determined for the telescoping arm and head panning.
In contrast, the head tilt backlash state depends on gravity
and where the center of mass of the head is relative to the
tilt joint's axis of rotation. As such, the head tilt backlash
state depends on the tilt angle of the head.

With these changes, keyboard_teleop now works in Noetic.
The main issue was a <= comparison in stretch_driver that
resulted in a failure when given a mobile base rotation command.

in set_goal
    if len(point.positions) <= self.index_translate_mobile_base and len(point.positions) <= self.index_rotate_mobile_base:
TypeError: '<=' not supported between instances of 'int' and 'NoneType'

In the process of fixing this, I made a number of other changes:

+ removed support for "manipulation" mode
+ added stricter and clearer error handling for the MobileBaseCommandGroup
+ trajectories should fail early if they include mobile base rotation and translation joints
+ simultaneous rotation and translation commands to the base are not allowed
+ only a mobile base rotation or translation joint can be present in a trajectory due to lack of null valued goal points (position, velocity, acceleration)
+ a future alternative might be to consider a 0.0 (< epsilon) position to mean no action, since mobile base translation and rotation specify incremental moves

In the process of trying to get keyboard_teleop to work,
I encountered a syntax error. It turned out to be due to
"async" being used in an argument and "async" being
a keyword in Python 3. I've replaced "async" with
"return_before_done". This is the main change in this
commit.

These changes make the following code run in ROS Noetic
with Ubuntu 20.04 and Python 3. However, this code has
not been fully tested. The robot moves and appears to
collect data. A fit error appears to be calculated.
The visualization of the fit may have a problem.

+ stretch_calibration collect_check_head_calibration_data.launch
+ stretch_calibration check_head_calibration.sh

For now, I've commented out "class HelloNode" in hello_misc.py
due to a syntax error without an obvious fix.

Detected ArUco markers can now be visualized in rviz.

+ Fixed shebang
+ removed "/" from the start of frame_ids

This simple change makes the following commands behave reasonably:

$ roslaunch stretch_core d435i_low_resolution.launch
$ rosrun rviz rviz