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']
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.
+ Users may define out-of-range goals intentionally if they want to move any joint to its limit without knowing the joint's calibrated limit for a specific robot