Move the UBI palm publish to the controller publisher

[ugocupcic]

On line 161 of sr_robot_lib/src/UBI0.cpp

[guihomework]

To fix this issue, there are several ways.

1. Register an interface to the palm data storage place
   or
2. Create a handle and then register the handle
   or
3. Add the palm data to the robot_state in the actuator which is extending the current hack.

I would not go for 3), actuators should not hold the tactile data. Tactile data are different objects and should be available to ros_control in an interface. I think 2) would be the best but also requires refactoring the storage places for the incoming data.

[guihomework]

@toliver suggested to maybe use CustomHW to enclose and update a specific tactile structure.
This would separate the incoming datagram processing in hand/motor stuff and tactile, but I think the access from outside the driver (from controllers) should go with the new handle register and not again with the robotstate (currently the robot state is registered and used instead of the specific joint_interfaces)
A tactile_publisher_controller could access a tactile interface and publish it. Then new tactile controllers (tactile servoing) could also access this interface for control feedback for instance, and gain from the full speed update on this interface and close the loop at 1 kHz.

[toliver]

The idea of registering an interface sounds good. But it seems that we would need different interfaces for the different types of tactiles. Is that your idea or would you want to make a common interface for all them?

[guihomework]

I looked deeper into ros_control, and some handler exist for ForceTorque sensors, or for ImuSensors and it is said somewhere that these are generic structures to store the typical information for the type of sensor they represent but not every sensor might fill in every field.
I looked for touch or tactile data structure in ros_control or in ros in general but did not find available code doing that (I mean a driver for sensors that could use ros_control)
Except ContactState messages, that were designed for gazebo in a first place, there is not much about generic tactile in ROS as far as I have seen, only specific things for specific sensors.

So maybe we could come up with a generic storage place like the actuators, that could cover our use cases and some other sensors too (some with dense arrays and such). It could be a wider discussion even.

At least I can say that there is one problem currently with the current controllers and topics, the data in the mid_prox tactile topic cover two links that have different frames, but there is only one header in the message. So it does not handle the two middle and proximal frames. Having one frame per sensor attached to a frame is better as one could extract the normal of contact of this sensor relative to this moving frame and transform that using tf. This is really important for cartesian tactile servoing for instance.

I will try think about a generic tactile structure.