Use angles library for math operations

[alsora]

Is your feature request related to a problem? Please describe.
There are some operations on angles in the irobot_create_toolbox package.
Rather than implementing them here, we should rely on the angles library that is already a dependency of the project.

[ckavanagh21]

Can you be more specific about where the math regarding angles is? I want to help if I can.

[alsora]

Hi, you can find the functions here https://github.com/iRobotEducation/create3_sim/blob/main/irobot_create_common/irobot_create_toolbox/include/irobot_create_toolbox/math.hpp

[ckavanagh21]

This is what I was able to come up with. I don't know how to run C++ files, so I couldn't test it. I just looked up the syntax for the angles library and modified the code. Do you think this looks right? Also, does the polar_coordinates.hpp file have to be changed as well?

[ksatyaki]

This might be a different issue, but somewhat related.

Here The calculations for transforming a pose are complicated. and possibly wrong.
Why not simply use tf2::Transform * tf2::Transform (just multiply the transformations). We need (world to some static link). That is simply (world to floating link) * (floating link to static link). Right?