launch file: uav_game.launch

launch uav gurobi node.cpp executable

instantiate GurobiPlayer ego

set ROS parameters at the same time

plan_horizon = x.x       // planning time horizon in (sec)
plan_steps = x.x          // planning horizon is discretized into 10 steps  
always_relax = true

Gurobi Player

solves and publishes commanded trajectory at (ctrl_rate)

important steps in constructor:

grab all ROS parameters
setup() ego controller and opp controller
subscribe: ego state and opp state
publisher: commands (remap to command/trajectory), ego_path, opp_path, track_cubes

mainTimer: loop at ctrl_rate

egoPars.timeStep = planHorizon / static_cast<double>(egoPars.planSteps);

GurobiController* egoCtrl, oppCtrl

egoCtrl->setOpponent(oppCtrl);
oppCtrl->setOpponent(egoCtrl);

egoCtrl->setup();
oppCtrl->setup();

subscribers and publishers:

egoStateSub, oppStateSub
- egoStateCB updateInitialConstraint
  updates position for single integrator, position & velocity for double integrator
cmdPub trajectory_msgs::MultiDOFJointTrajectory
egoPathPub, oppPathPub nav_msgs::Path
visPub (track cubes)

timers

mainTimer, mainCB (at control rate)
- initialize for only once
  initialize egoState with current initEgoState and egoInput
```
egoCtrl->initialize();
oppCtrl->initialize();
```
- solve
  - update initial guess with previous solution
```
egoCtrol->updatePreviousSolution()  
oppCtrol->updatePreviousSolution()
```
  - solve game iteration
```
egoCtrl->solve()
loop for gameIters {
  oppCtrl->solve()
  ego->solve()
}
```
- publish() egoState, egoInput

egoStateCB

egoCtrl->updateInitialConstraint()

oppStateCB

oppCtrl->updateInitialConstraint()

GurobiController

decVars are GRB variables
_Cons are GRB constraints
others are eigen matrices or vectors

GurobiController Method

trackFrame()

returns the tangent, normal and (curvature) of a given point.

the center point is determined by closest point

setup()

A = eye(2), B = eye(2)*dt

decision variables decVars (2*planSteps)

set lower bound and upper bound of decision variables (what is delta & sqpMaxStep)

add track constraints, input constraints, forward direction constraints

add collCons

relax the model so its always feasible

updateInitialConstraint()

update the RHS of initCons set initEgoState, set initEgoVelocity

initialize()

egoState = egoStatePrev = initEgoState egoInputPrev = egoInput = 0

updatePreviousSolution()

update egoState and egoInput
solve()

variables

decVars 4 * planSteps decVars_coeff 6 * planSteps

constraints

initCons 4 equation constraints 2 for velocity 2 for position dirCons planSteps collCons planSteps trackCons 2 * planSteps exponential penalty curvatureCons planSteps - 1 using sqp to linearize

not member constraints: no need to update during solving velocity constraints
quadratic constraints, not relaxed acceleration constraints quadratic constraints, not relaxed continuity constraints equation constrainst between coefficient variables and state variables

game code