Revert "Code fixes in the RViz enviroment and cartesian planning"

Reverts ros-industrial-consortium/fermi#13