Correct /tf calculation for base tracks

[ted-miller]

Fixes #170

Two problems:

1. The wrong parameters were being read in the lib
2. Scaling was wrong on the coordinates. For example, 1.234 m was getting converted to 1 um.

[gavanderhoorn]

I'll do a full review + test later, but as a first high-level comment: #78 includes the start of a more fleshed out M+ struct <-> ROS msg conversion library, of which MPCOORD -> geometry_msgs/Pose and geometry_msgs/Transform could be part. See this.

We could consider making that part of this PR instead: would help reduce the duplication, and reflect intent better.

(note: the referenced code obviously is also incorrect, just as the code changed/fixed by this PR, but that's a detail)

[gavanderhoorn]

I'll probably refactor this a bit so we can add tests.

That would also increase trust in these parts of the code.

[gavanderhoorn]

A quick test of the TF now being broadcast:

X axis displacement is now taken into account, so the main issue seems resolved.

We might want to open a discussion to see whether it'd be possible to include a Y and/or Z component as well (either through the config file, or via some other calibration that we may have available on the controller).

As-is, this is currently not usable by applications, unless another transform (broadcast by something else) adds the Z offset -- but making another frame parent of world feels at least conceptually 'weird'.

---

Edit: we could perhaps rename world to base_link, which would follow conventions (base_link is the start of any kinematic chain / urdf model). Together with the tf_prefix setting, this could be used to make a sane and unique TF (sub)tree.

Example: setting tf_prefix="my_robot/" would result in:

my_robot/base_link
  -> my_robot/r1/base
    -> my_robot/r1/flange
      --> my_robot/r1/tool0
      \-> my_robot/r1/tcp_0

another option could be the allow renaming all TF frames, instead of just prefixing.

[ted-miller]

We might want to open a discussion to see whether it'd be possible to include a Y and/or Z component as well (either through the config file, or via some other calibration that we may have available on the controller).

This is determined by the configuration of the robot during initial configuration.

Then the direction of travel is taken into account here:

motoros2/src/PositionMonitor.c

Lines 296 to 304 in 9492838

	switch (group->baseTrackInfo.motionType[i])
	{
	case MOTION_TYPE_X: coordTrackTravel.x = METERS_TO_MICROMETERS(track_pos_meters[i]); break;
	case MOTION_TYPE_Y: coordTrackTravel.y = METERS_TO_MICROMETERS(track_pos_meters[i]); break;
	case MOTION_TYPE_Z: coordTrackTravel.z = METERS_TO_MICROMETERS(track_pos_meters[i]); break;
	case MOTION_TYPE_RX: coordTrackTravel.rx = RAD_TO_DEG_0001(track_pos_meters[i]); break;
	case MOTION_TYPE_RY: coordTrackTravel.ry = RAD_TO_DEG_0001(track_pos_meters[i]); break;
	case MOTION_TYPE_RZ: coordTrackTravel.rz = RAD_TO_DEG_0001(track_pos_meters[i]); break;
	}

While I was debugging, I configured a RECT-XY track and did verify that I could jog along both axes.

I don't think it makes sense to make the direction "configurable" from the software side. The robot is going to be physically mounted to the track in a certain orientation. Plus we want the position reported by ROS to match what is shown on the teach pendant. (If you look at current-position on your pendant in base-frame, it will show travel along the x axis.)

As-is, this is currently not usable by applications, unless another transform (broadcast by something else) adds the Z offset

I'm not sure what you mean here. Are you referring to the height of the track relative to the floor?

The offset from the track-origin to the robot-origin is configured by Yaskawa during integration/commissioning. This is the baseTrackInfo.offsetFromBaseToRobotOrigin.

motoros2/src/PositionMonitor.c

Line 308 in 9492838

mpZYXeulerToFrame(&group->baseTrackInfo.offsetFromBaseToRobotOrigin, &frameTrackToRobot);

we could perhaps rename world to base_link, which would follow conventions

I'm going to rely on you to dictate naming conventions.

another option could be the allow renaming all TF frames, instead of just prefixing.

This seems excessive.

[gavanderhoorn]

We might want to open a discussion to see whether it'd be possible to include a Y and/or Z component as well (either through the config file, or via some other calibration that we may have available on the controller).

This is determined by the configuration of the robot during initial configuration.

Then the direction of travel is taken into account here:

motoros2/src/PositionMonitor.c

Lines 296 to 304 in 9492838

	switch (group->baseTrackInfo.motionType[i])
	{
	case MOTION_TYPE_X: coordTrackTravel.x = METERS_TO_MICROMETERS(track_pos_meters[i]); break;
	case MOTION_TYPE_Y: coordTrackTravel.y = METERS_TO_MICROMETERS(track_pos_meters[i]); break;
	case MOTION_TYPE_Z: coordTrackTravel.z = METERS_TO_MICROMETERS(track_pos_meters[i]); break;
	case MOTION_TYPE_RX: coordTrackTravel.rx = RAD_TO_DEG_0001(track_pos_meters[i]); break;
	case MOTION_TYPE_RY: coordTrackTravel.ry = RAD_TO_DEG_0001(track_pos_meters[i]); break;
	case MOTION_TYPE_RZ: coordTrackTravel.rz = RAD_TO_DEG_0001(track_pos_meters[i]); break;
	}

While I was debugging, I configured a RECT-XY track and did verify that I could jog along both axes.

See below.

I don't think it makes sense to make the direction "configurable" from the software side. The robot is going to be physically mounted to the track in a certain orientation. Plus we want the position reported by ROS to match what is shown on the teach pendant. (If you look at current-position on your pendant in base-frame, it will show travel along the x axis.)

No, that's not what I was thinking of.

And I agree with what you write.

As-is, this is currently not usable by applications, unless another transform (broadcast by something else) adds the Z offset

I'm not sure what you mean here. Are you referring to the height of the track relative to the floor?

The offset from the track-origin to the robot-origin is configured by Yaskawa during integration/commissioning. This is the baseTrackInfo.offsetFromBaseToRobotOrigin.

motoros2/src/PositionMonitor.c

Line 308 in 9492838

mpZYXeulerToFrame(&group->baseTrackInfo.offsetFromBaseToRobotOrigin, &frameTrackToRobot);

This was what I was thinking of.

It looks like this just wasn't done for my robot + track here.

So from the ROS 2 perspective, my robot is always "on the floor".

I'll just need to update that info on my controller.

we could perhaps rename world to base_link, which would follow conventions

I'm going to rely on you to dictate naming conventions.

I'll think about it a bit more, but I believe it would make sense.

world is really intended to be the singular root / origin of everything in the cell.

There's a very good chance more complex applications would have that not coincident with the world frame broadcast by MR2.

another option could be the allow renaming all TF frames, instead of just prefixing.

This seems excessive.

Users using a URDF or XACRO (from one of our description packages fi) would have that freedom though.

[gavanderhoorn]

@ted-miller: I've slightly refactored the MP_COORD to geometry_msgs/msg/Transform code (5236faf, 51afeae).

I've also added a couple of tests for the new functions, and while doing that, I've refactored some of the utility functions and put them in a separate file.

Output of the unit tests:

Performing unit tests
Testing CtrlGroup ConvertToRosPos - 6 DOF style: PASS
Testing CtrlGroup ConvertToMotoPos - 6 DOF style: PASS
Testing CtrlGroup ConvertToRosPos - Delta style: PASS
Testing CtrlGroup ConvertToMotoPos - Delta style: PASS
Testing CtrlGroup ConvertToRosPos - SIA style: PASS
Testing CtrlGroup ConvertToMotoPos - SIA style: PASS
Testing Ros_MpCoord_To_GeomMsgsPose: PASS
Testing Ros_MpCoord_To_GeomMsgsTransform: PASS
Testing SUCCESSFUL

It's just some very simple tests, but they should show the new functions are not doing anything really crazy.

I've also verified again the TF broadcast by this refactored version and everything still looks like it did in #172 (comment).

---

Seeing as I've now added commits to this PR as well: could you please review it and let me know whether you agree with the changes?

[gavanderhoorn]

we might want to start thinking about migrating to a "real" C testing library.

[ted-miller]

could you please review it and let me know whether you agree with the changes?

Everything looks good to me.

[gavanderhoorn]

Tested on a YRC1 + GP25 + TSL1000.