netNav - Networked mapping system and turtle pathfinding

This is my latest and greatest attempt at creating a turtle navigation / pathfinding system. It is based heavily on my previous attempt starNav, but this time it uses a shared map system that lets all connected turtles share the same map data to masssively speed up the process of mapping the world.

Video!:
A demo of the explore program I wrote to help map the world
[media]
http://www.youtube.com/watch?v=my8TubUddMI
[/media]

Setup:

Map Server:
The map server is where all the map data is stored. Your turtles will connect to this server to send/receive the latest map data. It can be run from any type of computer, just make sure it is connected to a modem.

mapServer <map_save_folder> <map_server_name>

netNav API:
The turtle will require:
- A wireless modem - to connect to the Map Server and to GPS
- Full GPS coverage of the area you want to navigate
- Fuel!
- (Optional) A sensor from openPeripherals - helps to massively speed up the mapping process

Once the netNav API is loaded, make sure rednet is open on the turtle, then you'll need to specify the name of the Map Server to connect to. Once connected you can proceed to use the 'goto' function to start pathfinding. See demo script below that does the above:

-- OPEN REDNET
for _, side in ipairs({"left", "right"}) do
	if peripheral.getType(side) == "modem" then
		rednet.open(side)
	end
end
if not rednet.isOpen() then
	printError("Could not open rednet")
	return
end

-- SET NETNAV MAP
netNav.setMap(<map_server_name>, 15) -- the second argument determines how frequently the turtle will check with the server for newer map data

-- START PATHFINDING
netNav.goto(<xPos>, <yPos>, <zPos>)

Explore + Remote Control Program:
The explore program sets your turtles exploring the world to improve the map data stored on the server. To get the program running you'll need to specify the Map Server to connect to, and the min + max coordinates of the area to explore (to define a rectangle area/bounding box).

explore <map_server_name> <minX> <minY> <minZ> <maxX> <maxY> <maxZ>

The remote control then lets you control the turtles whilst they explore. You can control them individually, or all together. The remote lets you pause them, call them to your current position, or return to where they first started.

netNavRemote <map_server_name>

Map Viewer Program:
The map viewer program allows you to directly view the map data on the Map Server. Simply pass it the name of the Map Server as the first argument and it will connect automatically.

mapViewer <map_server_name>

Downloads:
All code can be installed from the same pastebin installer script.

Map Server - pastebin run J8azvLQg remotemap_server
netNav API - pastebin run J8azvLQg netnav
Explore program - pastebin run J8azvLQg netnav_explore
Remote Control - pastebin run J8azvLQg netnav_remote
Map Viewer Program - pastebin run J8azvLQg remotemap_viewer

All code can also be viewed on GitHub.

Watched the entire video, and have to say, good work! Stuff like this has always interested me.

Actually, I started coding something like this, and kinda have a pathfinding algorithm, which works with obstacles, but doesn't work if the obstacles are in all three dimensions and in it's way from all three dimensions.

How does your algorithm work? I kinda looked into it, but don't quite get it.

This is what I mean by the obstacles:

Creator, on 23 December 2016 - 11:52 PM said:

How does your algorithm work? I kinda looked into it, but don't quite get it.

Sorry I completely missed this post!

It's just the A* algorithm but in 3 dimensions, so instead of just looking forward, back, left, and right, it also looks up and down when looking at which path to take.

It's not a straight forward implementation of A* though, it actually starts searching from both the start and end goals at the same time (essentially runs two instances of the A* algorithm), and detects when the two paths meet which ends up being the optimal path between the two points. It does all the calculation in one thread, but switches execution between the two algorithms when it notices the open nodes jumping around a lot (ie when it is not searching down a continuous path but is searching through lots of very different options). This helps it to find a path quicker when either the start or goal point is in a very complicated area and not in open space - there is a more mathematical explanation for why this is quicker if you want it!).

The other custom part of the algorithm is the heuristic it uses for tie breaking, it is set up to prefer going in straight lines instead of diagonals as they end up being quicker when you factor in the time it takes for the turtle to turn.

In terms of mapping the environment, the turtles all have 2 maps they use to determine the best path. The shared map that all turtles update is used as more of a suggestion of the best path - if a lot of turtles keep finding an object at the same point the value of that point in the shared map will keep increasing, so the turtles become less likely to try and go there. Then all turtles also maintain a local map that gets wiped between each call to netNav.goto, this is used to record obstacles that the turtle has ran into in this pathfinding session, so if they find an obstacle it gets recorded in the local map and the turtle will not attempt to go to that point again in that pathfinding session.

Hope that sheds a bit more light on it!? I will try and write all this down in a more ordered format at some point, just to remind myself how it works!!!

blunty666, on 28 July 2017 - 06:05 PM said:

Creator, on 23 December 2016 - 11:52 PM said:

How does your algorithm work? I kinda looked into it, but don't quite get it.

Sorry I completely missed this post!

It's just the A* algorithm but in 3 dimensions, so instead of just looking forward, back, left, and right, it also looks up and down when looking at which path to take.

It's not a straight forward implementation of A* though, it actually starts searching from both the start and end goals at the same time (essentially runs two instances of the A* algorithm), and detects when the two paths meet which ends up being the optimal path between the two points. It does all the calculation in one thread, but switches execution between the two algorithms when it notices the open nodes jumping around a lot (ie when it is not searching down a continuous path but is searching through lots of very different options). This helps it to find a path quicker when either the start or goal point is in a very complicated area and not in open space - there is a more mathematical explanation for why this is quicker if you want it!).

The other custom part of the algorithm is the heuristic it uses for tie breaking, it is set up to prefer going in straight lines instead of diagonals as they end up being quicker when you factor in the time it takes for the turtle to turn.

In terms of mapping the environment, the turtles all have 2 maps they use to determine the best path. The shared map that all turtles update is used as more of a suggestion of the best path - if a lot of turtles keep finding an object at the same point the value of that point in the shared map will keep increasing, so the turtles become less likely to try and go there. Then all turtles also maintain a local map that gets wiped between each call to netNav.goto, this is used to record obstacles that the turtle has ran into in this pathfinding session, so if they find an obstacle it gets recorded in the local map and the turtle will not attempt to go to that point again in that pathfinding session.

Hope that sheds a bit more light on it!? I will try and write all this down in a more ordered format at some point, just to remind myself how it works!!!

Doesn't A*'s design make accounting for turns simpler by adjusting the cost based on the turtle's facing?

I'd also be interested in the explanation as to why your approach is faster than simply pathfinding from the destination to the current location.

The Crazy Phoenix, on 28 July 2017 - 10:26 PM said:

Doesn't A*'s design make accounting for turns simpler by adjusting the cost based on the turtle's facing?
I'd also be interested in the explanation as to why your approach is faster than simply pathfinding from the destination to the current location.

Yes that's what it does in the algorthm I use, it keeps track of the parent coordinate for each coord it checks, and does a cross product of that with the direction of the neighbour coords in relation to the current coord it is adding to the open set, then multiplies it by a very small quantity so that it can be used to break ties in the heuristic function, but doesn't affect the overall cost of that node too much. This is the maths it uses:

0.0001*(preHeuristic + parent.length(parent:cross(neighbour - current)))

Here's the maths I came up with for deciding to search from both directions:

Spoiler

In the simplest case, with a zero cost heuristic, the A* algorithm in 3 dimensions can be pictured as a sphere expanding around the start coordinate (it becomes a breadth first search). For this we will simplify it even more and assume there are no obstacles in the world to block us. Therefore once the sphere has expanded enough that it reaches the goal coordinate we can say we have found the shortest path between the start and goal - i.e. the straight line between the two points.

From this we can see that the time taken to find the shortest path is the time it takes the radius of the sphere to expand enough to reach the goal. Therefore to measure the time it takes we need a function for time in terms of the radius of the sphere, or t = f( r )

The A* algorithm checks coordinates at a pretty constant rate, so the volume of the sphere (the number of checked coordinates increases at a constant rate, or in mathematic terms dV/dt = c where c is constant. By integrating with respect to time (t), we get a function for the volume (V) at any time (t), V = c x t

The last equation we need is for the volume of a sphere in terms of it's radius, or V = (4 x Pi x r³)/3

Putting those 2 equations together we get c x t = (4 x Pi x r³)/3, which rearranging gives t = (4 x Pi x r³)/(3 x c) which gives us the function f that we were after.

Now we just calculate the time taken for the single direction search and the both directions search to see what difference there is.

Single Direction:
Here we have one sphere expanding around the start coordinate until the sphere reaches the goal coordinate. So the rate of increase in the volume of the sphere is c and the radius it needs to reach is d, where d is the straight line difference between the start and goal. Plugging these figures into the equation gives t_s = (4 x Pi x d³)/(3 x c)


Both directions:
Here we have two spheres expanding around the start and goal coordinates, so seeing as the rate we can check coordinated is limited by the speed of our computer and assuming we alternate between checking a coord in the start sphere and a coord in the goal sphere, we can see that the colume of each sphere will increase by the constant rate c/2. Now we can see that to find the shortest path between the start and goal, we need each sphere to grow to a radius of d/2 before they collide and we have found a path between them, so that is the value for the radius that we need. Plugging all that in for one of the spheres we get t_b = (4 x Pi x (d/2)³)/(3 x (c/2)) = (Pi x d³) / (3 x c)


From this we can see that t_s = 4 x t_b so checking from both directions ends up being 4 times quicker!

In practice, when we also apply the heuristic, it turns out to be even quicker. Especially when either the start or goal coordinate are in complex areas like a cave and the other coordinate is in open space - it means that the search through the open space happens very quickly, even though the search through the cave side is happening very slowly.

Phew…….