FUNMAP cannot find valid base pose to navigate to object


I am running into an issue with FUNMAP where it fails to find a valid base pose for navigating to an object. I put together a demo that uses the ArUco marker detector to identify an object with an ArUco tag on it, and then utilizes FUNMAP to plan a path and navigate to this object to pick it up. At first, this happened every few times where it was unable to find a valid base pose. Usually, if I slightly moved the base of Stretch and tried again, it would then be able to find a base pose the next try. This does not work anymore and it fails to find a valid base pose every time. Has anyone ever run into this issue with FUNMAP? And if so, do you have any suggestions on what could be happening or how to fix it?

Thank you so much,

Hi @jgangemi ,
Thank you for reporting this issue, I spent some time trying to reproduce the issue you described. Looking at your output, I believe you are using the /clicked_point topic to publish the aruco marker’s pose goals to be reached by the robot. Unfortunately, I’m having the same issues you describe and I got it work only once by giving it a point on a raised table that was under 0.5m away. The issue seems to be happening from the ManipulationPlanner.base_pose() method, which is responsible for taking in the robot pose and goal pose in pixel coordinates w.r.t the MHI image and would output the best base pose and arm reach for grasping.

For further debugging, could send us the debug images from the ManipulationPlanner.The debug images can be displayed by setting the variable image_display_on to true in the funmap node.

Hi Mohamed,
Thank you for trying to reproduce this issue. I will send the debug images from the ManipulationPlanner once I get back to my Stretch model in my lab next week.
Do you know if this is a common problem users run into when utilizing FUNMap to plan a base pose? Is there an alternative that I could try for navigation planning? Thank you for your help!!