Remove graph nodes from locked-off hallways when searching for paths

[sea-bass]

This kind of started as a hack, but I'm really warming up to it in that I can't think of anything better that won't take some serious recursive / geometric analysis...

I recently ran into a "feature" with navigating to a hallway. Take this world:

If you are navigating to the hallway between office and closet, the default heuristic will tell the robot to go to the left side of the hallway because it's nearer to the current robot pose as the crow flies. But as you see, path planning will fail.

This PR will look for "landlocked" hallways as described above by checking that there is at least one open hallway on either room, or that the robot isn't inside that room. If there isn't (like in the case of closet in the screenshot above), the navigation graph node on that side gets eliminated from consideration.

If the hallway is locked on both rooms, then... yeah, path planning will fail in that there is no valid target. But I think that's okay.

[github-actions]

Coverage Report

File	Stmts	Miss	Cover	Missing
__init__.py	0	0	100%
core
__init__.py	9	0	100%
dynamics.py	43	0	100%
gazebo.py	136	8	94%	55, 124, 230, 244–248
hallway.py	93	8	91%	193, 235–238, 262–264
locations.py	131	21	83%	69, 71, 88, 103–104, 106, 117–118, 120–121, 138, 196, 211, 236, 246, 291, 302–303, 305–306, 335
objects.py	69	4	94%	78, 106, 137, 197
robot.py	458	67	85%	259–260, 272, 275, 279–280, 285–286, 332, 392–395, 399–402, 426, 429, 433–435, 462, 464, 506–507, 509–510, 527–529, 545–547, 614–616, 641–643, 694–696, 765–767, 819–821, 844, 850, 853, 855, 857, 860–862, 872, 880, 886, 892, 898, 910, 952, 970, 973, 978
room.py	48	1	97%	113
world.py	742	196	73%	147–148, 184–185, 189, 242–243, 245–246, 274–275, 326, 340–341, 343–344, 359–361, 381–385, 387–389, 391–392, 395, 397–400, 404–408, 411–413, 416–427, 443–444, 462–464, 478, 480, 483–484, 489–491, 532, 534, 537–538, 543–545, 557–559, 594, 596, 599–600, 605–607, 632, 634, 637–638, 643–645, 693–694, 697, 713, 743–744, 755, 758, 766, 782–786, 788–790, 793–794, 797–800, 802, 805, 807–809, 811–813, 815, 830, 837, 848–852, 887–888, 892–893, 911–912, 918–919, 921–922, 924, 926–927, 929, 931–932, 942–943, 946, 948, 962, 964, 971, 1018, 1049–1050, 1061, 1072–1074, 1076–1079, 1081, 1098–1099, 1103–1104, 1127, 1129–1130, 1163–1164, 1166, 1183–1184, 1215, 1229–1232, 1244–1247, 1261, 1272–1273, 1275, 1319–1320, 1372, 1452–1453, 1518, 1528
yaml_utils.py	108	6	94%	61, 65, 167–168, 187–188
gui
__init__.py	2	0	100%
main.py	234	52	77%	22–24, 45, 77–78, 219–227, 239–247, 254–257, 259, 262–268, 272, 277–278, 282–283, 294, 334–335, 344–345, 349–350, 361–364
world_canvas.py	301	62	79%	45–46, 48–52, 55, 61–62, 169–172, 176–177, 182–183, 190, 192, 249–251, 257, 274–276, 323, 380–381, 408–409, 411, 413–415, 418–421, 423, 427–433, 436, 439–440, 455–457, 460, 479, 522, 543, 547, 570, 587, 601
manipulation
__init__.py	1	0	100%
grasping.py	236	61	74%	102, 110–112, 114–122, 124, 132, 462, 464–466, 470–472, 566–568, 571–575, 583, 586–588, 593–594, 600–604, 607–614, 617–619, 628, 630, 632–638
navigation
__init__.py	11	1	90%	37
a_star.py	77	11	85%	83–88, 90–92, 180, 189
execution.py	90	7	92%	73–75, 80–82, 119
occupancy_grid.py	139	14	89%	44–47, 49–53, 56, 192, 226, 234, 238
prm.py	56	4	92%	60–61, 140, 149
rrt.py	161	0	100%
visualization.py	32	6	81%	88–93
world_graph.py	58	4	93%	117, 120, 130, 139
planning
__init__.py	0	0	100%
actions.py	112	0	100%
planning/pddlstream
__init__.py	7	1	85%	13
default_mappings.py	9	0	100%
planner.py	40	8	80%	119–126
primitives.py	61	4	93%	68, 82, 94, 232
utils.py	99	8	91%	66, 93, 105, 115–116, 207–208, 211
utils
__init__.py	0	0	100%
general.py	33	4	87%	23, 45, 51–52
knowledge.py	154	22	85%	86, 117, 156–161, 163–164, 167–168, 170–173, 189, 204, 208, 250, 266, 329
motion.py	70	3	95%	93–94, 96
polygon.py	111	0	100%
pose.py	89	0	100%
search_graph.py	93	3	96%	240–242
trajectory.py	67	2	97%	130, 165
TOTAL	4180	588	85%

Tests	Skipped	Failures	Errors	Time
195	0 💤	0 ❌	0 🔥	3m 20s ⏱️