GraphNav fixes

[IoTDan]

Hello!

thank you for creating and maintaining this repo. It has been extremely helpful.
I did some work with the GraphNav ROS2 services and action server.

While I was able to get it working, i encountered some issues, and wanted to share them.
I very well could have been using the ROS2 interfaces for SPOT incorrectly, but this usage pattern was built off reading the SPOT V4.0.3 documentation and samples, and looking at the examples, source code and documentation in this REPO.

To get SPOT working using GraphNav, I did the following:

1. ros2 service call /graph_nav_clear_graph spot_msgs/srv/GraphNavClearGraph
2. ros2 service call /graph_nav_upload_graph spot_msgs/srv/GraphNavUploadGraph "{upload_filepath: ./map.walk'}"
3. ros2 service call /graph_nav_set_localization spot_msgs/srv/GraphNavSetLocalization "{method: 'waypoint', waypoint_id: 'canny-mite-VQ.uUPtpNIimADm2yVAsWA=='}"

These work without any issue, with the 3rd step being the waypoint where our base station is.
we did all these from teh command line.

the fourth step , which calls the action server navigate_to had issues.

We found we had to make 3 changes to use this ROS2 action server:

1. in spot_wrapper/spot_Wrapper/spot_graph_nav.py, we had to add the request for a lease in set_initial_localization_waypoint (new lines 296-298)

    self._lease = self._get_lease()
    self._lease_keepalive = LeaseKeepAlive(self._lease_client)
   

2. in spot_driver/spot_driver/spot_ros2.py (lines 2823 - 2826) we had to change

       upload_path=goal_handle.request.upload_path,
        navigate_to=goal_handle.request.navigate_to,
        initial_localization_fiducial=goal_handle.request.initial_localization_fiducial,
        initial_localization_waypoint=goal_handle.request.initial_localization_waypoint,
   

to
waypoint_id=goal_handle.request.waypoint_id,

3. in spot_msgs/action/NavigateTO.action we needed to change the definition from

   string upload_path # Absolute path to map_directory, which is downloaded from tablet controller
   string navigate_to # Waypoint id string for where to go
   bool initial_localization_fiducial # Tells the initializer whether to use fiducials
   string initial_localization_waypoint # Waypoint id to trigger localization

   bool success # indicate successful run of triggered service
   string message # informational, e.g. for error messages

   string waypoint_id

to

string waypoint_id # waypoint ID to navigate to
---
bool success   # indicate successful run of triggered service
string message # informational, e.g. for error messages
---
string waypoint_id 

It looks as though the action server interface changed in implementation, but the action server interface was not updated.

did I call the API wrong, or did we find valid bugs?

thanks
Dan

[khughes-bdai]

Hi,
The navigate_to action looks like it hasn't been touched in a while. You are right that the call to spot_wrapper.spot_graph_nav._navigate_to in handle_navigate_to has the wrong arguments. EDIT: I am wondering if this is supposed to instead be spot_wrapper.spot_graph_nav.navigate_initial_localization as this appears to have the correct arguments. Would you mind trying this change?

I am unsure why you would need to rerequest the lease in set_initial_localization_waypoint 🤔 was this an issue with calling the /graph_nav_set_localization service?

@mhidalgo-bdai I know you have done work testing some graph nav functionality recently, had you run into any of these issues? (fyi @tcappellari-bdai)

[IoTDan]

Thank @khughes-bdai! you maybe correct, but the name navigate_initial_localization seems to imply that it is an initial localization, not go to a waypoint. and the parameters seem to match that understanding.

For clarity, what i am trying to do is set the target waypoint for the robot to go to.

[mhidalgo-bdai]

@mhidalgo-bdai I know you have done work testing some graph nav functionality recently, had you run into any of these issues?

We did basic smoke testing at the time, so I did not come across this issue. The fact that API calls don't match though is quite telling. Some git blame'ing shows this issue goes all the way back to the time GraphNav support was split out in bdaiinstitute/spot_wrapper#44. Before that PR, SpotWrapper.navigate_to() had the signature that the Spot ROS 2 node expects. So yeah, this hasn't been used a lot around lately.

Thanks for pointing it out @IoTDan and a PR would be most welcome.

[IoTDan]

Thank you for the quick response, due diligence and kind response. We will submit a PR to spot_Ros2 and spot_messages and work with you folks to best decide how to incorporate. From: mhidalgo-bdai ***@***.***> Sent: Monday, September 23, 2024 11:53 AM To: bdaiinstitute/spot_ros2 ***@***.***> Cc: Mention ***@***.***>; Comment ***@***.***>; Author ***@***.***> Subject: Re: [bdaiinstitute/spot_ros2] GraphNav fixes (Issue #486) @mhidalgo-bdai<https://github.com/mhidalgo-bdai> I know you have done work testing some graph nav functionality recently, had you run into any of these issues? We did basic smoke testing at the time, so I did not come across this issue. The fact that API calls don't match though is quite telling. Some git blame'ing shows this issue goes all the way back to the time GraphNav support was split out in bdaiinstitute/spot_wrapper#44<bdaiinstitute/spot_wrapper#44>. Before that PR, SpotWrapper.navigate_to() had the signature the Spot ROS 2 node expects. So yeah, this hasn't been used a lot around lately. Thanks for pointing it out @IoTDan<https://github.com/IoTDan> and a PR would be most welcome. - Reply to this email directly, view it on GitHub<#486 (comment)> or unsubscribe<https://github.com/notifications/unsubscribe-auth/AB5YFXTE5IRDUS4TUYFL4GLZYBPPRBFKMF2HI4TJMJ2XIZLTSOBKK5TBNR2WLJDUOJ2WLJDOMFWWLO3UNBZGKYLEL5YGC4TUNFRWS4DBNZ2F6YLDORUXM2LUPGBKK5TBNR2WLJDUOJ2WLJDOMFWWLLTXMF2GG2C7MFRXI2LWNF2HTAVFOZQWY5LFUVUXG43VMWSG4YLNMWVXI2DSMVQWIX3UPFYGLLDTOVRGUZLDORPXI6LQMWWES43TOVSUG33NNVSW45FGORXXA2LDOOJIFJDUPFYGLKTSMVYG643JORXXE6NFOZQWY5LFVE2DQMZSHE3TQMBSQKSHI6LQMWSWS43TOVS2K5TBNR2WLKRSGU2DGMJTGY2TQNFHORZGSZ3HMVZKMY3SMVQXIZI>. You are receiving this email because you were mentioned. Triage notifications on the go with GitHub Mobile for iOS<https://apps.apple.com/app/apple-store/id1477376905?ct=notification-email&mt=8&pt=524675> or Android<https://play.google.com/store/apps/details?id=com.github.android&referrer=utm_campaign%3Dnotification-email%26utm_medium%3Demail%26utm_source%3Dgithub>.

[IoTDan]

I submitted the following PRs:

"bdaiinstitute/spot_wrapper#141"
"#487"

thanks,

Dan

[khughes-bdai]

Going to close this issue as both of your PRs have been merged in! Thanks for the contribution