Custom pathplanner that takes preferences into account

[thaije]

Is your feature request related to a problem? Please describe. A pathplanner that could avoid specific areas has been requested, e.g. because it makes the user walk slower over specific tiles. At the moment, the A star pathplanner has been hardcoded and is not easily switchable to an alternative pathplanner, without changing the MATRX core code.

Describe the solution you would like

• A way to easily create your own pathplanner and occupation map (how is calculated what should be walked on or not), that can be added without changing the MATRX core code.
• A new pathplanner that can take a traversability_penalty into account that can be set on individual objects to indicate how hard the pathplanner should avoid walking over those objects.

Describe alternatives you have considered Extending the existing A star algorithm to include the weighted pathplanning as an option, however it also requires a different function for calculating the occupation map, so that is difficult and not really future proof.

Additional context As part of a TNO project, V2205.

[thaije]

See https://github.com/matrx-software/matrx/commit/fa15b1eac1d2ed942afeaccf20409096c7bf4095

[thaije]

Using the weighted pathplanner

• Add objects to your MATRX world with the traversability_penalty property, with a value > 0 and < 1, in addition to the is_traversable option (True or False). The agent will try to avoid objects with a high traversability_penalty (but will still walk over them if the path is much shorter).
• In the initialize function of your agent, initialize the navigator to use the new Navigator.WEIGHTED_A_STAR_ALGORITHM and with its settings like so:

  
  # Initialize this agent's state tracker
  self.state_tracker = StateTracker(agent_id=self.agent_id)

self.navigator = Navigator(agent_id=self.agent_id, action_set=self.action_set, algorithm=Navigator.WEIGHTED_A_STAR_ALGORITHM, algorithm_settings={"traversability_penalty_multiplier": 10})

For an example of an entire agent using the new pathplanner see this file: https://github.com/matrx-software/matrx/blob/319_custom_pathplanner(s)/matrx/agents/agent_types/patrolling_agent.py 

## Tweaking the weighted pathplanner 
By increasing `traversability_penalty_multiplier` in the `algorithm_settings` above, the algorithm will try to avoid objects more aggresively with a `traversability_penalty`. 

You can calculate this as follows: 
For an agent that uses the pathplanner, normally 1 step sideways or upwards has a 'cost' of 1. However, if there is a object on route with a `traversability_penalty=0.5` and the `{traversability_penalty_multiplier": 10}`, then walking over that object has a cost of `0.5*10=5`, thus equal to walking over 5 normal tiles. 

In this way, the agent will avoid to walk over this object. However, if the alternative is more than 5 normal tiles longer, it will opt t o walk over the slow tile instead. 

# Creating a custom pathplanner
It is also possible to create a new pathplanner, e.g. with a different pathplanner algorithm, or one that completely ignores the `is_traversable` such as maybe a drone would do. To do so:

- Change the navigator settings in your agent's initialize function as so: 
```python
self.navigator = Navigator(agent_id=self.agent_id, action_set=self.action_set,  algorithm="name_of_your_pathplanner",, custom_algorithm_class=YourCustomPathPlannerClass, traversability_map_func=your_custom_traversability_map_function, algorithm_settings={"settings_specific_to_your_pathplanner": True})

• Create the traversability_map_function that takes the current agent state, and converts it into a map of what is traversable and what not for your algorithm. See here for an example.
• Create a class for your pathplanner that takes the agent's action set (in what directions can it move) and settings for your algorithm. It should return a list of locations to traverse from the current location to the goal location. See here for an example.