work in progress, with pose in rviz