tudoroancea / ihm2

Display of my skills in optimal control for autonomous race cars

💫 About Me:

🔭 🚗 I am interested in numerical computing, optimal control, robotics, machine learning, data science, and their applications to fields like autonomous driving and aerospace.
🏁 🏎️ I have spent two years working for the EPFL Racing Team Driverless and implemented optimization-based nonlinear control and estimation algorithms.
🚀 🧑‍🚀 I am currently working for the Gruyere Space Program and developing optimal-control-based safety certifications for GNC algorithms.

💻 Tech Stack:

✍️ Random Dev Quote

ihm2's Issues

Debug foxglove bridge incompatible QoS

Sometimes, QoS policy incompatibilities arise when using foxglove bridge. When it happens, we cannot visualize the corresponding data on foxglove studio, whether it is coming from a ros bag/mcap or live publishers.

My bests guess for the moment: if foxglove bridge creates a subscriber on a certain topic, it will (supposedly) check the QoS policies of the other subscribers and will use the same one. If the foxglove bridge is already running when we launch our nodes, it could start before the others and use wrong QoS profiles.

Relevant parts of foxglove bridge source code:
choice of the QoS profile and PR where it was introduced

add lap time computation in simulation node

Goal

add lap time computation capabilities to the simulation node. We could publish these lap times in the sim_node's diagnostics (the last one and the best one since the last reset).

how to properly handle the track options for the controller

Basically, each ocp solver will be generated for one track and only one track. When we switch tracks, we have to switch controllers.

• Do we pre-generate controllers for all tracks and include them all ? => to be done properly, we would have to create some enums (maybe)
• do we generate them lazily by calling a python script? => would be hard to source the right env from c++

Add noises on velocity and cones observations

For the moment add Gaussian noises

add track name publisher

Goal

pretty-self explanatory

debug reset in dyn6 sim

even when we reset the simulation node, the car is still moving (even with 0 controls!)

Add `cones_observations` publisher to `sim_node`

Goal

In a first version, we could just add the ground truth for the cones observations.
In a second version, we could add noises to these observations and add parameters to choose from several noise models (gaussian, uniform, exponential, etc.).

Implementation

Parameters

• cones_observations_freq: float
• cones_observations_range_noise_model: string
• cones_observations_bearing_noise_model: string (in "uniform", "uni","exponential","exp","normal", "gaussian")
• cones_observations_range_noise_param: float (either max value for uniform (symmetric support), 1/mean for exp, std for normal)
• cones_observations_bearing_noise_param: float

Publishers

• /ihm2/cones_observations: ihm2/msg/ConesObservations

Custom interfaces

• ihm2/mgs/ConesObservations: based on the following .msg file:

  # ConesObservations
  std_msgs/Header header
  uint8 BLUE = 0
  uint8 YELLOW = 1
  uint8 SMALL_ORANGE = 2
  uint8 BIG_ORANGE = 3
  float64[] rho
  float64[] theta
  uint8[] colors
  float64[] rho_uncertainty # std dev of the rho (range) measurement
  float64[] theta_uncertainty # std dev of the theta (bearing) measurement
  float64[] colors_confidence # confidence of the color measurement, between 0 and 1
  

add accel publisher

Goal

pretty self-explanatory

add proper dependencies (with submodules)

To be added:

• acados. Options:
  - instructions to install it in the conda env and possibly bash scripts
  - add it as git submodule and bult by colcon
• foxglove bridge -> add it as submodule that will be built by colcon
• foxglove studio -> provide download link

Implement IHM2 controller with FKin6 model

Goal

First implementation of the principal goal of this repo: develop new fancy controllers.

Implementation

For the moment, the acados ocp solver is generated for a particular track because of the splines embedded in the model. Either we find a way to provide the parameters of those splines at runtime, or we generate several controllers. Cf issue #6

Implement simple controller (PI+Stanley)

Goal

have a simple controller to quickly test the simulation

write control node

Implementation

Implement an abstract class and first close this

• #9

then if time do this (otherwise keep for v2)

• #10

Don't forget about this for the message filters to synchronize /ihm2/pose and /ihm2/velocity

Decide on whether IHM2 can be a MP or a MT

add sim solver runtime in sim node diagnostics

Create launch files

Add weight transfers and 4WD to dyn6 model