Functions that allow simple plotting to rviz from Python