Merge pull request #1 from RoboGroup3/master

Updates
add node to get the costmap
Merge pull request #21 from qiqiapple/master

add node to get the costmap