Skip to content

Educational autonomous driving stack for the Polaris GEM vehicle

License

Notifications You must be signed in to change notification settings

krishauser/GEMstack

Folders and files

NameName
Last commit message
Last commit date

Latest commit

ย 
ย 
ย 
ย 
ย 
ย 
ย 
ย 
ย 
ย 
ย 
ย 
ย 
ย 
ย 
ย 
ย 
ย 
ย 
ย 
ย 
ย 
ย 
ย 
ย 
ย 
ย 
ย 
ย 
ย 
ย 
ย 
ย 
ย 
ย 

Repository files navigation

GEMstack: software for CS588 Autonomous Vehicle System Engineering

๐Ÿ“– Online documentation

๐Ÿš— About the GEM e2 vehicle

๐Ÿ—Ž ROS code for launching vehicle

Dependencies

GEMstack uses Python 3.7+ and ROS Noetic. (It is possible to do some offline and simulation work without ROS, but it is highly recommended to install it if you are working on any onboard behavior or training for rosbag files.)

You should also have the following Python dependencies installed, which you can install from this folder using pip install -r requirements.txt:

  • numpy
  • scipy
  • matplotlib
  • opencv-python
  • torch
  • klampt
  • shapely
  • dacite
  • pyyaml

In order to interface with the actual GEM e2 vehicle, you will need PACMOD2 - Autonomoustuff's low level interface to vehicle. You will also need Autonomoustuff's sensor message packages. The onboard computer uses Ubuntu 20.04 with Python 3.8, CUDA 11.6, and NVIDIA driver 515, so to minimize compatibility issues you should ensure that these are installed on your development system.

From a fresh Ubuntu 20.04 with ROS Noetic and CUDA 11.6 installed, you can install these dependencies by running setup/setup_this_machine.sh from the top-level GEMstack folder.

To build a Docker container with all these prerequisites, you can use the provided Dockerfile by running docker build -t gem_stack setup/. For GPU support you will need the NVidia Container Runtime (run setup/get_nvidia_container.sh from this directory to install, or see this tutorial to install) and run docker run -it --gpus all gem_stack /bin/bash.

In this folder

Your work will be typically confined to the GEMstack/ folder, and you may use the testing/, logs/, data/, and scenes/ folders.

  • GEMstack/: the software package (see below).
  • main.py โฏ๏ธ: the standard entry point to running onboard behavior (see below).
  • launch/ ๐Ÿš€ Launch configuration files are listed here. Specify these as an argument to main.py.
  • logs/ ๐Ÿชต: logs will be placed here. These will not be committed to the Github repo.
  • data/ ๐Ÿ’ฝ: standard location to place datasets for training, i.e., downloaded or curated from other sources. These will not be committed to the Github repo.
  • scenes/ ๐ŸŒŽ: standard location to place scenes for simulation.
  • testing/ ๐Ÿงช: test scripts to check whether GEMstack components are functioning.
  • docs/ ๐Ÿ“–: ReadTheDocs documentation source files are placed here. Used by automated tools to build the online documentation.
  • README.md: this file.
  • LICENSE: MIT license.
  • .gitignore: Git ignore file. All files that match these patterns will not be added to Git.
  • .readthedocs.yaml: ReadTheDocs configuration file.
  • pyproject.toml: Describes the GEMstack Python package for pip install.
  • requirements.txt: A list of Python dependencies for the software stack, used via pip install -r requirements.txt.

In addition, some tools (e.g., pip) will build temporary folders, such as build and GEMstack.egg-info. You can ignore these.

TODO list

  • Test ROS replay
  • Test behavior replay
  • More sophisticated simulator with sensor messages

Package structure

All algorithms and routines in the package, i.e., those that would be run onboard, are within the GEMstack/ folder.

Legend:

  • ๐ŸŸฅ: TODO
  • ๐ŸŸง: early development (not usable)
  • ๐ŸŸจ: in development (usable, but many features not complete or tested)
  • ๐ŸŸฉ: stable (most features complete and tested)
  • ๐ŸŸฆ: mature

mathutils/: ๐Ÿงฎ Math utilities common to onboard / offboard use.

  • ๐ŸŸฅ cameras: Contains standard camera models.
  • ๐ŸŸจ collisions: Provides collision detection and proximity detection.
  • ๐ŸŸฉ control: Contains standard control techniques, e.g., PID controller.
  • ๐ŸŸฆ differences: Finite differences for derivative approximation.
  • ๐ŸŸฆ dubins: Contains first- and second-order Dubins car dynamics models.
  • ๐ŸŸฆ dynamics: Contains standard dynamics models.
  • ๐ŸŸจ intelligent_driver_model: the IDM model used for adaptive cruise control behavior.
  • ๐ŸŸฉ signal: 1d signal processing.
  • ๐ŸŸฉ transforms: 2d and 3d rotations and rigid transforms.
  • ๐ŸŸจ units: constants to help with unit conversion.

utils/: ๐Ÿ› ๏ธ Other utilities common to onboard / offboard use.

  • ๐ŸŸฉ logging: Provides logging and log replay functionality.
  • ๐ŸŸจ mpl_visualization: Tools for plotting data on knowledge, state, etc. in Matplotlib.
  • ๐ŸŸจ klampt_visualization: Tools for plotting data on knowledge, state, etc. in Klampt.
  • ๐ŸŸฅ gazebo_visualization: Tools for converting data on knowledge, state, etc. to ROS messages used in Gazebo.
  • ๐ŸŸฆ settings: Tools for managing settings for onboard behaviour. If you're tempted to write a magic parameter or global variable, it should be placed in settings instead.
  • ๐ŸŸฆ config: Tools for loading config files.
  • ๐ŸŸฉ conversions: Tools for converting objects to and from standard Python objects, ROS messages, etc.
  • ๐ŸŸฆ serialization: Tools for serializing / deserializing objects.
  • ๐ŸŸฉ logging: Tools for logging data streams of serializable objects.
  • ๐ŸŸฆ loops: Tools for writing timed loops.

state/: ๐Ÿ’พ Representations of state of the vehicle and its environment, including internal state that persists from step to step.

  • ๐ŸŸฉ physical_object: A generic physical object base class.
  • ๐ŸŸฉ trajectory: Stores a generic path or trajectory.
  • ๐ŸŸฉ vehicle: Ego-vehicle state.
  • ๐ŸŸจ intent: Ego-vehicle intent that may involve special logic or signaling behavior, e.g., lane change, take exit, shutting down.
  • ๐ŸŸจ roadgraph: A section of the roadmap around the ego-vehicle.
  • ๐ŸŸจ roadmap: A map created for offline use.
  • ๐ŸŸจ environment: Environmental conditions, e.g., weather, road conditions.
  • ๐ŸŸจ obstacle: A static obstacle or debris.
  • ๐ŸŸจ sign: A traffic sign.
  • ๐ŸŸจ agent: Another moving object, e.g., pedestrian, bicyclist, vehicle.
  • ๐ŸŸฉ scene: All physical items that may be relevant to the current scene, i.e., vehicle, roadgraph, environment, obstacles, and agent states.
  • ๐ŸŸจ agent_intent: Maintains an estimate of agent intent.
  • ๐ŸŸจ entity_relation: Maintains an estimate of a relationship between entities, e.g. VISIBLE, FOLLOWING, PASSING, YIELDING.
  • ๐ŸŸจ mission: Stores the current mission objective, e.g., IDLE, DRIVE_ROUTE, ESTOP, used by routing, logic, planning, and execution.
  • ๐ŸŸฉ predicates: Any items predicates that are estimated to be true in the current world.
  • ๐ŸŸฉ route: Stores a 2d route, coming from the router.
  • ๐ŸŸฉ all: State or the current scene, all intent and relation estimates, and the driving logic (objective, predicates, route).

offboard/: ๐Ÿ’ป Programs for creation and management of data and knowledge.

  • ๐ŸŸฅ calibration/: Sensor calibration.
  • ๐ŸŸฅ log_management/: Provides log management, browsing, and query functionality.
  • ๐ŸŸฅ detection_learning/: Detection model learning.
  • ๐ŸŸฅ prediction_learning/: Prediction model learning.
  • ๐ŸŸฅ heuristic_learning/: Driving heuristic learning.

knowledge/: ๐Ÿง  Models and parameters common to onboard / offboard use. The file "current.py" in each directory will store the current model being used.

  • ๐ŸŸจ vehicle/: Vehicle geometry and physics. (needs calibration and testing)
  • ๐ŸŸจ calibration/: Calibrated sensor parameters.
  • ๐ŸŸฅ detection/: Stores detection models.
  • ๐ŸŸฅ prediction/: Stores prediction models.
  • ๐ŸŸฅ heuristics/: Stores heuristic models.
  • ๐ŸŸฅ roadmaps/: Stores roadmap knowledge, e.g., lanes, regions, obstacles, signs.
  • ๐ŸŸจ routes/: Stores precomputed routes.
  • ๐ŸŸฅ predicates/: Stores named predicates that may be true in a world state.
  • ๐ŸŸฉ defaults/: Stores the default settings.

onboard/: ๐Ÿš— All algorithms governing onboard behavior are located here. These algorithms may make use of items in the knowledge/ stack.

  • perception/: Perception components.

    • ๐ŸŸจ state_estimation: State estimators.
    • ๐ŸŸจ roadgraph_update: Roadgraph updaters.
    • ๐ŸŸจ perception_normalization: Normalizes the scene before planning.
    • ๐ŸŸฅ lane_detection: Lane detection.
    • ๐ŸŸฅ sign_detection: Sign detection.
    • ๐ŸŸฅ obstacle_detection: Obstacle detction.
    • ๐ŸŸฅ agent_detection: Agent detection.
    • ๐ŸŸฅ environment_detection: Environment condition detection.
    • ๐ŸŸฅ intent_estimation: Agent intent estimation.
    • ๐ŸŸฅ relation_estimation: Entity relation estimation.
    • ๐ŸŸฅ agent_prediction: Agent motion prediction.
  • planning/: Planning components.

    • ๐ŸŸฉ route_planning: Decides which route to drive from the roadgraph.
    • ๐ŸŸฅ driving_logic: Performs all necessary logic to develop a planning problem specification, e.g., select obstacles, design cost functions, etc.
    • ๐ŸŸฅ heuristics: Implements various planning heuristics.
    • ๐ŸŸฅ motion_planning: Implements one or more motion planners.
    • ๐ŸŸฅ optimization: Implements one or more trajectory optimizers.
    • ๐ŸŸฅ selection: Implements best-trajectory selection.
    • ๐ŸŸจ pure_pursuit: Implements a pure pursuit controller. Needs some tuning.
    • ๐ŸŸฉ recovery: Implements standard recovery behavior.
  • execution/: Executes the onboard driving behavior.

    • ๐ŸŸฉ entrypoint: The entrypoint that launches all onboard behavior. Configured by settings in 'run'.
    • ๐ŸŸฉ executor: Base classes for executors.
    • ๐ŸŸฉ logging: A manager to log components / replay messages from a log.
    • ๐ŸŸจ multiprocess_execution: Component executors that work in separate process. (Stdout logging not done yet. Still hangs on exception.)
  • visualization/: Visualization components on-board the vehicle

    • ๐ŸŸจ mpl_visualization: Matplotlib visualization
    • ๐ŸŸฉ klampt_visualization: Klampt visualization
  • interface/: Defines interfaces to vehicle hardware and simulators.

    • ๐ŸŸฉ gem: Base class for the Polaris GEM e2 vehicle.
    • ๐ŸŸฉ gem_hardware: Interface to the real GEM vehicle.
    • ๐ŸŸฉ gem_simulator: Interfaces to simulated GEM vehicles.
    • ๐ŸŸฉ gem_mixed: Interfaces to the real GEM e2 vehicle's sensors but simulated motion.

Launching the stack

You will launch a simulation using:

  • python3 main.py --variant=sim launch/LAUNCH_FILE.yaml where LAUNCH_FILE.yaml is your preferred launch file. Try python3 main.py --variant=sim launch/fixed_route.yaml. Inspect the simulator classes in GEMstack/onboard/interface/gem_simulator.py for more information about configuring the simulator.

To launch onboard behavior you will open Terminator / tmux and split it into three terminal windows. In each of them run:

  • cd GEMstack
  • source catkin_ws/devel/setup.bash to get all of the appropriate ROS environment variables.

Then run:

  • (window 1) roslaunch basic_launch sensor_init.launch
  • (window 2) roslaunch basic_launch dbw_joystick.launch (TODO: switch this to dbw_no_joystick.launch)
  • (window 3) python3 main.py launch/LAUNCH_FILE.yaml where LAUNCH_FILE.yaml is your preferred launch file.

Note that if you try to use import GEMstack in a script or Jupyter notebook anywhere outside of this directory, Python will not know where the GEMstack module is. If you wish to import GEMstack from a script located in a separate directory, you can put

import sys
import os
sys.path.append(os.getcwd())   #or enter the absolute path of this directory

import GEMstack

at the top of your script. Then, you can run the script from this directory via python3 PATH/TO/SCRIPT/myscript.py. See the scripts in testing for an example of how this is done.

You can also install GEMstack into the system Python by calling pip install ., but this is not recommended because has a couple of drawbacks:

  • You might make changes in this directory, e.g., via git pull, and then forget to reinstall, so the changes won't be reflected when you run your code.
  • If you added model or roadgraph files, e.g., to the knowledge directory, they may not be installed. You will need to edit pyproject.toml to include those files.

Communication and serialization protocols

Sending commands to the vehicle is handled by the ROS-PACMOD interface. Receiving sensor messages is handled through standard ROS sensor messages.

Generally speaking, the only onboard components that should be reading from sensors are the Perception components. The only onboard components that should be sending commands to the vehicle are the Execution comopnents.

For internal state messages, which changes rapidly during development, we use raw Python dictionaries, lists, and primitives. This is also known as JSON format. The utils.serialization library makes this easy for you. We convert strings to and from Python classes that are annotated with the @dataclass decorator and you can add your own classes using the @utils.serialization.register decorator. You can then use the utils.serialization.serialize and utils.serialization.deserialize functions to convert to/from strings or ROS std_msgs/String messages.

Note that all registered class names must be unique. Also, versioning is a major problem if you wish to use legacy logs. If you gather some logs, change your class' attributes, and then attempt to view those logs again, you may encounter an error or missing data. The serialization module will do as much as it can to fail silently and enter None into missing fields, but it can still fail. If you wish to parse logs that contain legacy data, you can use the version keyword to register, as follows.

from utils.serialization import register
from dataclasses import dataclass

@dataclass
@register(name="MyClass",version="1")
class MyClass_Original:
  x : float
  y : float

@dataclass
@register(name="MyClass",version="2")
class MyClass:
  x : float
  y : float
  time : float

Keep in mind that your functions will need to distinguish between the old and new classes. It may be better in this case just to use a single class and tag time as having type Optional[float]. Then, your functions can see whether time is None, and if so, invoke the old-style behavior.

Settings

Magic parameters and global variables are a scourge and must be eliminated in production code. Instead, you will declare parameters in configuration files. In your code, you will access settings using the utils.settings module. For example,

from GEMutils.utils import settings
settings.get('key1.key2.attribute')

To override a setting temporarily (just for a few run), you can run your script with an optional --key=value command-line argument. For example, to set the simulation scene, you can use --simulator.scene=PATH/TO/SCENE/FILE.

To create new settings or override a setting more permanently, you should dive into GEMstack/knowledge/defaults/current.yaml. This YAML formatted configuration file specifies the entire configuration that can be accessed through the utils.settings module. One of these files may !include other configuration files, so if you are adding a large number of related settings, e.g., for some component module, it would make sense to create that module's own YAML file. For example, you may create a YAML file mymodule_default_config.yaml add it to current.yaml under the mymodule key, e.g., mymodule: !include mymodule_default_config.yaml. (Of course, replace mymodule with a descriptive name of your module, duh.)

Note that there are settings that configure an algorithm's behavior that persist between runs, and there are settings that configure a particular run. If you want to configure an algorithm, put it in current.yaml, a descendant configuration file, or elsewhere in knowledge. If you want to configure a single run, you should place those options in the launch file. The main.py entrypoint will consume a run launch file and a settings file, and will place all the run configurations in the run attribute of the global settings. So if you wish to inspect run details or specify per-run behavior, e.g., see whether we are in a simulation run or a hardware run, your algorithm can check settings.get('run.mode'). In general, you should try to minimize how dependent your algorithms are on run settings.

Another way to think about this is that we are trying to evolve the onboard software stack to generate better behavior by changing algorithms and their settings. The evolution mechanism is implemented by commits to the repository. On a day to day level, you will be performing different types of runs, such as simulation tests, unit tests, and full integration tests. You may be testing a lot of different conditions but the software stack should remain constant for that suite of tests. If you wish to do an apples-to-apples comparison against a different version of the stack, you should git check out another commit ID, and then perform those same tests. So if you are configuring the software stack, the setting changes should go into knowledge. If you are configuring how the software stack works just for a single test, the setting changes should go into the launch script or a keyword argument.

Launch files, pipeline state machine, and the computation graph

Onboard behavior begins by launching an executor, which maintains a pipeline state machine that can switch between different top-level behaviors. Pipelines are usually switched depending on the health state of the system, and are not appropriate for handling driving logic. For example, the recovery pipeline is a mandatory fallback pipeline in case an essential component fails on the vehicle. For most cases, drive and recovery are sufficient.

Each pipeline defines a computation graph consisting of Component subclasses (see GEMstack.onboard.component), such as state estimators, object detectors, routing, planners, etc. Each component operates in a loop on attributes of the AllState object (see GEMstack.state.allstate). Each component defines a rate at which its loop should be executed, a set of state inputs (part or all of the AllState), a set of state outputs, and initialize, update, and cleanup callbacks. The basic idea is that all components in the computation graph will be run in a loop as follows:

state = [SHARED_STATE]
component = MyComponent()
component.initialize()
for every 1/component.rate() seconds, and while still active:
    inputs = [state.X for X in component.state_inputs()]
    outputs = component.update(*inputs)
    for Y,outY in zip(component.state_outputs(),outputs)
        state.Y = outY
component.cleanup()

Creating the computation graph and customizing your component in a launch file

The computation graph defines an execution order of components and a set of allowable inputs and outputs for each component. This structure is defined in the run.computation_graph setting and by default uses GEMstack/knowledge/defaults/computation_graph.yaml.

In a launch file, you can specify a component by name, i.e.,

drive:
  planning:
    motion_planner: MyMotionPlanner

which will look for the MyMotionPlanner class in the GEMstack/onboard/planning/motion_planner.py file. You can also specify module.Class, i.e.,

drive:
  planning:
    motion_planner: my_motion_planner.MyMotionPlanner

which will look in the GEMstack/onboard/planning/my_motion_planner.py file.

You can modify how the component is constructed and run by specifying a dictionary. The valid values of this dictionary are as follows:

drive:
  planning:
    motion_planner: 
      type: my_motion_planner.MyMotionPlanner
      args: #specify a dict, or you can just specify a list of arguments, i.e., [3.0]
        some_argument: 3.0  
      rate: 10.0   #overrides MyMotionPlanner.rate() to run at 10Hz 
      print: True  #whether to include print output (default True)
      debug: True  #whether to save debug output (default True)
      multiprocess: False  #whether to use multiprocessing (default False).  Multiprocessing makes the stack run faster, but logging is not yet mature.

Variants

A launch file can contain a variants key that may specify certain changes to the launch stack that may be named via --variant=X on the command line. As an example, see launch/fixed_route.yaml. This specifies two variants, sim and log_ros which would run a simulation or log ROS topics. You can specify multiple variants on the command line using the format --variant=X,Y.

Managing and modifying state

When implementing your computation graph, you should think of AllState as a strictly typed blackboard architecture in which items can be read from and written to. If you need to pass data between components, you should add it to the state rather than use alternative techniques, e.g., global variables. This will allow the logging / replay to save and restore system state. Over a long development period, it would be best to be disciplined at versioning.

It is generally assumed that components will not maintain significant internal state. If you implement a component that does update internal state, then the executor will not be able to reproduce prior behavior from logs. This causes headaches with replay tools and A/B testing.

New pipelines

If you wish to override the executor to add more pipelines, you will need to create a new executor by subclassing from ExecutorBase. This will need to implement the pipeline switching and termination logic as detailed in the begin, update, done, and end callbacks.

Branches and submitting pull requests

To count as a contribution to the team, you will need to check in your code via pull requests (PRs). PRs should be reviewed by at least one other approver.

  • main: will contain content that persists between years. Approver: Kris Hauser.
  • s2024: is the "official class vehicle" for this semester's class. Approver: instructor, TAs.
  • s2024_groupX: will be your group's branch. Approver: instructor, TAs, team members.

Guidelines:

  • DO NOT check in large datasets. Instead, keep these around on SSDs.
  • DO check in trained models, named descriptively. In your PR, describe how you evaluated the model and its results. Choose which model you use in your tests in the settings.

About

Educational autonomous driving stack for the Polaris GEM vehicle

Topics

Resources

License

Stars

Watchers

Forks

Releases

No releases published

Packages

No packages published

Languages