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.
The demo launch files started IMU nodes that were
unused, produced warnings, and created problematic tf trees.
With this commit, they no longer start these nodes.
Quick tests of some of the demos indicate that they
work as well as they did before. However, I did not test
hello_world.launch.
FUNMAP obstacle detection failed due to the frustum of the
RGB colored 3D points not overlapping with the region in front
of the robot that is required to detect the floor when moving
forward. Since the IR camera 3D point frustum still overlaps
with the region, it seems that an upstream Realsense D435i
change may be causing only the RGB colored 3D points to be used
now.
For now, this is a quick fix that makes the robot look down lower
when moving forward, so that the color camera frustum overlaps
with the obstacle detection region.
The FUNMAP mapping launch file started IMU nodes that were
unused, produced warnings, and created a problematic tf tree.
With this commit, mapping.launch no longer starts these nodes.
Mapping and navigation continued to function in a brief test
and the IMU related warnings were no longer produced.
I used the following command for testing.
$ roslaunch stretch_funmap mapping.launch
After these changes, FUNMAP mapping capabilities worked for me
in a small room. I tested the following capabilities:
+ head scans
+ merging head scans
+ navigating to a clicked pose
+ navigating and reaching to a clicked 3D point
+ autonomously navigating to a good place to scan
+ changing pose to a clicked pose
+ estimating nearby pose on a map
+ estimating global pose on a map
I tested these using the following launch command:
$ roslaunch stretch_funmap mapping.launch
I made a variety of changes, including the following:
+ \ => \\ for divisions that appeared to need an integer output
+ changed height bounds for floor segments (-0.05 m to 0.05 m)
Two notable remaining issues follow:
[ WARN] [1622754350.710109968]: Could not obtain transform from imu_mobile_base to base_link. Error was Could not find a connection between 'base_link' and 'imu_mobile_base' because they are not part of the same tree.Tf has two or more unconnected trees.
Transforms and URDF visualization sometimes fail after launching.
It can take multiple attempts for FUNMAP mapping to work. There is
significant nondeterminism when launching with mapping.launch.
NOT FULLY TESTED
After these very minor changes, the following command
appeared to execute properly. However, I didn't bother
attaching the dry-erase marker tool. Instead, I placed
the robot in front of a whiteboard and caught and followed
the gripper with my hand at the surface of the board
to simulate lower-friction dry-erase marker contact.
It successfully aligned itself with the board, stopped on
contact with my hand, and completed all of the writing
motions.
$ roslaunch stretch_demos hello_world.launch
After these very minor changes, surface cleaning with the
following command worked for me. Stretch wiped the top of
a small nightstand.
$ roslaunch stretch_demos clean_surface.launch