Path planning integration testing

[AbBaSaMo]

• [ ] create, test and integrate costmap fusion in light of issues jiawei listed
• [ ] merge in nav2 stack and test in rviz2