FUNMAP implementation using wrist D405 camera

Ask
1

We are looking to utilize the D405 wrist camera for plane detection using FUNMAP. In our application, the head camera’s vision may become occluded, so switching to the wrist camera would be beneficial.
Could you please provide suggestions or guidance on how to modify the ros2_stretch_FUNMAP package to use the wrist camera instead of the head scans?

Thank you for your assistance!

2

Hey @Davide_Peron, FUNMAP’s fit_closest_flat_surface() method could work for this.

image
image665×262 22.3 KB

It takes in a max height image (MHI), the robot’s position in image space, and a debug boolean. The FUNMAP node already tracks the robot’s position in image space, so you could just use that. Constructing the MHI from the D405 wrist camera is possible; here’s code showing how it’s done with the D435if head camera:

image
image865×324 45.4 KB

Github Link: stretch_ros2/stretch_funmap/stretch_funmap/mapping.py at 9a44eee2933c5c171f9504aae69c3bb6becf0443 · hello-robot/stretch_ros2 · GitHub

3

Thanks, and we will try to follow your suggestions.