Created by [ Rowan Dempster] on Dec 27, 2019
Architecture and Y2 Transfer of Information
https://docs.google.com/document/d/1gwdH9bKd3mHhI1z9mOiTxt8OIrB3cUg9yiQ4aN5J3aA/edit?usp=sharing
What is an Occupiable Object?
An Occupiable Object is any object that is “Dynamic” or has an area of
influence that is greater than just it’s bounding box.
A complicated example would be Pedestrians, as they require STATE to function. A pedestrian can be either waiting, moving, or stopped. The Cutting layer / costmap layer that represents the object must change based on the pedestrian’s state. If the pedestrian is not crossing, nothing is drawn. Logic is also needed to determine what state the pedestrian is in.
A simpler example would be a Cyclist, who needs a 3m padding. The only logic required is a modified costmap layer.
Occupiable Objects are: Pedestrians, Railroad Bar, Cyclist, Stop Sign, Traffic Light, Parking signs (Handicap parking only etc), Intersection Signs (Left turn/Right turn only, Do not enter)
Occupiable Object API:
All occupiable objects will have their own classes that are customized to the needs of the object. All occupiable objects will inhereit from an OccupiableObject template class with the following methods:
c++
OccupiableObject:
void update(Env)
query()
bool is_cache_invalid()
The update function will receive the new environment / time and
update the required fields in the Occupiable Object. This includes
states, etc.
Query will use the CostmapUtils functions to return a union of the
cutting lines that should be drawn and a costmap layer (The matrix
representation of the bounding box that will be drawn to the Costmap).
For is_cache_invalid, The object will also decide (probably in Update), whether or not the previous costmapLayer returned is still valid or not. This logic is dependent on the Obstacle. Cyclists might depend on the object’s orientation. Pedestrians will depend about the state of the pedestrian.
Example:
c++
Pedestrian: OccupiableObject
override update(env, time):
// LOGIC HERE
If (PEDESTRIAN IS CROSSING):
state = waiting
If (time > 5)
state = stopped
…
override query():
switch (state):
case stopped:
return none
case waiting:
return cutting_line(crosswalk)
case moving:
return costmap(draw_vel_cost(ped.vel.vector))
Occupiable Master API
OccupiableMaster:
Map <id, OccupiableObject> objects
// Accessors or helper functions
Occupiable master will be a long lived class that is defined when the ros node starts, and destroyed when the ros node finishes.
c++
Ros_main:
OccupiableMaster = new Master()
We will need to pass around OccupiableMaster or use it like a singleton. This depends on how implementation goes.
c++
On Receive New Environment (Wherever the environment message is received)
env_callback(env_msg):
env = ros_msgs(env_msg)
custting_lines, layers, costmap, costmaputils
f_env = costmaputils.filter(env)
// STOPLINE ASSOCIATION
for obj in f_env:
if (master.contains(obj.id)):
obj.update(env, delta)
if obj.is_cache_invalid():
lines, layers <= obj.query
costmapCache.update(obj.id, lines, layers)
else:
lines, layers <= costmapCache.get(lines, layers)
else:
switch objtype
case PED:
master.put(obj.id, new Ped(obj))
lines, layers <= obj.query
costmapCache.update(obj.id, lines, layers)
...
occupiableMaster.update(env)
dynamicLayer, cuttingLayer = occupiableMaster.query()
costmap.setDynamicLayer(dynamicLayer)
costmap.getStaticFromEnv(env)
PASS COSTMAP AND cutting_lines TO TRAJECTORY
Whenever a new environment is received, we need to update all the objects inside of occupiable master and also update the costmap cache. We compile the dynamic + static layers of the costmap and the cutting layer, then pass both to trajectory planner.
Caching
There are two caches. One is the CostmapCache, and one is in the Occupiable Master. CostmapCache will contain a cache of costmap layers - eigen matrix representations of an object. This is used to avoid reproducing the intensive process of drawing obstacles every tick. Note that each “layer” is not a full costmap layer, but a drawing of the bounding box that is then compiled onto the costmap at the correct position.
If the red is the object, the yellow is the blur, and the green is the
extent of the eigen matrix that is cached.
Costmap utility functions are defined
here: https://docs.google.com/document/d/1fdoAPptk-d9cl6WdLzySRhGZM9_qRT1uBCZaDsCWZw4/edit#heading=h.dfickkdd0o8d
Occupiable also has a “cache” or a mapping of Object IDs to Occupiable Classes. There needs to be logic here about when to draw an object (do you draw an object if it’s recent in the mapping but not in the environment currently?) and when to clean up the map (remove an object immediately after it disappears from the env or wait 3s?).
In the future we should also have some checking as a fallback to
Perception’s Object Tracking. Eg: An object disappeared from the cache,
but the same type of object appears with a new ID half a meter away.
Mark those as the same ID. This depends on how reliable processing
object tracking is. Since all logic will be dependent on object tracking
to work, we want to be extra careful.
Note that specifics about what gets cached, what the exact function
arguments are etc. are subject to change.
Be sure to read through the Costmap wiki as we use their tools and API: https://phabricator.watonomous.ca/w/software/path_planning/costmap/
Anqi Wu
Dhruv Rajani
Thomas Gao
Tirth Patel
John Philips
This is a Google Doc outline of the Occupiable Objects project plan written by [\@321329096]{.phui-tag-core .phui-tag-color-person style=”color: inherit;”}
Document generated by Confluence on Dec 10, 2021 04:01