Clean-up MPC

* Remove cbit route checker.

* Change corridor slider to use meters.

* Add error checking to pose graph relaxation.

* Improve pose interpolation to be in the lie algebra.

* Update ui colours to match the slider bar.

* Improve speed scheduling at the start of the path

* Modify RViz to show poses along MPC solution.

* Make all chain accessors const

* Refactor cbit to start and stop based on state machine events.

* Change the planning thread creation

* Remove duplication of getChainInfo

* Use the timestamp of the last command being sent out instead of time of point cloud.

* Add Path typename rename 2d vectors.

* Change MPC to use casadi as solver instead of STEAM

* Removing some old config parameters

* Refactor helper MPC functions into mpc_common

* Remove old speed scheduler code

* Move to fork of casadi for compilation reasons

* Make the angle_weight for pa calculation a config parameter

* Modify cmake to force a header export on every build

* Add Ouster Jackal configs + add additional visualization/publications for external navigation

* add localization index to PathInforForExternalNavigation

* Fix error in evaluating so2 error

* Change default corridor settings. Increase minimum to 20cm

* tuning changes, log addition, timestamp fix

* update jackal visualize flags

* Add profiling options.

* tune mpc params

* Modify preprocessing to reduce computation time

* Updated configs to match new MPC

* lmprove the multi-threading shutdown behaviour.

* add in angle threshold for route completion. Switch ouster jackal to cyclonedds

* Update configs to set new angle threshold

---------

Co-authored-by: James Han <jameshanjameshan@gmail.com>

Dockerfile

@@ -169,13 +169,22 @@ RUN rm libtorch.zip
 ENV TORCH_LIB=/opt/torch/libtorch
 ENV LD_LIBRARY_PATH=$TORCH_LIB/lib:${LD_LIBRARY_PATH:+:${LD_LIBRARY_PATH}}
 ENV CMAKE_PREFIX_PATH=$TORCH_LIB:$CMAKE_PREFIX_PATH
+
+RUN apt install swig liblapack-dev libmetis-dev -y -q --install-recommends
+RUN mkdir -p ${HOMEDIR}/.casadi && cd ${HOMEDIR}/.casadi \
+  && git clone https://github.com/utiasASRL/casadi.git .
+RUN cd ${HOMEDIR}/.casadi \
+  && mkdir -p build && cd build \
+  && cmake build -DWITH_PYTHON=ON -DWITH_PYTHON3=ON -DWITH_IPOPT=ON -DWITH_BUILD_IPOPT=ON -DWITH_BUILD_REQUIRED=ON -DWITH_SELFCONTAINED=ON .. \
+  && make -j${NUMPROC} install
+ENV PYTHONPATH=${PYTHONPATH}:/usr/local
+ENV LD_LIBRARY_PATH=/usr/local/casadi:${LD_LIBRARY_PATH}
+
 
 ENV NVIDIA_DRIVER_CAPABILITIES compute,utility,graphics
 
-RUN apt install htop
+RUN apt install -q -y vim htop
 
-# Install vim
-RUN apt update && apt install -q -y vim
 
 ## Switch to specified user
 USER ${USERID}:${GROUPID}