Fmilano/cpt pointlaser ee align laser with marker

francescomilano172 commented 3 years ago

This is just a secondary PR that mainly adds the features needed for Ardian's experiments. I would recommend reviewing https://github.com/ethz-asl/cad-percept/pull/110, then optionally this PR, and then https://github.com/ethz-asl/cad-percept/pull/101.

Details of this PR:

Adds the services needed to perform Ardian's experiments:
- Align the laser to the x axis of the building model
- Rotate pointlaser A by a certain input rotation (w.r.t. the pose after the x-axis alignment).
Adds debug features:
- Further prints during optimization
- Publish intersections of laser with building model, as obtained when adding measurements to factor graph
- Highlight intersected triangles in building model
- Update rviz format file accordingly.

ethzasl-jenkins commented 3 years ago

Test FAILed.

ethzasl-jenkins commented 3 years ago

Test FAILed.

ethz-asl / cad-percept