Feature/mid level planner/main #30

MultyXu · 2025-10-10T21:44:07Z

As a complement to this PR.

… all of them should be in map frame]

Co-authored-by: Aryan Naveen <aryan_naveen@college.harvard.edu>

…need to figure out.

…rame now.

…d a plan, it will fall back to follow the original high level plan.

GoldenZephyr

Great progress! I left some specific comments. At a high level, I think there are two structural changes we should make.

Let's plan to always have a mid-level planner, but the mid-level planner that is used is flexible. For the case where we want to fall back to the current behavior, the mid-level planner should just be an "identity" planner, i.e. it returns the high-level plan.
I think we want to split up the mid-level planner, Occupancy Map, and ROS logic for updating the occupancy map slightly differently. The mid-level planner should be in charge of instantiating the occupancy map (and ROS pub/subs for updating the map)

Also, it would be great to add the instructions for how to test this functionality offboard the robot (either from a bag or the fake occupancy map), either here or in the ADT4 README

...xecutor_interface/robot_executor_interface/src/robot_executor_interface/mid_level_planner.py

spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py

GoldenZephyr · 2025-10-15T19:28:58Z

spot_tools_ros/src/spot_tools_ros/fake_path_publisher.py

+            return [m, start, end]
+
+
+    def to_viz_msg(self, action, marker_ns):


Is there any reason that these functions for creating marker arrays from the action descriptions won't work here? https://github.com/robustrobotics/spot_tools/blob/main/robot_executor_interface/robot_executor_interface_ros/src/robot_executor_interface_ros/action_descriptions_ros.py

When we use the existing functions we get this strange error Failed to publish path: Unknown action type <class 'robot_executor_msgs.msg._action_sequence_msg.ActionSequenceMsg the issue is for whatever reason our action type has _ before _action_sequence_msg and it should not be there?

MultyXu · 2025-10-16T01:52:22Z

@GoldenZephyr Now the branch should be mostly refactored according to the suggestions you gave. But there might be further small changes needed.

GoldenZephyr

Good progress. Made a couple more comments (and I think there might be a small number of comments from last time to wrap up). Also,

You'll want to rebase on main (and resolve the conflict, let me know if you need any help with that) so that CI will run
I think some of the functions used to publish viz markers for the fake path can be used from the existing robot_executor action_description utilities rather than implemented here (see comment below).

I haven't had a chance to try running on my side yet, but thanks for adding the steps in the README

GoldenZephyr · 2025-10-16T20:32:22Z

...xecutor_interface/robot_executor_interface/src/robot_executor_interface/mid_level_planner.py

+    def clone_grid(self):
+        return self.occupancy_map.copy() if self.occupancy_map is not None else None
+
+    def set_grid(self, grid, resolution, origin, robot_pose, update_time):


Need to acquire the lock for the duration of this function (what if one thread tries accessing this information while it is being set, and only some of the fields have updated?)

...xecutor_interface/robot_executor_interface/src/robot_executor_interface/mid_level_planner.py

spot_tools/src/spot_executor/spot_executor.py

...xecutor_interface/robot_executor_interface/src/robot_executor_interface/mid_level_planner.py

spot_tools_ros/src/spot_tools_ros/fake_occupancy_publisher.py

GoldenZephyr · 2025-10-16T21:00:29Z

spot_tools_ros/src/spot_tools_ros/fake_path_publisher.py

+from robot_executor_msgs.msg import ActionSequenceMsg, ActionMsg
+
+
+def get_robot_pose(tf_buffer, parent_frame: str, child_frame: str):


Let's make a utils.py in this directory for now. We can think about a more centralized utils directory outside of spot_tools_ros later

GoldenZephyr · 2025-10-16T21:03:44Z

spot_tools_ros/src/spot_tools_ros/occupancy_grid_ros_updater.py

+
+    def occupancy_map_callback(self, msg):
+
+        with self.occupancy_map:


I would recommend handling the locking inside the implementation of set_grid, and then this function doesn't need to worry about acquiring the lock at all.

What if the ros updater updates the grid in the middle of the plan_path function?

What specifically is the worry? During the time when plan_path assumes the occupancy map isn't changing, it should acquire the lock. If it has the lock and we receive a message here to update the occupancy map, we wait here until we can get the lock. That should work fine, although we do need to make sure that spot_executor_ros has its ROS callbacks setup up properly to ensure that blocking in the occupancy map callback doesn't block other ROS updates (specifically, we need to make sure spot_executor_ros has a MultiThreadedExecutor and also puts this callback in its own mutually exclusive callback group.

As a starting point, make sure that only the necessary lines are within this locking block. I think its should only be the call to set_map. And then I think it will be clear that we can just move the acquisition of the lock into the top of the set_map function (as opposed to right before calling the function).

GoldenZephyr · 2025-10-16T21:04:57Z

spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py

    ma.markers.append(m2)
    return ma

+def build_marker(pt, ns, frame, mid=0, color=[1,0,0]):


Just to follow up on this, can the two marker-building functions be combined into one? If not, they need docstrings explaining the difference.

spot_tools_ros/src/spot_tools_ros/utils.py

MultyXu · 2025-10-20T20:20:27Z

There is small one line change in hydra_ros for the active window sink config. Do we want to make a PR on that repo as well?

…_level_planner/main

GoldenZephyr

Looks very close, just a couple of things:

I think there might still be a couple of comments unaddressed from last time. Let me know if you have any questions on those.
Most of the time, there's no reason to commit commented out code. For any commented out line of code, I would either delete it, uncomment it, or leave a more extensive comment discussing the code, what it does, and why it's there as a comment.
any TODOs or similar notes in comments should either be deleted (if they aren't adding much information / aren't very important), or enough context should be added to understand what's involved with implementing the TODO.
I think this was an unresolved comment, but in the PR there's some code for converting an ActionDescription message to an RVIZ marker message. The robot_executor package has implementations of this already, can we use those instead of adding code here?

GoldenZephyr · 2025-10-21T02:31:39Z

...xecutor_interface/robot_executor_interface/src/robot_executor_interface/mid_level_planner.py

+            a_star_path_execute = a_star_path_metric
+
+            # project global point to local index
+            # TODO: add comment to explain code


should resolve this TODO

GoldenZephyr · 2025-10-21T02:32:23Z

...xecutor_interface/robot_executor_interface/src/robot_executor_interface/mid_level_planner.py

+        if frontier_cells.size == 0:
+            return free_cell_candidate
+
+        # dists_frontier = np.linalg.norm(frontier_cells - np.array(goal).T, axis=1)


GoldenZephyr · 2025-10-21T02:32:40Z

...xecutor_interface/robot_executor_interface/src/robot_executor_interface/mid_level_planner.py

+            return 0 <= cell[0] < rows and 0 <= cell[1] < cols
+
+        def is_obstacle(cell):
+            # return self.occupancy_map[cell[0], cell[1]] != 0


GoldenZephyr · 2025-10-21T02:32:57Z

...xecutor_interface/robot_executor_interface/src/robot_executor_interface/mid_level_planner.py

+            # return self.occupancy_map[cell[0], cell[1]] != 0
+            # -1 unknown, 100 occupied, 0 free
+            return self.occupancy_map[cell[0], cell[1]] > 0 # threshold
+            # TODO: might be the case


remove? or clarify?

GoldenZephyr · 2025-10-21T02:33:15Z

...xecutor_interface/robot_executor_interface/src/robot_executor_interface/mid_level_planner.py

+        Input: high level path in <robot>/odom frame, Nx2 numpy array
+        Output: (bool, path) -> (success, path in odom frame)
+        '''
+        # self.feedback.print("INFO", f"{high_level_path_metric[-1]}")


uncomment or remove

GoldenZephyr · 2025-10-21T02:35:23Z

spot_tools/src/spot_skills/navigation_utils.py

            x=target_point.x, y=target_point.y, angle=yaw_angle
        )
+        feedback.print("INFO", f"Navigating to waypoint {current_waypoint}")
+        # feedback.print("INFO", f"Navigating to waypoint {target_point}")


uncomment or remove

GoldenZephyr · 2025-10-21T02:50:27Z

spot_tools_ros/src/spot_tools_ros/occupancy_grid_ros_updater.py

+            occ_map = np.array(msg.data, dtype=np.int8).reshape((h, w))
+
+            # put test code here to see status
+            # robot_pose = self.spot_interface.get_pose() # not working in sim ??


is this resolved?

GoldenZephyr · 2025-10-21T02:53:09Z

spot_tools_ros/src/spot_tools_ros/occupancy_grid_ros_updater.py

+
+    def occupancy_map_callback(self, msg):
+
+        with self.occupancy_map:


What specifically is the worry? During the time when plan_path assumes the occupancy map isn't changing, it should acquire the lock. If it has the lock and we receive a message here to update the occupancy map, we wait here until we can get the lock. That should work fine, although we do need to make sure that spot_executor_ros has its ROS callbacks setup up properly to ensure that blocking in the occupancy map callback doesn't block other ROS updates (specifically, we need to make sure spot_executor_ros has a MultiThreadedExecutor and also puts this callback in its own mutually exclusive callback group.

As a starting point, make sure that only the necessary lines are within this locking block. I think its should only be the call to set_map. And then I think it will be clear that we can just move the acquisition of the lock into the top of the set_map function (as opposed to right before calling the function).

GoldenZephyr · 2025-10-21T03:02:56Z

Oh also I think CI is failing because of formatting or code linting -- I would recommend running the pre-commit hooks locally to check what the issues are.

GoldenZephyr · 2025-10-21T03:08:51Z

There is small one line change in hydra_ros for the active window sink config. Do we want to make a PR on that repo as well?

I think we should be able to just include the active window sinks in the hydra config that's already in ADT4. If you put the contents of this file (https://github.com/MIT-SPARK/Hydra-ROS/blob/3d7d24ed88232ab6173c7e009504a5f762539d6c/hydra_ros/config/sinks/active_window_sinks.yaml) (plus whatever modifications you have) in the ADT4 Hydra config under the active_window yaml namespace, it should work (and then you don't need to extra link in master.launch that refers to the extra yaml file at all).

MultyXu and others added 22 commits September 23, 2025 17:03

draft structure for the mid level planner.

f3ba828

Add high level structure for using MidLevelPlanner in spto_executor

4509da3

add more info about the map into MidLevelPlanner

40eab01

add todos after meeting with Aaron and Nathan

f970725

test

b317dca

Figure out the tf for robot pose, map origin, and planed path. [maybe…

a49b371

… all of them should be in map frame]

clean code, proper transformation between grid index and global pose

a015bfc

wip

aba7143

projection heuristic

60107f2

pre-merge commit

0cef18b

First draft of mid-level-planner put into test.

478fba6

Co-authored-by: Aryan Naveen <aryan_naveen@college.harvard.edu>

after fist test

0210ae1

ugly update but path is correctly passed. Visualization not working, …

bcfe296

…need to figure out.

Path plan is in <robot>/odom frame. Everything plan respect to this f…

900d90a

…rame now.

debugging global point projection

3f23a61

working path plan

8f05462

Now outputing plan iteratively untill reached goal. If A* fail to fin…

a705ac9

…d a plan, it will fall back to follow the original high level plan.

Adding flag for fake occupancy and fake path.

3b2aea1

cleaned & added projection viz

8848a73

Visualization working

d18db04

further clean up and add back informative log messages.

461d2f6

independent fake occupancy and path publisher. Naive map inflation.

d33ea7e

GoldenZephyr requested changes Oct 14, 2025

View reviewed changes

Aryan-Naveen and others added 7 commits October 14, 2025 15:16

Visualizes test path now

c7a316e

Fixed lookahead bug

d9171ea

Heurestic for projection

64225fe

Code clean up: move occupancy grid outside of mid-level planner

523944a

further clean up

4ecaf26

Improved heurestic for projection

c342875

Documentation

d87b1e5

Documentation and better testing

6c609e3

GoldenZephyr reviewed Oct 15, 2025

View reviewed changes

MultyXu and others added 6 commits October 15, 2025 18:41

major refactor

a07ae46

merge MLP/main

0f0069e

furhter bug fix after merge

63560b5

add parameters to launch file.

650e011

After robot test fixes

3178a06

minor fix

41a1af5

GoldenZephyr requested changes Oct 16, 2025

View reviewed changes

rebase main

b25a5ef

Aryan-Naveen and others added 7 commits October 20, 2025 17:35

cleanup

fa3240c

Address some of Aaron's comments

5e9e54d

Merge branch 'feature/mid_level_planner/apn-cleanup' into feature/mid…

6493bbb

…_level_planner/main

viz cleanup

33fe777

More comments on code.

8f363f2

Merge branch 'feature/mid_level_planner/apn-cleanup' into feature/mid…

af06d6b

…_level_planner/main

Bug fix after addressing comments

386c76f

GoldenZephyr requested changes Oct 21, 2025

View reviewed changes

Aryan-Naveen and others added 7 commits October 21, 2025 08:59

pre-commit fixed

5de3298

modifying OccupancyMap lock.

b2d19f7

precommit fix

420db91

timeout for failing

9385a5a

broken viz

60cb8e9

viz update

1fb95a8

viz and pre-commit

384cb1d

GoldenZephyr merged commit f99f1dd into main Oct 22, 2025
2 checks passed

		return [m, start, end]


		def to_viz_msg(self, action, marker_ns):

		from robot_executor_msgs.msg import ActionSequenceMsg, ActionMsg


		def get_robot_pose(tf_buffer, parent_frame: str, child_frame: str):


		def occupancy_map_callback(self, msg):

		with self.occupancy_map:

Feature/mid level planner/main #30

Feature/mid level planner/main #30

Uh oh!

Conversation

MultyXu commented Oct 10, 2025

Uh oh!

GoldenZephyr left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

MultyXu commented Oct 16, 2025

Uh oh!

GoldenZephyr left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

MultyXu commented Oct 20, 2025

Uh oh!

GoldenZephyr left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

GoldenZephyr commented Oct 21, 2025

Uh oh!

GoldenZephyr commented Oct 21, 2025

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

5 participants