Forward trajectory controller

[URJala]

The controller should be able to forward a cartesian tracjectory to the robot for interpolation.
Creates errors on compilation due to multiple definition of functions, when inheriting from the joint_trajectory_controller. But looking at the passthrough controllers from ROS1 that may not be the right controller to use anyway?
All inputs are welcome.

closes #839

[RobertWilbrandt]

I cannot reproduce your build errors, are you up-to-date with the packages listed in Universal_Robots_ROS2_Driver.rolling.repos?

As for the general idea, i don't think the forwarding controller should be derived from the JTC at all as it doesn't do any spline interpolation itself. You can just derive directly from controller_interface::ControllerInterface.

The main problem we will have is encoding the trajectory in a way that it reaches the hardware interface. In ROS 1, we could just define TrajectoryInterface and use it in the hardware and controller. This is not really possible in ROS 2, as you access LoanedStateInterfaces from the controller that only give you access to one double value each. I know that some people are working on allowing more complex interface types, but am also not sure if this would help us (as we ideally want to use data that does not have a statically defined size). @fmauch do you know about the current state of these efforts?

[URJala]

I am up-to-date on everything, as far as i can tell, but I have started working while deriving from ControllerInterface and that works fine, so i will just continue on that.
Mads and i talked about making an action_server in the controller that will send over one point at a time to the robot, that way we can have a variable input, while only using the space required for one point in the state interface. This sounds like a good way of doing it to me.

[URJala]

The controller is now functional, and you should be able to test it out yourself. A user can give the controller a trajectory to forward via the action server running on the controller. It is possible to define trajectories with just positions, or with both positions and velocities and/or accelerations. The trajectory is transferred to the hardware interface via the command interfaces, before the hardware interface then transfers it to the robot.
Only one trajectory will be accepted at a time, if one is executing others will be denied.
The controller monitors the execution time of the trajectory, and gives a warning if it takes too long, although it isn't counting correctly currently (It warns too early). This has no effect on the execution of the trajectory.
When the robot driver is launched this controller is not running, and needs to be loaded/activated manually, while the scaled joint trajectory controller is unloaded, as they cannot run at the same time. The example load_switch_controller_example.py does this. The robot needs to be running while this is done, so the controller can be loaded correctly. (This should be fixed, so it doesn't need to run, but i don't know how currently.)
The examples example_movej.py and example_spline.py send a trajectory with only positions and one with positions, velocities and accelerations, respectively.
Let me know what you think of the construction of the controller and the communication between the controller and the hardware interface.

[fmauch]

This is looking very promising but I think we have to iterate this a couple of times, since this is a rather big change :-)

[VinDp]

Works nicely and looks good, I just have some minor suggestions.

[fmauch]

Approving so my previous change request gets reset.