Add ROS interface to `cpt_pointlaser_loc`

francescomilano172 commented 3 years ago

The structure is now the following:

cpt_pointlaser_comm: ROS-independent interface for communication with the Distobox;
cpt_pointlaser_comm_ros: ROS-wrapper around cpt_pointlaser_comm;
cpt_pointlaser_loc: ROS-independent
- localizer: handles measurements and maintains an internal optimizer that is used to obtain the base pose in the map;
- model: contains the GTSAM traits for the MeshModel, no changes in this PR;
- optimizer: contains the optimizer class, no changes in this PR.
cpt_pointlaser_loc_ros: ROS-interface for cpt_pointlaser_loc, could in theory contain a 'localizer' class for any 'arm' that one would need to use. Contains a KinovaLocalizer class that should perform highAccuracyLocalization with the Kinova arm, and a wrapping ROS node.

I could not perform the actual HAL with the KinovaLocalizer because the launch files from cpt_pointlaser_loc_ros depend on a mesh file that is not include in the original hilti_slam repo.

I instead checked that the laser measurements can be read correctly:

roslaunch cpt_pointlaser_comm_ros pointlaser_comm.launch