Motor speeds synchronizer/sanitizer

[petrlmat]

Subscribes motor speeds in both simulation and realworld uav. @parakhm95 does it make sense like this for you? This will allow us to have the same topic for both simulation and uav.

Simulation

• Individual motor speeds published from gazebo plugin on separate topics /uav1/motor_speed/X of type std_msgs/Float64.
• Subscribes separate motor speed topics and publishes most recent data on a merged topic /uav1/motor_speeds_sync of type mrs_mgs/Float64ArrayStamped every time any of the separate motor speed messages arrives.
• Timestamp is the same as the latest separate motor speed message received.

Realworld

• Subscribes /uav1/mavros/esc_status topic of type mavros_msgs/ESCStatus.
• The message contains speeds of all motors with separate timestamps but sometimes the motor speed value is 0, which is pretty bad for state estimation.
• The 0 values are filtered out and the previous valid measurement is used instead.
• Publishes most recent data on a merged topic /uav1/motor_speeds_sync of type mrs_mgs/Float64ArrayStamped every time any of the separate motor speed messages arrives.
• Timestamp is the average of all timestamps in the subscribed message.

[parakhm95]

I see some issues here :

• If one of the motors stops responding, we cannot really find out if that has happened in this method.
• The time would start to move slower and slower as motors disconnect and then jump around as they reconnect. We won't go back in time, it is monotonic for sure, but there would be so many jumps.
• If we want to combine these motors into a message based on the update, we should make an array that contains time stamps for each motor's last update. That would retain all the information at the expense of some bandwidth use.
• How will you be dealing with a non-responding or frequently-dropping-out motor in state estimation? It would also depend on that because if a motor has frequent dropouts due to vibrations of that ESC, I think that it would start biasing your measurements in a way that would be very difficult to detect without a timeout gating (i might be wrong). And for such a gating, you need the last timestamp of a motor's response. Do you disagree?
• The full-proof solution would be to only accept packets where all the motors have responded. But what percentage of these are fully-formed non-zero messages? This could lead to a large drop in our "real" frequency of motor feedback.
• What about re-using or forwarding the esc_status message as it is? I mean, the gazebo layer can also just transmit using the esc_status msg format right? And the real one can just be subscribed directly with only a pass-through (if you deem fit)?

[petrlmat]

I agree that the approach is not ideal and it is tailored to the way I am fusing it right now but we want to make it more general and future-proof. Probably every approach will require different drop-out handling so let's just republish the message with 0 values and let the fusion methods handle the drop-outs. I will also keep the timestamp for each motor for timestamp-based processing.

I'm not a fan of forwarding the esc_status message. All fusion methods would then have to depend on mavros. So I proposed making a new mrs msg:
ArrayFloat64Stamped.msg

std_msgs/Header header
Float64Stamped[] data

[klaxalk]

Ye, the core should not depend on Mavros or Gazebo. So I am vouching for creating the hw_api msgs. I will leave it to you.