Everything you require except camera positioning is in snake_visualize_jc_as.m

You will need to extract parts of the code and include to the code you
are running with ROS. Things to extract:
1) stl rendering, scaling of heart. Feel free to play with its
positioning. Instead of rendering obstacle as stl, I use one of the
snake links with bigger radius. That is faster to render.
2) Add points (zdd,yadd,zadd) to snakePoints before computing bounding
box. Line 148 describes how to use boundingbox and find volume,
edgelength etc for features and plot the same.
3) Lines 134-138 describe distance to goal and angle to goal.