Merge pull request #1 from RoboGroup3/master

Updates
sometimes it shows in rviz
Now it is published to visualization_marker but still not show up in rviz
Merge pull request #4 from qiqiapple/master

Now indeed published but still not visualized