Support Four Axes TCP Robots

[Yadunund]

This PR extends EGM support to four axis robots such as the ABB IRB910SC. It fixes #141

I've tested this with an IRB910SC robot in RobotStudio using abb_egm_rws_managers and the abb_ros2 driver from PickNik. Will be opening PRs in those repositories with supporting changes. But the changes here will need to be merged in first.

abb_libegm_scara_demo.mp4

[Yadunund]

Hmm I just noticed that the joint position read for the linear axis, ie, joint_3 is incorrect.

Here's the printout of the value of all the joint positions obtained from the incoming protobuf msg. ie _inputs.feedback().robot().joints().position().values()

Positions: [0.00409085, 0.00435426, -5.15469, 0.00300584]

The value at index 2 , -5.15469, is supposed to be the value of the linear joint in mm. However, the true value of the joint as read from the teach pendant is -90mm. So clearly there is a discrepancy.

Does this mean the EGM client on the robot is not publishing the right joint position?

[Yadunund]

Managed to overcome the issue above with 60bcc39 which updates the protobuf parsing to compensate for this error by deriving the correct value from the cartesian Z-value of the robot.

Tested this out with the same IRB910SC in RobotStudio and seems to work well. Notice how the linear axis moves this time in the robot state visualization in RViz. In the video in the PR description, the axis doesn't move due to the protobuf error in the input joint value.

abb_libegm_scara_fixed.mp4

[Yadunund]

Update: I've been using these changes to command a physical irb910sc robot for a few months without any issues.

[Yadunund]

@gavanderhoorn could you provide some feedback on this PR?

[Yadunund]

@gavanderhoorn another ping.