Fmilano/cpt pointlaser ee poses visitor

francescomilano172 commented 3 years ago

Big PR that adds all the functionalities to run the HAL routine with current setup. I consider cpt_pointlaser_loc as the base branch for the HAL routine that I would eventually like to merge to master, but I'm setting cpt_pointlaser_loc_localizer as the base branch here because there is an old PR (https://github.com/ethz-asl/cad-percept/pull/101) that is still open and which adds some basic ROS interface that has not been reviewed yet. https://github.com/ethz-asl/cad-percept/pull/101 should be nothing major (I was just porting the old repo and refactoring), and I performed all the final tests with this branch.

The README here contains the instructions to build the stack, and to set up and reproduce the test.

The main changes in this PR are:

Implementation of the interface to control the arm, in the package cpt_pointlaser_ctrl_ros
Slight refactoring: in addition to the structure described in https://github.com/ethz-asl/cad-percept/pull/101, now I added the following packages:
- cpt_pointlaser_common: contains a config file with common parameters and util functions.
- cpt_pointlaser_ctrl_ros: see above.
- cpt_pointlaser_hal: contains a node and two launch files that facilitate running the whole routine in a state-machine-like fashion (cf. README). Following the instructions in the README, the user can execute the whole routine just by writing ready multiple times to move from one step to the subsequent one.
- cpt_pointlaser_msgs: Contains all the custom-defined messages/services, so that if an external package needs to use just the latter, it doesn't have to depend on the other packages.
Slight modification to mapper in cpt_selective_icp so that the map-to-scanner transform, rather than scanner-to-map, gets appended to the TF tree. This is needed in the HAL routine so that the TF trees of the robot and of the building model are not disconnected. A parameter is added to the MapperParameters to ensure back-compatibility with the previous version of cpt_selective_icp.

NOTE (still TODO): The laser extrinsics are not the real ones, and will need to be updated when a new URDF is available. Now, the pointlaser-frame origin is assumed to coincide with the end-effector frame, and I rotated the pointlaser transforms from the previous file to make them match the orientation of the lasers with these "wrong" extrinsics.