Working on rviz display