handover demo updates for noetic

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']

hello_helpers/src/hello_helpers/hello_misc.py

@@ -121,9 +121,12 @@ class HelloNode:
         # Query TF2 to obtain the current estimated transformation
         # from the robot's base_link frame to the frame.
         robot_to_odom_mat, timestamp = get_p1_to_p2_matrix('base_link', floor_frame, self.tf2_buffer)
+        print('robot_to_odom_mat =', robot_to_odom_mat)
+        print('timestamp =', timestamp)
 
         # Find the robot's current location in the frame.
         r0 = np.array([0.0, 0.0, 0.0, 1.0])
+        print('r0 =', r0)
         r0 = np.matmul(robot_to_odom_mat, r0)[:2]
 
         # Find the current angle of the robot in the frame.

stretch_demos/nodes/handover_object

@@ -1,6 +1,6 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 
-from __future__ import print_function
+#from __future__ import print_function
 
 from sensor_msgs.msg import JointState
 from geometry_msgs.msg import Twist
@@ -107,7 +107,8 @@ class HandoverObjectNode(hm.HelloNode):
                     lookup_time = rospy.Time(0) # return most recent transform
                     timeout_ros = rospy.Duration(0.1)
 
-                    old_frame_id = self.mouth_point.header.frame_id[1:]
+                    #old_frame_id = self.mouth_point.header.frame_id[1:]
+                    old_frame_id = self.mouth_point.header.frame_id[:]
                     new_frame_id = 'base_link'
                     stamped_transform = self.tf2_buffer.lookup_transform(new_frame_id, old_frame_id, lookup_time, timeout_ros)
                     points_in_old_frame_to_new_frame_mat = rn.numpify(stamped_transform.transform)

stretch_demos/rviz/handover_object.rviz

@@ -5,7 +5,7 @@ Panels:
     Property Tree Widget:
       Expanded: ~
       Splitter Ratio: 0.5
-    Tree Height: 728
+    Tree Height: 719
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -220,11 +220,6 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
-        link_puller:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
         link_right_wheel:
           Alpha: 1
           Show Axes: false
@@ -249,8 +244,8 @@ Visualization Manager:
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: 3.660540819168091
-        Min Value: -0.17842411994934082
+        Max Value: 10
+        Min Value: -10
         Value: true
       Axis: Z
       Channel Name: intensity
@@ -261,16 +256,14 @@ Visualization Manager:
       Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 4096
       Min Color: 0; 0; 0
-      Min Intensity: 0
       Name: PointCloud2
       Position Transformer: XYZ
       Queue Size: 10
       Selectable: true
       Size (Pixels): 3
       Size (m): 0.009999999776482582
-      Style: Points
+      Style: Flat Squares
       Topic: /camera/depth/color/points
       Unreliable: false
       Use Fixed Frame: true
@@ -281,7 +274,7 @@ Visualization Manager:
       Marker Topic: /nearest_mouth/axes
       Name: MarkerArray
       Namespaces:
-        {}
+        "": true
       Queue Size: 100
       Value: true
   Enabled: true
@@ -318,6 +311,7 @@ Visualization Manager:
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
+      Field of View: 0.7853981852531433
       Focal Point:
         X: -0.28927484154701233
         Y: -0.1668422967195511
@@ -329,16 +323,15 @@ Visualization Manager:
       Near Clip Distance: 0.009999999776482582
       Pitch: 0.3497966527938843
       Target Frame: <Fixed Frame>
-      Value: Orbit (rviz)
       Yaw: 0.9340142011642456
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1025
+  Height: 1016
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd0000000400000000000001c900000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002edfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002ed000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000056e0000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001c90000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002edfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002ed000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005690000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -347,6 +340,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 1853
-  X: 67
+  Width: 1848
+  X: 72
   Y: 27

stretch_funmap/src/stretch_funmap/compile_cython_code.sh

@@ -1,11 +1,17 @@
 #!/bin/bash
 
 echo "Building cython_min_cost_path.pyx..."
-cython ./cython_min_cost_path.pyx 
-gcc -shared -pthread -fPIC -fwrapv -ffast-math -O3 -Wall -fno-strict-aliasing -I/usr/include/python2.7 -o cython_min_cost_path.so cython_min_cost_path.c
+cython3 ./cython_min_cost_path.pyx 
+gcc -shared -pthread -fPIC -fwrapv -ffast-math -O3 -Wall -fno-strict-aliasing -I/usr/include/python3.8 -o cython_min_cost_path.so cython_min_cost_path.c
 echo "Done."
 echo ""
 
+# In the future, consider the following recommendation from the Cython
+# documentation found at
+# https://cython.readthedocs.io/en/latest/src/quickstart/build.html
+#
+# "Write a setuptools setup.py. This is the normal and recommended way."
+#
 
 # In the future try adding -DNPY_NO_DEPRECATED_API=NPY_1_7_API_VERSION to address the following error (adding this argument currently results in compilation failure). 
 #

stretch_funmap/src/stretch_funmap/manipulation_planning.py

@@ -1,6 +1,6 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 
-from __future__ import print_function
+#from __future__ import print_function
 
 import numpy as np
 import scipy.ndimage as nd
@@ -8,16 +8,16 @@ import scipy.signal as si
 import cv2
 import skimage as sk
 import math
-import max_height_image as mh
-import segment_max_height_image as sm
-import ros_max_height_image as rm
+import stretch_funmap.max_height_image as mh
+import stretch_funmap.segment_max_height_image as sm
+import stretch_funmap.ros_max_height_image as rm
 import hello_helpers.hello_misc as hm
 import ros_numpy as rn
 import rospy
 import os
 
-from numba_manipulation_planning import numba_find_base_poses_that_reach_target, numba_check_that_tool_can_deploy
-from numba_check_line_path import numba_find_contact_along_line_path, numba_find_line_path_on_surface
+from stretch_funmap.numba_manipulation_planning import numba_find_base_poses_that_reach_target, numba_check_that_tool_can_deploy
+from stretch_funmap.numba_check_line_path import numba_find_contact_along_line_path, numba_find_line_path_on_surface
 
 def plan_surface_coverage(tool_start_xy_pix, tool_end_xy_pix, tool_extension_direction_xy_pix, step_size_pix, max_extension_pix, surface_mask_image, obstacle_mask_image):
     # This was designed to be used when planning to clean a flat

stretch_funmap/src/stretch_funmap/mapping.py

@@ -1,23 +1,24 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
+
+#from __future__ import print_function
 
-from __future__ import print_function
 import numpy as np
 import ros_numpy as rn
-import ros_max_height_image as rm
+import stretch_funmap.ros_max_height_image as rm
 from actionlib_msgs.msg import GoalStatus
 import rospy
 import hello_helpers.hello_misc as hm
-import ros_max_height_image as rm
+import stretch_funmap.ros_max_height_image as rm
 import ros_numpy
 import yaml
-import navigation_planning as na
+import stretch_funmap.navigation_planning as na
 import time
 import cv2
 import copy
 import scipy.ndimage as nd
-import merge_maps as mm
+import stretch_funmap.merge_maps as mm
 import tf_conversions
-import segment_max_height_image as sm
+import stretch_funmap.segment_max_height_image as sm
 
 
 def stow_and_lower_arm(node):

stretch_funmap/src/stretch_funmap/max_height_image.py

@@ -1,6 +1,6 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 
-from __future__ import print_function
+#from __future__ import print_function
 
 import sys
 import cv2
@@ -14,8 +14,8 @@ import struct
 import threading
 from collections import deque
 
-import numba_height_image as nh
-from numba_create_plane_image import numba_create_plane_image, numba_correct_height_image, transform_original_to_corrected, transform_corrected_to_original
+import stretch_funmap.numba_height_image as nh
+from stretch_funmap.numba_create_plane_image import numba_create_plane_image, numba_correct_height_image, transform_original_to_corrected, transform_corrected_to_original
 
 from scipy.spatial.transform import Rotation
 

stretch_funmap/src/stretch_funmap/merge_maps.py

@@ -1,8 +1,8 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 
-from __future__ import print_function
+#from __future__ import print_function
 
-import max_height_image as mh
+import stretch_funmap.max_height_image as mh
 import numpy as np
 import scipy.ndimage as nd
 import scipy.signal as si
@@ -10,7 +10,7 @@ import cv2
 import skimage as sk
 import math
 import hello_helpers.hello_misc as hm
-import navigation_planning as na
+import stretch_funmap.navigation_planning as na
 import copy
 import time
 
@@ -18,8 +18,8 @@ import time
 from scipy.optimize import minimize, minimize_scalar
 
 
-from numba_compare_images import numba_compare_images_2
-import mapping as ma
+from stretch_funmap.numba_compare_images import numba_compare_images_2
+import stretch_funmap.mapping as ma
 
 import tf_conversions
 

stretch_funmap/src/stretch_funmap/navigate.py

@@ -1,15 +1,16 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
+
+#from __future__ import print_function
 
-from __future__ import print_function
 import numpy as np
 import ros_numpy as rn
-import ros_max_height_image as rm
+import stretch_funmap.ros_max_height_image as rm
 from control_msgs.msg import FollowJointTrajectoryResult
 from actionlib_msgs.msg import GoalStatus
 import rospy
 from std_srvs.srv import Trigger, TriggerRequest
 import hello_helpers.hello_misc as hm
-import navigation_planning as na
+import stretch_funmap.navigation_planning as na
 import cv2
 
 class ForwardMotionObstacleDetector():

stretch_funmap/src/stretch_funmap/navigation_planning.py

@@ -2,10 +2,10 @@ import copy
 import numpy as np
 import scipy.ndimage as nd
 import cv2
-import cython_min_cost_path as cm
-from numba_check_line_path import numba_check_line_path
-from numba_sample_ridge import numba_sample_ridge, numba_sample_ridge_list
-import segment_max_height_image as sm
+import stretch_funmap.cython_min_cost_path as cm
+from stretch_funmap.numba_check_line_path import numba_check_line_path
+from stretch_funmap.numba_sample_ridge import numba_sample_ridge, numba_sample_ridge_list
+import stretch_funmap.segment_max_height_image as sm
 import hello_helpers.hello_misc as hm
 
 ####################################################
@@ -424,7 +424,7 @@ def estimate_navigation_channels( floor_mask, idealized_height_image, m_per_pix,
 
     # find exit regions
     number_of_exits, exit_label_image = simple_connected_components(map_exits)
-    print 'number_of_exits =', number_of_exits
+    print class="p">('number_of_exits =', number_of_exits)
 
     distance_map = original_dist_transform.copy()
     distance_map[closed_floor_mask < 1] = 0
@@ -924,9 +924,9 @@ def split_paths(pix_path, max_segment_length_pix):
                 new_path.append(list(p_mid))
             new_path.append(p1)
     else:
-        print 'WARNING: split_paths given pix_path input with length <= 1.'
-        print '         returning pix_path without modification'
-        print '         pix_path =', pix_path
+        print class="p">('WARNING: split_paths given pix_path input with length <= 1.')
+        print class="p">('         returning pix_path without modification')
+        print class="p">('         pix_path =', pix_path)
         return pix_path
     return new_path, split_made
 
@@ -955,9 +955,9 @@ def chop_path_at_location(pix_path, best_stopping_location):
         new_path = pix_path[:min_index+1]
         new_path.append(best_stopping_location)
     else:
-        print 'WARNING: chop_path_at_location given pix_path input with length <= 1.'
-        print '         returning pix_path without modification'
-        print '         pix_path =', pix_path
+        print class="p">('WARNING: chop_path_at_location given pix_path input with length <= 1.')
+        print class="p">('         returning pix_path without modification')
+        print class="p">('         pix_path =', pix_path)
         return pix_path
     return new_path
     

stretch_funmap/src/stretch_funmap/ros_max_height_image.py

@@ -1,6 +1,6 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 
-from __future__ import print_function
+#from __future__ import print_function
 
 import sys
 import rospy
@@ -30,9 +30,9 @@ from scipy.spatial.transform import Rotation
 
 from copy import deepcopy
 
-from max_height_image import *
+from stretch_funmap.max_height_image import *
 
-import navigation_planning as na
+import stretch_funmap.navigation_planning as na
     
 class ROSVolumeOfInterest(VolumeOfInterest):
 

stretch_funmap/src/stretch_funmap/segment_max_height_image.py

@@ -1,8 +1,8 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 
-from __future__ import print_function
+#from __future__ import print_function
 
-import max_height_image as mh
+import stretch_funmap.max_height_image as mh
 import numpy as np
 import scipy.ndimage as nd
 import scipy.signal as si
@@ -11,9 +11,9 @@ import skimage as sk
 from skimage.morphology import convex_hull_image
 import math
 import hello_helpers.hello_misc as hm
-import navigation_planning as na
+import stretch_funmap.navigation_planning as na
 
-from numba_height_image import numba_create_segment_image_uint8
+from stretch_funmap.numba_height_image import numba_create_segment_image_uint8
 import hello_helpers.fit_plane as fp