[Enhancement] ROS2 support

[SamoaChen]

@ChongjianYUAN Hi Chongjian, as part of my personal project, I used your code and migrated it to support ROS2. Are we currently working on official ROS2 support? If not, I'd be delighted to create pull requests to get the code blessed by you the owner, and share the blessed results with the audiences of your excellent works here

By the rules of github, comments in the issue section tend to get buried. Thus cc'ed some other MARS researchers just for viz 😊 cc: @Ecstasy-EC @Joanna-HE