Add local grid publisher #629

ryan-roche · 2025-03-18T17:36:15Z

Change Overview

This PR adds a module /spot_local_grid with a ROS2 node providing a publisher for Spot's local grid.

(Grid messages visualized in Foxglove Studio)

It is currently hard-coded to publish the obstacle_distance grid, but I can refactor it to take an argument for which grid to publish.

Since this was something I developed internally as part of another project, I separated it to its own module. Should you choose to merge this into the driver repo, it might make more sense to integrate it into the driver itself and have it optionally activated when using the driver launchfile, similar to the --publish-point-clouds argument.

Testing Done

Please create a checklist of tests you plan to do and check off the ones that have been completed successfully. Ensure that ROS 2 tests use domain_coordinator to prevent port conflicts. Further guidance for testing can be found on the ros utilities wiki.

Being that this module is so dependent on data from the robot, I wasn't able to make any unit tests for it.

tcappellari-bdai · 2025-03-18T18:35:47Z

Can you run pre-commit install then pre-commit run --all at the repo's root, then commit those changes to ensure the linter passes?

tcappellari-bdai · 2025-03-19T19:59:51Z

Actually, after thinking about this a bit more, I think it would be better to have this live in the existing spot_driver package rather than making a new one. I also like your idea of adding an optional argument to activate it from the driver launchfile.

Could you make those changes and also modify your node to follow our conventions for homogeneity (i.e. going through spot_wrapper to remove some boilerplate)? Thanks!

ryan-roche · 2025-03-22T22:53:28Z

Where exactly in the spot_driver package should I put the source file for the grid publisher? Most of the nodes appear to be written in C++, but the python source directory /spot_driver/spot_driver doesn't have so should I just put it at /spot_driver/spot_driver/local_grid_pub.py?

If I recall correctly, you can have executable Nodes written in both languages within the same package, so it shouldn't be an issue that the code is in Python.

ryan-roche · 2025-03-24T20:21:56Z

Bumping to re-open the PR. I've tried looking into a way to add the python node to the base spot_driver package, but while you can have python files installed as part of an ament_cmake package, I'm trying to figure out how you add the python package dependencies.

I could try rewriting the node in C++, but I'm unfamiliar with the Spot C++ SDK so it'd take some time. The Python local grid publisher node is working as-is, so I would rather figure out how to integrate that into the driver and have rewriting it in C++ as a possible future improvement down the line. I'm sure there is a way to do this- I just need to look through more of this documentation.

khughes-bdai · 2025-03-24T20:34:38Z

@ryan-roche no need to convert to C++ -- we already have some python code in spot_driver/spot_driver, living side by side with the C++ code. I think the two main options for where to put this code are a) leave it as is in its own file, as you are doing now, or b) try to add the relevant code into spot_driver/spot_driver/spot_ros2.py, which is where almost all of our python based driver code lives.
Ideally the second option would reduce some boilerplate (as we already create the robot object, authenticate, etc. in there, using spot_wrapper as a common entry point) but I'm also not super opposed to the first option as this node seems pretty self contained. @tcappellari-bdai thoughts?

ryan-roche · 2025-03-25T22:05:56Z

I'll keep it in its own node- think that'll make it easier to not run it unless the user explicitly asks for it with the launch arguments. Just gotta figure out how you specify package dependencies for your python nodes in an ament_cmake package since there isn't a setup.py to list them in.

…osdep :(

ryan-roche · 2025-03-26T01:08:57Z

Got the local grid publisher implemented as a node within the spot_driver package!!! Visualizing it in Foxglove in the picture above.

The publishing of local grids is configured like the pointclouds- you need to specify publish_local_grids:=True when you launch the driver, and since there are multiple local grids to choose from, there's an additional parameter local_grid_name to specify which one you want (it'll default to obstacle_distance)

The topic that the grids are published is constructed like the other topics in the driver- it'll prepend the name of the robot if one is specified, publishing at /{robot name}/{grid name}

All that's left (aside from stuff like the linting checks) is to add a check that the specified local grid name is the name of an actual grid that Spot produces, and to remove the vestigial "cutoff" logic from when the node was a part of the project I'm working on at my university robotics lab.

Might have to translate the values differently depending on what grid it is so long as it makes sense in ROS conventions- right now it just assigns one of 3 values based on some value ranges (I'm using it to estimate the footprint of objects in my project), but that doesn't make sense for a general-purpose publisher, so I'm going to have it just translate the values to whatever makes sense with the conventions of the ROS OccupancyGrid messages.

One extra thing to note is that I had to add a line to the setup script to install the ros2_numpy Python package I use to convert between NumPy objects and ROS messages since it doesn't have a rosdep binding, so it doesn't get installed by rosdep. It's been a godsend in my work so I'm kinda surprised it's not on rosdep, but I digress. The other Python packages without rosdep bindings are installed from a requirements.txt file in the spot_wrapper repository, but since this package is only used by the local grid publisher it didn't make sense to add it there.

tl;dr

The node has been integrated into the driver instead of being a separate package
Launchfiles have been updated so that you just do publish_local_grids:=True when launching the driver, and local_grid_name:=<grid_name> to specify which grid to publish
I had to add a line to the setup script to install a package directly since it didn't make sense to add it to the requirements.txt in the spot_wrapper repo since nothing in there actually uses this package

What's left for me to do

Remove vestigial grid probability cutoffs
Verify the name of the requested local grid and raise an error if the grid doesn't exist
Appease the linting gods
Rebase commits with signoff messages (didn't realize that was required, my bad!)

…onger clipping through Spot's ankles)

ryan-roche · 2025-03-26T23:15:59Z

I've fixed the linting and un-hardcoded the local grid that the publisher node queries from the robot, but I've run into an issue.

ROS OccupancyGrid message data can only be in int8 format, and from the basic script I wrote to print out the data types of each of the response protobufs I get from the SDK client, they're all either uint8 or int16 values. I wrote two separate conversion cases, but it seems like the grid somehow fell through and was received in a different format to what I did earlier?

Regardless, I'm going to need some guidance on how to handle converting the data from the different grids into the int8 format needed by the OccupancyGrid messages. I know that the visualizer example in the SDK repository reads the grid data from the protobuf in as if it's floating point values, so I think I don't fully understand the workings of the local grid data.

As of now, I'm not sure how to proceed, as the format of the data in each grid is kind of its own thing, and I'm unable to figure out an appropriate conversion from the information available in the SDK documentation.

tcappellari-bdai

Looks good to me! Thanks for making the requested edits! I just want to test this out on one of our robots before merging as a sanity check

ryan-roche · 2025-03-27T19:08:02Z

I wouldn't merge the changes as-is- the node currently converts the grid data from the SDK response into 8-bit integers since that's the only supported format for the OccupancyGrid message data. In order for the data to still be usable, there's going to need to be some unique conversion logic for each grid- otherwise the data's just going to be clipped since most of it is in a 16-bit format

khughes-bdai

Thanks for the PR! This seems like a really helpful feature. Just left a few high level comments

khughes-bdai · 2025-03-28T13:52:54Z

spot_driver/spot_driver/spot_local_grid_publisher.py

+from spot_driver.manual_conversions import se3_pose_to_ros_pose
+from spot_driver.ros_helpers import get_from_env_and_fall_back_to_param
+
+VALID_GRIDS = ["terrain", "terrain_valid", "intensity", "no_step", "obstacle_distance"]


nit: can you leave a comment here with where these options are coming from? (I'm assuming it's in some BD docs)

khughes-bdai · 2025-03-28T13:54:21Z

spot_driver/launch/local_grid.launch.py

+def generate_launch_description() -> launch.LaunchDescription:
+    # Define launch arguments
+    local_grid_name = DeclareLaunchArgument(
+        "local_grid_name", default_value="obstacle_distance", description="Name of the local_grid you want published"


if the list of valid local_grids is the same as the list hardcoded in the node, I'd reccomend enforcing that too here in a choices field (makes it easier at runtime to see what the options are)

khughes-bdai · 2025-03-28T13:54:46Z

spot_driver/launch/spot_driver.launch.py

+            "local_grid_name",
+            default_value="obstacle_distance",
+            description="Name of the local_grid you want published (i.e. obstacle_distance, no_step, etc.)",
+        )


same comment here, you can have a choices option

khughes-bdai · 2025-03-28T14:00:42Z

spot_driver/spot_driver/spot_local_grid_publisher.py

+        if not self.ip or not self.username or not self.password:
+            self.get_logger().error("Robot credentials not found")
+            raise ValueError("Robot credentials not found")


nit: i think this block can be deleted, as these are guaranteed to fall back to the default values specified above

khughes-bdai · 2025-03-28T14:03:44Z

spot_driver/launch/local_grid.launch.py

+        output="screen",
+        parameters=[{"local_grid_name": LaunchConfiguration("local_grid_name")}],
+        namespace=LaunchConfiguration("spot_name"),
+        remappings=[("grid_topic_REMAP_ME", local_grid_topic)],


It seems like it's not necessary to include this remapping -- you are already getting the local grid name as a parameter inside your node, so you can internally just use that as the topic name. This should also be able to pick up the namespace of the spot_name

khughes-bdai · 2025-03-28T14:10:41Z

spot_driver/spot_driver/spot_local_grid_publisher.py

+        # "terrain_valid" and "intensity" grid protos are uint8
+
+        if raw_cells.dtype == np.uint8:
+            converted_cells = (raw_cells.astype(np.int16) - 128).astype(
+                np.int8
+            )  # Subtract a bias value so values remain correctly relative to each other
+
+        # "terrain", "no_step", and "obstacle_distance" grid protos are int16
+        else:
+            converted_cells = raw_cells.astype(np.int8)


Just to be clear, what lines here are the ones that are clipping data (from your earlier comment)?

khughes-bdai · 2025-03-28T16:49:36Z

spot_driver/spot_driver/spot_local_grid_publisher.py

+        # Set runtime variables
+        self.first_draw_done = False
+        self.im = None
+        self.fig = None
+        self.ax = None


another nit -- seems like these variables aren't used, and can be deleted

ryan-roche requested review from METEORITENMAX, amessing-bdai, bhung-bdai, jbarry-bdai, khughes-bdai, mpickett-bdai and tcappellari-bdai as code owners March 18, 2025 17:36

ryan-roche closed this Mar 24, 2025

ryan-roche force-pushed the main branch from 2267f04 to 3f3a5b3 Compare March 24, 2025 18:58

add grid publisher python node

3ae9132

ryan-roche reopened this Mar 24, 2025

ryan-roche added 3 commits March 25, 2025 18:40

add launchfile logic for local_grid publisher

8c34b17

Now get credentials using helper function from spot_driver

8082d0a

add module dependencies

137f855

ryan-roche mentioned this pull request Mar 26, 2025

Missing dependency from setup script #634

Closed

ryan-roche added 2 commits March 26, 2025 00:36

move ros2_numpy install to setup script- it isn't available through r…

942903a

…osdep :(

corrected read-only parameter configuration

e5364fb

ryan-roche added 5 commits March 26, 2025 15:13

add check for valid local_grid name

d68c297

Implemented type conversions for different grid datatypes

76e5f16

Set local grid origin's height to estimated ground plane height (no l…

74b857a

…onger clipping through Spot's ankles)

linting

cc06e2b

fix typo

e56e7c1

tcappellari-bdai approved these changes Mar 27, 2025

View reviewed changes

khughes-bdai reviewed Mar 28, 2025

View reviewed changes

Add local grid publisher #629

Are you sure you want to change the base?

Add local grid publisher #629

Uh oh!

Conversation

ryan-roche commented Mar 18, 2025

Change Overview

Testing Done

Uh oh!

tcappellari-bdai commented Mar 18, 2025

Uh oh!

tcappellari-bdai commented Mar 19, 2025

Uh oh!

ryan-roche commented Mar 22, 2025

Uh oh!

ryan-roche commented Mar 24, 2025

Uh oh!

khughes-bdai commented Mar 24, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

ryan-roche commented Mar 25, 2025

Uh oh!

ryan-roche commented Mar 26, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

tl;dr

What's left for me to do

Uh oh!

ryan-roche commented Mar 26, 2025

Uh oh!

tcappellari-bdai left a comment

Choose a reason for hiding this comment

Uh oh!

ryan-roche commented Mar 27, 2025

Uh oh!

khughes-bdai left a comment

Choose a reason for hiding this comment

Uh oh!

khughes-bdai Mar 28, 2025

Choose a reason for hiding this comment

Uh oh!

khughes-bdai Mar 28, 2025

Choose a reason for hiding this comment

Uh oh!

khughes-bdai Mar 28, 2025

Choose a reason for hiding this comment

Uh oh!

khughes-bdai Mar 28, 2025

Choose a reason for hiding this comment

Uh oh!

khughes-bdai Mar 28, 2025

Choose a reason for hiding this comment

Uh oh!

khughes-bdai Mar 28, 2025

Choose a reason for hiding this comment

Uh oh!

khughes-bdai Mar 28, 2025

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

khughes-bdai commented Mar 24, 2025 •

edited

Loading

ryan-roche commented Mar 26, 2025 •

edited

Loading