Mutable joint state node/publishing should be replaced with single transform publisher about industrial_calibration

[shaun-edwards]

@geoffreychiou, could you take a look at this.

[gavanderhoorn]

@shaun-edwards wrote:

Manually tweaking a transform (possibly replaced with this)

Perhaps rqt_frame_editor_plugin is a bit nicer?

[drchrislewis]

It would be straight forward to add the capability to mutable joint state publisher to simply parse and publish transforms in xyz angle axis, xyzrpy or homogeneous transforms. The critical aspect is that the calibration data needs to be saved in a file, read at start up, and modifiable through a service call. There is already a yaml transform parser in industrial_extrinisc_cal. I'd probably copy M.T.P. and make a Mutable Transform Publisher node or nodes for each type.

[gavanderhoorn]

Would it be an idea to propose something like a dynamic_transform_publisher (as a companion for the static_transform_publisher) for inclusion in the tf and / or tf2 packages?

That would remove some of our maintenance burden while also covering a use-case which I think many outside the users of industrial_calibration would seem to have.

[Jmeyer1292]

Worth throwing out that I'm not sure if moveit supports floating transforms anywhere but the base of the robot. In the past, this was the case (I think). This caused serious practical issues for many of our applications.

[shaun-edwards]

Thanks for bringing this up @Jmeyer1292. Does this only matter for end-effector mounted cameras? What about static cameras?

[Jmeyer1292]

I don't know. I'd have to look.

[AustinDeric]

@shaun-edwards @Jmeyer1292 @gavanderhoorn @drchrislewis @geoffreychiou
We are rapidly approaching the point in our revamp of this repo that we need a decision made about this. Let me summarize the ideas in this thread:

• rqt_frame_editor_plugin
• dynamic_transform_publisher

Both these ideas revolve around publishing new a new series of tf. Jonathan brought up that moveit does not accept floating transforms.

Would an option be to update the /robot_description parameter directly? This would flow through to the robot_state_publisher. Another benefit of this approach is that we could then save off this parameter as a 'calibrated' URDF.

If this or any of the above approaches are not acceptable, then it may be nessesary to agree on a short-term solution.

Here is the latest effort: https://github.com/geoffreychiou/IC2
In depth conversation can be had with the dev-team at: https://rosindustrial.slack.com

Thank you,
-Austin

[gavanderhoorn]

@AustinDeric wrote:

Would an option be to update the /robot_description parameter directly? This would flow through to the robot_state_publisher. Another benefit of this approach is that we could then save off this parameter as a 'calibrated' URDF.

I think you'll have a hard time getting that to work robustly. Nothing in ROS is written to expect robot_description to ever change, so it's typically only parsed once -- at init -- and then never looked at again.

Parsing it only once would mean that the application would have to be restarted, which I believe was considered unacceptable and which was what prompted the creation of the mutable joint state publisher in the first place.

[shaun-edwards]

I would suggest you create a dynamic_transform_publisher, as this is the cleanest solution. You could support both options and then allow the user to decide which one to use.

[gavanderhoorn]

@shaun-edwards: would the issue with floating transforms in MoveIt that @Jmeyer1292 mentioned not be a problem?

[shaun-edwards]

No. It has not been fixed as far as I know.

[johnwason]

I ran into a similar problem for the RPI ARM project and put together a minimalist publisher for floating transforms. It will read the robot_description URDF and publish the "floating" joints it finds in the description. There is an "update_floating_transform" service to modify the transform as needed.

https://github.com/rpiRobotics/floating_transform_publisher

[shaun-edwards]

@johnwason, thanks for this. I wasn't aware that you were looking at the industrial calibration library. I'll point some of the Plus One developers at your package.

[johnwason]

@shaun-edwards we are not currently using the industrial calibration library, but we are looking into it. Right now we are using some MATLAB scripts to calibrate the camera. I wanted the ability to move around frames in our testbed model dynamically, and came across this thread during my prior art search. The current implementation is simple but does what we need.

[Jmeyer1292]

I also made something similar for my uses. Does not parse a URDF though.

https://github.com/Jmeyer1292/mutable_transform_publisher

[drchrislewis]

@shaun-edwards @Jmeyer1292 @johnwason
First, does MoveIt! now support floating transforms? If so, then I'm all for switching over to a floating_tranform_publisher or mutable_transform_publisher.
Two questions.

1. Do we want to add it as a package to industrial_calibration, or just have industrial_calibration depend on a separate package?
   2.Does anyone have any specific functional requirements that I'm unaware of?
   a. I require that it has services to save the results to a file, and restore those results from a file on startup.
   b. I'd like it provide services for get and set functions compatible with TF, Ceres-solver, bullet, and Eigen
   (The only reason I created my own Pose6d class was to provide conversions between all these interfaces).
   Anything else?

Please consider that the next order of business for industrial calibration is to add kinematic calibration. Typically, this may only adjust the DH parameters for each link. In this case, mutable_joint_states may be ok for times like this where at most 4 parameters are changing for the transform, not all 6.

[johnwason]

@drchrislewis It appears that MoveIt! does not handle the floating joints correctly. In the startup messages it says that the floating joints are being converted to fixed joints. When I then give it a different transform using the floating_transform_publisher, it adds the new transform relative to the URDF joint origin instead of using the new relative to the parent frame. This is incorrect behavior, as TF specifies the transform relative to the parent frame with no regard to the URDF joint origin. It also appears that MoveIt! will not update the parent frame properly if it is changed by the floating joint.

[drchrislewis]

@johnwason @shaun-edwards I vaguely recall Shaun mentioning that it had been fixed. Is there a newer version of MoveIt!?
The main drawback of the mutable_joint_state publisher is that it requires a fairly complex change the URDF which we solve by including and calling "calibration_transform_macro.xacro" defined in the industrial_extrinsic_cal/urdf directory.
We require being able to get, set, and store results to a file. Does anyone think we need to include a restore() service to get back to the original launch values?

[johnwason]

@drchrislewis @shaun-edwards I am using MoveIt! packages version 0.9.12-1xenial-20180530. I am not familiar enough with the guts of MoveIt! to know where the change would have been made.

[johnwason]

I implemented /reset_floating_transform as suggested by @drchrislewis.

I am having another weird problem with MoveIt! Now when I publish the floating transforms with updated stamp (I omitted this update previously) the robot is blinking back between its default home position and the current position from the robot controller. It appears that it is assuming that the pose is missing, since the TF message from floating_transform_publisher does not contain the link poses. Those come from robot_state_publisher. It appears that MoveIt! is not correctly merging the different messages.

Nevermind, I had an error in my configuration causing an extra joint_publisher to be running. The robot flickering was not an error in MoveIt!