ROS wrapper for costmap supports 3D costmap