Merge into Master (#69)

* Revised readme.md, created low level control dir

* fixes #16

* Updated README.md, shell script requires root

* Initial commit of control

* start future improvements lists

* fix typo

* Commit for work transfer to linux machine

* #21 start dev guide

* #9 configure usb_cam and add cv_camera, just in case

* minor change commit

* remove old version directory cv_camera

* remove old version usb_cam directory

* updated files from usb_cam

* fix cv_camera, cleanup lanes-v1

* Implemented pathHeading Calc

* cleanup and add debug code

* launch views video1, does not display image (intentinoally)

* Completed lat_controller core algo

* Imported fix to GPS error

* fixed error message. calculation likely still incorrect

* reformat and add __future__ imports

* Add debugger node to verify camera functionality

* Fix typo in docstring

* Reformat and add main() methods

* Changed exception handler style

* Add lane detection v2 source. Need to convert to python 2

* Built control core library

* Update module README with credits

* Added RAZOR 9DOF

* Attempt to fix the repo link for razor

* rm razor

* Final IMU

* Initial commit of the control node

* Advanced Lanes work in python 2

* Added Interface and Common pkg

* committing all changes

* Added three ROS nodes. Lane offset data of a video feed can now be published

* Removed swp, swn, swo files

* Converting Vec3 to vector of cluster indices

* Fixed some bugs

* Fixed bugs

* mm

* Display label instead of intesnity

* Maybe sped up gpf

* missed semicolon

* Edited params

* this code is so bad :(

* whoops

* this is getting annoying

* mm

* Hopefully fixed

* Implemented scanline wrapping

* fix bug

* mm

* Maybe fixed scanline wrapping

* moar bugs

* mm

* mm

* mmm

* ugh

* sadasdfas

* last time

* Update README.md

* Complete lane v2 node structure

* Cleaned up lane v2 nodes

* Add condition for no detected lane

* Calibrate ELP camera

* - Finished making Segmentation as stable as possible (for now).
- Improved readability
-Removed old files

* Fixed gitignore

* Delete .vscode files

* revert to old gpf code

* newest_steering_test.ino

* Fixed over-segmentation of ground-plane

* Add files via upload

Initial low-level MCU node for low-level control.

* Update redcar_control.ino

Initial low-level control code for driving and steering control.

* Add files via upload

Initial commit of Control_Adapter_Arduino_node.

* Delete low_lvl_mcu_node.py

* Change no lane reading to "nan" Float32

* temp gpf fix

* Implemented Point Filtering - NOT TESTED

* Completed Rev1 Control node w/ custom message

* Remove default test videos

* Cleanup and refresh cached files

* Added an IMU callback for control node to retrieve orientation

* Updates after testing

* #25 Add placeholder for face detector

* #25 Add initial face detector

* #25 Add haarcascade xml files

* Add updated cache files

* New point count based obstacle detection algorithm

* #25 Altered CMakeLists to integrate cpp files

* #25 Update face detector

* Update control_node.cpp

* Updated Object detection field

* Change swiftnav to output vel_down

* Fixed size_t error

* fixed range to type float

* Start launch file

* Added control launch file

* initial path interpolation file

* Added launchfile, now publishing  boolean

* edited launch file

* Fixed launch file ..again

* Finished and tested lidar code

* final parameter tweak

* Fixed timer function bug, Added obstacle detection and driving mode mechanism

* Add new arduino test code

* Update path interpolation

* Update new steering test

* Fixed path heading check issue, placed CATKIN_IGNORE in localization pkg to prevent build error

* Removed catkin_ignore from localization, planned to merge into ISS-29

* Added geodetic to ECEF conversion

* Path interpolation publishes a path message

* Set y position message to correct ouput

* Path interpolation works with global reference frame

* Add relative X and Y calculation

* Improve interpolation function

* Updated IMU Accel, Gyro Calibation

* Fixed compilation error

* Read locations from file

* Fixed missing last point corner case in #42

* Hotfix commented out x_n

* Fixed file parser #42

* Quick cleanup documentation #42

* Remove unnecessary globals

* Modularize path interpolation

* Add adaptive point density calculation

* Add steering angle conversion

* #42 debug in process for finePointsY equation

* Fixed interpolation formula #42

* Cleanup #42

* Update launch file for path interpolation

* Create geodesy package

* Started real time position output node

* Add mgrs library for UTM conversion

* Remove mgrs library

* Add geodetic to UTM calculation from utm pypy library

* Update launch file

* Add adaptive point density calc for UTM

* Reorganize data #42

* Quick cleanup #42

* Cleanup Arduino and Python code

* Cleanup Arduino Code

* fix imu config

* imu usb port change

* Changed UTM to OOP implementation

* Add OOP implementation for ECEF interpolation

* Refactor ECEF path interpolation

* Refactor UTM path interpolation

* Update .gitignore

* Start ENU converter

* Stopped tracking cache files

* Update manifest

* Add live GPS position converter

* Add velocity calculation

* Hotfix debug message

* Update geodesy launch

* Including instructions at the topic for running code

* Including instructions at the topic for running code

* Added heading estimation into path cb

* Fixed small struct bug

* Fix for reseting prev_pos, added control debug layer

* Added control files

* Small update

* Tuned PID parameters for 12 Volts and added timeout safety features

* fixed rc, fixed control launch to include python node

* Added rosserial launch

* Added 2 new Paths

* Fixed some low lvl python logic

* removed old python low lvl node

* launch files for path

* updated joy installation

* pip install

* control bug fix

* updated path

* Obstacle detection fix

* changed mitchell's code to reduce buffer size

* Control Bug Fix

* New student union path

* Most updated Arduino code, tested some filtering at idle to lower oscillation

* Control update

* Arduino Fix

* Preliminary error confirmation

* Add point converter #53

* Fixed ENU data to ECEF data bug #53

* Update formatting for debugging #53

* Updated ardu_adapter.py and newest_steering_test.ino to remove magic numbers

* Forgot an indent

* working low level control code

* Controller logic fix, fit was having error in having duplicated points

* Start tf master node

* Small fix

* Fixed the wrapping angle issue where i forgot to remap the output to the right variable

* Least squares gpf method w/ Eigen

* Kalman Filter Core function implementationn

* Fixed eigen compile errors

* Update IMU message published

* Move validation data to appropriate location

* Note future TF msg improvements

* Implemented SVD approach to groun plane fitting (NOT TESTED YET)

* Fixed Eigen compilation bug

* Verified that new CalculatePlaneNormal works as desired

* fixed compilation bugs

* Added include guards to all header files, tweaked gpf parameters

* fixed z_min error

* Gpf working commit

* Update initial framework messages for tf

* Update executable target

* IMU Calibration, Control node update

* Removed libuvc from util