Adding CPM Access-Functions and RViz Plugin; and improve utils package for all message types

[jpbusch]

This PR includes:

• fix the inclusion of multiple access headers in one node
• adding access functions for the CPM to the utils package
• adding a RViz plugin for CPMs (ROS 2)