This reverts commit dc69b7d420.

I primarily removed commented code.

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

After these changes, object grasping with the following command
worked for me. Stretch grasped a cup from a small nightstand.

$ roslaunch stretch_demos grasp_object.launch

Changes include the following:

+ / => // for some places where an integer output is required

+ changes for scikit_image (import skimage) 0.18.1
  + skimage.measure.label argument change
  + skimage.measure.regionprops argument change

After this minor change, drawer opening worked with the
following command.

$ roslaunch stretch_demos open_drawer.launch

The following persistent warning would be good to resolve.

[ WARN] [1622728896.394958823]: 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.

With these updates, the handover_object.launch demo succeeded.
The robot successfully moved its hand to me three times in a
row when I moved myself to different positions. The handoff
poses seemed good.

A variety of changes were required to make this work, including
the following:

  + tab vs. spaces cleanup for funmap using "autopep8 -i funmap"

  + updates to cython compilation for Python 3 and recent cython

  + changed import statement pattern to "stretch_funmap.*"

Prior to my successful run of this demo, I encountered an error
related to get_robot_floor_pose_xya in HelloNode in hello_misc.py
(details below). It seems like it may have been a failed transform
lookup by get_p1_to_p2_matrix, which queries TF2 to obtain the
current estimated transformation from the robot's base_link frame
to the frame.

I did not make changes that I would expect to resolve this issue.
I only added a few print statements to hello_misc.py that I
intended to use for debugging. I have left the print statements
in place.

[ERROR] [1622656837.765018]: Error processing request: matmul: Input operand 0 does not have enough dimensions (has 0, gufunc core with signature (n?,k),(k,m?)->(n?,m?) requires 1)
['Traceback (most recent call last):\n', '  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/impl/tcpros_service.py", line 632, in _handle_request\n    response = convert_return_to_response(self.handler(request), self.response_class)\n', '  File "/home/ck/catkin_ws/src/stretch_ros/stretch_demos/nodes/handover_object", line 178, in trigger_handover_object_callback\n    at_goal = self.move_base.forward(self.mobile_base_forward_m, detect_obstacles=False, tolerance_distance_m=tolerance_distance_m)\n', '  File "/home/ck/catkin_ws/src/stretch_ros/stretch_funmap/src/stretch_funmap/navigate.py", line 245, in forward\n    xya, timestamp = self.node.get_robot_floor_pose_xya()\n', '  File "/home/ck/catkin_ws/src/stretch_ros/hello_helpers/src/hello_helpers/hello_misc.py", line 127, in get_robot_floor_pose_xya\n    r0 = np.matmul(robot_to_odom_mat, r0)[:2]\n', 'ValueError: matmul: Input operand 0 does not have enough dimensions (has 0, gufunc core with signature (n?,k),(k,m?)->(n?,m?) requires 1)\n']

+ move up or down to contact handles
+ use new stretch_driver custom effort thresholds
+ successful with tool chest drawer, but has 10 second timeouts after each move until contact