Browse Source

sleep before trying to configure the D435i

This appears to fix an issue where the D435i's mode was
not being successfully changed on launch. The code now
sleeps for 3 seconds before trying to configure the D435i,
which appears to fix the problem for the typical launch
files in stretch_ros.

This also appears to resolve an issue that resulted in
warnings from the ArUco marker detection node and
calibration issues. Without a successful mode change,
depth images from the D435i often failed to provide
depth values for the pixels corresponding with the
ArUco markers on Stretch's body.

Addressing this issue will likely obviate the need
for Hello Robot's D435i patch that pins earlier
versions of the D435i software.
pull/49/head
Charlie Kemp 3 years ago
parent
commit
4de2780473
1 changed files with 1 additions and 0 deletions
  1. +1
    -0
      stretch_core/nodes/d435i_configure

+ 1
- 0
stretch_core/nodes/d435i_configure View File

@ -40,6 +40,7 @@ class D435iConfigureNode:
)
def main(self):
rospy.sleep(3) # sleep for 3 seconds to wait for D435i to be ready
rospy.init_node('configure_d435i')
self.node_name = rospy.get_name()
rospy.loginfo("{0} started".format(self.node_name))

Loading…
Cancel
Save