Added category 3 and tested on gazebo. Next step, test on robot and mayb...

...e create a script to easily set all parameters