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
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']
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
Python 3 was not well supported by ROS Melodic.
stretch_deep_perception required Python 3, which could
cause issues and confusion. To help clarify when code
used Python 3, the filenames had the pattern *_python3.py.
I've changed the filenames to remove this pattern,
since all of the ROS Noetic code will be using Python 3
by default, and ROS Noetic supports Python 3.
stretch_deep perception now works with ROS Noetic, Ubuntu 20.04, and
Python 3. The only change for this commit is to two rviz
configuration files that were rendering very large points,
which dramatically slowed down the system.
I tested the following launch files:
stretch_detect_faces.launch
stretch_detect_objects.launch
stretch_detect_body_landmarks.launch
stretch_detect_nearest_mouth.launch
The following demo appears to work now.
roslaunch stretch_deep_perception stretch_detect_faces.launch
One key change was to make sure to use a Python 3 version of
OpenCV (import cv2) with the Inference Engine. Otherwise, you
get errors like the following:
self.head_pose_model = cv2.dnn.readNet(head_pose_weights_filename, head_pose_config_filename)
cv2.error: OpenCV(4.2.0) ../modules/dnn/src/dnn.cpp:3298: error: (-2:Unspecified error) Build OpenCV with Inference Engine to enable loading models from Model Optimizer. in function 'readFromModelOptimizer'
My installation had two different pip versions of OpenCV and
an apt version of OpenCV, which created problems.
The last change I made was to fix frame_id by remove the
initial slash.
Used grep to find from_dcm and as_dcm methods in stretch_ros code.
scipy.spatial.Rotation changed the method names, so I attempted
to correct them throughout the code with the following conversions:
as_dcm => as_matrix
from_dcm => from_matrix
Displaying point cloud points with unusually large sizes in RViz
resulted in a dramatic system slowdown. The face detection
RViz configuration now has a standard small size for the points,
which fixed the problem.
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.
stretch_calibration appears to now work with ROS Noetic, Python 3, and Ubuntu 20.04.
tilt_angle_backlash_transition is now added to the calibration results file.
To do in the future:
+ The camera pan doesn't seem to consistently move a little to the back of the robot when collecting calibration data. It does seem to consistently pan a little to the front of the robot. This could cause issues with pan backlash calibration.
+ Check on the calibration quality. The fit was not as good as I expected to the mobile base and the arm. I need to check on this.
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
Charlie Kemp
Charlie Kemp
Binit Shah