-
Notifications
You must be signed in to change notification settings - Fork 38
Topological Navigation Restrictions
Topological restrictions are a mechanism to set hard constraints to the robots navigation. The constraints can be set on top of the underlying navigation structure, i.e. the nodes and edges, to define under which conditions a path can be navigated by a certain robot. Under the same topological map, different robots performing different tasks are ultimately allowed to navigate in different subsets of the entire topology.
There are two types of restrictions that can be specified:
- planning restrictions, they create a subset of the entire topological map by eliminating those nodes/edges that do not respect the restrictions. This means that from the robot point-of-view only this subset of the map is accessible for planning and localisation.
- runtime restrictions, these restrictions are checked at runtime during the robot navigation. Used for highly dynamic conditions, like the presence of obstacles in the path. They only affect the current navigation plan and actions, the robot still can see the nodes/edges restricted at runtime for planning and localisation.
In the example image below we can see how the planning restrictions produce a different "sub-map", from the same combined map, for each different robot.
Restrictions can be defined in the topological map definition for each edge and node. Each restriction is a boolean expression as a combination of the operators &
(and), |
(or), ~
(not) and boolean conditions.
If the expression evaluates as True the node/edge is not restricted (the robot can traverse it), otherwise is it restricted for the robot.
For example:
...
nodes:
- node:
localise_by_topic: ''
name: WayPoint140
parent_frame: map
restrictions_planning: 'robot_tall & (task_uv | task_data)' # the node is available only to "tall" robots that are performing "uv" or "data collection" tasks
restrictions_runtime: 'obstacleFree_1' # the node can be traversed when free of obstacles within 1 meter of distance
edges:
- edge_id: WayPoint140_WayPoint74
node: WayPoint74
action: move_base
action_type: move_base_msgs/MoveBaseGoal
fluid_navigation: True
recovery_behaviours_config: ''
restrictions_planning: 'True' # if True, the edge is not restricted; if False, the edge is always restricted
restrictions_runtime: 'obstacleFree_1' # the edge can be traversed when free of obstacles within 1 meter of distance
...
The boolean conditions are in the format NAME_ARGUMENT
, where NAME
specifies the name of the condition and ARGUMENT
the argument to pass for the specific instance. Because of how sympy works, only string and positive integer arguments are allowed (e.g. 'obstacleFree_0.5' not allowed).
The services /topological_map_manager2/update_edge_restrictions
and /topological_map_manager2/update_node_restrictions
can be used to update the restrictions at run-time.
Name | Arguments | Type | Description |
---|---|---|---|
robot | {tall, short} | planning | Specifies the type of robot |
task | {uv, transportation, data} | planning | Specifies the task that the robot is performing |
obstacleFree | runtime | Specifies the range, in meters, of free space around node/edge to successfully navigate |
In order to create a new restriction, you simply need to extend the abstract class AbstractRestriction.
It's important you set the name
property, which is the one that will be used to refer to the condition in the boolean expression. Also, you need to provide an implementation to all the abstract methods.
Look at the conditions that are already implemented to see how you can access the robot state and other context information... the whole system can be improved so open a pull-req if you find a way to make this more easy to extend to any new condition!
If getting an error such like 'Or' object has no attribute simplify, ensure sympy version is >= 1.5.1