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
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.
Checking the calibration now collects fewer higher quality observations.
When checking or performing a new fit, the fit error is checked against a threshold that likely indicates a problem with the calibration.
Code to check the existing calibration. It does the following:
+ Collects a reduced number of new observations with which to check the current calibration.
+ Visualizes and computes the fit of the current calibration to the new observations.
Future work:
+ Improve the reduced number of observations to increase efficiency.
+ Check the numeric results of the fit and provide a warning if the quality is low.
+ 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
Charlie Kemp
hello-binit
Aaron Edsinger
David V. Lu
Charlie Kemp
Aaron Edsinger